Whole-Body Control for Humanoid Robots: QP Task Prioritization, Centroidal Dynamics and MPC

A humanoid has between 25 and 40 actuated joints, two feet that may or may not be on the ground, two hands that want to do something useful, and a torso that will fall over if you stop thinking about it for a few hundred milliseconds. Nothing about it fits the textbook industrial arm, where a fixed base, a rigid link chain and a position-controlled joint loop are enough. The base here is not bolted down. It is the robot’s own pelvis, it moves, and the only thing that can push it is friction at the soles of the feet.

Whole-body control humanoid systems solve this by treating the robot as one coupled mechanical system and computing, many hundreds of times per second, the joint torques and contact forces that satisfy physics while pursuing a ranked list of goals. In 2026 the dominant recipe is layered: a model predictive controller on simplified centroidal dynamics decides where momentum should go, and a quadratic program (QP) at the whole-body level turns that into torques. Learned reinforcement learning (RL) policies increasingly compete with, or wrap around, that stack.

This article gives you the mechanism, not the marketing: the floating-base equation of motion, contact constraints and friction cones, strict versus weighted task prioritization, centroidal dynamics and MPC, the classical zero moment point (ZMP) and capture point ideas, whole-body MPC, and how RL policies fit. It includes a runnable numpy example of prioritized task allocation.

What this covers: floating-base dynamics, the whole-body QP, hierarchical versus weighted priorities, centroidal MPC, ZMP and capture point, whole-body MPC, the software stack (Pinocchio, OCS2, Crocoddyl, OSQP, qpOASES), RL hybrids, failure modes and a deployment checklist.

Context and Background

Humanoid control grew from two lineages. The first is the walking-pattern tradition: Vukobratovic’s zero moment point criterion, and Kajita and colleagues’ 2003 work on pattern generation by preview control of the ZMP, which reduced the robot to a cart on a table (the linear inverted pendulum) and tracked a ZMP reference (Kajita et al., ICRA 2003). Those controllers generated joint position trajectories offline or semi-online, and a stiff position-controlled servo followed them. That is why classic Honda ASIMO-era and HRP-series robots walk with a characteristic flat-footed, bent-knee gait.

The second lineage is torque-controlled, force-aware control, which came out of the DARPA Robotics Challenge era and the hydraulic and series-elastic robots built around it. Here the controller commands joint torques, reasons explicitly about contact forces, and treats balance as an optimization problem. This is the lineage of operational-space control (Khatib), of hierarchical inverse dynamics, and of the QP controllers now standard on research and commercial humanoids.

Two things changed between roughly 2018 and 2026. First, efficient rigid-body algorithms made the full dynamics cheap. The open-source library Pinocchio computes inverse dynamics, the joint-space inertia matrix and analytical derivatives in microseconds to low milliseconds for a humanoid-sized model, which makes whole-body optimal control tractable. Second, hardware caught up: actuators with low reflected inertia and transparent torque sensing made it realistic to command torque at 1 kHz and expect the robot to do what the model says.

The industry context matters too. Commercial humanoid programs such as the one analyzed in our piece on Tesla Optimus Gen 3 humanoid architecture have shifted public attention toward learned policies, but nearly every serious program still has a model-based layer somewhere: in the training simulator, in the safety fallback, in the contact-rich manipulation controller, or in the reference generator that an RL policy imitates. Understanding the model-based stack is therefore not nostalgia; it is the vocabulary for everything else, including why learned policies sometimes fail where a QP would not.

A note on scope. Where this article gives numerical rates, they are typical ranges reported across the literature and open-source examples, not specifications of any product. Illustrative figures are labeled as illustrative.

The Reference Architecture: Layers, Rates and Responsibilities

In short: a modern whole-body control humanoid stack separates concerns by time scale. A planner picks footsteps, a centroidal MPC (tens of hertz) plans momentum and contact forces, a whole-body QP (hundreds of hertz to 1 kHz) allocates torques to satisfy ranked tasks and contact constraints, and a joint-level loop tracks those torques. Each layer uses a simpler model than the one below it.

Figure 1: The layered control stack. Simplified models run slowly and far ahead; the full-dynamics QP runs fast and only one step ahead.

The figure shows the standard decomposition. The task planner emits footstep locations and end-effector goals at roughly 1 to 10 Hz. The centroidal MPC consumes them and produces center-of-mass (CoM) and momentum references plus planned contact forces. The whole-body QP combines those references with the instantaneous measured state and returns joint torques. A state estimator, fusing IMU and joint encoders, and often contact detection, feeds both upper layers. Joint-level torque or impedance loops close the last gap to the motors.

