The _enforce_quat_continuity function previously skipped dragged parts,
assuming the GUI directly controls their placement. However, Newton
re-solves all free params (including the dragged part's) to satisfy
constraints, and can converge to an equivalent but distinct quaternion
branch. The C++ validateNewPlacements() then sees a >91 degree rotation
and rejects the step.
Two-level fix:
1. Remove the dragged_ids skip — apply continuity to ALL non-grounded
bodies, including the dragged part.
2. Add rotation angle check beyond simple hemisphere negation: compute
the relative quaternion angle using the same formula as the C++
validator (2*acos(w)). If it exceeds 91 degrees, reset to the
previous step's quaternion. This catches branch jumps where the
solver finds a geometrically different but constraint-satisfying
orientation (e.g. Cylindrical + Planar with 180-degree ambiguity).
Verified: all 291 solver tests pass.
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.
Add half-space tracking for all compound constraints with branch
ambiguity: Planar, Revolute, Concentric, Cylindrical, Slider, Screw,
Universal, PointInPlane, and LineInPlane. Previously only
DistancePointPoint, Parallel, Angle, and Perpendicular were tracked,
so the Newton-Raphson solver could converge to the wrong branch for
compound constraints — causing parts to drift through plane
constraints while honoring revolute joints.
Add quaternion continuity enforcement in drag_step(): after solving,
each non-dragged body's quaternion is checked against its pre-step
value and negated if in the opposite hemisphere (standard SLERP
short-arc correction). This prevents the C++ validateNewPlacements()
from rejecting valid solutions as 'flipped orientation' due to the
quaternion double-cover ambiguity (q and -q encode the same rotation
but measure as ~340° apart).
The weight vector was built before substitution_pass and
single_equation_pass, which can fix variables and reduce the free
parameter count. This caused a shape mismatch in newton_solve when
the Jacobian had fewer columns than the weight vector had entries:
ValueError: operands could not be broadcast together with shapes
(55,27) (1,28)
Move build_weight_vector() after both pre-passes so its length
matches the actual free parameters used by the Jacobian.
The parallel-normal constraints (ParallelConstraint, PlanarConstraint,
ConcentricConstraint, RevoluteConstraint, CylindricalConstraint,
SliderConstraint, ScrewConstraint) and point-on-line constraints
previously used only the x and y components of the cross product,
dropping the z component.
This created a singularity when both normal vectors lay in the XY
plane: a yaw rotation produced a cross product entirely along Z,
which was discarded, making the constraint blind to the rotation.
Fix: return all 3 cross-product components. The Jacobian has a
rank deficiency at the solution (3 residuals, rank 2), but the
Newton solver handles this correctly via its pseudoinverse.
Similarly, point_line_perp_components now returns all 3 components
of the displacement cross product to avoid singularity when the
line direction aligns with a coordinate axis.
During interactive drag, the constraint topology is invariant — only the
dragged part's parameter values change between steps. Previously,
drag_step() called solve() which rebuilt everything from scratch each
frame: new ParamTable, new Expr trees, symbolic differentiation, CSE,
and compilation (~150 ms overhead per frame).
Now pre_drag() builds and caches the system, symbolic Jacobian, compiled
evaluator, half-spaces, and weight vector. drag_step() reuses all cached
artifacts, only updating the dragged part's 7 parameter values before
running Newton-Raphson.
Expected ~1.5-2x speedup on drag step latency (eliminating rebuild
overhead, leaving only the irreducible Newton iteration cost).
DistancePointPointConstraint uses a squared residual (||p_i-p_j||^2 - d^2)
which has a degenerate Jacobian when d=0 and the constraint is satisfied
(all partial derivatives vanish). This made the constraint invisible to
the Newton solver during drag, allowing constrained points to drift apart.
When distance=0, use CoincidentConstraint instead (3 linear residuals:
dx, dy, dz) which always has a well-conditioned Jacobian.
Add a code generation pipeline that compiles Expr DAGs into flat Python
functions, eliminating recursive tree-walk dispatch in the Newton-Raphson
inner loop.
Key changes:
- Add to_code() method to all 11 Expr node types (expr.py)
- New codegen.py module with CSE (common subexpression elimination),
sparsity detection, and compile()/exec() compilation pipeline
- Add ParamTable.env_ref() to avoid dict copies per iteration (params.py)
- Newton and BFGS solvers accept pre-built jac_exprs and compiled_eval
to avoid redundant diff/simplify and enable compiled evaluation
- count_dof() and diagnostics accept pre-built jac_exprs
- solver.py builds symbolic Jacobian once, compiles once, passes to all
consumers (_monolithic_solve, count_dof, diagnostics)
- Automatic fallback: if codegen fails, tree-walk eval is used
Expected performance impact:
- ~10-20x faster Jacobian evaluation (no recursive dispatch)
- ~2-5x additional from CSE on quaternion-heavy systems
- ~3x fewer entries evaluated via sparsity detection
- Eliminates redundant diff().simplify() in DOF/diagnostics
Add a Python decomposition layer using NetworkX that partitions the
constraint graph into biconnected components (rigid clusters), orders
them via a block-cut tree, and solves each cluster independently.
Articulation-point bodies propagate as boundary conditions between
clusters.
New module kindred_solver/decompose.py:
- DOF table mapping BaseJointKind to residual counts
- Constraint graph construction (nx.MultiGraph)
- Biconnected component detection + articulation points
- Block-cut tree solve ordering (root-first from grounded cluster)
- Cluster-by-cluster solver with boundary body fix/unfix cycling
- Pebble game integration for per-cluster rigidity classification
Changes to existing modules:
- params.py: add unfix() for boundary body cycling
- solver.py: extract _monolithic_solve(), add decomposition branch
for assemblies with >= 8 free bodies
Performance: for k clusters of ~n/k params each, total cost drops
from O(n^3) to O(n^3/k^2).
220 tests passing (up from 207).
Some files were not shown because too many files have changed in this diff
Show More
Reference in New Issue
Block a user
Blocking a user prevents them from interacting with repositories, such as opening or commenting on pull requests or issues. Learn more about blocking a user.
