[BUG FIX] Honor joint equality polynomials in SAPCoupler.

[jeetrex17]

Description

Honor the full joint-equality polynomial in SAPCoupler, updating its residual and Jacobian every substep. Support omitted joint2 instead of rejecting it at build time. This also applies to URDF mimic multipliers and offsets.

Equality constraints now trigger the SAP solve without contacts, adding per-substep solve overhead: about 69 ms versus 0.12 ms per step on macOS CPU for the 10-DOF test scene, because the SAP Newton, PCG, and line-search launches now run every substep. Regularization now uses the computed Delassus term, which also changes the regularization of existing identity-polynomial equalities.

Related Issue

Resolves #3324.

Rebased on main after #3290 merged.

Motivation and Context

SAP previously assumed an identity polynomial and skipped equality-only scenes without contacts, leaving valid joint constraints unenforced.

How Has This Been / Can This Be Tested?

PYTHONPATH="$PWD" QD_OFFLINE_CACHE=0 uv run pytest \
  tests/coupling/test_hybrid.py::test_sap_joint_equality_polynomial \
  tests/coupling/test_hybrid.py::test_sap_rigid_rigid_hydroelastic_contact \
  tests/rigid/test_constraints.py::test_equality_joint_scaling \
  -p no:pytest-retry -p no:rerunfailures -q

5 passed on macOS CPU (fp64, since SAPCoupler rejects fp32). Pinned Ruff lint and format pass. CUDA and the full suite were not run.

The base fails it: the omitted-joint2 scene is rejected at build, restoring the contact-only gate fails both variants at the correction assertion, and freezing the derivative fails both at the impulse-ratio assertion.

Covered: batched and unbatched, all hinge/slide combinations, scaling, constant offset (a0), all coefficients, derivative updates, omitted joint2, an unrelated joint. The Delassus path runs but its value is not asserted.

scaled_mjcf_joint_equalities moves to tests/conftest.py because tests/rigid and tests/coupling both use it. Exact checks use the tol fixture; the one-step residual-ratio bound reflects SAP's constraint regularization, not floating-point error.

Checklist:

• I read the CONTRIBUTING document.
• I followed the Submitting Code Changes section of CONTRIBUTING document.
• I tagged the title correctly (including BUG FIX/FEATURE/MISC/BREAKING)
• I updated the documentation accordingly or no change is needed.
• I tested my changes and added instructions on how to test it for reviewers.
• I have added tests to cover my changes.
• All new and existing tests passed.