diff --git a/kindred_solver/constraints.py b/kindred_solver/constraints.py index 9c56875..4c9100f 100644 --- a/kindred_solver/constraints.py +++ b/kindred_solver/constraints.py @@ -416,6 +416,7 @@ class PlanarConstraint(ConstraintBase): self.marker_j_pos = marker_j_pos self.marker_j_quat = marker_j_quat self.offset = offset + self.reference_normal = None # Set by _build_system; world-frame Const normal def residuals(self) -> List[Expr]: # Parallel normals (3 cross-product residuals, rank 2 at solution) @@ -423,10 +424,20 @@ class PlanarConstraint(ConstraintBase): z_j = marker_z_axis(self.body_j, self.marker_j_quat) cx, cy, cz = cross3(z_i, z_j) - # Point-in-plane + # Point-in-plane distance — use world-anchored reference normal when + # available so the constraining plane direction doesn't rotate with + # the body (prevents axial drift when combined with Cylindrical). p_i = self.body_i.world_point(*self.marker_i_pos) p_j = self.body_j.world_point(*self.marker_j_pos) - d = point_plane_distance(p_i, p_j, z_j) + if self.reference_normal is not None: + nz = ( + Const(self.reference_normal[0]), + Const(self.reference_normal[1]), + Const(self.reference_normal[2]), + ) + else: + nz = z_j + d = point_plane_distance(p_i, p_j, nz) if self.offset != 0.0: d = d - Const(self.offset) diff --git a/kindred_solver/solver.py b/kindred_solver/solver.py index 0a3205b..781fa5d 100644 --- a/kindred_solver/solver.py +++ b/kindred_solver/solver.py @@ -39,6 +39,7 @@ from .constraints import ( UniversalConstraint, ) from .decompose import decompose, solve_decomposed +from .geometry import marker_z_axis from .diagnostics import find_overconstrained from .dof import count_dof from .entities import RigidBody @@ -407,9 +408,7 @@ class KindredSolver(kcsolve.IKCSolver): # old and new quaternion — if we're in the -q branch, that # shows up as a ~340° flip and gets rejected. dragged_ids = self._drag_parts or set() - _enforce_quat_continuity( - params, cache.system.bodies, cache.pre_step_quats, dragged_ids - ) + _enforce_quat_continuity(params, cache.system.bodies, cache.pre_step_quats, dragged_ids) # Update the stored quaternions for the next drag step env = params.get_env() @@ -606,6 +605,19 @@ def _build_system(ctx): c.type, ) continue + # For PlanarConstraint, snapshot the world-frame normal at the + # initial configuration so the distance residual uses a constant + # reference direction. This prevents axial drift when a Planar + # is combined with a Cylindrical on the same body pair. + if isinstance(obj, PlanarConstraint): + env = params.get_env() + z_j = marker_z_axis(obj.body_j, obj.marker_j_quat) + obj.reference_normal = ( + z_j[0].eval(env), + z_j[1].eval(env), + z_j[2].eval(env), + ) + constraint_objs.append(obj) constraint_indices.append(idx) all_residuals.extend(obj.residuals()) diff --git a/tests/test_drag.py b/tests/test_drag.py index 4f674d6..b7c5731 100644 --- a/tests/test_drag.py +++ b/tests/test_drag.py @@ -19,10 +19,12 @@ import math import pytest from kindred_solver.constraints import ( CoincidentConstraint, + CylindricalConstraint, PlanarConstraint, RevoluteConstraint, ) from kindred_solver.entities import RigidBody +from kindred_solver.geometry import marker_z_axis from kindred_solver.newton import newton_solve from kindred_solver.params import ParamTable from kindred_solver.prepass import single_equation_pass, substitution_pass @@ -251,3 +253,118 @@ class TestDragDoesNotBreakStaticSolve: assert abs(env["arm/ty"]) < 1e-8 assert abs(env["arm/tz"]) < 1e-8 assert abs(env["plate/tz"]) < 1e-8 + + +def _set_reference_normal(planar_obj, params): + """Snapshot world-frame normal as reference (mirrors _build_system logic).""" + env = params.get_env() + z_j = marker_z_axis(planar_obj.body_j, planar_obj.marker_j_quat) + planar_obj.reference_normal = ( + z_j[0].eval(env), + z_j[1].eval(env), + z_j[2].eval(env), + ) + + +class TestPlanarCylindricalAxialDrift: + """Planar + Cylindrical on the same body pair: axial drift regression. + + Bug: PlanarConstraint's distance residual uses a body-attached normal + (z_j) that rotates with the body. When combined with Cylindrical + (which allows rotation about the axis), the normal can tilt during + Newton iteration, allowing the body to drift along the cylinder axis + while technically satisfying the rotated distance residual. + + Fix: snapshot the world-frame normal at system-build time and use + Const nodes in the distance residual (reference_normal). + """ + + def _setup(self, offset=0.0): + """Cylindrical + Planar along Z between ground and a free body. + + Free body starts displaced along Z so the solver must move it. + """ + pt = ParamTable() + ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True) + free = RigidBody("free", pt, (0, 0, 5), ID_QUAT) + + cyl = CylindricalConstraint( + ground, + (0, 0, 0), + ID_QUAT, + free, + (0, 0, 0), + ID_QUAT, + ) + planar = PlanarConstraint( + ground, + (0, 0, 0), + ID_QUAT, + free, + (0, 0, 0), + ID_QUAT, + offset=offset, + ) + bodies = [ground, free] + constraints = [cyl, planar] + return pt, bodies, constraints, planar + + def test_reference_normal_prevents_axial_drift(self): + """With reference_normal, free body converges to z=0 (distance=0).""" + pt, bodies, constraints, planar = self._setup(offset=0.0) + _set_reference_normal(planar, pt) + raw, qg = _build_residuals(bodies, constraints) + + residuals = substitution_pass(raw, pt) + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + env = pt.get_env() + assert abs(env["free/tz"]) < 1e-8, ( + f"free/tz = {env['free/tz']:.6e}, expected ~0.0 (axial drift)" + ) + + def test_reference_normal_with_offset(self): + """With reference_normal and offset=3.0, free body converges to z=-3. + + Sign convention: point_plane_distance = (p_ground - p_free) · n, + so offset=3 means -z_free - 3 = 0, i.e. z_free = -3. + """ + pt, bodies, constraints, planar = self._setup(offset=3.0) + _set_reference_normal(planar, pt) + raw, qg = _build_residuals(bodies, constraints) + + residuals = substitution_pass(raw, pt) + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + env = pt.get_env() + assert abs(env["free/tz"] + 3.0) < 1e-8, f"free/tz = {env['free/tz']:.6e}, expected ~-3.0" + + def test_drag_step_no_drift(self): + """After drag perturbation, re-solve keeps axial position locked.""" + pt, bodies, constraints, planar = self._setup(offset=0.0) + _set_reference_normal(planar, pt) + raw, qg = _build_residuals(bodies, constraints) + + residuals = substitution_pass(raw, pt) + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + env = pt.get_env() + assert abs(env["free/tz"]) < 1e-8 + + # Simulate drag: perturb the free body's position + pt.set_value("free/tx", 3.0) + pt.set_value("free/ty", 2.0) + pt.set_value("free/tz", 1.0) # axial perturbation + + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + env = pt.get_env() + # Cylindrical allows x/y freedom only on the axis line, + # but Planar distance=0 must hold: z stays at 0 + assert abs(env["free/tz"]) < 1e-8, ( + f"free/tz = {env['free/tz']:.6e}, expected ~0.0 after drag re-solve" + )