Section 46.3: Whole-body and operational-space control

"Whole-body control is where kinematics stops making promises it cannot keep under contact."

A Contact Dynamics Seminar

Figure 46.3A shows the shape of the problem: a humanoid reaching across a table on one foot, where end-effector goals, balance, and contact feasibility must all be satisfied at once. Consider what happens when a humanoid tries to hand a heavy object to a person while standing on one foot. A joint-space controller that ignores the floating base will fight itself: the arm motion shifts the center of mass, the balance correction pulls the torso, and the torso motion disturbs the arm. Whole-body control exists precisely because these interactions are not small perturbations; they are the dominant coupling. The controller must satisfy the hand goal, the balance goal, and the contact feasibility requirement simultaneously, in a single optimization, or the robot fails at all three. In practice, teams that switch from a sequential decompose-then-correct strategy to a unified whole-body QP on the Unitree G1 typically report cutting the number of training episodes needed to achieve stable loco-manipulation from roughly 80,000 to under 5,000, though the exact ratio depends on the task panel and reward shaping, because the policy no longer has to learn compensatory hacks for the coupling the controller was hiding.

A floating-base humanoid is often modeled as \(M(q)\ddot q + h(q, \dot q) = S^\top \tau + J_c(q)^\top \lambda\), with configuration \(q\), mass matrix \(M\), bias term \(h\), selection matrix \(S\), joint torques \(\tau\), and contact wrench multipliers \(\lambda\). This is the minimum equation needed to keep manipulation, balance, and contact in one mathematical object.

Operational-space control adds task variables \(x = \phi(q)\) with Jacobian \(J = \partial \phi / \partial q\). The task-space inertia \(\Lambda = (J M^{-1} J^\top)^{-1}\) captures the apparent mass the controller must move. It reveals how hard the robot must work to accelerate a hand, torso, or center of mass in a given configuration. Reaching with the hand changes the effective control problem for the whole body. On a Unitree G1 with the arm fully extended forward, \(\Lambda\) in the forward direction grows two to three times larger than when the arm is tucked. The same desired hand acceleration then demands proportionally more torque from the shoulder and back joints. A controller that ignores this variation will overshoot when the arm is close and stall when it is extended. Operational-space control makes this configuration dependence explicit and compensates for it automatically.

Theory

Figure 46.3.1 lays out this process as an inspectable loop: observe the state and contacts, build the floating-base model, solve for a command, and verify feasibility before execution. Whole-body control usually combines equality constraints, such as rigid contacts or desired accelerations, with inequality constraints, such as torque limits, friction cones, and joint bounds. A friction cone is the set of contact forces a foot or hand can exert without slipping: when the tangential force exceeds the normal force multiplied by the coefficient of friction, the contact slides. On physical hardware this is not recoverable mid-step. Mechanically, the cone is linearized into a pyramid of half-space constraints on the contact wrench, letting the QP solver check feasibility alongside torque commands in a single pass; tasks weighted too aggressively to respect the cone are softened automatically. The most common implementation is a hierarchy or quadratic program (QP) that trades exact satisfaction of high-priority constraints against soft lower-priority objectives.

This is where model-based libraries remain essential even in learned systems. Architectures such as the GR00T N1 diffusion transformer (a generative model that produces motion trajectories by iteratively denoising a noise sample, described further in section 46.4) or HumanPlus retargeting policies output reference trajectories or joint targets at 10-50 Hz. The execution stack must still map those references through Pinocchio's rigid-body dynamics. It then verifies friction-cone feasibility at 500-1000 Hz before issuing torque commands. Without that inner loop, a policy that learned plausible-looking motions in IsaacSim will command foot contact forces outside the friction cone on physical hardware. The Unitree G1 or H1 then slips and falls within the first contact transition.

Because that inner loop is where feasibility is actually won or lost, the way you evaluate a whole-body controller has to look inside it rather than at the final motion alone. The strongest evaluation artifact is a synchronized trace with task errors, solver status, contact wrench estimates, torque saturation events, and any recovery actions the QP triggered.

Worked Example

The algorithm above lists what the controller must check, but on real hardware those checks only matter if you can read them off a single trial. A small whole-body ledger is enough to show whether a reach task is balance-limited or simply poorly tuned.

trial = {
    "hand_error_cm": 1.8,
    "com_error_cm": 0.9,
    "max_torque_ratio": 0.87,
    "contact_slip_cm": 0.2,
    "qp_status": "solved",
}
print(trial)
print({"feasible": trial["qp_status"] == "solved" and trial["max_torque_ratio"] < 1.0})

Expected output interpretation. The hand target is being tracked within a small error while torque and slip remain inside the envelope. This is the kind of evidence that distinguishes a successful whole-body trial from a lucky reach.

Practical Recipe

1. Write the exact task stack and task priority policy before tuning weights.
2. Instrument contact consistency and torque saturation from the first day.
3. Test reaching while perturbing support, payload, and friction.
4. Compare one model-based baseline to one learned or residual-augmented variant on the same task panel.
5. Archive the solver traces, not only the end-effector plots.

Multibody modeling has not disappeared in the age of foundation models; it has moved inside the inner loop. When GR00T N1 emits joint targets at 30 Hz for a Unitree H1, those targets are meaningless until Pinocchio computes the floating-base mass matrix and TSID checks each foot's friction cone at 1 kHz. The diffusion policy proposes; the whole-body QP decides what is dynamically affordable.

This is where 'the H1 reached the target' and 'the H1 reached the target with 13 percent torque margin and 0.2 cm of foot slip' diverge. Both look identical in a rendered IsaacSim clip, but only the second survives the first contact transition on hardware. A saturated shoulder joint or a violated friction cone produces a fall the renderer never shows.

If whole-body control fails, first isolate whether the task is dynamically infeasible, incorrectly prioritized, or simply under-instrumented. Those are three very different debugging paths. A controller that satisfies the hand goal but silently bankrupts the torque budget is not a success; it is a failure that has not yet fallen over.