Robotics · Shuja Abidi

Robot Control

MPC and WBC: planning forces, then moving joints

A legged robot has to answer two questions at very different speeds. Model predictive control (MPC) looks a moment ahead: what forces should my feet push with? Whole-body control (WBC) answers right now: what torque should each joint produce?

Goalspeed, footsteps1–10 Hz MPCsimple model, futureTENS OF Hz WBCfull model, right nowHUNDREDS OF Hz Motorsjoint torqueABOUT 1 kHz+ Robot forcesτ measured state: body pose and speed, joint angles, which feet are down
Exact rates vary by robot. The point is that each layer runs faster, and thinks less far ahead, than the one above.

Why two layers? Predicting ahead with a full robot model is too slow, and reacting with no prediction misses what's coming. So MPC predicts ahead with a simple model, and WBC reacts with the full one.

1

Model predictive control (MPC)

Plan the next moment, carry out only the first step, then plan again.

The idea

Think of driving at night. You plan for as far as your headlights reach, you steer a little, and then you plan again from where you now are. MPC works the same way, and that's called a receding horizon:

  1. Measure the robot's current state.
  2. Predict how it would move over the next fraction of a second, for candidate foot forces.
  3. Optimise to find the forces that best follow the goal, while staying within the limits.
  4. Apply only the first step of the plan, then throw the rest away and repeat.

Because the plan is redone many times a second, a push or a slip is simply a new starting state. No separate recovery plan is needed.

Step 1: A simple model of the body

A robot's legs are often light compared with its body, so MPC can treat the whole robot as a single rigid body pushed by forces at the feet:

