diff --git a/cadquery/assembly.py b/cadquery/assembly.py index 510ce6f5e..830d43065 100644 --- a/cadquery/assembly.py +++ b/cadquery/assembly.py @@ -357,13 +357,16 @@ def solve(self) -> "Assembly": ents = {} i = 0 - locked = [] + locked: List[int] = [] + for c in self.constraints: for name in c.objects: if name not in ents: ents[name] = i i += 1 - if c.kind == "Fixed" or name == self.name: + if (c.kind == "Fixed" or name == self.name) and ents[ + name + ] not in locked: locked.append(ents[name]) # Lock the first occuring entity if needed. @@ -402,15 +405,32 @@ def solve(self) -> "Assembly": if not constraints: raise ValueError("At least one constraint required") + # check if at least two entities are present + if len(ents) < 2: + raise ValueError("At least two entities need to be constrained") + # instantiate the solver - solver = ConstraintSolver(locs, constraints, locked=locked) + scale = self.toCompound().BoundingBox().DiagonalLength + solver = ConstraintSolver(locs, constraints, locked=locked, scale=scale) # solve locs_new, self._solve_result = solver.solve() # update positions + + # find the inverse root loc + loc_root_inv = Location() + + if self.obj: + for loc_new, n in zip(locs_new, ents): + if n == self.name: + loc_root_inv = loc_new.inverse + break + + # update the positions for loc_new, n in zip(locs_new, ents): - self.objects[n].loc = loc_new + if n != self.name: + self.objects[n].loc = loc_root_inv * loc_new return self diff --git a/cadquery/occ_impl/geom.py b/cadquery/occ_impl/geom.py index 0ad3da5a9..7b997442c 100644 --- a/cadquery/occ_impl/geom.py +++ b/cadquery/occ_impl/geom.py @@ -790,6 +790,9 @@ class BoundBox(object): zmax: float zlen: float + center: Vector + DiagonalLength: float + def __init__(self, bb: Bnd_Box) -> None: self.wrapped = bb XMin, YMin, ZMin, XMax, YMax, ZMax = bb.Get() diff --git a/cadquery/occ_impl/solver.py b/cadquery/occ_impl/solver.py index 3188e1f7b..2c43d79f9 100644 --- a/cadquery/occ_impl/solver.py +++ b/cadquery/occ_impl/solver.py @@ -10,12 +10,11 @@ cast as tcast, Type, ) -from nptyping import NDArray as Array -from nptyping import Float -from math import radians + +from math import radians, pi from typish import instance_of, get_type -from numpy import array, eye, pi -import nlopt + +import casadi as ca from OCP.gp import ( gp_Vec, @@ -24,10 +23,10 @@ gp_Pnt, gp_Trsf, gp_Quaternion, - gp_XYZ, gp_Lin, - gp_Intrinsic_XYZ, + gp_Extrinsic_XYZ, ) + from OCP.BRepTools import BRepTools from OCP.Precision import Precision @@ -39,12 +38,10 @@ NoneType = type(None) -DOF6 = Tuple[float, float, float, float, float, float] +DOF6 = Tuple[Tuple[float, float, float], Tuple[float, float, float]] ConstraintMarker = Union[gp_Pln, gp_Dir, gp_Pnt, gp_Lin, None] -UnaryConstraintKind = Literal[ - "Fixed", "FixedPoint", "FixedAxis", "FixedRotation", "FixedRotationAxis" -] +UnaryConstraintKind = Literal["Fixed", "FixedPoint", "FixedAxis", "FixedRotation"] BinaryConstraintKind = Literal["Plane", "Point", "Axis", "PointInPlane", "PointOnLine"] ConstraintKind = Literal[ "Plane", @@ -56,7 +53,6 @@ "FixedAxis", "PointOnLine", "FixedRotation", - "FixedRotationAxis", ] # (arity, marker types, param type, conversion func) @@ -73,11 +69,11 @@ "Fixed": (1, (None,), Type[None], None), "FixedPoint": (1, (gp_Pnt,), Tuple[Real, Real, Real], None), "FixedAxis": (1, (gp_Dir,), Tuple[Real, Real, Real], None), - "FixedRotationAxis": ( + "FixedRotation": ( 1, (None,), - Tuple[int, Real], - lambda x: (x[0], radians(x[1])), + Tuple[Real, Real, Real], + lambda x: tuple(map(radians, x)), ), } @@ -86,10 +82,6 @@ ConstraintKind, Tuple[Tuple[ConstraintKind, ...], Callable[[Any], Tuple[Any, ...]]] ] = { "Plane": (("Axis", "Point"), lambda x: (radians(x) if x is not None else None, 0)), - "FixedRotation": ( - ("FixedRotationAxis", "FixedRotationAxis", "FixedRotationAxis"), - lambda x: tuple(enumerate(map(radians, x))), - ), } # constraint POD type @@ -97,6 +89,8 @@ Tuple[ConstraintMarker, ...], ConstraintKind, Optional[Any], ] +NDOF_V = 3 +NDOF_Q = 3 NDOF = 6 DIR_SCALING = 1e2 DIFF_EPS = 1e-10 @@ -311,70 +305,244 @@ def toPODs(self) -> Tuple[Constraint, ...]: # Cost functions of simple constraints +def Quaternion(R): + + m = ca.sumsqr(R) + + u = 2 * R / (1 + m) + s = (1 - m) / (1 + m) + + return s, u + + +def Rotate(v, R): + + s, u = Quaternion(R) + + return 2 * ca.dot(u, v) * u + (s ** 2 - ca.dot(u, u)) * v + 2 * s * ca.cross(u, v) + + +def Transform(v, T, R): + + return Rotate(v, R) + T def point_cost( - m1: gp_Pnt, m2: gp_Pnt, t1: gp_Trsf, t2: gp_Trsf, val: Optional[float] = None, + problem, + m1: gp_Pnt, + m2: gp_Pnt, + T1_0, + R1_0, + T2_0, + R2_0, + T1, + R1, + T2, + R2, + val: Optional[float] = None, + scale: float = 1, ) -> float: val = 0 if val is None else val - return val - (m1.Transformed(t1).XYZ() - m2.Transformed(t2).XYZ()).Modulus() + m1_dm = ca.DM((m1.X(), m1.Y(), m1.Z())) + m2_dm = ca.DM((m2.X(), m2.Y(), m2.Z())) + + dummy = ( + Transform(m1_dm, T1_0 + T1, R1_0 + R1) - Transform(m2_dm, T2_0 + T2, R2_0 + R2) + ) / scale + + if val == 0: + return ca.sumsqr(dummy) + + return (ca.sumsqr(dummy) - (val / scale) ** 2) ** 2 def axis_cost( - m1: gp_Dir, m2: gp_Dir, t1: gp_Trsf, t2: gp_Trsf, val: Optional[float] = None, + problem, + m1: gp_Dir, + m2: gp_Dir, + T1_0, + R1_0, + T2_0, + R2_0, + T1, + R1, + T2, + R2, + val: Optional[float] = None, + scale: float = 1, ) -> float: val = pi if val is None else val - return DIR_SCALING * (val - m1.Transformed(t1).Angle(m2.Transformed(t2))) + m1_dm = ca.DM((m1.X(), m1.Y(), m1.Z())) + m2_dm = ca.DM((m2.X(), m2.Y(), m2.Z())) + + d1, d2 = (Rotate(m1_dm, R1_0 + R1), Rotate(m2_dm, R2_0 + R2)) + + if val == 0: + dummy = d1 - d2 + + return ca.sumsqr(dummy) + + elif val == pi: + dummy = d1 + d2 + + return ca.sumsqr(dummy) + + dummy = ca.dot(d1, d2) - ca.cos(val) + + return dummy ** 2 def point_in_plane_cost( - m1: gp_Pnt, m2: gp_Pln, t1: gp_Trsf, t2: gp_Trsf, val: Optional[float] = None, + problem, + m1: gp_Pnt, + m2: gp_Pln, + T1_0, + R1_0, + T2_0, + R2_0, + T1, + R1, + T2, + R2, + val: Optional[float] = None, + scale: float = 1, ) -> 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.Distance(m1.Transformed(t1)) + m1_dm = ca.DM((m1.X(), m1.Y(), m1.Z())) + + m2_dir = m2.Axis().Direction() + m2_pnt = m2.Axis().Location().Translated(val * gp_Vec(m2_dir)) + + m2_dir_dm = ca.DM((m2_dir.X(), m2_dir.Y(), m2_dir.Z())) + m2_pnt_dm = ca.DM((m2_pnt.X(), m2_pnt.Y(), m2_pnt.Z())) + + dummy = ( + ca.dot( + Rotate(m2_dir_dm, R2_0 + R2), + Transform(m2_pnt_dm, T2_0 + T2, R2_0 + R2) + - Transform(m1_dm, T1_0 + T1, R1_0 + R1), + ) + / scale + ) + + return dummy ** 2 def point_on_line_cost( - m1: gp_Pnt, m2: gp_Lin, t1: gp_Trsf, t2: gp_Trsf, val: Optional[float] = None, + problem, + m1: gp_Pnt, + m2: gp_Lin, + T1_0, + R1_0, + T2_0, + R2_0, + T1, + R1, + T2, + R2, + val: Optional[float] = None, + scale: float = 1, ) -> float: val = 0 if val is None else val - m2_located = m2.Transformed(t2) + m1_dm = ca.DM((m1.X(), m1.Y(), m1.Z())) + + m2_dir = m2.Direction() + m2_pnt = m2.Location() - return val - m2_located.Distance(m1.Transformed(t1)) + m2_dir_dm = ca.DM((m2_dir.X(), m2_dir.Y(), m2_dir.Z())) + m2_pnt_dm = ca.DM((m2_pnt.X(), m2_pnt.Y(), m2_pnt.Z())) + d = Transform(m1_dm, T1_0 + T1, R1_0 + R1) - Transform( + m2_pnt_dm, T2_0 + T2, R2_0 + R2 + ) + n = Rotate(m2_dir_dm, R2_0 + R2) -def fixed_cost(m1: Type[None], t1: gp_Trsf, val: Optional[Type[None]] = None): + dummy = (d - n * ca.dot(d, n)) / scale - return 0 + if val == 0: + return ca.sumsqr(dummy) + return (ca.sumsqr(dummy) - val) ** 2 -def fixed_point_cost(m1: gp_Pnt, t1: gp_Trsf, val: Tuple[float, float, float]): - return (m1.Transformed(t1).XYZ() - gp_XYZ(*val)).Modulus() +# dummy cost, fixed constraint is handled on variable level +def fixed_cost( + problem, + m1: Type[None], + T1_0, + R1_0, + T1, + R1, + val: Optional[Type[None]] = None, + scale: float = 1, +): + return None -def fixed_axis_cost(m1: gp_Dir, t1: gp_Trsf, val: Tuple[float, float, float]): - return DIR_SCALING * (m1.Transformed(t1).Angle(gp_Dir(*val))) +def fixed_point_cost( + problem, + m1: gp_Pnt, + T1_0, + R1_0, + T1, + R1, + val: Tuple[float, float, float], + scale: float = 1, +): + m1_dm = ca.DM((m1.X(), m1.Y(), m1.Z())) -def fixed_rotation_axis_cost(m1: gp_Dir, t1: gp_Trsf, val: Tuple[int, float]): + dummy = (Transform(m1_dm, T1_0 + T1, R1_0 + R1) - ca.DM(val)) / scale - ix, v0 = val - v = t1.GetRotation().GetEulerAngles(gp_Intrinsic_XYZ)[ix] + return ca.sumsqr(dummy) - return v - v0 + +def fixed_axis_cost( + problem, + m1: gp_Dir, + T1_0, + R1_0, + T1, + R1, + val: Tuple[float, float, float], + scale: float = 1, +): + + m1_dm = ca.DM((m1.X(), m1.Y(), m1.Z())) + m_val = ca.DM(val) / ca.norm_2(ca.DM(val)) + + dummy = Rotate(m1_dm, R1_0 + R1) - m_val + + return ca.sumsqr(dummy) + + +def fixed_rotation_cost( + problem, + m1: Type[None], + T1_0, + R1_0, + T1, + R1, + val: Tuple[float, float, float], + scale: float = 1, +): + + q = gp_Quaternion() + q.SetEulerAngles(gp_Extrinsic_XYZ, *val) + q_dm = ca.DM((q.W(), q.X(), q.Y(), q.Z())) + + dummy = 1 - ca.dot(ca.vertcat(*Quaternion(R1_0 + R1)), q_dm) ** 2 + + return dummy # dictionary of individual constraint cost functions @@ -386,7 +554,18 @@ def fixed_rotation_axis_cost(m1: gp_Dir, t1: gp_Trsf, val: Tuple[int, float]): Fixed=fixed_cost, FixedPoint=fixed_point_cost, FixedAxis=fixed_axis_cost, - FixedRotationAxis=fixed_rotation_axis_cost, + FixedRotation=fixed_rotation_cost, +) + +scaling: Dict[str, bool] = dict( + Point=True, + Axis=False, + PointInPlane=True, + PointOnLine=True, + Fixed=False, + FixedPoint=True, + FixedAxis=False, + FixedRotation=False, ) # Actual solver class @@ -394,20 +573,51 @@ def fixed_rotation_axis_cost(m1: gp_Dir, t1: gp_Trsf, val: Tuple[int, float]): class ConstraintSolver(object): - entities: List[DOF6] + opti: ca.Opti + variables: List[Tuple[ca.MX, ca.MX]] + starting_points: List[Tuple[ca.MX, ca.MX]] constraints: List[Tuple[Tuple[int, ...], Constraint]] locked: List[int] ne: int nc: int + scale: float def __init__( self, entities: List[Location], constraints: List[Tuple[Tuple[int, ...], Constraint]], locked: List[int] = [], + scale: float = 1, ): - self.entities = [self._locToDOF6(loc) for loc in entities] + self.scale = scale + self.opti = opti = ca.Opti() + self.variables = [ + (scale * opti.variable(NDOF_V), opti.variable(NDOF_Q)) + if i not in locked + else (opti.parameter(NDOF_V), opti.parameter(NDOF_Q)) + for i, _ in enumerate(entities) + ] + self.start_points = [ + (opti.parameter(NDOF_V), opti.parameter(NDOF_Q)) for _ in entities + ] + + # initialize, add the unit quaternion constraints and handle locked + for i, ((T, R), (T0, R0), loc) in enumerate( + zip(self.variables, self.start_points, entities) + ): + T0val, R0val = self._locToDOF6(loc) + + opti.set_value(T0, T0val) + opti.set_value(R0, R0val) + + if i in locked: + opti.set_value(T, (0, 0, 0)) + opti.set_value(R, (0, 0, 0)) + else: + opti.set_initial(T, (0.0, 0.0, 0.0)) + opti.set_initial(R, (1e-2, 1e-2, 1e-2)) + self.constraints = constraints # additional book-keeping @@ -418,22 +628,24 @@ def __init__( @staticmethod def _locToDOF6(loc: Location) -> DOF6: - T = loc.wrapped.Transformation() - v = T.TranslationPart() - q = T.GetRotation() + Tr = loc.wrapped.Transformation() + v = Tr.TranslationPart() + q = Tr.GetRotation() alpha_2 = (1 - q.W()) / (1 + q.W()) a = (alpha_2 + 1) * q.X() / 2 b = (alpha_2 + 1) * q.Y() / 2 c = (alpha_2 + 1) * q.Z() / 2 - return (v.X(), v.Y(), v.Z(), a, b, c) + return (v.X(), v.Y(), v.Z()), (a, b, c) + + def _build_transform(self, T: ca.MX, R: ca.MX) -> gp_Trsf: - def _build_transform( - self, x: float, y: float, z: float, a: float, b: float, c: float - ) -> gp_Trsf: + opti = self.opti rv = gp_Trsf() + + a, b, c = opti.value(R) m = a ** 2 + b ** 2 + c ** 2 rv.SetRotation( @@ -441,109 +653,75 @@ def _build_transform( 2 * a / (m + 1), 2 * b / (m + 1), 2 * c / (m + 1), (1 - m) / (m + 1), ) ) - - rv.SetTranslationPart(gp_Vec(x, y, z)) + rv.SetTranslationPart(gp_Vec(*opti.value(T))) return rv - def _cost( - self, - ) -> Tuple[ - Callable[[Array[Any, Float]], float], - Callable[[Array[Any, Float], Array[Any, Float]], None], - ]: - - constraints = self.constraints - ne = self.ne - delta = DIFF_EPS * eye(NDOF) - - def f(x): - """ - Function to be minimized - """ - - rv = 0 - - transforms = [ - self._build_transform(*x[NDOF * i : NDOF * (i + 1)]) for i in range(ne) - ] - - for ks, (ms, kind, params) in constraints: - ts = tuple( - transforms[k] if k not in self.locked else gp_Trsf() for k in ks - ) - cost = costs[kind] - - rv += cost(*ms, *ts, params) ** 2 - - return rv - - def grad(x, rv): - - rv[:] = 0 - - transforms = [ - self._build_transform(*x[NDOF * i : NDOF * (i + 1)]) for i in range(ne) - ] - - transforms_delta = [ - self._build_transform(*(x[NDOF * i : NDOF * (i + 1)] + delta[j, :])) - for i in range(ne) - for j in range(NDOF) - ] - - for ks, (ms, kind, params) in constraints: - ts = tuple( - transforms[k] if k not in self.locked else gp_Trsf() for k in ks - ) - cost = costs[kind] - - tmp_0 = cost(*ms, *ts, params) - - for ix, k in enumerate(ks): - if k in self.locked: - continue - - for j in range(NDOF): - tkj = transforms_delta[k * NDOF + j] - - ts_kj = ts[:ix] + (tkj,) + ts[ix + 1 :] - tmp_kj = cost(*ms, *ts_kj, params) - - rv[k * NDOF + j] += 2 * tmp_0 * (tmp_kj - tmp_0) / DIFF_EPS - - return f, grad - def solve(self) -> Tuple[List[Location], Dict[str, Any]]: - x0 = array([el for el in self.entities]).ravel() - f, grad = self._cost() - - def func(x, g): - - if g.size > 0: - grad(x, g) - - return f(x) + opti = self.opti - opt = nlopt.opt(nlopt.LD_CCSAQ, len(x0)) - opt.set_min_objective(func) + constraints = self.constraints + variables = self.variables + start_points = self.start_points + + # construct a penalty term + penalty = 0.0 + + for T, R in variables: + penalty += ca.sumsqr(ca.vertcat(T / self.scale, R)) + + # construct the objective + objective = 0.0 + for ks, (ms, kind, params) in constraints: + + # select the relevant variables and starting points + s_ks: List[ca.DM] = [] + v_ks: List[ca.MX] = [] + + for k in ks: + s_ks.extend(start_points[k]) + v_ks.extend(variables[k]) + + c = costs[kind]( + opti, + *ms, + *s_ks, + *v_ks, + params, + scale=self.scale if scaling[kind] else 1, + ) - opt.set_ftol_abs(0) - opt.set_ftol_rel(0) - opt.set_xtol_rel(TOL) - opt.set_xtol_abs(TOL * 1e-3) - opt.set_maxeval(MAXITER) + if c is not None: + objective += c + + opti.minimize(objective + 1e-16 * penalty) + + # solve + opti.solver( + "ipopt", + {"print_time": False}, + { + "acceptable_obj_change_tol": 1e-12, + "acceptable_iter": 1, + "tol": 1e-14, + "hessian_approximation": "exact", + "nlp_scaling_method": "none", + "honor_original_bounds": "yes", + "bound_relax_factor": 0, + "print_level": 5, + "print_timing_statistics": "no", + "linear_solver": "mumps", + }, + ) + sol = opti.solve_limited() - x = opt.optimize(x0) - result = { - "cost": opt.last_optimum_value(), - "iters": opt.get_numevals(), - "status": opt.last_optimize_result(), - } + result = sol.stats() + result["opti"] = opti # this might be removed in the future locs = [ - Location(self._build_transform(*x[NDOF * i : NDOF * (i + 1)])) - for i in range(self.ne) + Location(self._build_transform(T + T0, R + R0)) + for (T, R), (T0, R0) in zip(variables, start_points) ] + return locs, result diff --git a/conda/meta.yaml b/conda/meta.yaml index 605aaa978..67045d5fc 100644 --- a/conda/meta.yaml +++ b/conda/meta.yaml @@ -24,6 +24,7 @@ requirements: - nptyping >=2.0.0 - nlopt - multimethod 1.6 + - casadi test: requires: diff --git a/environment.yml b/environment.yml index 7e1cb5f1c..756bc0007 100644 --- a/environment.yml +++ b/environment.yml @@ -23,6 +23,7 @@ dependencies: - nptyping>=2.0.0 - nlopt - path + - casadi - pip - pip: - "--editable=." diff --git a/mypy.ini b/mypy.ini index f78b4f7a8..5f61b5df4 100644 --- a/mypy.ini +++ b/mypy.ini @@ -31,3 +31,7 @@ ignore_missing_imports = True [mypy-typish.*] ignore_missing_imports = True + +[mypy-casadi.*] +ignore_missing_imports = True + diff --git a/tests/test_assembly.py b/tests/test_assembly.py index dec5d3497..e5d014da0 100644 --- a/tests/test_assembly.py +++ b/tests/test_assembly.py @@ -1,8 +1,8 @@ import pytest import os from itertools import product +from math import degrees -import nlopt import cadquery as cq from cadquery.occ_impl.exporters.assembly import ( exportAssembly, @@ -84,6 +84,7 @@ def box_and_vertex(): 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 @@ -103,6 +104,7 @@ def metadata_assy(): b2, loc=cq.Location(cq.Vector(1, 1, 1)), name="sub", metadata={"b2": "sub-data"} ) assy.add(sub_assy) + return assy @@ -131,9 +133,8 @@ def test_metadata(metadata_assy): def solve_result_check(solve_result: dict) -> bool: checks = [ - solve_result["status"] == nlopt.XTOL_REACHED, - solve_result["cost"] < 1e-9, - solve_result["iters"] > 0, + solve_result["success"] == True, + solve_result["iterations"]["inf_pr"][-1] < 1e-9, ] return all(checks) @@ -641,7 +642,7 @@ def test_fixed_rotation(simple_assy2): assert w.solids("Z").size() == 1 -def test_validation(simple_assy2): +def test_constraint_validation(simple_assy2): with pytest.raises(ValueError): simple_assy2.constrain("b1", "Fixed?") @@ -650,6 +651,13 @@ def test_validation(simple_assy2): cq.assembly.Constraint((), (), (), "Fixed?") +def test_single_unary_constraint(simple_assy2): + + with pytest.raises(ValueError): + simple_assy2.constrain("b1", "FixedRotation", (45, 0, 45)) + simple_assy2.solve() + + def test_point_on_line(simple_assy2): assy = simple_assy2 @@ -667,3 +675,30 @@ def test_point_on_line(simple_assy2): assert w.solids(">Z").val().Center().z == pytest.approx(0.5) assert w.solids(">Z").val().Center().y == pytest.approx(0.5) assert w.solids(">Z").val().Center().x == pytest.approx(0.0) + + +def test_axis_constraint(simple_assy2): + + assy = simple_assy2 + + assy.constrain("b1@faces@>Z", "b2@faces@>Z", "Axis", 0) + assy.constrain("b1@faces@>X", "b2@faces@>X", "Axis", 45) + + assy.solve() + + q2 = assy.children[1].loc.wrapped.Transformation().GetRotation() + + assert degrees(q2.GetRotationAngle()) == pytest.approx(45) + + +def test_point_constraint(simple_assy2): + + assy = simple_assy2 + + assy.constrain("b1", "b2", "Point", 1) + + assy.solve() + + t2 = assy.children[1].loc.wrapped.Transformation().TranslationPart() + + assert t2.Modulus() == pytest.approx(1)