Merge 8d9ffbc into a71a93e

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,14 +106,25 @@ 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.
  """
 
  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:

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"""

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}")