Why layering exists

The reason for layers is computational and structural, not tradition. The full nonlinear dynamics of a 30-DoF floating-base robot, combined with contact-mode switching, makes a long-horizon optimization expensive. A one-step QP on the instantaneous dynamics is a convex problem with tens to a couple hundred variables, and well-tuned solvers handle that in well under a millisecond. The trade is horizon: a one-step QP is myopic. It cannot know that a foot must land 20 centimeters further forward in 300 ms to avoid a fall. The centroidal MPC carries that foresight, using a model small enough to solve over a horizon of roughly one to two seconds.

The state of each layer

The footing of the system is the state estimator. The floating base has six degrees of freedom (three position, three orientation) that are not directly measured by any joint encoder. An IMU gives orientation and angular rate; position and linear velocity come from fusing leg kinematics with the assumption that stance feet do not slip, often through an extended Kalman filter. If that assumption is wrong, the base velocity estimate is wrong, and every layer above inherits the error. This is the first of several places where the controller is only as good as its estimated contact state, a theme we return to in the failure modes section.

Interface contracts between layers

Each layer hands the next a reference, not a command. The MPC sends desired CoM position, linear and angular momentum, foot trajectories, and contact force profiles. The QP treats these as soft or hard targets, balanced against whatever the instantaneous physics permits. If the MPC reference is infeasible given a disturbance that arrived 5 ms ago, the QP degrades gracefully: it satisfies contact and torque limits first and tracks the reference as closely as it can. That graceful degradation, with constraints as the backbone, is the reason QP-based control is trusted in safety-critical contexts.

Floating-Base Dynamics and Contact Constraints

The foundation is one equation. For a robot with n joints and a free-floating base, the generalized coordinates have n + 6 degrees of freedom (the base pose in 3D plus the joint angles; velocities are n + 6 dimensional). The equation of motion is:

M(q) qdd + h(q, qd) = S^T tau + J_c^T f

Here M(q) is the joint-space inertia matrix, qdd the generalized acceleration, h(q, qd) collects Coriolis, centrifugal and gravity terms, tau is the vector of n actuated joint torques, f stacks the contact forces, and J_c is the stacked contact Jacobian. The matrix S = [0_(n x 6) I_(n x n)] is the selection matrix: it says torques act only on the actuated joints, never directly on the six base coordinates. That structural fact is the whole difficulty of legged robots, called underactuation.

Reading the equation

Split the equation into its first six rows (the base) and last n rows (the joints). The base rows contain no tau term at all:

M_b(q) qdd + h_b = J_cb^T f

The robot can only accelerate its base through contact forces. Torque affects the base indirectly, by changing what the limbs do and therefore what contact forces the ground can supply. This is why a humanoid in flight cannot correct its angular momentum except by swinging limbs, and why balance is fundamentally about managing contact forces, not about commanding the pelvis directly.

Contact as a constraint

When a foot is firmly planted, its point has zero acceleration in the world frame. Writing the contact point acceleration as J_c qdd + Jdot_c qd = 0 gives an equality constraint that couples to the dynamics. Combined with the dynamics above, the unknowns are qdd, tau and f, and the equations form a linear system in them. This is the key reformulation: inverse dynamics under contact becomes a linear (and therefore convex) problem in (qdd, tau, f), which is exactly what a QP wants.

Friction cones and why you linearize them

A planted foot cannot pull on the ground, and it cannot push sideways with arbitrary force. The Coulomb friction model says the tangential force magnitude must not exceed mu times the normal force, with f_n >= 0. That is a second-order cone, a convex constraint but not a linear one. Real-time controllers commonly replace the cone with a pyramid of four (or more) linear inequalities:

|f_x| <= mu f_z / sqrt(2), |f_y| <= mu f_z / sqrt(2), f_z >= 0

The inscribed pyramid is conservative: it admits slightly less force than the true cone (its corners touch the cone, but along the flat faces it allows about 29 percent less tangential force), but it keeps the problem a QP, which has mature, fast solvers. A finer approximation with eight facets costs more constraints and recovers more friction capacity. Which to choose depends on how close to slipping the task runs. For walking on a high-friction floor, four facets are routine; for pushing a heavy cart or standing on a slope, you want the better approximation.

Foot geometry and the center of pressure

A foot is not a point. A rectangular sole can generate a moment about its center as long as the center of pressure stays inside the support polygon. Controllers handle this either by modeling each foot as four corner point contacts (each with its own cone and non-negative normal force, which implicitly keeps the pressure inside the sole) or by adding a wrench cone with explicit moment limits. The corner-point model is easier to build and extends naturally to partial foot contact, such as rolling over the toe, at the cost of four times as many contact force variables per foot.

