fix(solver): use world-anchored reference normal in PlanarConstraint distance residual

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

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

@@ -21,12 +21,21 @@ import numpy as np
 
 from .constraints import (
     AngleConstraint,
+    ConcentricConstraint,
     ConstraintBase,
+    CylindricalConstraint,
     DistancePointPointConstraint,
+    LineInPlaneConstraint,
     ParallelConstraint,
     PerpendicularConstraint,
+    PlanarConstraint,
+    PointInPlaneConstraint,
+    RevoluteConstraint,
+    ScrewConstraint,
+    SliderConstraint,
+    UniversalConstraint,
 )
-from .geometry import cross3, dot3, marker_z_axis
+from .geometry import cross3, dot3, marker_z_axis, point_plane_distance, sub3
 from .params import ParamTable
 
 
@@ -107,6 +116,33 @@ def _build_half_space(
     if isinstance(obj, PerpendicularConstraint):
         return _perpendicular_half_space(obj, constraint_idx, env, params)
 
+    if isinstance(obj, PlanarConstraint):
+        return _planar_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, RevoluteConstraint):
+        return _revolute_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, ConcentricConstraint):
+        return _concentric_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, PointInPlaneConstraint):
+        return _point_in_plane_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, LineInPlaneConstraint):
+        return _line_in_plane_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, CylindricalConstraint):
+        return _axis_direction_half_space(obj, constraint_idx, env)
+
+    if isinstance(obj, SliderConstraint):
+        return _axis_direction_half_space(obj, constraint_idx, env)
+
+    if isinstance(obj, ScrewConstraint):
+        return _axis_direction_half_space(obj, constraint_idx, env)
+
+    if isinstance(obj, UniversalConstraint):
+        return _universal_half_space(obj, constraint_idx, env)
+
     return None
 
 
@@ -212,6 +248,319 @@ def _parallel_half_space(
     )
 
 
