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)

LoopRateOutput
trajectory sampling + re-anchoring100 to 500 Hz, twist
arm IK100 to 500 Hz
lower body RL policy50 Hz
joint PD1 kHztorques

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
StageNoteQuestion it answers
time law (jerk limited)S-Curve Trajectory Profilehow fast can the hand move without shaking?
time law (smooth reach)Quintic Polynomial Trajectoryhow to match position, velocity and acceleration at both ends?
orientationSpherical Linear Interpolation (SLERP)how to turn the hand along the shortest arc?
moving baseCartesian Target Re-anchoringhow to keep the hand still in the world while the body sways?
balanceUpper-Lower Body Decouplinghow 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.


  • the five notes in the table above