From 5802d45a7f6541d90fed4d7a057cbe2673a42977 Mon Sep 17 00:00:00 2001 From: forbes-0023 Date: Wed, 25 Feb 2026 12:57:43 -0600 Subject: [PATCH] fix(solver): skip single_equation_pass during drag to prevent stale constraints single_equation_pass analytically solves variables and bakes their values as Const() nodes into downstream residual expressions. During drag, the cached residuals use these stale constants even though part positions have changed, causing constraints like Planar distance=0 to silently stop being enforced. Skip single_equation_pass in the pre_drag() path. Only substitution_pass (which replaces genuinely grounded parameters) is safe to cache across drag steps. Newton-Raphson converges in 1-2 iterations from a nearby initial guess anyway, so the prepass optimization is unnecessary for drag. Add regression tests covering the bug scenario and the fix. --- kindred_solver/solver.py | 39 +++--- tests/test_drag.py | 253 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 268 insertions(+), 24 deletions(-) create mode 100644 tests/test_drag.py diff --git a/kindred_solver/solver.py b/kindred_solver/solver.py index f6467b5..76e625f 100644 --- a/kindred_solver/solver.py +++ b/kindred_solver/solver.py @@ -130,8 +130,7 @@ class KindredSolver(kcsolve.IKCSolver): for c in ctx.constraints: if c.limits: log.warning( - "Joint limits on '%s' ignored " - "(not yet supported by Kindred solver)", + "Joint limits on '%s' ignored (not yet supported by Kindred solver)", c.id, ) self._limits_warned = True @@ -200,9 +199,7 @@ class KindredSolver(kcsolve.IKCSolver): # Build result result = kcsolve.SolveResult() - result.status = ( - kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed - ) + result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed result.dof = dof # Diagnostics on failure @@ -227,9 +224,7 @@ class KindredSolver(kcsolve.IKCSolver): ) if not converged and result.diagnostics: for d in result.diagnostics: - log.warning( - " diagnostic: [%s] %s — %s", d.kind, d.constraint_id, d.detail - ) + log.warning(" diagnostic: [%s] %s — %s", d.kind, d.constraint_id, d.detail) return result @@ -263,7 +258,13 @@ class KindredSolver(kcsolve.IKCSolver): post_step_fn = None residuals = substitution_pass(system.all_residuals, system.params) - residuals = single_equation_pass(residuals, system.params) + # NOTE: single_equation_pass is intentionally skipped for drag. + # It permanently fixes variables and removes residuals from the + # list. During drag the dragged part's parameters change each + # frame, which can invalidate those analytic solutions and cause + # constraints (e.g. Planar distance=0) to stop being enforced. + # The substitution pass alone is safe because it only replaces + # genuinely grounded parameters with constants. # Build symbolic Jacobian + compile once from .codegen import try_compile_system @@ -312,9 +313,7 @@ class KindredSolver(kcsolve.IKCSolver): # Build result dof = count_dof(residuals, system.params, jac_exprs=jac_exprs) result = kcsolve.SolveResult() - result.status = ( - kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed - ) + result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed result.dof = dof result.placements = _extract_placements(system.params, system.bodies) @@ -389,9 +388,7 @@ class KindredSolver(kcsolve.IKCSolver): ) result = kcsolve.SolveResult() - result.status = ( - kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed - ) + result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed result.dof = -1 # skip DOF counting during drag for speed result.placements = _extract_placements(params, cache.system.bodies) @@ -581,9 +578,7 @@ def _run_diagnostics(residuals, params, residual_ranges, ctx, jac_exprs=None): if not hasattr(kcsolve, "ConstraintDiagnostic"): return diagnostics - diags = find_overconstrained( - residuals, params, residual_ranges, jac_exprs=jac_exprs - ) + diags = find_overconstrained(residuals, params, residual_ranges, jac_exprs=jac_exprs) for d in diags: cd = kcsolve.ConstraintDiagnostic() cd.constraint_id = ctx.constraints[d.constraint_index].id @@ -613,9 +608,7 @@ def _extract_placements(params, bodies): return placements -def _monolithic_solve( - all_residuals, params, quat_groups, post_step=None, weight_vector=None -): +def _monolithic_solve(all_residuals, params, quat_groups, post_step=None, weight_vector=None): """Newton-Raphson solve with BFGS fallback on the full system. Returns ``(converged, jac_exprs)`` so the caller can reuse the @@ -647,9 +640,7 @@ def _monolithic_solve( ) nr_ms = (time.perf_counter() - t0) * 1000 if not converged: - log.info( - "_monolithic_solve: Newton-Raphson failed (%.1f ms), trying BFGS", nr_ms - ) + log.info("_monolithic_solve: Newton-Raphson failed (%.1f ms), trying BFGS", nr_ms) t1 = time.perf_counter() converged = bfgs_solve( all_residuals, diff --git a/tests/test_drag.py b/tests/test_drag.py new file mode 100644 index 0000000..4f674d6 --- /dev/null +++ b/tests/test_drag.py @@ -0,0 +1,253 @@ +"""Regression tests for interactive drag. + +These tests exercise the drag protocol at the solver-internals level, +verifying that constraints remain enforced across drag steps when the +pre-pass has been applied to cached residuals. + +Bug scenario: single_equation_pass runs during pre_drag, analytically +solving variables from upstream constraints and baking their values as +constants into downstream residual expressions. When a drag step +changes those variables, the cached residuals use stale constants and +downstream constraints (e.g. Planar distance=0) stop being enforced. + +Fix: skip single_equation_pass in the drag path. Only substitution_pass +(which replaces genuinely grounded parameters) is safe to cache. +""" + +import math + +import pytest +from kindred_solver.constraints import ( + CoincidentConstraint, + PlanarConstraint, + RevoluteConstraint, +) +from kindred_solver.entities import RigidBody +from kindred_solver.newton import newton_solve +from kindred_solver.params import ParamTable +from kindred_solver.prepass import single_equation_pass, substitution_pass + +ID_QUAT = (1, 0, 0, 0) + + +def _build_residuals(bodies, constraint_objs): + """Build raw residual list + quat groups (no prepass).""" + all_residuals = [] + for c in constraint_objs: + all_residuals.extend(c.residuals()) + + quat_groups = [] + for body in bodies: + if not body.grounded: + all_residuals.append(body.quat_norm_residual()) + quat_groups.append(body.quat_param_names()) + + return all_residuals, quat_groups + + +def _eval_raw_residuals(bodies, constraint_objs, params): + """Evaluate original constraint residuals at current param values. + + Returns the max absolute residual value — the ground truth for + whether constraints are satisfied regardless of prepass state. + """ + raw, _ = _build_residuals(bodies, constraint_objs) + env = params.get_env() + return max(abs(r.eval(env)) for r in raw) + + +class TestPrepassDragRegression: + """single_equation_pass bakes stale values that break drag. + + Setup: ground --Revolute--> arm --Planar(d=0)--> plate + + The Revolute pins arm's origin to ground (fixes arm/tx, arm/ty, + arm/tz via single_equation_pass). The Planar keeps plate coplanar + with arm. After prepass, the Planar residual has arm's position + baked as Const(0.0). + + During drag: arm/tz is set to 5.0. Because arm/tz is marked fixed + by prepass, Newton can't correct it, AND the Planar residual still + uses Const(0.0) instead of the live value 5.0. The Revolute + constraint (arm at origin) is silently violated. + """ + + def _setup(self): + pt = ParamTable() + ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True) + arm = RigidBody("arm", pt, (10, 0, 0), ID_QUAT) + plate = RigidBody("plate", pt, (10, 5, 0), ID_QUAT) + + constraints = [ + RevoluteConstraint(ground, (0, 0, 0), ID_QUAT, arm, (0, 0, 0), ID_QUAT), + PlanarConstraint(arm, (0, 0, 0), ID_QUAT, plate, (0, 0, 0), ID_QUAT, offset=0.0), + ] + bodies = [ground, arm, plate] + return pt, bodies, constraints + + def test_bug_stale_constants_after_single_equation_pass(self): + """Document the bug: prepass bakes arm/tz=0, drag breaks constraints.""" + pt, bodies, constraints = self._setup() + raw_residuals, quat_groups = _build_residuals(bodies, constraints) + + # Simulate OLD pre_drag: substitution + single_equation_pass + residuals = substitution_pass(raw_residuals, pt) + residuals = single_equation_pass(residuals, pt) + + ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10) + assert ok + + # Verify prepass fixed arm's position params + assert pt.is_fixed("arm/tx") + assert pt.is_fixed("arm/ty") + assert pt.is_fixed("arm/tz") + + # Simulate drag: move arm up (set_value, as drag_step does) + pt.set_value("arm/tz", 5.0) + pt.set_value("plate/tz", 5.0) # initial guess near drag + + ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10) + # Solver "converges" on the stale cached residuals + assert ok + + # But the TRUE constraints are violated: arm should be at z=0 + # (Revolute pins it to ground) yet it's at z=5 + max_err = _eval_raw_residuals(bodies, constraints, pt) + assert max_err > 1.0, ( + f"Expected large raw residual violation, got {max_err:.6e}. " + "The bug should cause the Revolute z-residual to be ~5.0" + ) + + def test_fix_no_single_equation_pass_for_drag(self): + """With the fix: skip single_equation_pass, constraints hold.""" + pt, bodies, constraints = self._setup() + raw_residuals, quat_groups = _build_residuals(bodies, constraints) + + # Simulate FIXED pre_drag: substitution only + residuals = substitution_pass(raw_residuals, pt) + + ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10) + assert ok + + # arm/tz should NOT be fixed + assert not pt.is_fixed("arm/tz") + + # Simulate drag: move arm up + pt.set_value("arm/tz", 5.0) + pt.set_value("plate/tz", 5.0) + + ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10) + assert ok + + # Newton pulls arm back to z=0 (Revolute enforced) and plate follows + max_err = _eval_raw_residuals(bodies, constraints, pt) + assert max_err < 1e-8, f"Raw residual violation {max_err:.6e} — constraints not satisfied" + + +class TestCoincidentPlanarDragRegression: + """Coincident upstream + Planar downstream — same bug class. + + ground --Coincident--> bracket --Planar(d=0)--> plate + + Coincident fixes bracket/tx,ty,tz. After prepass, the Planar + residual has bracket's position baked. Drag moves bracket; + the Planar uses stale constants. + """ + + def _setup(self): + pt = ParamTable() + ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True) + bracket = RigidBody("bracket", pt, (0, 0, 0), ID_QUAT) + plate = RigidBody("plate", pt, (10, 5, 0), ID_QUAT) + + constraints = [ + CoincidentConstraint(ground, (0, 0, 0), bracket, (0, 0, 0)), + PlanarConstraint(bracket, (0, 0, 0), ID_QUAT, plate, (0, 0, 0), ID_QUAT, offset=0.0), + ] + bodies = [ground, bracket, plate] + return pt, bodies, constraints + + def test_bug_coincident_planar(self): + """Prepass fixes bracket/tz, Planar uses stale constant during drag.""" + pt, bodies, constraints = self._setup() + raw, qg = _build_residuals(bodies, constraints) + + residuals = substitution_pass(raw, pt) + residuals = single_equation_pass(residuals, pt) + + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + assert pt.is_fixed("bracket/tz") + + # Drag bracket up + pt.set_value("bracket/tz", 5.0) + pt.set_value("plate/tz", 5.0) + + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + # True constraints violated + max_err = _eval_raw_residuals(bodies, constraints, pt) + assert max_err > 1.0, f"Expected raw violation from stale prepass, got {max_err:.6e}" + + def test_fix_coincident_planar(self): + """With the fix: constraints satisfied after drag.""" + pt, bodies, constraints = self._setup() + raw, qg = _build_residuals(bodies, constraints) + + residuals = substitution_pass(raw, pt) + # No single_equation_pass + + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + assert not pt.is_fixed("bracket/tz") + + # Drag bracket up + pt.set_value("bracket/tz", 5.0) + pt.set_value("plate/tz", 5.0) + + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + max_err = _eval_raw_residuals(bodies, constraints, pt) + assert max_err < 1e-8, f"Raw residual violation {max_err:.6e} — constraints not satisfied" + + +class TestDragDoesNotBreakStaticSolve: + """Verify that the static solve path (with single_equation_pass) still works. + + The fix only affects pre_drag — the static solve() path continues to + use single_equation_pass for faster convergence. + """ + + def test_static_solve_still_uses_prepass(self): + """Static solve with single_equation_pass converges correctly.""" + pt = ParamTable() + ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True) + arm = RigidBody("arm", pt, (10, 0, 0), ID_QUAT) + plate = RigidBody("plate", pt, (10, 5, 8), ID_QUAT) + + constraints = [ + RevoluteConstraint(ground, (0, 0, 0), ID_QUAT, arm, (0, 0, 0), ID_QUAT), + PlanarConstraint(arm, (0, 0, 0), ID_QUAT, plate, (0, 0, 0), ID_QUAT, offset=0.0), + ] + bodies = [ground, arm, plate] + raw, qg = _build_residuals(bodies, constraints) + + # Full prepass (static solve path) + residuals = substitution_pass(raw, pt) + residuals = single_equation_pass(residuals, pt) + + ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10) + assert ok + + # All raw constraints satisfied + max_err = _eval_raw_residuals(bodies, constraints, pt) + assert max_err < 1e-8 + + # arm at origin (Revolute), plate coplanar (z=0) + env = pt.get_env() + assert abs(env["arm/tx"]) < 1e-8 + assert abs(env["arm/ty"]) < 1e-8 + assert abs(env["arm/tz"]) < 1e-8 + assert abs(env["plate/tz"]) < 1e-8