+def _planar_half_space(
+    obj: PlanarConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Planar: track which side of the plane the point is on
+    AND which direction the normals face.
+
+    A Planar constraint has parallel normals (cross product = 0) plus
+    point-in-plane (signed distance = 0).  Both have branch ambiguity:
+    the normals can be same-direction or opposite, and the point can
+    approach the plane from either side.  We track the signed distance
+    from marker_i to the plane defined by marker_j — this captures
+    the plane-side and is the primary drift mode during drag.
+    """
+    # Point-in-plane signed distance as indicator
+    p_i = obj.body_i.world_point(*obj.marker_i_pos)
+    p_j = obj.body_j.world_point(*obj.marker_j_pos)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    d_expr = point_plane_distance(p_i, p_j, z_j)
+
+    d_val = d_expr.eval(env)
+
+    # Also track normal alignment (same as Parallel half-space)
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    dot_expr = dot3(z_i, z_j)
+    dot_val = dot_expr.eval(env)
+    normal_ref_sign = math.copysign(1.0, dot_val) if abs(dot_val) > 1e-14 else 1.0
+
+    # If offset is zero and distance is near-zero, we still need the normal
+    # direction indicator to prevent flipping through the plane.
+    # 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 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)
+
+        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:
+            return d_expr.eval(e)
+
+        ref_sign = math.copysign(1.0, d_val)
+
+    # Correction: reflect the moving body's position through the plane
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    def correction(p: ParamTable, _val: float) -> None:
+        e = p.get_env()
+        # Recompute signed distance and normal direction
+        d_cur = d_expr.eval(e)
+        nx = z_j[0].eval(e)
+        ny = z_j[1].eval(e)
+        nz = z_j[2].eval(e)
+        n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
+        if n_len < 1e-15:
+            return
+        nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
+        # Reflect through plane: move body by -2*d along normal
+        sign = -1.0 if moving_body is obj.body_j else 1.0
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=indicator,
+        correction_fn=correction,
+    )
+
+
+def _revolute_half_space(
+    obj: RevoluteConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Revolute: track hinge axis direction.
+
+    A revolute has coincident origins + parallel Z-axes.  The parallel
+    axes can flip direction (same ambiguity as Parallel).  Track
+    dot(z_i, z_j) to prevent the axis from inverting.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    ref_val = dot_expr.eval(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: dot_expr.eval(e),
+    )
+
+
+def _concentric_half_space(
+    obj: ConcentricConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Concentric: track axis direction.
+
+    Concentric has parallel axes + point-on-line.  The parallel axes
+    can flip direction.  Track dot(z_i, z_j).
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    ref_val = dot_expr.eval(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: dot_expr.eval(e),
+    )
+
+
+def _point_in_plane_half_space(
+    obj: PointInPlaneConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for PointInPlane: track which side of the plane.
+
+    The signed distance to the plane can be satisfied from either side.
+    Track which side the initial configuration is on.
+    """
+    p_i = obj.body_i.world_point(*obj.marker_i_pos)
+    p_j = obj.body_j.world_point(*obj.marker_j_pos)
+    n_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    d_expr = point_plane_distance(p_i, p_j, n_j)
+
+    d_val = d_expr.eval(env)
+    if abs(d_val) < 1e-10:
+        return None  # already on the plane, no branch to track
+
+    ref_sign = math.copysign(1.0, d_val)
+
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    def correction(p: ParamTable, _val: float) -> None:
+        e = p.get_env()
+        d_cur = d_expr.eval(e)
+        nx = n_j[0].eval(e)
+        ny = n_j[1].eval(e)
+        nz = n_j[2].eval(e)
+        n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
+        if n_len < 1e-15:
+            return
+        nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
+        sign = -1.0 if moving_body is obj.body_j else 1.0
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: d_expr.eval(e),
+        correction_fn=correction,
+    )
+
+
+def _line_in_plane_half_space(
+    obj: LineInPlaneConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for LineInPlane: track which side of the plane.
+
+    Same plane-side ambiguity as PointInPlane.
+    """
+    p_i = obj.body_i.world_point(*obj.marker_i_pos)
+    p_j = obj.body_j.world_point(*obj.marker_j_pos)
+    n_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    d_expr = point_plane_distance(p_i, p_j, n_j)
+
+    d_val = d_expr.eval(env)
+    if abs(d_val) < 1e-10:
+        return None
+
+    ref_sign = math.copysign(1.0, d_val)
+
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    def correction(p: ParamTable, _val: float) -> None:
+        e = p.get_env()
+        d_cur = d_expr.eval(e)
+        nx = n_j[0].eval(e)
+        ny = n_j[1].eval(e)
+        nz = n_j[2].eval(e)
+        n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
+        if n_len < 1e-15:
+            return
+        nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
+        sign = -1.0 if moving_body is obj.body_j else 1.0
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: d_expr.eval(e),
+        correction_fn=correction,
+    )
+
+
+def _axis_direction_half_space(
+    obj,
+    constraint_idx: int,
+    env: dict[str, float],
+) -> HalfSpace | None:
+    """Half-space for any constraint with parallel Z-axes (Cylindrical, Slider, Screw).
+
+    Tracks dot(z_i, z_j) to prevent axis inversion.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    ref_val = dot_expr.eval(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: dot_expr.eval(e),
+    )
+
+
+def _universal_half_space(
+    obj: UniversalConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+) -> HalfSpace | None:
+    """Half-space for Universal: track which quadrant of perpendicularity.
+
+    Universal has dot(z_i, z_j) = 0 (perpendicular).  The cross product
+    sign distinguishes which "side" of perpendicular.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    cx, cy, cz = cross3(z_i, z_j)
+
+    cx_val = cx.eval(env)
+    cy_val = cy.eval(env)
+    cz_val = cz.eval(env)
+
+    components = [
+        (abs(cx_val), cx, cx_val),
+        (abs(cy_val), cy, cy_val),
+        (abs(cz_val), cz, cz_val),
+    ]
+    _, best_expr, best_val = max(components, key=lambda t: t[0])
+
+    if abs(best_val) < 1e-14:
+        return None
+
+    ref_sign = math.copysign(1.0, best_val)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: best_expr.eval(e),
+    )
+
+
 # ============================================================================
 # Minimum-movement weighting
 # ============================================================================

kindred_solver/solver.py

@@ -4,6 +4,7 @@ expression-based Newton-Raphson solver."""
 from __future__ import annotations
 
 import logging
+import math
 import time
 
 import kcsolve
@@ -38,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
@@ -142,8 +144,6 @@ class KindredSolver(kcsolve.IKCSolver):
             system.constraint_indices,
             system.params,
         )
-        weight_vec = build_weight_vector(system.params)
-
         if half_spaces:
             post_step_fn = lambda p: apply_half_space_correction(p, half_spaces)
         else:
@@ -153,6 +153,10 @@ class KindredSolver(kcsolve.IKCSolver):
         residuals = substitution_pass(system.all_residuals, system.params)
         residuals = single_equation_pass(residuals, system.params)
 
