Overview
- this is the hub note for reaching with the arms of a humanoid while an RL policy keeps it balanced
- the chain is: plan a smooth Cartesian path in the world frame, re-express it in the moving base frame, solve IK for the arms, and let the leg policy absorb the reaction
- each step has its own atomic note, linked below
Mathematical Formulation
Three equations carry the whole pipeline:
1. Time law and pose interpolation (one shared path parameter ):
2. Re-anchoring into the moving base frame:
3. Coupling through the unactuated base rows:
The first two make the hand motion smooth and fixed in the world. The third says that the same smooth arm motion is still a disturbance for the legs.
Typical loop rates (generic humanoid)
| Loop | Rate | Output |
|---|---|---|
| trajectory sampling + re-anchoring | 100 to 500 Hz | , twist |
| arm IK | 100 to 500 Hz | |
| lower body RL policy | 50 Hz | |
| joint PD | 1 kHz | torques |
Algorithmic / Architectural Flow
flowchart TB GOAL["Goal pose in world frame"] --> TL["Time law: S-curve or quintic"] TL --> SL["Position lerp + SLERP"] SL --> RA["Re-anchor with live T_WB"] EST["State estimator"] --> RA RA --> IK["Arm IK"] IK --> ARM["Arm PD"] IK -- "q_U des" --> OBS["Policy observation"] ARM -- "q_U, qdot_U" --> OBS EST --> OBS OBS --> POL["Lower body RL policy"] POL --> LEG["Leg PD"] ARM -. "reaction wrench, CoM shift" .-> LEG
| Stage | Note | Question it answers |
|---|---|---|
| time law (jerk limited) | S-Curve Trajectory Profile | how fast can the hand move without shaking? |
| time law (smooth reach) | Quintic Polynomial Trajectory | how to match position, velocity and acceleration at both ends? |
| orientation | Spherical Linear Interpolation (SLERP) | how to turn the hand along the shortest arc? |
| moving base | Cartesian Target Re-anchoring | how to keep the hand still in the world while the body sways? |
| balance | Upper-Lower Body Decoupling | how does the leg policy survive the arm motion? |
Sim-to-Real & Hardware Failure Modes
Top failures across the pipeline:
- Cartesian limits that are safe on paper can still break joint speed limits near a singularity
- a missing quaternion sign flip makes the wrist take the long way round
- foot impact spikes and estimator jumps in go straight to the hand unless filtered
- a leg policy trained with slow or fixed arms falls when real reaches are fast or carry a load
- the arm correcting pelvis sway and the pelvis reacting to the arm can form a coupled oscillation
Details and fixes are in each linked note.
Cross-Links
- the five notes in the table above