m p̈ = Σᵢ fᵢ + m g (position: Newton's second law) d/dt ( I ω ) = Σᵢ rᵢ × fᵢ (rotation: torque from each foot force)

Here p is the body's position, ω is how fast it's turning, fᵢ is the force at foot i, and rᵢ is the arm from the centre of mass to that foot. It has far fewer states than the full robot, which makes it fast to solve.

Step 2: Make it linear

The rotation part above isn't linear, because of the cross product and how orientation is written. The MIT Cheetah 3 controller makes it linear by assuming that roll and pitch stay small, and by planning around a reference path with known foot positions. The future then becomes:

x_{k+1} = A_k x_k + B_k f_k + g x = [ orientation, position, angular velocity, linear velocity ]

Step 3: The optimisation problem

min over f₀ … f_{N−1} Σₖ ‖ x_{k+1} − x_ref,k+1 ‖²_Q + ‖ f_k ‖²_R subject to x_{k+1} = A_k x_k + B_k f_k + g (the model) |f_x| ≤ μ f_z, |f_y| ≤ μ f_z (no slipping) f_min ≤ f_z ≤ f_max (push only, within motor strength) f = 0 for feet in the air (the gait schedule)

Each term has a plain meaning:

This form is a quadratic program (QP), which is convex: it has one best answer, and fast solvers can find it reliably. Di Carlo and colleagues (2018) report horizons of up to 0.5 s, running at 20–30 Hz with solves under 1 ms, and speeds up to 3 m/s on the MIT Cheetah 3 across several gaits.

A worked example: friction

Take a 35 kg humanoid standing on a floor with friction μ = 0.6. Its weight is 35 × 9.81 ≈ 343 N, so the ground pushes up with about 343 N in total. Friction can then provide at most:

f_xy,max = μ f_z = 0.6 × 343 ≈ 206 N a_max = 206 / 35 ≈ 5.9 m/s²

On ice, with μ = 0.05, this falls to about 17 N, or about 0.5 m/s². MPC can plan within that limit, instead of commanding a force that would make the foot slip. This is one of its biggest advantages: the limits are written into the problem itself.

Humanoids are harder than quadrupeds

Quadruped MPCHumanoid MPC
FeetSmall points: force onlyFlat feet: force and torque, kept within the foot's area
Leg massOften small enough to ignoreLegs and arms are heavy; they change balance as they swing
Common modelSingle rigid bodyLinear inverted pendulum for walking, or centroidal dynamics for more motion
Margin for errorLarger, with a low, wide stanceSmaller, with a high body over small feet

Centroidal dynamics is the more general model, and includes the angular momentum L of all the moving parts, such as swinging arms. It's exact for the whole robot's momentum, but makes the problem non-convex:

m c̈ = Σᵢ fᵢ + m g L̇ = Σᵢ (pᵢ − c) × fᵢ + Σᵢ τᵢ

Variations

Convex MPC

Simple model, linearised. Very fast and reliable, but less accurate far from its assumptions, for example with a large body tilt.

Nonlinear MPC

Keeps the true model, such as centroidal dynamics. More capable, but slower and may find a poor local answer.

Whole-body MPC

Plans over every joint, so it needs no separate WBC in principle. It's the most complete, and heavy to compute in real time.

2

Whole-body control (WBC)

Take many goals at once, and find the single set of joint torques that best satisfies them.

The problem it solves

At any instant, a robot has many goals: keep the body upright, hold the stance feet still, swing the other foot to its target, follow the arm motion, and apply the forces MPC planned. They share the same motors, and may conflict. WBC decides how to share them, while obeying physics.

Step 1: The full dynamics

Unlike MPC's simple model, WBC uses every link and joint:

M(q) q̈ + h(q, q̇) = Sᵀ τ + J_cᵀ f
M(q)Mass matrix: how hard each joint is to accelerate in this pose
h(q, q̇)Gravity, plus forces caused by motion
Sᵀ τMotor torques, which act only on the actuated joints, not on the floating body
J_cᵀ fContact forces from the ground, mapped to all the joints

Step 2: Write each goal as a task

A task is anything you want to control, such as the position of a foot. The Jacobian J relates joint motion to task motion, so a desired task acceleration becomes a condition on joint acceleration:

ẍ = J q̈ + J̇ q̇ (task acceleration from joint acceleration) ẍ* = ẍ_des + K_p (x_des − x) + K_d (ẋ_des − ẋ) (the acceleration we want)

The second line is a spring and damper in task space: it pulls the task back when it strays from its target.

Step 3: The optimisation problem

WBC is commonly written as a QP that's solved at every control step:

min over q̈, f, τ Σᵢ wᵢ ‖ Jᵢ q̈ + J̇ᵢ q̇ − ẍᵢ* ‖² + ‖ f − f_MPC ‖²_W + ε ‖τ‖² subject to M q̈ + h = Sᵀ τ + J_cᵀ f (physics) J_c q̈ + J̇_c q̇ = 0 (stance feet stay still) |f_xy| ≤ μ f_z, f_z ≥ 0 (friction, push only) τ_min ≤ τ ≤ τ_max (motor limits) joint angle and speed limits

The hard limits can never be broken. The tasks in the cost are what's traded off when everything can't be satisfied at once.

Deciding what matters most

PriorityTaskWhy here
Hard limitPhysics, contact, friction, motor limitsThese can't be negotiated
HighestCentre of mass and body orientationIf this fails, the robot falls
HighSwing foot positionA misplaced foot can trip the robot, or ruin the next step
MediumHand or arm goalsThe actual job, but never at the cost of falling
LowNatural postureUses whatever freedom remains, and keeps the pose sensible

This order is a typical example, and designs vary. It can be enforced in two ways:

Weighted

All tasks go in one cost, with bigger weights on important ones. Simple and flexible, but a low-priority task can still disturb a high-priority one slightly.

Strict hierarchy

A lower task may only use motion that doesn't disturb the higher ones, using null-space projection. It gives a firm order, but is more complex, and can behave abruptly when a task reaches a limit.

A worked example: raising an arm while standing

A humanoid stands and raises a heavy arm forward. The arm's weight moves the centre of mass forward too.

  1. Arm task: asks the shoulder to accelerate the arm upward.
  2. Centre of mass task: notices the forward shift, and asks the hips and ankles to lean back slightly.
  3. Contact condition: requires the feet to stay still, and keeps the pressure inside the foot area.
  4. Solver result: the arm rises and the body leans back at the same moment. No one programmed this lean; it follows from the tasks.

3

How MPC and WBC work together

MPC says what forces to apply. WBC says how the joints can produce them.

This division was shown clearly on the MIT Mini Cheetah by Kim and colleagues (2019). MPC plans ground forces over a longer horizon with a simple model, and WBC turns them into joint commands, with a focus on tracking those forces. They report six gaits, with speeds up to 3.7 m/s.

One moment in a trot

  1. Gait scheduler: front-left and rear-right feet are down, and the others are swinging.
  2. MPC: the robot is commanded forward and is tilted slightly, so it plans forces for the two stance feet over the next fraction of a second.
  3. Foot placement: a simple rule picks where the swinging feet should land, depending on the speed.
  4. WBC: takes the planned forces, the body goal and the swing-foot goals, and solves for joint torques using the full model. It may run several times before the next MPC plan arrives.
  5. Motors: apply the torques and joint commands.
  6. Sensors: report what really happened, and everything repeats from the new state.

Side by side

MPCWBC
Question it answersWhat forces do I need over the next moment?What torque should each joint produce now?
Looks aheadYes: a fraction of a second or moreNo: only the current instant
ModelSimple: rigid body, pendulum or centroidalFull: all links and joints
DecidesContact forces, and sometimes footstepsJoint accelerations, forces and torques
Typical rateTens of hertzHundreds of hertz or more
Good atAnticipating, such as slowing before a stopCoordinating many joints and goals at once
WeaknessIts simple model ignores joint limits and leg swingIt can't foresee a problem that's coming

4

Compared with reinforcement learning

Optimise while running, or learn beforehand and just run a network.

MPC + WBCRL policy
When the hard work happensOn the robot, many times a secondIn simulation, before deployment
NeedsAn accurate model, plus expert tuning of weights and gainsA good simulator, plus reward design, and lots of GPU time
LimitsWritten explicitly; force and friction limits can be set and inspectedLearned; usually followed, but not guaranteed
FailuresEasier to trace to a model, cost or constraintHarder to explain
Rough ground and surprisesHarder without good contact and terrain informationOften very robust, when trained with randomisation
Copying human motionPossible, but needs contact timing and careToday's most common choice
Compute on the robotSolvers run constantlyRunning the network is usually cheap
They aren't really rivals. Modern systems often combine them, for example with RL that adjusts an MPC, or with model-based planning above a learned walking policy. MPC + WBC also suits tasks where someone must be able to state and check the limits, such as forces on a person.

5

From math to AI

How the same problem moved from equations solved on the robot to a network trained in simulation.

The problem stays the same

A walking robot spends much of its time with legs in the air, so it's often briefly unstable. It can only move its body by pushing against the earth with its feet, and it must keep deciding how hard, in which direction and when to push, even on uneven ground or after a kick. What changed is who works out the answer, and when.

MPC: the strategist

Like a driver looking through the windscreen, it looks a fraction of a second to about a second ahead. It treats the body as a single flying brick and plans the pushing forces.

WBC: the muscle coordinator

It takes the planned forces and considers the whole robot: every joint, motor limit and leg. It works out the motor torques, while keeping the feet from slipping.

RL: muscle memory

One neural network replaces both layers on the robot. It reads the robot's sensors and directly outputs motor commands, with the hard work already done in training.

What moved where

Classical: MPC + WBCEnd-to-end RL
Who writes the behaviourEngineers: models, costs and constraintsEngineers write rewards; training finds the behaviour
Where the physics livesIn the equations solved on the robotIn the simulator used for training
On the robotOptimisers running constantlyOne network, typically about 50 times a second
OutputForces, then joint torquesUsually joint target angles, followed by low-level motor controllers
Why it won attentionExplicit limits; explainableHandles full-body motion that's hard to write equations for, such as fast running and recovering from kicks
RL hasn't made classical control obsolete. It's now the common choice for many agile skills, but many systems still combine the two, and explicit limits remain valuable where forces and safety have to be checked.

Open resources you can start from

You don't have to start from nothing, but always check what each one really contains:

ResourceWhat it isCheck first
Unitree RL GymRL training code, with pretrained .pt walking policies for the G1, H1 and H1-2 humanoidsHas Go2 training settings, but no Go2 pretrained policy in the copy we checked
Unitree Go2 velocity policyA community .onnx policy for the Go2 on flat groundFlat ground only; community model, BSD-3
Kyu3224 quadruped policiesFlat and rough-terrain policies in .pt and .onnx, for the KAIST Hound-1 and Hound-2 robotsOther robots need retraining; outputs torque; no licence listed
HIW-500A dataset, not a policy: hundreds of hours of a G1 being teleoperated in real homesCC BY 4.0; suited to imitation learning

Running a trained policy in MuJoCo

A policy works only if it receives exactly the inputs it was trained on. This loop shows the parts that matter, with example values from the Go2 policy above:

import time import mujoco, mujoco.viewer import numpy as np import onnxruntime as ort # 1. Load the robot and the trained policy model = mujoco.MjModel.from_xml_path("go2_scene.xml") # motors must take torque data = mujoco.MjData(model) policy = ort.InferenceSession("policy.onnx") input_name = policy.get_inputs()[0].name # 2. Settings from THIS policy's training configuration. Don't guess these. DEFAULT = np.array([-0.1, 0.9, -1.8, 0.1, 0.9, -1.8, -0.1, 0.9, -1.8, 0.1, 0.9, -1.8]) ACTION_SCALE, KP, KD = 0.5, 20.0, 1.0 model.opt.timestep = 0.005 # physics at 200 Hz DECIMATION = 4 # policy at 50 Hz model.dof_damping[6:18] = 0.0 # this policy was trained without joint damping or friction model.dof_frictionloss[6:18] = 0.0 command = np.array([0.5, 0.0, 0.0]) # forward speed, sideways speed, turn rate def gravity_in_body(q): # the robot's tilt, from its orientation w, x, y, z = q return np.array([2*(-z*x + w*y), -2*(z*y + w*x), 1 - 2*(w*w + z*z)]) data.qpos[2] = 0.33 # start standing data.qpos[3:7] = [1, 0, 0, 0] data.qpos[7:19] = DEFAULT last_action = np.zeros(12, dtype=np.float32) target = DEFAULT.copy() step = 0 with mujoco.viewer.launch_passive(model, data) as viewer: while viewer.is_running(): # 3. The network runs only every few physics steps if step % DECIMATION == 0: obs = np.concatenate([ data.qvel[3:6], # how fast the body is turning gravity_in_body(data.qpos[3:7]), # tilt command, data.qpos[7:19] - DEFAULT, # joint angles, relative to standing data.qvel[6:18], # joint speeds last_action, ]).astype(np.float32) last_action = policy.run(None, {input_name: obs[None]})[0][0] target = DEFAULT + ACTION_SCALE * last_action # 4. A spring and damper at each joint follows the target, at every physics step data.ctrl[:12] = KP * (target - data.qpos[7:19]) - KD * data.qvel[6:18] mujoco.mj_step(model, data) viewer.sync() step += 1 time.sleep(model.opt.timestep) # roughly real time

What must match training

Must matchIf it doesn't
Inputs: which values, their order, their scale, and any historyWrong size stops with an error. Right size in the wrong order runs, and the robot falls
Joint orderCommands go to the wrong joints
Default pose and action scaleThe network's output means the wrong angle
Motor gains and limits, and model physicsThe robot walks too slowly, shakes or falls
Update rateThe policy sees motion at a speed it never practised

These settings are usually in the configuration files saved with the policy, and some exported ONNX files also store them as metadata.

Training a policy for a new scenario

A pretrained policy is a quick start, but a new robot or a new behaviour, such as climbing specific obstacles, usually means training your own. Training runs in a GPU-accelerated simulator, with thousands of robots at once. Isaac Lab is widely used and replaced the older Isaac Gym; mjlab offers a MuJoCo-based route.

  1. The environment. Put about 4,000 virtual robots in the simulator, and randomise the terrain, friction, mass and pushes, so the policy doesn't rely on one exact world.
  2. The reward. Write rules that score behaviour: points for moving at the requested speed and keeping the body level, penalties for falling and wasting energy.
  3. The optimisation loop. Movements start out close to random, with exploration noise added. PPO (Proximal Policy Optimization) favours what earned more reward, and gradually updates the network over millions to billions of simulated steps, until stable walking emerges.
  4. Test before trusting it. Run it in a second simulator, then carefully on the real robot.

What hardware do you need?

A strong GPU helps, because it runs the physics and the learning together, but a top-end card isn't a hard requirement. A modest GPU can also run thousands of robots, at the cost of time.

As one example, a laptop RTX 4060 with 8 GB ran 4,096 robots in mjlab at about 51,000 steps per second, using under 2 GB of GPU memory. That's practical for one clip or a simple task, if you can leave it running for hours.

Rough guide

  • Mid-range or laptop GPU: learning, smoke tests, simple tasks
  • High-end desktop GPU: faster runs and more experiments
  • Rented cloud GPU: large motion datasets and many runs, from providers such as RunPod or Lambda. Compare prices first

6

Tools and papers

Where to read further and try it yourself.

Papers

Open software