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)
 

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:

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())

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