Support initial body velocities during reset

[rubengrandia]

Problem

Kamino currently ignores the model's initial body velocities when resetting with SolverKamino.ResetConfig.ToDefault. Body poses are restored to their initial values, but body and base velocities are reset to zero.

Users can request non-zero reset velocities explicitly through FromJointU, FromActuatorU, and FromBaseU, but there is no reset option that consistently restores Model.body_qd as the initial velocity state.

Why this cannot be changed by copying initial body velocities

A naive implementation that copies the stored initial body twists instead of zeroing them is incorrect for several supported reset configurations:

• Model.body_qd contains world-frame twists at each body's center of mass. These values depend on the body pose and cannot generally be copied unchanged when resetting to a non-default pose.
• With a non-default base pose, initial velocities must be transported from the initial base pose to the target base pose. Applying the transform from the current pose to the target pose gives the wrong result when the current and initial poses differ.
• Combining default base velocity with body velocities reconstructed through FromJointU or FromActuatorU can produce inconsistent body twists unless both are evaluated at the same target pose.
• A raw body twist is not interchangeable with FromBaseU: base velocity may be expressed in the base-joint frame and referenced at the joint origin, whereas body_qd is expressed in world coordinates and referenced at the center of mass.
• Copying maximal-coordinate body velocities at a different pose may violate joint constraints. A kinematically consistent result likely needs velocity-level FK using initial generalized/actuator velocities and a correctly transported initial base velocity.

Proposed direction

Define explicit semantics for restoring initial velocities independently of the selected pose reset mode. The implementation should reconstruct a consistent velocity state at the target pose rather than copying stored world-frame body twists.

At minimum, cover combinations of:

• default, preserved, joint-derived, and actuator-derived body velocities;
• default, preserved, and custom base poses;
• default, preserved, joint-derived, and custom base velocities;
• models with and without a base joint;
• non-trivial initial base poses and velocities;
• a current base pose that differs from both the initial and target poses.

Until these semantics are implemented, ToDefault should retain its documented zero-velocity behavior, and users should pass desired non-zero velocities explicitly.