The unilateral and timing constraints nobody draws

Contact forces are only nonzero when the foot is actually in contact, and the controller must know when. A foot that the planner thinks has landed but physically has not will have its force variables fighting for traction that is not there. A foot that is declared swing while still pressed to the floor will be driven by a position task into the ground. Contact schedule and contact detection are therefore inputs to the QP, not outputs, and a production system typically combines the planned schedule with force-threshold or kinematic detection for robustness.

The Whole-Body QP: Tasks, Priorities and the Solver

In short: a whole-body QP stacks the equation of motion, contact constraints, friction pyramids and actuator limits as constraints, then expresses goals such as CoM tracking, swing-foot placement and posture as quadratic cost terms in (qdd, tau, f). Prioritization is either strict (null-space or lexicographic) or weighted (one summed cost).

Figure 2: What goes into the whole-body QP. Physics and limits are constraints; goals are costs. The solve repeats every control tick.

Task-space control in one paragraph

A task is a desired behaviour of some quantity x = phi(q), such as the position of the left hand or the CoM. Differentiating twice gives xdd = J qdd + Jdot qd. A task-space controller sets a desired acceleration with a PD law, xdd_des = xdd_ref + Kp (x_ref - x) + Kd (xd_ref - xd), then asks the optimizer to make J qdd + Jdot qd match it. This is task-space control: you describe goals in the coordinates where they are natural (Cartesian space, momentum, orientation) and the solver does the mapping to joints. It is the humanoid counterpart of the operational-space framework, and the compliance side of the story is covered in impedance and admittance control for compliant manipulation.

The standard QP formulation

Collect the decision variable z = [qdd; tau; f] (some formulations eliminate tau by substituting the dynamics, which shrinks the problem). The QP is:

minimize sum_i w_i || J_i qdd + Jdot_i qd - xdd_i_des ||^2 + regularization

subject to the equation of motion, J_c qdd + Jdot_c qd = 0 for stance feet, the friction pyramid for each contact force, torque limits tau_min <= tau <= tau_max, and optionally joint position and velocity limits expressed as acceleration bounds over one step.

Typical sizes: for a 30-DoF humanoid with two feet as four-point contacts, qdd has 36 entries, tau 30, and f 24, so roughly 90 variables, with a few hundred inequality rows. Dense active-set solvers such as qpOASES and operator-splitting solvers such as OSQP handle problems of that size in a fraction of a millisecond to a few milliseconds on an embedded CPU, depending on warm start and tolerance. Treat those timings as indicative; they depend heavily on the model, solver settings and hardware, and you should benchmark on your target computer.

Strict priorities: the null-space approach

In a strict hierarchy, a lower-priority task may not degrade a higher one. The classical solution projects each task into the null space of those above it. With task 1 solved by qdd_1 = J1^+ (xdd1_des - Jdot1 qd), the second task is solved only inside N1 = I - J1^+ J1:

qdd = qdd_1 + (J2 N1)^+ (xdd2_des - Jdot2 qd - J2 qdd_1)

and so on down the list. When tasks do not conflict this gives the exact solution of all of them. When they conflict, the higher priority one is satisfied exactly (as far as it can be) and the lower one is satisfied only in the leftover subspace. The cost is numerical: every pseudo-inverse of a near-singular J N amplifies noise, which we demonstrate below. Inequality constraints (torque limits, friction) do not fit the projection framework directly, which is why modern implementations instead use a cascade of QPs, solving one QP per priority level and passing the optimal value of previous levels as equality constraints to the next. This is the hierarchical QP (HQP) approach.

Weighted priorities: one QP, one set of weights

The weighted approach sums all tasks with weights spanning orders of magnitude, such as 1000 for balance, 10 for the swing foot and 1 for posture. It needs only one solve per tick, so it is faster and has more predictable timing than a cascade. The price is softness: a large weight does not give a hard guarantee, and a very large weight ratio makes the Hessian ill-conditioned, which hurts solver accuracy. A task with weight 1000 conflicting with one of weight 1 will still be violated a little, and in a pinch both can be sacrificed in unexpected proportions.

Figure 3: Two ways to rank tasks. Strict hierarchy passes freedom downward; the weighted sum trades all tasks inside one cost.

A runnable example: null-space versus weighted allocation

