fix(wbt): correct velocities, body poses and clip boundaries in default-pose transitions

[tkevinbest]

MotionCommand can splice a synthetic lead-in and lead-out between the robot's default pose and a retargeted clip, so a whole-body-tracking policy has something trackable at the clip boundary instead of a step change. It is gated by enable_default_pose_prepend / enable_default_pose_append, both default off, and no shipped config turns either on. That is also why the path had zero test coverage and seven defects.

What was wrong:

• Stored velocities were not the derivative of the stored positions. Position and velocity were each interpolated linearly between the endpoints, but the derivative of a linear position ramp is a constant, not a ramp. On the test fixture the stored value disagreed with the true derivative by up to 5.3 rad/s, sometimes with the wrong sign (+5.07 rad/s stored where the derivative was a constant -0.23 rad/s).
• Body poses in a frame were not forward-kinematics-consistent with that frame's joint angles. Blending each body's world position independently cuts an arc to a chord: link lengths shrank mid-transition, and no joint configuration corresponded to the emitted poses.
• With motion_dir, the transition was registered as its own motion interval. step() resamples a new clip the instant time_steps reaches a motion's end index, so the robot played the lead-in and was teleported to a random clip exactly at the handoff. Only the first and last clip of the concatenation got a transition at all.
• Motion body slots with no corresponding robot body were zero-filled, leaving a zero quaternion in those slots.
• A fake foot contact point aliases onto its ankle link, and both names are in the robot's body list, so the scatter into motion order was last-write-wins: the contact point, 3.7 cm below the ankle, overwrote the ankle. Both ankle links are tracked bodies, so this landed in the tracking reward.
• Body linear velocities were differenced from link-frame origins, but a motion file stores centre-of-mass velocities (the retargeting converter writes body_pos_w at the link origin and body_lin_vel_w at the CoM). Up to 16 cm of offset on this robot.
• Root rotation was slerped, which matches the clip's orientation at the seam but not its turn rate: a 0.97 rad/s angular-velocity step into a clip turning at 0.60 rad/s.

Joint angles and root position now come from a cubic Hermite segment that returns position and its analytic derivative from one polynomial, so the stored velocity is the derivative of the stored position by construction. Root rotation runs the same cubic in rotation-vector space, with the endpoint derivative mapped through the inverse left Jacobian of SO(3). Body poses are per-frame forward kinematics of the interpolated joint angles, batched across environments and clips rather than looped frame by frame. Body velocities are finite-differenced from those poses and referenced to each body's CoM. Every clip gets its own lead-in and lead-out, spliced into that clip's own frame interval, so time_steps walks from the transition straight into the clip's frames without a reset. The math lives in a new pure-math module, utils/transition_trajectory.py, with no env or simulator imports. FK stays IsaacSim-only, as before, and there are no new config knobs.

Both endpoints are at rest, which makes the cubic a smoothstep and the joint path monotone between the two poses. That is a deliberate trade. Matching the clip's actual arrival velocity would make the seam velocity-continuous, but it forces the path out past the target and back whenever the net displacement is small.

Verification: about 1500 of the 2400 added lines are tests. utils/tests/test_transition_trajectory.py covers the math (36 tests, no sim); test_wbt_transition_fields.py, test_wbt_transition_splice.py and test_wbt_motion_scatter.py cover the emitted fields, the splice and the body scatter under no_sim; and an IsaacSim harness asserts the root pose round-trips through FK, that distinct joint configurations give distinct body poses, and that the batched result matches looping one frame at a time. Batched-vs-per-frame agreement is 2.9e-06, which is what rules out the stale-cache failure mode, and link lengths hold to 2.4e-07 m while the root travels 64 cm. Full runs: no_sim 603 passed, IsaacSim 63 passed, pre-commit and mypy clean. On a 19 s 29-DoF clip (959 frames at 50 fps) the emitted joint angles arrive monotonically at the clip's first frame with no detour, against a stored velocity that is the derivative of the plotted position. A 300-iteration training run on this path completed with no NaNs.

Known limitations: feet can slide during the transition, and the default-pose root height still comes from the nominal spawn height rather than from FK, which is entangled with foot anchoring.

[tkevinbest]

Follow up tests and plots. This figure shows the pre-pr behavior and the post-pr behavior. Velocities are actually the derivatives of the positions in the post, and weren't in the pre.

[tkevinbest]

Also a simple video showing the results. No appending/prepending is on the far left, the pre-PR behavior is in the middle, and the post PR behavior is on the right. There's minimal visual difference between middle and right, but the velocity references fed to the policy are now correctly differentiated.

wbt_transition_boxlift_3pane.mp4