Locomotion, Humanoids, and Navigation › Whole-Body Control

We have identified quantities a locomotion model may need to predict. The next question is how a controller uses them to choose actions. Whole-body control is one structured answer; direct learned policies and other feedback designs are also possible.

"Whole body" refers to coordinated motion across the torso, relevant joints, and active contacts. A controller may jointly choose foot placement, feasible contact forces, torso orientation, and limb posture; the exact decision variables depend on the robot and task. The coupling matters: changing foot placement affects contact forces and torso acceleration, which in turn affect posture and where the feet can reach next. Optimizing those pieces independently can miss a feasible or stable coordinated motion.

One formulation is nominal model predictive control (MPC) over the hybrid dynamics. Recall from Part VII, Ch 1: Sampling-Based Planning and Model Predictive Control that MPC at each control step solves a finite-horizon optimization problem, applies the first action, then re-solves. Here the controller plans with one terrain estimate $\hat\theta_t$ and minimizes stage costs subject to dynamics, actuator limits, and contact/friction constraints. This formulation does not propagate the full terrain belief; scenario or belief-space MPC would account for that uncertainty explicitly:

$$\begin{aligned} \min_{\{a_t\}_{t=0}^{H-1}} \quad & \sum_{t=0}^{H-1} c(s_t, a_t) + c_H(s_H) \\ \text{subject to} \quad & s_{t+1} = f(s_t, a_t, \hat\theta_t) \\ & s_0 = \hat{s}_0, \quad a_t \in \mathcal{A}(s_t) \\ & \text{contact constraints and friction cones} \end{aligned}$$

where:

• $H$: planning horizon (number of steps)
• $a_t$: control input at time step $t$ (e.g., joint torques). A planner may also optimize feasible desired contact forces, but a lower-level controller must realize them through the robot's actuators and contacts.
• $s_t$: full robot state (generalized coordinates and velocities)
• $f(s_t, a_t, \hat\theta_t)$: hybrid dynamics evaluated at the current terrain estimate $\hat\theta_t$
• $c(s_t, a_t)$: stage cost at time $t$ (e.g., tracking error, control effort)
• $c_H(s_H)$: terminal cost at the horizon
• $\hat{s}_0$: estimated initial state (from state estimator)
• $\mathcal{A}(s_t)$: set of admissible actions at state $s_t$ (respecting actuator limits)
• Contact constraints: constraints ensuring feet are on the ground and forces lie in friction cones

The objective sums costs over a horizon. A major difficulty lies in the constraints: the nominal dynamics $s_{t+1} = f(s_t, a_t, \hat\theta_t)$ are hybrid, so contact-mode choices create different feasible regions. The admissible-action constraint encodes hardware limits, while contact and friction constraints restrict forces and motion. A low-cost trajectory that violates one of these constraints is not executable as planned. A trajectory feasible for $\hat\theta_t$ can still fail on uncertain terrain, which is why robust or belief-aware variants matter.

Three choices make this formulation useful for the legged example, though related choices also appear in manipulation:

• $f$ is the hybrid rigid-body dynamics above. It contains contact events, and small differences in their timing can produce large differences in future state on trajectories sensitive to those events.
• $c$ can penalize center-of-mass (CoM) tracking error, footstep error, body orientation, control effort, or torque rates; its form depends on the task.
• At a sticking contact, the force $f_i$ must satisfy a unilateral normal-force condition and a friction cone $\{f : \|f_{\text{tangential}}\| \leq \mu f_{\text{normal}}, f_{\text{normal}} \geq 0\}$. A force outside that cone cannot be sustained as a sticking contact. Force feasibility also depends on contact location, joint kinematics, and actuator torque limits; being inside the cone alone does not make a whole-body plan executable.

The cone is easiest to read directly in force coordinates, where it is the wedge bounded by the two lines $f_{\text{tangential}} = \pm \mu f_{\text{normal}}$. Admissible contact forces sit inside the wedge, and slipping forces sit outside it.

Solving the full hybrid optimization at a high control rate can be expensive; feasibility depends on the model, horizon, solver, and hardware. Three common strategies simplify different parts of the problem.

