Split qpos/qvel into translation/attitude StateData for better integrator tolerances

[ReeceHumphreys]

• Review: By commit
• Merge strategy: Merge (no squash)

Description

The adaptive integrator's step-size controller computes an error norm per StateData object. With a single unified mujocoQpos/mujocoQvel state, orbital translational velocity (~7600 m/s) dominated the norm which would make the adaptive tolerance far too loose to properly control integration error in the for things like attitude and hinge DOFs as found by my testing in #1287.

As a temporary workaround I was forcing relTol = 0 on mujocoQvel, which effectively disabled relative tolerance entirely. However a better solution is to split this tolerancing out:

mujocoQposTranslation / mujocoQposAttitude
mujocoQvelTranslation / mujocoQvelAttitude

Now each gets its own error norm, so orbital and attitude DOFs are controlled independently at their natural scales. A new MJQPosAttitudeStateData class handles the quaternion-aware propagation (mju_quatIntegrate) needed for the attitude qpos block. Index maps are also built at compile and recompile time to pack and unpack between the split states and MuJoCo's flat qpos/qvel arrays.

Context

RKF45 integrator computes its step-size tolerance as |state| * relTol + absTol. With the velocities on the order of orbital speeds in the mujocoQvel state, the tolerance became 7616[m/s] * 1e-4. This allowed substeps large enough to numerically destabilise the stiff panel joint ODE used in the solar panel deploy scenario I made. Thats how the issue was identified and why this needs a fix.

Verification

Ran a MuJoCo vizard scenario I have been developing to validate everything works.

Documentation

N/A

Future work

Sliding joints would get looped into the new attitude bucket alongside hinge joints. A slide joint is in metres, not radians, so its scale is different from both orbital translation and angular DOFs. In practice this is probably fine because a regular boom like a 2-meter boom extension is small compared to orbital distances, so it won't dominate the attitude norm. In the future we should probably handle this separately though.

[juan-g-bonilla]

This is definitely a move in the right direction. Packaging the entire multibody state into a single StateData was always going to be problematic for adaptive integrators. However, I think the better move if we go this direction of unraveling a single MJQPosStateData is to have individual state data for every joint. So,

• MJScalarJoint would get a StateData of size 1x1.
• MJBallJoint would get a new QuaternionStateData of pos size 4x1 and vel size 3x1. We could put this class in src/simulation/dynamics/_GeneralModuleFiles if we steal the mju_quatIntegrate from mujoco and just put it in our code (it's small and standard math enough that this is reasonable).
• MJFreeJoint would get a StateData of size 3x1 and a QuaternionStateData.

This actually completely removes the need for MJQPosStateData. What do you think?