diff --git a/cadquery/assembly.py b/cadquery/assembly.py index cfe3537c8..60c2d42b6 100644 --- a/cadquery/assembly.py +++ b/cadquery/assembly.py @@ -4,8 +4,8 @@ from uuid import uuid1 as uuid from .cq import Workplane -from .occ_impl.shapes import Shape, Face, Edge -from .occ_impl.geom import Location, Vector +from .occ_impl.shapes import Shape, Face, Edge, Wire +from .occ_impl.geom import Location, Vector, Plane from .occ_impl.assembly import Color from .occ_impl.solver import ( ConstraintSolver, @@ -18,7 +18,7 @@ # type definitions AssemblyObjects = Union[Shape, Workplane, None] -ConstraintKinds = Literal["Plane", "Point", "Axis"] +ConstraintKinds = Literal["Plane", "Point", "Axis", "PointInPlane"] ExportLiterals = Literal["STEP", "XML"] PATH_DELIM = "/" @@ -79,7 +79,7 @@ def __init__( ): """ Construct a constraint. - + :param objects: object names refernced in the constraint :param args: subshapes (e.g. faces or edges) of the objects :param sublocs: locations of the objects (only relevant if the objects are nested in a sub-assembly) @@ -106,6 +106,17 @@ def _getAxis(self, arg: Shape) -> Vector: return rv + def _getPlane(self, arg: Shape) -> Plane: + + if isinstance(arg, Face): + normal = arg.normalAt() + elif isinstance(arg, (Edge, Wire)): + normal = arg.normal() + else: + raise ValueError(f"Can not get normal from {arg}.") + origin = arg.Center() + return Plane(origin, normal=normal) + def toPOD(self) -> ConstraintPOD: """ Convert the constraint to a representation used by the solver. @@ -113,7 +124,7 @@ def toPOD(self) -> ConstraintPOD: rv: List[Tuple[ConstraintMarker, ...]] = [] - for arg, loc in zip(self.args, self.sublocs): + for idx, (arg, loc) in enumerate(zip(self.args, self.sublocs)): arg = arg.located(loc * arg.location()) @@ -123,6 +134,11 @@ def toPOD(self) -> ConstraintPOD: rv.append((arg.Center().toPnt(),)) elif self.kind == "Plane": rv.append((self._getAxis(arg).toDir(), arg.Center().toPnt())) + elif self.kind == "PointInPlane": + if idx == 0: + rv.append((arg.Center().toPnt(),)) + else: + rv.append((self._getPlane(arg).toPln(),)) else: raise ValueError(f"Unknown constraint kind {self.kind}") @@ -157,22 +173,22 @@ def __init__( """ construct an assembly - :param obj: root object of the assembly (deafault: None) - :param loc: location of the root object (deafault: None, interpreted as identity transformation) + :param obj: root object of the assembly (default: None) + :param loc: location of the root object (default: None, interpreted as identity transformation) :param name: unique name of the root object (default: None, reasulting in an UUID being generated) :param color: color of the added object (default: None) :return: An Assembly object. - - + + To create an empty assembly use:: - + assy = Assembly(None) - + To create one constraint a root object:: - + b = Workplane().box(1,1,1) assy = Assembly(b, Location(Vector(0,0,1)), name="root") - + """ self.obj = obj @@ -211,12 +227,15 @@ def add( color: Optional[Color] = None, ) -> "Assembly": """ - add a subassembly to the current assembly. - + Add a subassembly to the current assembly. + :param obj: subassembly to be added - :param loc: location of the root object (deafault: None, resulting in the location stored in the subassembly being used) - :param name: unique name of the root object (default: None, resulting in the name stored in the subassembly being used) - :param color: color of the added object (default: None, resulting in the color stored in the subassembly being used) + :param loc: location of the root object (default: None, resulting in the location stored in + the subassembly being used) + :param name: unique name of the root object (default: None, resulting in the name stored in + the subassembly being used) + :param color: color of the added object (default: None, resulting in the color stored in the + subassembly being used) """ ... @@ -229,18 +248,20 @@ def add( color: Optional[Color] = None, ) -> "Assembly": """ - add a subassembly to the current assembly with explicit location and name - + Add a subassembly to the current assembly with explicit location and name. + :param obj: object to be added as a subassembly - :param loc: location of the root object (deafault: None, interpreted as identity transformation) - :param name: unique name of the root object (default: None, resulting in an UUID being generated) + :param loc: location of the root object (default: None, interpreted as identity + transformation) + :param name: unique name of the root object (default: None, resulting in an UUID being + generated) :param color: color of the added object (default: None) """ ... def add(self, arg, **kwargs): """ - add a subassembly to the current assembly. + Add a subassembly to the current assembly. """ if isinstance(arg, Assembly): @@ -270,18 +291,18 @@ def add(self, arg, **kwargs): def _query(self, q: str) -> Tuple[str, Optional[Shape]]: """ - Execute a selector query on the assembly. + Execute a selector query on the assembly. The query is expected to be in the following format: - + name[?tag][@kind@args] - + valid example include: - + obj_name @ faces @ >Z - obj_name?tag1@faces@>Z + obj_name?tag1@faces@>Z obj_name ? tag obj_name - + """ tmp: Workplane @@ -311,7 +332,7 @@ def _query(self, q: str) -> Tuple[str, Optional[Shape]]: def _subloc(self, name: str) -> Tuple[Location, str]: """ Calculate relative location of an object in a subassembly. - + Returns the relative posiitons as well as the name of the top assembly. """ @@ -422,9 +443,9 @@ def save( ) -> "Assembly": """ save as STEP or OCCT native XML file - + :param path: filepath - :param exportType: export format (deafault: None, results in format being inferred form the path) + :param exportType: export format (default: None, results in format being inferred form the path) """ if exportType is None: diff --git a/cadquery/occ_impl/geom.py b/cadquery/occ_impl/geom.py index 018d46b35..a1334f850 100644 --- a/cadquery/occ_impl/geom.py +++ b/cadquery/occ_impl/geom.py @@ -2,7 +2,18 @@ from typing import overload, Sequence, Union, Tuple, Type, Optional -from OCP.gp import gp_Vec, gp_Ax1, gp_Ax3, gp_Pnt, gp_Dir, gp_Trsf, gp_GTrsf, gp, gp_XYZ +from OCP.gp import ( + gp_Vec, + gp_Ax1, + gp_Ax3, + gp_Pnt, + gp_Dir, + gp_Pln, + gp_Trsf, + gp_GTrsf, + gp_XYZ, + gp, +) from OCP.Bnd import Bnd_Box from OCP.BRepBndLib import BRepBndLib from OCP.BRepMesh import BRepMesh_IncrementalMesh @@ -500,30 +511,35 @@ def bottom(cls, origin=(0, 0, 0), xDir=Vector(1, 0, 0)): plane._setPlaneDir(xDir) return plane - def __init__(self, origin, xDir, normal): - """Create a Plane with an arbitrary orientation - - TODO: project x and y vectors so they work even if not orthogonal - :param origin: the origin - :type origin: a three-tuple of the origin, in global coordinates - :param xDir: a vector representing the xDirection. - :type xDir: a three-tuple representing a vector, or a FreeCAD Vector - :param normal: the normal direction for the new plane - :type normal: a FreeCAD Vector - :raises: ValueError if the specified xDir is not orthogonal to the provided normal. - :return: a plane in the global space, with the xDirection of the plane in the specified direction. + def __init__( + self, + origin: Union[Tuple[float, float, float], Vector], + xDir: Optional[Union[Tuple[float, float, float], Vector]] = None, + normal: Union[Tuple[float, float, float], Vector] = (0, 0, 1), + ): + """ + Create a Plane with an arbitrary orientation + + :param origin: the origin in global coordinates + :param xDir: an optional vector representing the xDirection. + :param normal: the normal direction for the plane + :raises ValueError: if the specified xDir is not orthogonal to the provided normal """ zDir = Vector(normal) if zDir.Length == 0.0: raise ValueError("normal should be non null") - xDir = Vector(xDir) - if xDir.Length == 0.0: - raise ValueError("xDir should be non null") - self.zDir = zDir.normalized() + + if xDir is None: + ax3 = gp_Ax3(Vector(origin).toPnt(), Vector(normal).toDir()) + xDir = Vector(ax3.XDirection()) + else: + xDir = Vector(xDir) + if xDir.Length == 0.0: + raise ValueError("xDir should be non null") self._setPlaneDir(xDir) - self.origin = origin + self.origin = Vector(origin) def _eq_iter(self, other): """Iterator to successively test equality""" @@ -725,6 +741,10 @@ def location(self) -> "Location": return Location(self) + def toPln(self) -> gp_Pln: + + return gp_Pln(gp_Ax3(self.origin.toPnt(), self.zDir.toDir(), self.xDir.toDir())) + class BoundBox(object): """A BoundingBox for an object or set of objects. Wraps the OCP one""" diff --git a/cadquery/occ_impl/solver.py b/cadquery/occ_impl/solver.py index 66961c06d..f5850c18e 100644 --- a/cadquery/occ_impl/solver.py +++ b/cadquery/occ_impl/solver.py @@ -4,12 +4,12 @@ from numpy import array, eye, zeros, pi from scipy.optimize import minimize -from OCP.gp import gp_Vec, gp_Dir, gp_Pnt, gp_Trsf, gp_Quaternion +from OCP.gp import gp_Vec, gp_Pln, gp_Lin, gp_Dir, gp_Pnt, gp_Trsf, gp_Quaternion from .geom import Location DOF6 = Tuple[float, float, float, float, float, float] -ConstraintMarker = Union[gp_Dir, gp_Pnt] +ConstraintMarker = Union[gp_Pln, gp_Dir, gp_Pnt] Constraint = Tuple[ Tuple[ConstraintMarker, ...], Tuple[Optional[ConstraintMarker], ...], Optional[Any] ] @@ -117,7 +117,25 @@ def dir_cost( DIR_SCALING * (val - m1.Transformed(t1).Angle(m2.Transformed(t2))) ** 2 ) + def pnt_pln_cost( + m1: gp_Pnt, + m2: gp_Pln, + t1: gp_Trsf, + t2: gp_Trsf, + val: Optional[float] = None, + ) -> float: + + val = 0 if val is None else val + + m2_located = m2.Transformed(t2) + # offset in the plane's normal direction by val: + m2_located.Translate(gp_Vec(m2_located.Axis().Direction()).Multiplied(val)) + return m2_located.SquareDistance(m1.Transformed(t1)) + def f(x): + """ + Function to be minimized + """ constraints = self.constraints ne = self.ne @@ -133,10 +151,12 @@ def f(x): t2 = transforms[k2] if k2 not in self.locked else gp_Trsf() for m1, m2 in zip(ms1, ms2): - if isinstance(m1, gp_Pnt): + if isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pnt): rv += pt_cost(m1, m2, t1, t2, d) elif isinstance(m1, gp_Dir): rv += dir_cost(m1, m2, t1, t2, d) + elif isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pln): + rv += pnt_pln_cost(m1, m2, t1, t2, d) else: raise NotImplementedError(f"{m1,m2}") @@ -166,7 +186,7 @@ def jac(x): t2 = transforms[k2] if k2 not in self.locked else gp_Trsf() for m1, m2 in zip(ms1, ms2): - if isinstance(m1, gp_Pnt): + if isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pnt): tmp = pt_cost(m1, m2, t1, t2, d) for j in range(NDOF): @@ -197,6 +217,22 @@ def jac(x): if k2 not in self.locked: tmp2 = dir_cost(m1, m2, t1, t2j, d) rv[k2 * NDOF + j] += (tmp2 - tmp) / DIFF_EPS + + elif isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pln): + tmp = pnt_pln_cost(m1, m2, t1, t2, d) + + for j in range(NDOF): + + t1j = transforms_delta[k1 * NDOF + j] + t2j = transforms_delta[k2 * NDOF + j] + + if k1 not in self.locked: + tmp1 = pnt_pln_cost(m1, m2, t1j, t2, d) + rv[k1 * NDOF + j] += (tmp1 - tmp) / DIFF_EPS + + if k2 not in self.locked: + tmp2 = pnt_pln_cost(m1, m2, t1, t2j, d) + rv[k2 * NDOF + j] += (tmp2 - tmp) / DIFF_EPS else: raise NotImplementedError(f"{m1,m2}") diff --git a/tests/test_assembly.py b/tests/test_assembly.py index f88c23c8d..fa06a403a 100644 --- a/tests/test_assembly.py +++ b/tests/test_assembly.py @@ -1,5 +1,6 @@ import pytest import os +from itertools import product import cadquery as cq from cadquery.occ_impl.exporters.assembly import exportAssembly, exportCAF @@ -43,6 +44,16 @@ def nested_assy(): return assy +@pytest.fixture +def box_and_vertex(): + + box_wp = cq.Workplane().box(1, 2, 3) + assy = cq.Assembly(box_wp, name="box") + vertex_wp = cq.Workplane().newObject([cq.Vertex.makeVertex(0, 0, 0)]) + assy.add(vertex_wp, name="vertex") + return assy + + def test_color(): c1 = cq.Color("red") @@ -225,3 +236,158 @@ def test_expression_grammar(nested_assy): nested_assy.constrain( "TOP@faces@>Z", "SECOND/BOTTOM@vertices@>X and >Y and >Z", "Point" ) + + +def test_PointInPlane_constraint(box_and_vertex): + + # add first constraint + box_and_vertex.constrain( + "vertex", + box_and_vertex.children[0].obj.val(), + "box", + box_and_vertex.obj.faces(">X").val(), + "PointInPlane", + param=0, + ) + box_and_vertex.solve() + + x_pos = ( + box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart().X() + ) + assert x_pos == pytest.approx(0.5) + + # add a second PointInPlane constraint + box_and_vertex.constrain("vertex", "box@faces@>Y", "PointInPlane", param=0) + box_and_vertex.solve() + + vertex_translation_part = ( + box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart() + ) + # should still be on the >X face from the first constraint + assert vertex_translation_part.X() == pytest.approx(0.5) + # now should additionally be on the >Y face + assert vertex_translation_part.Y() == pytest.approx(1) + + # add a third PointInPlane constraint + box_and_vertex.constrain("vertex", "box@faces@>Z", "PointInPlane", param=0) + box_and_vertex.solve() + + # should now be on the >X and >Y and >Z corner + assert ( + box_and_vertex.children[0] + .loc.wrapped.Transformation() + .TranslationPart() + .IsEqual(gp_XYZ(0.5, 1, 1.5), 1e-6) + ) + + +def test_PointInPlane_3_parts(box_and_vertex): + + cylinder_height = 2 + cylinder = cq.Workplane().circle(0.1).extrude(cylinder_height) + box_and_vertex.add(cylinder, name="cylinder") + box_and_vertex.constrain("box@faces@>Z", "cylinder@faces@Z", "PointInPlane") + box_and_vertex.constrain("vertex", "box@faces@>X", "PointInPlane") + box_and_vertex.solve() + vertex_translation_part = ( + box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart() + ) + assert vertex_translation_part.Z() == pytest.approx(1.5 + cylinder_height) + assert vertex_translation_part.X() == pytest.approx(0.5) + + +@pytest.mark.parametrize("param1", [-1, 0, 2]) +@pytest.mark.parametrize("param0", [-2, 0, 0.01]) +def test_PointInPlane_param(box_and_vertex, param0, param1): + + box_and_vertex.constrain("vertex", "box@faces@>Z", "PointInPlane", param=param0) + box_and_vertex.constrain("vertex", "box@faces@>X", "PointInPlane", param=param1) + box_and_vertex.solve() + + vertex_translation_part = ( + box_and_vertex.children[0].loc.wrapped.Transformation().TranslationPart() + ) + assert vertex_translation_part.Z() - 1.5 == pytest.approx(param0, abs=1e-6) + assert vertex_translation_part.X() - 0.5 == pytest.approx(param1, abs=1e-6) + + +def test_constraint_getPlane(): + """ + Test that _getPlane does the right thing with different arguments + """ + ids = (0, 1) + sublocs = (cq.Location(), cq.Location()) + + def make_constraint(shape0): + return cq.Constraint(ids, (shape0, shape0), sublocs, "PointInPlane", 0) + + def fail_this(shape0): + c0 = make_constraint(shape0) + with pytest.raises(ValueError): + c0._getPlane(c0.args[0]) + + def resulting_plane(shape0): + c0 = make_constraint(shape0) + return c0._getPlane(c0.args[0]) + + # point should fail + fail_this(cq.Vertex.makeVertex(0, 0, 0)) + + # line should fail + fail_this(cq.Edge.makeLine(cq.Vector(1, 0, 0), cq.Vector(0, 0, 0))) + + # planar edge (circle) should succeed + origin = cq.Vector(1, 2, 3) + direction = cq.Vector(4, 5, 6).normalized() + p1 = resulting_plane(cq.Edge.makeCircle(1, pnt=origin, dir=direction)) + assert p1.zDir == direction + assert p1.origin == origin + + # planar edge (spline) should succeed + # it's a touch risky calling a spline a planar edge, but lets see if it's within tolerance + points0 = [cq.Vector(x) for x in [(-1, 0, 1), (0, 1, 1), (1, 0, 1), (0, -1, 1)]] + planar_spline = cq.Edge.makeSpline(points0, periodic=True) + p2 = resulting_plane(planar_spline) + assert p2.origin == planar_spline.Center() + assert p2.zDir == cq.Vector(0, 0, 1) + + # non-planar edge should fail + points1 = [cq.Vector(x) for x in [(-1, 0, -1), (0, 1, 1), (1, 0, -1), (0, -1, 1)]] + nonplanar_spline = cq.Edge.makeSpline(points1, periodic=True) + fail_this(nonplanar_spline) + + # planar wire should succeed + # make a triangle in the XZ plane + points2 = [cq.Vector(x) for x in [(-1, 0, -1), (0, 0, 1), (1, 0, -1)]] + points2.append(points2[0]) + triangle = cq.Wire.makePolygon(points2) + p3 = resulting_plane(triangle) + assert p3.origin == triangle.Center() + assert p3.zDir == cq.Vector(0, 1, 0) + + # non-planar wire should fail + points3 = [cq.Vector(x) for x in [(-1, 0, -1), (0, 1, 1), (1, 0, 0), (0, -1, 1)]] + wonky_shape = cq.Wire.makePolygon(points3) + fail_this(wonky_shape) + + # all faces should succeed + for length, width in product([None, 10], [None, 11]): + f0 = cq.Face.makePlane( + length=length, width=width, basePnt=(1, 2, 3), dir=(1, 0, 0) + ) + p4 = resulting_plane(f0) + if length and width: + assert p4.origin == cq.Vector(1, 2, 3) + assert p4.zDir == cq.Vector(1, 0, 0) + + f1 = cq.Face.makeFromWires(triangle, []) + p5 = resulting_plane(f1) + # not sure why, but the origins only roughly line up + assert (p5.origin - triangle.Center()).Length < 0.1 + assert p5.zDir == cq.Vector(0, 1, 0) + + # shell... not sure? + + # solid should fail + fail_this(cq.Solid.makeBox(1, 1, 1)) diff --git a/tests/test_cadquery.py b/tests/test_cadquery.py index 1c5480adb..55f3b7091 100644 --- a/tests/test_cadquery.py +++ b/tests/test_cadquery.py @@ -378,6 +378,27 @@ def testPlaneRotateConcatRandom(self): assert before[direction] == approx(after[direction]) assert plane.origin.toTuple() == origin + def testPlaneNoXDir(self): + """ + Plane should pick an arbitrary x direction if None is passed in. + """ + for z_dir in [(0, 0, 1), (1, 0, 0), (-1, 0, 0), Vector(-1, 0, 0)]: + result = Plane(origin=(1, 2, 3), xDir=None, normal=z_dir) + assert result.zDir == Vector(z_dir) + assert result.xDir.Length == approx(1) + assert result.origin == Vector(1, 2, 3) + + # unspecified xDir should be the same as xDir=None + result2 = Plane(origin=(1, 2, 3), normal=z_dir) + assert result2 == result + + def testPlaneToPln(self): + plane = Plane(origin=(1, 2, 3), xDir=(-1, 0, 0), normal=(0, 1, 0)) + gppln = plane.toPln() + assert Vector(gppln.XAxis().Direction()) == Vector(-1, 0, 0) + assert Vector(gppln.YAxis().Direction()) == plane.yDir + assert Vector(gppln.Axis().Direction()) == plane.zDir + def testRect(self): x = 10 y = 11