[BUG FIX] Fix light objects being kicked away when the contact solve stops short of convergence.

[duburcqa]

Description

• Under the Euler and implicitfast integrators, the implicit damping pass applies the damped acceleration as a correction of the constraint solver's acceleration, a' = a - (M + hD)^-1 (hD a), instead of re-solving it from the smooth and constraint forces. hD is the diagonal the damped mass factor already adds, formed elementwise into a scratch dof-state field and solved in place with the existing block solve. In MuJoCo compatibility mode the acceleration is re-solved from the forces, (M + hD) a' = f, as MuJoCo does.
• The mass matrix passes (assembly, factor, block solve, implicit damping, manual backward) iterate the kinematic trees, gated on the sleep flag of the tree root, and walk the mass blocks of each tree. The per-entity mask mass_mat_mask and the per-entity block tables are gone: under implicit damping a tree takes the damped factor when one of its dofs carries damping or an implicit actuator bias (func_tree_has_implicit_damping), the others keep their smooth factor and the acceleration the constraint solver converged to. MuJoCo compatibility mode keeps its re-solve from the forces: on the damped trees under Euler, on every awake tree under implicitfast, as before. The Cartesian acceleration walk and the acceleration copy iterate the trees as well.
• mass_mat_mask was part of the recorded rigid state, so a trajectory recorded before this change is refused by the loader (test_trajectory_replay_across_backends needs its snapshots regenerated).

Motivation and Context

When the constraint solve exits with a force-balance residual r, re-solving the acceleration from the forces integrates M^-1 r, which the inertia of a light body turns into a large spurious impulse: on MuJoCo Warp's aloha_clutter scene in single precision a 10 g stick resting on the table drifts, gets kicked, and ends in Invalid constraint forces causing 'nan'. The correction integrates the bounded step the solve took. MuJoCo integrates the residual, which its compatibility mode keeps for parity.

The per-entity mask mismatched the unit the factor works on: after attach() the dofs of a child entity live in the mass block rooted in its parent, so the mask set on the parent left the child's dofs out of the correction. A mass block lies within one kinematic tree, and a tree sleeps as a unit, so the tree is the unit of every mass pass.

How Has This Been / Can This Be Tested?

test_contact_never_injects_energy drops a light tilted box next to a damped pendulum with a single Newton iteration, so every impact solve stops short of its fixed point, and asserts the box never exceeds its free-fall speed and comes to rest. test_implicit_joint_damping_spans_attached_entities attaches a damped pendulum to another pendulum's moving link and checks it against the same mechanism authored as one two-link URDF. Both fail on main. tests/rigid and tests/grad on CPU, Metal and CUDA. go2 at 4096 environments with damping on every joint, Euler: main 1.739 / 1.756 ms per step, this branch 1.750 / 1.765.

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.

[chatgpt-codex-connector]

💡 Codex Review

Here are some automated review suggestions for this pull request.

Reviewed commit: 0cb8b88342

ℹ️ About Codex in GitHub

Codex has been enabled to automatically review pull requests in this repo. Reviews are triggered when you

• Open a pull request for review
• Mark a draft as ready
• Comment "@codex review".

If Codex has suggestions, it will comment; otherwise it will react with 👍.

When you sign up for Codex through ChatGPT, Codex can also answer questions or update the PR, like "@codex address that feedback".

[chatgpt-codex-connector]

Use the mass-block root for correction masks

When attach() merges a child entity's DOFs into a block rooted in its parent, mass_mat_mask is set only for the parent entity (as the preceding rooted-range scan intends), while dyn_info.dofs.entity_idx still identifies each DOF's owning child. This condition therefore omits the child's hD*a entries; the analogous check at line 2392 also omits applying any correction that the full-block solve produces for those DOFs. Consequently Euler/implicitfast computes an incomplete coupled damping update for attached articulated entities, potentially leaving stiff child damping explicit and unstable. Gate initialization and subtraction using the entity that roots dofs_mass_block_start[i_d], or iterate each masked rooted block directly.

Useful? React with 👍 / 👎.