The following script uses numpy only, no solver dependency. It builds two random linear tasks on a 7-DoF system and solves them both ways. The check: when the tasks are compatible, both methods satisfy both. When the system is over-constrained (4 DoF, 6 task rows), the strict method satisfies task 1 exactly and sacrifices task 2, while the weighted method only approaches that as the weight ratio grows.

import numpy as np

def pinv(A):
    # SVD pseudo-inverse with truncation: avoids noise blow-up near singularities
    return np.linalg.pinv(A, rcond=1e-9)

def hierarchical(tasks, n):
    """Strict priority via null-space projection. tasks: [(J, xdd_des, Jdot_qd)]"""
    qdd, N = np.zeros(n), np.eye(n)
    for J, xdd_des, Jdot_qd in tasks:
        Jt = J @ N
        qdd = qdd + pinv(Jt) @ (xdd_des - Jdot_qd - J @ qdd)
        N = N @ (np.eye(n) - pinv(Jt) @ Jt)
    return qdd

def weighted(tasks, weights, n, reg=1e-6):
    """Weighted least squares: min sum w_i |J_i qdd - (xdd_i - Jdot_i qd)|^2"""
    H, g = reg * np.eye(n), np.zeros(n)
    for (J, xdd_des, Jdot_qd), w in zip(tasks, weights):
        H += w * J.T @ J
        g += -w * J.T @ (xdd_des - Jdot_qd)
    return np.linalg.solve(H, -g)

rng = np.random.default_rng(0)

# Case A: 7 DoF, two compatible 3D tasks
n = 7
J1, J2 = rng.standard_normal((3, n)), rng.standard_normal((3, n))
t1 = (J1, np.array([1., 0., .5]), np.zeros(3))
t2 = (J2, np.array([0., 2., -1.]), np.zeros(3))
qh = hierarchical([t1, t2], n)
print("A hier  e1=%.1e e2=%.1e" % (np.linalg.norm(J1 @ qh - t1[1]),
                                   np.linalg.norm(J2 @ qh - t2[1])))

# Case B: 4 DoF, tasks conflict (6 equations, 4 unknowns)
n = 4
J1, J2 = rng.standard_normal((3, n)), rng.standard_normal((3, n))
t1 = (J1, np.array([1., 0, 0]), np.zeros(3))
t2 = (J2, np.array([0, 1., 0]), np.zeros(3))
qh = hierarchical([t1, t2], n)
print("B hier  e1=%.1e e2=%.2f" % (np.linalg.norm(J1 @ qh - t1[1]),
                                   np.linalg.norm(J2 @ qh - t2[1])))
for w in (1, 1e3, 1e6):
    qw = weighted([t1, t2], [w, 1.], n)
    print("B w=%-7g e1=%.1e e2=%.2f" % (w, np.linalg.norm(J1 @ qw - t1[1]),
                                        np.linalg.norm(J2 @ qw - t2[1])))
assert np.linalg.norm(J1 @ qh - t1[1]) < 1e-9   # strict priority holds

With the seed above, running it on a recent numpy prints task 1 error near 1e-15 for the hierarchical case and a task 2 error of about 2.17, while the weighted errors on task 1 fall from roughly 0.86 at weight 1 to about 4.4e-3 at weight 1000 and 4.4e-6 at weight 1e6, with task 2 settling near the same 2.17. The qualitative lesson is robust to the seed: weights approach strict priority asymptotically, but never get there exactly, and the price of a bigger ratio is a worse-conditioned H. If you replace the truncated SVD pseudo-inverse by a damped inverse with a tiny damping constant, you will see the hierarchical solution leak: the second task pollutes the first because near-singular directions are amplified. That is a real failure mode, covered below.

Centroidal Dynamics and Model Predictive Control

In short: centroidal dynamics describe how the robot’s total linear and angular momentum change under gravity and contact forces, ignoring the detailed joint structure. It is exact for the momentum but ignores limb-level detail, so it plans CoM motion and contact forces quickly, and the whole-body layer then realizes them.

The centroidal momentum equations

Define the robot’s total mass m, CoM position c, and the contact forces f_k applied at points p_k. Newton’s and Euler’s laws for the whole body, written about the CoM, give:

m cdd = sum_k f_k + m g

Ldot = sum_k (p_k - c) x f_k

where L is the centroidal angular momentum. These two equations are exact for any articulated body in contact. The approximation enters through the next step: the centroidal momentum is related to joint velocities through the centroidal momentum matrix, h_G = A_G(q) qd, and a planner that treats L and the contact positions as the state ignores the constraint that L must be producible by actual limb motion. Many controllers further assume L is small and regulate it near zero, which recovers the familiar single rigid body or linear inverted pendulum models.

