kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

fix(solver): use all 3 cross-product components to avoid XY-plane singularity

Browse Source 
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.
This commit is contained in:
forbes-0023
2026-02-22 15:51:59 -06:00
parent bfb787157c
commit 8e521b4519
5 changed files with 53 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
kindred_solver/constraints.py
View File

@@ -196,8 +196,8 @@ class PointOnLineConstraint(ConstraintBase):
p_i = self.body_i.world_point(*self.marker_i_pos)
p_j = self.body_j.world_point(*self.marker_j_pos)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy = point_line_perp_components(p_i, p_j, z_j)
return [cx, cy]
cx, cy, cz = point_line_perp_components(p_i, p_j, z_j)
return [cx, cy, cz]
class PointInPlaneConstraint(ConstraintBase):
@@ -244,8 +244,9 @@ class ParallelConstraint(ConstraintBase):
"""Parallel axes — 2 DOF removed.
marker Z-axes are parallel: z_i x z_j = 0.
2 residuals from the cross product (only 2 of 3 components are
independent for unit vectors).
3 cross-product residuals (rank 2 at the solution). Using all 3
avoids a singularity when the axes lie in the XY plane, where
dropping cz would leave the constraint blind to yaw rotations.
"""
def __init__(
@@ -264,7 +265,7 @@ class ParallelConstraint(ConstraintBase):
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, cz = cross3(z_i, z_j)
return [cx, cy]
return [cx, cy, cz]
class PerpendicularConstraint(ConstraintBase):
@@ -350,17 +351,17 @@ class ConcentricConstraint(ConstraintBase):
self.distance = distance
def residuals(self) -> List[Expr]:
# Parallel axes (2 residuals)
# Parallel axes (3 cross-product residuals, rank 2 at solution)
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, _cz = cross3(z_i, z_j)
cx, cy, cz = cross3(z_i, z_j)
# Point-on-line: marker_i origin on line through marker_j along z_j
p_i = self.body_i.world_point(*self.marker_i_pos)
p_j = self.body_j.world_point(*self.marker_j_pos)
lx, ly = point_line_perp_components(p_i, p_j, z_j)
lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
return [cx, cy, lx, ly]
return [cx, cy, cz, lx, ly, lz]
class TangentConstraint(ConstraintBase):
@@ -417,10 +418,10 @@ class PlanarConstraint(ConstraintBase):
self.offset = offset
def residuals(self) -> List[Expr]:
# Parallel normals
# Parallel normals (3 cross-product residuals, rank 2 at solution)
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, _cz = cross3(z_i, z_j)
cx, cy, cz = cross3(z_i, z_j)
# Point-in-plane
p_i = self.body_i.world_point(*self.marker_i_pos)
@@ -429,7 +430,7 @@ class PlanarConstraint(ConstraintBase):
if self.offset != 0.0:
d = d - Const(self.offset)
return [cx, cy, d]
return [cx, cy, cz, d]
class LineInPlaneConstraint(ConstraintBase):
@@ -527,12 +528,12 @@ class RevoluteConstraint(ConstraintBase):
p_j = self.body_j.world_point(*self.marker_j_pos)
pos = [p_i[0] - p_j[0], p_i[1] - p_j[1], p_i[2] - p_j[2]]
# Parallel Z-axes
# Parallel Z-axes (3 cross-product residuals, rank 2 at solution)
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, _cz = cross3(z_i, z_j)
cx, cy, cz = cross3(z_i, z_j)
return pos + [cx, cy]
return pos + [cx, cy, cz]
class CylindricalConstraint(ConstraintBase):
@@ -559,17 +560,17 @@ class CylindricalConstraint(ConstraintBase):
self.marker_j_quat = marker_j_quat
def residuals(self) -> List[Expr]:
# Parallel Z-axes
# Parallel Z-axes (3 cross-product residuals, rank 2 at solution)
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, _cz = cross3(z_i, z_j)
cx, cy, cz = cross3(z_i, z_j)
# Point-on-line
p_i = self.body_i.world_point(*self.marker_i_pos)
p_j = self.body_j.world_point(*self.marker_j_pos)
lx, ly = point_line_perp_components(p_i, p_j, z_j)
lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
return [cx, cy, lx, ly]
return [cx, cy, cz, lx, ly, lz]
class SliderConstraint(ConstraintBase):
@@ -598,22 +599,22 @@ class SliderConstraint(ConstraintBase):
self.marker_j_quat = marker_j_quat
def residuals(self) -> List[Expr]:
# Parallel Z-axes
# Parallel Z-axes (3 cross-product residuals, rank 2 at solution)
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, _cz = cross3(z_i, z_j)
cx, cy, cz = cross3(z_i, z_j)
# Point-on-line
p_i = self.body_i.world_point(*self.marker_i_pos)
p_j = self.body_j.world_point(*self.marker_j_pos)
lx, ly = point_line_perp_components(p_i, p_j, z_j)
lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
# Rotation lock: x_i . y_j = 0
x_i = marker_x_axis(self.body_i, self.marker_i_quat)
y_j = marker_y_axis(self.body_j, self.marker_j_quat)
twist = dot3(x_i, y_j)
return [cx, cy, lx, ly, twist]
return [cx, cy, cz, lx, ly, lz, twist]
class ScrewConstraint(ConstraintBase):
@@ -648,14 +649,14 @@ class ScrewConstraint(ConstraintBase):
self.pitch = pitch
def residuals(self) -> List[Expr]:
# Cylindrical residuals (4)
# Cylindrical residuals (5: 3 parallel + 2 point-on-line)
z_i = marker_z_axis(self.body_i, self.marker_i_quat)
z_j = marker_z_axis(self.body_j, self.marker_j_quat)
cx, cy, _cz = cross3(z_i, z_j)
cx, cy, cz = cross3(z_i, z_j)
p_i = self.body_i.world_point(*self.marker_i_pos)
p_j = self.body_j.world_point(*self.marker_j_pos)
lx, ly = point_line_perp_components(p_i, p_j, z_j)
lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
# Pitch coupling: axial_disp = pitch * angle / (2*pi)
# Axial displacement
@@ -684,7 +685,7 @@ class ScrewConstraint(ConstraintBase):
# = axial - pitch * qz / pi
coupling = axial - Const(self.pitch / math.pi) * rel[3]
return [cx, cy, lx, ly, coupling]
return [cx, cy, cz, lx, ly, lz, coupling]
class UniversalConstraint(ConstraintBase):

14
kindred_solver/geometry.py
View File

@@ -117,15 +117,15 @@ def point_line_perp_components(
point: Vec3,
line_origin: Vec3,
line_dir: Vec3,
) -> tuple[Expr, Expr]:
"""Two independent perpendicular-distance components from point to line.
) -> tuple[Expr, Expr, Expr]:
"""Three perpendicular-distance components from point to line.
The line passes through line_origin along line_dir.
Returns the x and y components of (point - line_origin) x line_dir,
which are zero when the point lies on the line.
Returns all 3 components of (point - line_origin) x line_dir,
which are zero when the point lies on the line. Only 2 of 3 are
independent, but using all 3 avoids a singularity when the line
direction aligns with a coordinate axis.
"""
d = sub3(point, line_origin)
cx, cy, cz = cross3(d, line_dir)
# All three components of d x line_dir are zero when d is parallel
# to line_dir, but only 2 are independent. We return x and y.
return cx, cy
return cx, cy, cz

20
tests/test_constraints_phase2.py
View File

@@ -65,7 +65,7 @@ class TestPointOnLine:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = PointOnLineConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
assert len(c.residuals()) == 2
assert len(c.residuals()) == 3
class TestPointInPlane:
@@ -135,7 +135,7 @@ class TestParallel:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = ParallelConstraint(b1, ID_QUAT, b2, ID_QUAT)
assert len(c.residuals()) == 2
assert len(c.residuals()) == 3
class TestPerpendicular:
@@ -222,7 +222,7 @@ class TestConcentric:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = ConcentricConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
assert len(c.residuals()) == 4
assert len(c.residuals()) == 6
class TestTangent:
@@ -279,7 +279,7 @@ class TestPlanar:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = PlanarConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
assert len(c.residuals()) == 3
assert len(c.residuals()) == 4
class TestLineInPlane:
@@ -334,7 +334,7 @@ class TestRevolute:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = RevoluteConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
assert len(c.residuals()) == 5
assert len(c.residuals()) == 6
class TestCylindrical:
@@ -353,7 +353,7 @@ class TestCylindrical:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = CylindricalConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
assert len(c.residuals()) == 4
assert len(c.residuals()) == 6
class TestSlider:
@@ -375,15 +375,15 @@ class TestSlider:
c = SliderConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
env = pt.get_env()
vals = [r.eval(env) for r in c.residuals()]
# First 4 should be ~0 (parallel + on-line), but twist residual should be ~1
assert abs(vals[4]) > 0.5
# First 6 should be ~0 (parallel + on-line), but twist residual should be ~1
assert abs(vals[6]) > 0.5
def test_residual_count(self):
pt = ParamTable()
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = SliderConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
assert len(c.residuals()) == 5
assert len(c.residuals()) == 7
class TestUniversal:
@@ -411,7 +411,7 @@ class TestScrew:
b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
c = ScrewConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT, pitch=10.0)
assert len(c.residuals()) == 5
assert len(c.residuals()) == 7
def test_zero_displacement_zero_rotation(self):
"""Both at origin with identity rotation → all residuals 0."""

6
tests/test_geometry.py
View File

@@ -173,15 +173,17 @@ class TestPointLinePerp:
pt = (Const(0.0), Const(0.0), Const(5.0))
origin = (Const(0.0), Const(0.0), Const(0.0))
direction = (Const(0.0), Const(0.0), Const(1.0))
cx, cy = point_line_perp_components(pt, origin, direction)
cx, cy, cz = point_line_perp_components(pt, origin, direction)
assert abs(cx.eval({})) < 1e-10
assert abs(cy.eval({})) < 1e-10
assert abs(cz.eval({})) < 1e-10
def test_off_line(self):
pt = (Const(3.0), Const(0.0), Const(0.0))
origin = (Const(0.0), Const(0.0), Const(0.0))
direction = (Const(0.0), Const(0.0), Const(1.0))
cx, cy = point_line_perp_components(pt, origin, direction)
cx, cy, cz = point_line_perp_components(pt, origin, direction)
# d = (3,0,0), dir = (0,0,1), d x dir = (0*1-0*0, 0*0-3*1, 3*0-0*0) = (0,-3,0)
assert abs(cx.eval({})) < 1e-10
assert abs(cy.eval({}) - (-3.0)) < 1e-10
assert abs(cz.eval({})) < 1e-10

6
tests/test_joints.py
View File

@@ -421,8 +421,10 @@ class TestSliderCrank:
bodies = [ground, crank, rod, piston]
dof = _dof(pt, bodies, constraints)
# 3D slider-crank: planar motion + out-of-plane fold modes
assert dof == 3
# With full 3-component cross products, the redundant constraint rows
# eliminate the out-of-plane fold modes, giving the correct 1 DOF
# (crank rotation only).
assert dof == 1
def test_slider_crank_solves(self):
"""Slider-crank converges from displaced state."""