The first is contact-implicit trajectory optimization, exemplified by Posa, Cantu, and Tedrake (2014), "A Direct Method for Trajectory Optimization of Rigid Bodies Through Contact". Their formulation optimizes motion and contact forces together with complementarity constraints rather than fixing a contact schedule in advance. This mathematical program with complementarity constraints can be numerically difficult. Related contact-implicit formulations may be used in receding-horizon control, but the cited work is trajectory optimization, not a claim that a particular smoothing parameter is always used in MPC.

The second is contact-scheduled optimization, used in several legged-control systems, including MIT Cheetah-style controllers; Crocoddyl is a library that can formulate such problems. A gait pattern or higher-level planner supplies a candidate contact schedule, and continuous optimization then solves for motion and forces under it. This avoids searching over every schedule in each solve, but a poor schedule can rule out an otherwise feasible motion. Feedback can revise timing or the schedule when conditions change. These examples do not establish how prevalent the approach is across deployed quadrupeds.

The third category needs a distinction. A learned dynamics model can be used inside a model-predictive planner; TD-MPC, from Part VIII, Ch 5: TD-MPC and Control-Centric Representations, is an example that uses learned latent dynamics, rewards, and value estimates for short-horizon planning. A direct policy trained by imitation or reinforcement learning may instead map observations to actions without planning through an explicit learned model. Analytic and learned models offer different inductive biases and failure modes; neither has a general guarantee of better behavior on hardware.

The two optimization strategies and learned-model MPC share a predict–optimize–execute–replan loop, but direct learned policies need not. The relevant comparison is therefore between how a controller represents dynamics and contacts, how it selects actions, and how it uses feedback, not an assertion that all reinforcement-learning controllers run MPC.

One reason whole-body control can be tractable is that the net motion of the center of mass (CoM) and its angular momentum can be expressed in a smaller space than the full $6+n_j$ robot coordinates. Internal forces cancel from the whole-body balance equations. In differential form:

$$\begin{aligned} m \ddot{p}_{\text{CoM}} &= m \mathbf{g} + \sum_i f_i \\ \dot{L} &= \sum_i \big[(r_i - p_{\text{CoM}}) \times f_i + \tau_i^{\text{contact}}\big] \end{aligned}$$

Here $\mathbf g$ is the gravity-acceleration vector, $r_i$ the contact point, and $\tau_i^{\text{contact}}$ any direct contact moment, which is zero in a point-contact model. Internal forces do not appear explicitly in these net equations. That does not make joint motion irrelevant: kinematics, torque limits, and available contacts determine which centroidal trajectories the robot can realize. A low-dimensional planner can propose CoM and momentum behavior, while a whole-body controller checks and realizes it through the joints.

The centroidal model is a useful low-dimensional planning surrogate. Its CoM dynamics depend on gravity and contact forces, while the contact sequence remains hybrid. A further simplification is the linear inverted pendulum model (LIPM): with constant CoM height, a point support, and negligible rate of centroidal angular momentum about the CoM, the horizontal equation becomes linear and foot placement becomes a planning input. These assumptions make the LIPM useful for illustration, not a full robot simulator.

The first equation is Newton's second law for the whole-body CoM: gravity and the vector sum of contact forces determine its acceleration. The second is the angular-momentum balance about the CoM: contact forces acting at moment arms $r_i-p_{\text{CoM}}$, plus any direct contact moments, change $L$. In the LIPM special case, the CoM height is fixed at $z_0$, support is a point at $p_{\text{foot}}$, and the centroidal angular-momentum rate is neglected. The horizontal dynamics then become $\ddot{p}_x = (g/z_0)\,(p_x-p_{\text{foot}})$, where $g$ is the positive magnitude of gravitational acceleration. Changing $p_{\text{foot}}$ between steps supplies the control input in the example below.

The instability and the role of foot placement are visible in a short numerical integration of that ODE. With a fixed foot, the CoM accelerates away from the contact point. In the plotted feedback run, re-placing the foot at the capture-point estimate $p + \dot{p}/\omega$ every step interval brings the CoM toward a bounded position.