Why this model suits MPC

Compare model sizes. The full model has 36 generalized coordinates and 72 state variables; its dynamics involve the inertia matrix, recomputed from configuration. The centroidal model has a handful of states (CoM position and velocity, angular momentum, plus maybe base orientation) and bilinear dynamics: the torque (p_k - c) x f_k is a product of unknowns when contact positions are also decision variables. If contact positions come from a fixed footstep plan, the dynamics become linear in the forces, and with a linearized rotation the MPC becomes a convex QP. That is the structure exploited by well-known quadruped and biped controllers (convex MPC over a single rigid body model, with friction pyramid constraints on every force over every timestep).

What an MPC step solves

At each cycle, say every 20 ms at 50 Hz, the MPC takes the measured state, rolls a model forward over a horizon (often 0.5 to 2 seconds, divided into 10 to 50 nodes), and finds the force and CoM trajectory that minimize tracking error and force effort subject to dynamics, friction pyramids, and a contact schedule. Only the first control is applied or, more commonly, passed down as reference; the problem is re-solved from the new state. This re-solving is what gives disturbance rejection: a shove changes the measured state, and the next solve produces a new plan. For the general theory, constraint handling and solver choices, see our tutorial on model predictive control with OSQP and acados. The humanoid case differs in that the model switches with the contact schedule, and constraints are dominated by friction rather than by process limits.

A worked sizing example

The following numbers are illustrative, not from a specific robot. Take a horizon of 1.0 s at 25 nodes (dt = 40 ms). Let the state be 12-dimensional (CoM position, velocity, base orientation, angular velocity) and the input be 24 contact force components (two feet by four corners by three components). The QP then has 25 x (12 + 24) = 900 variables, with around 25 x 8 x 5 = 1,000 inequality rows (a friction pyramid has five facets including the non-negativity of the normal force, per corner point). Sparse solvers such as OSQP exploit the banded structure and, with warm starting from the shifted previous solution, can solve such problems in the low single-digit millisecond range on a modern CPU; treat that as an order of magnitude and measure it. Fitting a solve inside a 10 to 20 ms cycle with margin is realistic; a 1 kHz cycle is not.

The simplified-model gap

The centroidal MPC may output a force profile that the real robot cannot produce. It might ask for a CoM height the legs cannot reach, an angular momentum that requires limb swings colliding with the body, or a swing-foot touchdown the leg cannot achieve in time. The gap between the simplified model and the full robot is the main reason the second layer exists and the main reason people have moved toward whole-body MPC, discussed shortly. Kinematic feasibility can be restored either by adding conservative constraints to the MPC (restricting CoM height, stride length, and step time) or by running a kinematic feasibility check on the reference before handing it down.

ZMP, Capture Point and the Classical Balance Toolbox

Before centroidal MPC became standard, balance was managed with two related ideas, still widely used as stability margins and as cheap sanity checks.

Zero moment point

The ZMP is the point on the ground about which the net horizontal moment of gravity and inertial forces is zero. If it lies strictly inside the support polygon, the foot stays flat; if it reaches the polygon’s edge, the robot starts to tip about that edge. For a flat ground and a point-mass CoM at constant height z_c, the ZMP relates to the CoM by p_x = c_x - (z_c / g) cdd_x. This is the linear inverted pendulum model (LIPM) relation, and it makes planning trivial: choose a ZMP trajectory inside the footprints and solve for the CoM motion, which is exactly the preview control idea in the 2003 Kajita paper cited above.

The ZMP is equivalent to the center of pressure for flat contacts, so a ZMP outside the support polygon is physically impossible to realize: the real center of pressure saturates at the edge, and the unbalanced moment starts rotating the robot. That is why the ZMP criterion is a statement about feasibility for flat-foot walking, and why it is silent on multi-contact tasks (hands on a wall, stepping on uneven blocks), where the contact surfaces are not coplanar. Wrench cone formulations are the generalization.

Capture point

The instantaneous capture point (or divergent component of motion) of the LIPM is xi = c + cd / omega, with omega = sqrt(g / z_c). It is the point where the robot must step to come to rest. The LIPM’s unstable mode is exactly xi: xidot = omega (xi - p), so if the ZMP p is held fixed, xi diverges exponentially away from it with time constant 1/omega. For a CoM height of 0.9 m, omega is about 3.3 rad/s and the time constant is about 0.3 s. That 0.3 s figure is a useful intuition for why balance recovery must be decided on a time scale of a few hundred milliseconds, and it is derived from the model rather than measured from a robot. See also the capture steps work on robust humanoid walking for a modern treatment.

