diff --git a/kindred_solver/solver.py b/kindred_solver/solver.py index 8de9f1d..3d8e76a 100644 --- a/kindred_solver/solver.py +++ b/kindred_solver/solver.py @@ -538,6 +538,16 @@ def _build_constraint( if kind == kcsolve.BaseJointKind.DistancePointPoint: distance = c_params[0] if c_params else 0.0 + if distance == 0.0: + # Distance=0 is point coincidence. Use CoincidentConstraint + # (3 linear residuals) instead of the squared form which has + # a degenerate Jacobian when the constraint is satisfied. + return CoincidentConstraint( + body_i, + marker_i_pos, + body_j, + marker_j_pos, + ) return DistancePointPointConstraint( body_i, marker_i_pos,