For a fixed foot placement, the linear LIPM has a closed-form CoM solution; choosing the next $p_{\text{foot}}$ changes that solution. A controller can use a centroidal planner for a candidate CoM and momentum trajectory, then a whole-body inverse-dynamics layer to seek joint torques that realize it. The layers must be checked together: joint reach, torque limits, and contact availability can invalidate the candidate. Their update rates depend on the robot and implementation, not on a universal planner-versus-controller frequency split.

The LIPM gives a concrete interpretation of foot placement. With a fixed point support, its horizontal CoM dynamics are unstable: displacement from the support tends to grow. Repositioning the support can change that trajectory. The capture-point-style rule in the code illustrates one feedback choice, not a general solution for every gait, contact geometry, or robot.

Analytic whole-body controllers use approximate body dynamics, parameter estimates, and contact assumptions. Feedback can tolerate some mismatch, but unmodeled compliance, soft terrain, or damage may degrade performance. Learned corrections and adaptation are possible responses; their value must be tested against a well-tuned analytic baseline.

One pattern is residual learning: fit a model of the mismatch between analytic and observed dynamics and use it alongside the analytic model. This resembles system identification with a flexible model class (Part II, Ch 3: System Identification). Training data can come from simulation, hardware, or both, and the residual must be evaluated on the target states and contacts. An unconstrained residual does not automatically fall back to physics outside its training data; that behavior requires an explicit gate, bound, or uncertainty-aware design.

Another pattern learns dynamics or a policy from data. The Dreamer family from Part VIII, Ch 3: The Dreamer Family learns a recurrent latent model from experience and trains an actor and critic using imagined trajectories. This is distinct from privileged teacher–student locomotion training: some methods train with simulator-only terrain information and then distill or adapt a controller that uses onboard sensing. Neither approach implies that every humanoid or quadruped learns a single latent world model of body and terrain, and a privileged teacher does not guarantee successful extraction from noisy sensors.

The distinction is about inductive bias, not a strict body-versus-terrain split. An analytic backbone can preserve useful structure, but a learned residual may still produce physically implausible outputs unless constrained. An end-to-end model can learn unmodeled effects and can also incorporate physical structure. Which combination works best depends on the robot, data, and validation regime.

Whole-body control can encounter several failure modes worth testing explicitly; their frequency and severity depend on the system.

• Contact mistiming. The controller expects a foot to be in contact at time $t$ and it is not, or vice versa. This happens when terrain height is misestimated, when the compliance of the surface is not modeled, or when the actuator bandwidth limits how fast the leg can extend.
• Friction cone violation. A requested tangential force may exceed the available sticking-friction bound, causing slip; whether the robot recovers depends on its feedback and remaining contacts. A model trained on one surface may misestimate this bound on another.
• Model exploitation. The optimizer finds a plan that is optimal under an inaccurate model but impossible in reality: for example, a step that relies on force that the model thinks is available but the actuator cannot produce. This is the canonical "model-based RL gone wrong" story from Part XII, Ch 1: Failure Modes and Model Exploitation.
• Compounding rollout error. Repeated model errors can accumulate over an open-loop horizon, though they can also contract under stable dynamics. The relevant horizon depends on the model, controller, and task.

The four failure modes interact rather than dividing neatly into physical and computational categories. Contact mistiming or slip may follow a poor terrain estimate, while model exploitation can end in a physical loss of balance. Compounding prediction error can also misrank otherwise feasible plans without making any action physically impossible. Better contact and friction estimates help, as does measuring where the predictive model is unreliable; a controller needs both kinds of checks because an unexpected contact changes the state from which the next prediction begins.

No single mitigation covers all four. Re-observation and replanning can reduce exposure to open-loop error, subject to sensing and computation limits. Contact and friction failures also require feasible constraints, calibration, and monitoring. An ensemble may reveal model disagreement, but disagreement is not a calibrated detector of every out-of-distribution state or model-exploitation risk. Feedback update rate and rollout horizon should be chosen from measured closed-loop behavior, not a universal short-horizon rule.

We test whether a learned hopper model can sustain the multi-step predictions that MPC would use. The experiment collects simulator transitions and fits a linear model of vertical dynamics; it does not optimize actions or close a control loop.