How these live inside a modern stack

The ZMP and capture point survive in three roles. First, they are cheap stability monitors and fallback controllers. Second, they appear as soft constraints or terminal costs in MPC (for instance, forcing the final capture point to a feasible region). Third, they provide the intuition for foot placement controllers that adjust step location in proportion to the capture point error. The generalization beyond flat ground, and to angular momentum regulation, is what the centroidal formulation delivers.

Whole-Body MPC and the Software Stack

In short: whole-body MPC drops the simplification and optimizes over the full rigid-body dynamics (or a close variant) across a horizon, via differential dynamic programming or sequential quadratic programming. It removes the model gap at the price of compute, and it is practical today because of efficient analytic derivatives in libraries such as Pinocchio.

Figure 4: Multi-rate message flow. The MPC re-solves at tens of hertz; the QP closes the loop at hundreds to a thousand hertz.

What changes with full dynamics

In whole-body MPC, the state is (q, qd) of the full floating-base system and the control is tau (or joint accelerations plus contact forces). The optimal control problem is nonlinear: the dynamics involve the articulated body algorithm, contact constraints enter as equalities or complementarity, and costs include posture, CoM, and end-effector terms. Solvers exploit the problem’s multiple-shooting or DDP structure: for each node, a Riccati-style backward pass over a linearization of the dynamics is cheap if the derivatives are cheap.

The Crocoddyl framework, introduced by Mastalli and colleagues, was built for precisely this: efficient multi-contact optimal control using analytic derivatives of rigid-body dynamics with a feasibility-driven DDP solver (Crocoddyl, arXiv 1909.04947). It builds on Pinocchio. OCS2, developed at ETH Zurich, implements sequential linear-quadratic and SQP-type solvers for switched systems and has been used for legged and mobile manipulators; I describe it here from its public documentation rather than from a measured comparison. Both can run a full-body or kinematic-plus-centroidal model in a receding-horizon loop at a few tens of hertz on a single CPU core for typical humanoid models, though exact rates depend on horizon, node count, and model size (unverified here as a general claim; always benchmark your own model).

Whole-body versus hybrid centroidal-kinematic

A popular middle path is the centroidal-kinematic model: dynamics are centroidal (momentum and contact forces), while the joint trajectory is obtained from kinematics tied to the centroidal momentum through the centroidal momentum matrix. It keeps the MPC state small while enforcing joint limits and reach. Recent work such as whole-body inverse dynamics MPC for legged loco-manipulation (arXiv 2511.19709) shows the field exploring where to place the boundary between the MPC and the instantaneous controller. The practical question is rarely “which is right” but “which parts of the model do I need to see in the horizon”. If the task depends on joint limits at touchdown or on arm-leg momentum coupling, the horizon needs them; if the task is walking on flat ground, a simplified model with a good QP below it is hard to beat for cost.

The solver and library map

Layer	Typical tool	What it gives you	Watch out for
Rigid-body dynamics	Pinocchio	M(q), h, Jacobians, analytic derivatives	Contact and constraint dynamics need care to model
Whole-body OCP	Crocoddyl	Feasibility-driven DDP, multi-contact	Nonconvex; warm starting matters
Switched-system OCP	OCS2	SLQ and SQP-type solvers, ROS tooling	Setup effort; build complexity
Dense QP	qpOASES	Active-set, quick warm starts, accurate	Scales poorly with size; degeneracy
Sparse QP	OSQP	ADMM, sparse, embeddable, warm start	Moderate accuracy; needs scaling and polishing

Both qpOASES and OSQP are open-source and widely used in controls; check each project’s current license and release status before adopting, since I have not re-verified those specifics this run. A rule of thumb from practice: use a dense active-set QP for the small whole-body problem where you need a precise solution within a hard time budget, and a sparse first-order solver for the long-horizon MPC where the structure helps and moderate accuracy is acceptable.

Timing budgets and real-time discipline

A controller that is right on average and late once is a controller that is wrong. A 1 kHz loop has a 1 ms budget, and the QP plus state estimation plus communication must fit with margin, on a real-time kernel with pinned threads and no dynamic memory allocation in the loop. Because the MPC runs slower, it is a common pattern to run it on a separate thread, with the QP always using the most recently completed MPC reference and extrapolating it forward by the elapsed time. Latency from the MPC to the QP thus appears as an effective delay on the reference, which should be included in the design margin. The ordering in Figure 4, where the QP reads fresh state every tick but the reference changes only every MPC period, is the practical meaning of multi-rate control.

