From 85a607228d25fb6fe6821d49b0e9cb8ca521a23b Mon Sep 17 00:00:00 2001 From: forbes-0023 Date: Thu, 26 Feb 2026 07:46:10 -0600 Subject: [PATCH 1/2] fix: remove planar half-space correction for on-plane distance=0 constraints When a PlanarConstraint has distance=0 (point already on plane), the half-space tracker was using dot(z_i, z_j) as its indicator and providing a correction function that reflects the body through the plane. When combined with a Cylindrical joint, legitimate rotation about the cylinder axis causes the body's planar face normal to rotate. After ~90 degrees, the dot product crosses zero, triggering the correction which teleports the body to the other side of the plane. The C++ validator then rejects every subsequent drag step as 'flipped orientation'. Fix: return a tracking-only HalfSpace (no correction_fn) when the point is already on the plane. This matches the pattern used by Cylindrical, Revolute, and Concentric half-space trackers. The cross-product residuals in PlanarConstraint already enforce normal alignment via the solver itself, so the correction is redundant and harmful during drag. --- kindred_solver/preference.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/kindred_solver/preference.py b/kindred_solver/preference.py index 85613f4..f6ea7da 100644 --- a/kindred_solver/preference.py +++ b/kindred_solver/preference.py @@ -283,11 +283,18 @@ def _planar_half_space( # Use the normal dot product as the primary indicator when the point # is already on the plane (distance ≈ 0). if abs(d_val) < 1e-10: - # Point is on the plane — track normal direction instead + # Point is on the plane — track normal direction only. + # No correction: when combined with rotational joints (Cylindrical, + # Revolute), the body's marker normal rotates legitimately and + # reflecting through the plane would fight the rotation. def indicator(e: dict[str, float]) -> float: return dot_expr.eval(e) - ref_sign = normal_ref_sign + return HalfSpace( + constraint_index=constraint_idx, + reference_sign=normal_ref_sign, + indicator_fn=indicator, + ) else: # Point is off-plane — track which side def indicator(e: dict[str, float]) -> float: -- 2.49.1 From 000f54adaadf3670414cbe7b1c94586c03e885f4 Mon Sep 17 00:00:00 2001 From: forbes-0023 Date: Thu, 26 Feb 2026 11:06:55 -0600 Subject: [PATCH 2/2] fix(solver): use world-anchored reference normal in PlanarConstraint distance residual MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit PlanarConstraint's point-in-plane residual used a body-attached normal (z_j) that rotates with body_j. When combined with CylindricalConstraint, which allows rotation about the shared axis, the plane normal tilts during Newton iteration. This allows 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 (as Const nodes) and use it in the distance residual. The cross-product residuals still use body-attached normals to enforce parallelism. Only the distance residual needs the fixed reference direction. Works for any offset value: (p_i - p_j) · z_ref - offset = 0. --- kindred_solver/constraints.py | 15 ++++- kindred_solver/solver.py | 18 +++++- tests/test_drag.py | 117 ++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+), 5 deletions(-) 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" + ) -- 2.49.1