+        # Build weight vector *after* pre-passes so its length matches the
+        # remaining free parameters (single_equation_pass may fix some).
+        weight_vec = build_weight_vector(system.params)
+
         # Solve (decomposed for large assemblies, monolithic for small)
         jac_exprs = None  # may be populated by _monolithic_solve
         if n_free_bodies >= _DECOMPOSE_THRESHOLD:
@@ -250,7 +254,6 @@ class KindredSolver(kcsolve.IKCSolver):
             system.constraint_indices,
             system.params,
         )
-        weight_vec = build_weight_vector(system.params)
 
         if half_spaces:
             post_step_fn = lambda p: apply_half_space_correction(p, half_spaces)
@@ -266,6 +269,10 @@ class KindredSolver(kcsolve.IKCSolver):
         # The substitution pass alone is safe because it only replaces
         # genuinely grounded parameters with constants.
 
+        # Build weight vector *after* pre-passes so its length matches the
+        # remaining free parameters (single_equation_pass may fix some).
+        weight_vec = build_weight_vector(system.params)
+
         # Build symbolic Jacobian + compile once
         from .codegen import try_compile_system
 
@@ -308,6 +315,14 @@ class KindredSolver(kcsolve.IKCSolver):
         cache.half_spaces = half_spaces
         cache.weight_vec = weight_vec
         cache.post_step_fn = post_step_fn
+
+        # Snapshot solved quaternions for continuity enforcement in drag_step()
+        env = system.params.get_env()
+        cache.pre_step_quats = {}
+        for body in system.bodies.values():
+            if not body.grounded:
+                cache.pre_step_quats[body.part_id] = body.extract_quaternion(env)
+
         self._drag_cache = cache
 
         # Build result
@@ -387,6 +402,20 @@ class KindredSolver(kcsolve.IKCSolver):
                 compiled_eval=cache.compiled_eval,
             )
 
+        # Quaternion continuity: ensure solved quaternions stay in the
+        # same hemisphere as the previous step.  q and -q encode the
+        # same rotation, but the C++ side measures angle between the
+        # 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)
+
+        # Update the stored quaternions for the next drag step
+        env = params.get_env()
+        for body in cache.system.bodies.values():
+            if not body.grounded:
+                cache.pre_step_quats[body.part_id] = body.extract_quaternion(env)
+
         result = kcsolve.SolveResult()
         result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
         result.dof = -1  # skip DOF counting during drag for speed
@@ -448,6 +477,7 @@ class _DragCache:
         "half_spaces",  # list[HalfSpace]
         "weight_vec",  # ndarray or None
         "post_step_fn",  # Callable or None
+        "pre_step_quats",  # dict[str, tuple] — last-accepted quaternions per body
     )
 
 
@@ -465,6 +495,46 @@ class _System:
     )
 
 
+def _enforce_quat_continuity(
+    params: ParamTable,
+    bodies: dict,
+    pre_step_quats: dict,
+    dragged_ids: set,
+) -> None:
+    """Ensure solved quaternions stay in the same hemisphere as pre-step.
+
+    For each non-grounded, non-dragged body, check whether the solved
+    quaternion is in the opposite hemisphere from the pre-step quaternion
+    (dot product < 0).  If so, negate it — q and -q represent the same
+    rotation, but staying in the same hemisphere prevents the C++ side
+    from seeing a large-angle "flip".
+
+    This is the standard short-arc correction used in SLERP interpolation.
+    """
+    for body in bodies.values():
+        if body.grounded or body.part_id in dragged_ids:
+            continue
+        prev = pre_step_quats.get(body.part_id)
+        if prev is None:
+            continue
+
+        pfx = body.part_id + "/"
+        qw = params.get_value(pfx + "qw")
+        qx = params.get_value(pfx + "qx")
+        qy = params.get_value(pfx + "qy")
+        qz = params.get_value(pfx + "qz")
+
+        # Quaternion dot product: positive means same hemisphere
+        dot = prev[0] * qw + prev[1] * qx + prev[2] * qy + prev[3] * qz
+
+        if dot < 0.0:
+            # Negate to stay in the same hemisphere (identical rotation)
+            params.set_value(pfx + "qw", -qw)
+            params.set_value(pfx + "qx", -qx)
+            params.set_value(pfx + "qy", -qy)
+            params.set_value(pfx + "qz", -qz)
+
+
 def _build_system(ctx):
     """Build the solver's internal representation from a SolveContext.
 
@@ -535,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"
+        )