RL Locomotion Policies and the Hybrid Picture

Since about 2019 learned locomotion has gone from curiosity to commercial default for quadrupeds, and since roughly 2023 it has done the same for humanoid walking and running demonstrations. An RL policy is typically a neural network mapping proprioceptive history (joint positions, velocities, IMU, previous action, a command) to joint position targets, trained in massively parallel simulation with domain randomization and then deployed zero-shot. A PD controller at the joint tracks the targets at 1 kHz or so, with the policy running at 50 Hz or thereabouts.

What RL does better

RL excels where the model-based pipeline is brittle. It discovers contact sequences, gaits and recovery behaviours without a hand-written contact schedule, and it absorbs unmodeled effects (actuator dynamics, backlash, compliance, soft ground) because the simulator, once randomized, covers them statistically. It also exploits the whole dynamic range of the robot: running, jumping and getting up from the ground are hard to script in a QP and routine for policies that were rewarded for them.

What it does worse

There are no hard guarantees. A friction constraint in a QP is satisfied by construction; in an RL policy it is encouraged by a reward and satisfied most of the time. Out-of-distribution inputs, such as a payload the randomization range did not include, a stair geometry never seen in training or an actuator fault, can produce a failure that is difficult to predict or diagnose. Policies are also bad at precise, constraint-dense manipulation, where millimetre-level force control matters and where the QP’s explicit task structure and compliance control shine. Tuning a reward is an art; tuning a QP weight is at least interpretable.

Hybrids that work

Several hybrid patterns are now common, and I present them as design patterns rather than claims about any one company:

RL for the lower body, QP or impedance for the upper body. The policy keeps balance and locomotion; a task-space controller handles arm manipulation with explicit force and compliance behaviour.
Model-based reference, RL tracker. A centroidal MPC or motion library generates reference motion, and an RL policy is trained to imitate it while being robust to disturbances. This inherits the model’s feasibility structure and the policy’s robustness.
RL residual on a QP. The policy outputs corrections to task targets or weights rather than raw torques, so the QP’s constraints always bound the final command.
Safety shield. A model-based controller monitors the policy output (capture point, joint limits, contact forces) and overrides it when a violation is imminent.

The design trade is between authority and guarantees. The more authority you grant the learned component, the less you can prove; the more you constrain it with a QP layer, the more you pay in capability. For industrial and safety-critical deployments, patterns 3 and 4 are the pragmatic ones because they keep a verifiable constraint layer in the loop.

Why a digital twin helps either way

Training an RL policy and tuning a QP controller both depend on a faithful simulation of contact, actuators and sensing. That is a digital twin problem: the identified inertial parameters, friction coefficients, actuator torque-speed curves and delays determine whether anything transfers. A well-maintained twin lets you regression-test both a QP weight change and a new policy checkpoint against the same disturbance suite before either touches hardware.

Trade-offs, Gotchas, and What Goes Wrong

Contact estimation errors. Everything above assumes you know which feet carry load. A false positive (believing a foot is in contact when it is airborne) makes the QP request forces nobody can supply; a false negative lets the foot slam down with a controller that thinks it is in swing. Fuse planned schedule, joint torque or force-sensor thresholds and kinematic consistency, and add hysteresis.

Model mismatch. Rigid-body inertial parameters from CAD can be off by several percent, cables and covers add unmodeled mass, and actuators are not ideal torque sources. Joint-level torque loops and friction compensation matter as much as the QP. A frequent symptom is steady-state sag of the CoM or a standing sway that no weight tuning removes. The fix is system identification, not more tuning.

Ill-conditioning and singularities. Strict hierarchies via pseudo-inverse blow up near kinematic singularities (a fully stretched knee is the classic). The example above showed a damped inverse leaking across priorities. Use truncated SVD, damping that scales with the smallest singular value, or QP-based cascades. Weighted formulations avoid singular behaviour but trade it for conditioning issues when weight ratios exceed about 1e6, because the Hessian’s condition number grows with the ratio (this is a rule of thumb, not a bound).

Infeasibility. A QP with hard constraints can become infeasible, for instance when a requested torque limit conflicts with a required contact force. Production controllers keep slack variables on the less critical constraints, penalized heavily, so that a solve always returns something. A solver failure flag must be handled explicitly, typically by holding the last valid torque for one tick and then falling to a safe posture.

Chattering between contact modes. Rapid toggling of the contact set causes force discontinuities. Ramp contact force limits over 20 to 50 ms around a transition (illustrative values) rather than switching them instantaneously, and smooth task weights during phase changes.

