2 Commits

Author SHA1 Message Date
forbes-0023
8e521b4519 fix(solver): use all 3 cross-product components to avoid XY-plane singularity
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.
2026-02-22 15:51:59 -06:00
forbes-0023
533ca91774 feat(solver): full constraint vocabulary — all 24 BaseJointKind types (phase 2)
Add 18 new constraint classes covering all BaseJointKind types from Types.h:
- Point: PointOnLine (2r), PointInPlane (1r)
- Orientation: Parallel (2r), Perpendicular (1r), Angle (1r)
- Surface: Concentric (4r), Tangent (1r), Planar (3r), LineInPlane (2r)
- Kinematic: Ball (3r), Revolute (5r), Cylindrical (4r), Slider (5r),
  Screw (5r), Universal (4r)
- Mechanical: Gear (1r), RackPinion (1r)
- Stubs: Cam, Slot, DistanceCylSph

New modules:
- geometry.py: marker axis extraction, vector ops (dot3, cross3, sub3),
  geometric primitives (point_plane_distance, point_line_perp_components)
- bfgs.py: L-BFGS-B fallback solver via scipy for when Newton fails

solver.py changes:
- Wire all 20 supported types in _build_constraint()
- BFGS fallback after Newton-Raphson in solve()

183 tests passing (up from 82), including:
- DOF counting for every joint type
- Solve convergence from displaced initial conditions
- Multi-body mechanisms (four-bar linkage, slider-crank, revolute chain)
2026-02-20 21:15:15 -06:00