Latency and rate mismatch. A reference arriving 15 ms late at a 1 kHz loop is treated as current. If the plan requires a rapid change, that delay is a gain-margin loss. Time-stamp references and extrapolate.

Angular momentum neglect. Setting centroidal angular momentum to zero breaks down in fast turning or arm-swing-heavy motion, producing yaw drift and foot slip. Either regulate it explicitly in the MPC or add a heading task at the whole-body level.

Anti-pattern: tuning weights to hide a bad model. If the CoM task needs a weight of 1e5 to hold, something is wrong with the estimate or the contact model, and the symptom will reappear on a different terrain.

Practical Recommendations

If you are building or evaluating a humanoid control stack, do not begin with a grand architecture. Begin with the lowest layer and make it correct, because every layer above inherits its flaws. Get the floating-base dynamics right in a library you trust, validate against a simulator that models contact, and make the QP’s constraints (friction pyramid, torque and joint limits, contact consistency) airtight before adding tasks. Then add tasks in descending importance, checking each one’s tracking error against a disturbance suite.

Choose the prioritization method by the problem. Weighted QPs are the best default: one solve, predictable timing, and tuning that engineers can reason about. Move to a strict hierarchy only for the one or two behaviors where a guarantee is worth the compute, usually balance and contact consistency over everything else. Use a centroidal MPC when you need foresight over footsteps or momentum; do not use one to compensate for a weak low-level loop.

Treat learned policies as components with an authority budget. If the application tolerates occasional falls, such as research or entertainment, give the policy wide authority. If it works near people or expensive equipment, wrap it with a model-based shield and log every override.

Checklist:

Validate M(q), h, and Jacobians against a second implementation before tuning anything.
Use a conservative friction coefficient; measure it on the real floor rather than assuming it.
Add slack variables to non-critical constraints so the QP is always feasible.
Fuse contact detection from schedule, torque, and kinematics, with hysteresis.
Run QP and estimator on a real-time kernel; log solve times and worst case, not average.
Time-stamp MPC references and extrapolate to the QP tick.
Benchmark your solver on target hardware; treat published timings as orders of magnitude only.
Regression-test every controller or policy change against the same disturbance suite in simulation first.
Keep a documented safe-stop behavior for solver failure.

Frequently Asked Questions

What is whole-body control for humanoid robots?

Whole-body control is a method that computes joint torques and contact forces for all of a robot’s joints together, rather than controlling each limb separately. It uses the floating-base dynamics, contact and friction constraints, and a prioritized list of tasks such as balance, foot placement and hand motion. Usually it is solved as a quadratic program at 500 to 1000 Hz, with centroidal MPC supplying references from above.

What is the difference between a hierarchical and a weighted QP?

A hierarchical QP solves tasks in strict order: each lower-priority task is optimized only within the freedom left by higher ones, so high priorities are never compromised. A weighted QP sums all tasks into one cost with different weights, which is faster and simpler but only approaches strict priority as weight ratios grow, at the cost of ill-conditioning. Most practical controllers use weights, with a few hard constraints.

What are centroidal dynamics?

Centroidal dynamics describe the evolution of a robot’s total linear and angular momentum about its center of mass under gravity and contact forces. They are exact for the momentum but hide joint-level detail, so the model is small enough for fast model predictive control. The result is a plan of CoM motion and contact forces that a whole-body controller then realizes with actual joint torques.

Do I still need model-based control if I use reinforcement learning?

Often yes. RL policies are strong at robust locomotion but give no hard guarantees on friction limits, joint limits or contact forces, and they struggle with precise force-controlled manipulation. Model-based layers supply reference motions for imitation, constrain or shield the policy, and control the arms. Many practical systems pair RL for the legs with a QP or impedance controller for manipulation.

How fast must a humanoid whole-body controller run?

The whole-body QP typically runs at 500 Hz to 1 kHz so that torque commands track fast contact dynamics. The centroidal or whole-body MPC runs slower, about 20 to 100 Hz, because it solves a longer horizon. Learned policies commonly run near 50 Hz with a faster joint-level PD loop. These are common ranges rather than fixed requirements, and the right numbers depend on actuators and hardware.

Which libraries should I start with?

For dynamics and derivatives, start with Pinocchio. For whole-body optimal control, Crocoddyl and OCS2 are the established open-source options. For the QP itself, qpOASES suits small dense problems with tight timing, while OSQP suits larger sparse problems such as MPC. Simulation in a contact-capable physics engine is essential for validating anything before it reaches hardware.