An Efficiently Solvable Quadratic Program for Stabilizing Dynamic Locomotion

In plain words

A walking robot has to keep its balance while its joints, motors and feet all obey physics. The authors first use a simple model of the robot's centre of mass and its balance point (the zero-moment point, ZMP) to work out how the body should move to stay balanced and follow a planned path. That gives a score for each possible state of the robot: how far it is from a good state, and how much it will cost to recover.

Then the full robot is controlled by solving a small optimisation problem at every tick. The problem asks for joint accelerations that bring the robot closest to the simple model's recovery plan, while never breaking the rules of the real robot: motors have limits, feet must not slide, and the ground can only push, not pull. Most of the time the answer does not change much from one tick to the next, so the solver starts from the previous answer and usually finishes in a single step. That speed is what lets the controller run at about 1000 times per second on a 34-joint humanoid model in simulation.

TL;DR

• A time-varying LQR on the centre-of-mass and zero-moment-point (ZMP) model gives a value function that the full-robot controller tries to descend §2.
• A convex QP picks joint accelerations and contact forces under the full dynamics, torque limits, friction-cone and no-slip constraints §3.
• A custom active-set solver reuses the previous step's active set. The paper reports a single linear solve in about 97% of control steps, and about 0.2 ms solve time, faster than Gurobi and CVXGEN on the tested walk §4 §5.1.
• Use it if you need a torque-level humanoid balance and walking controller with a fast, warm-started QP. All performance numbers are author-reported and come from simulation of one robot.

Problem

Whole-body humanoid control needs constraints on dynamics, actuation and contacts at every control step. Model predictive control over the full model is too expensive for a 34-DOF humanoid §1. Earlier work either used low-dimensional linear models or relaxed constraints to keep the problem smooth §1. Several QP approaches with one-step horizons exist for bipedal control, but the paper argues they lack a principled and reliable way to stabilise walking trajectories §1 §6.

Method

Stage 1: ZMP model and LQR cost-to-go §2. The planar centre of mass (COM) is modelled with state x = [x, y, ẋ, ẏ] and input u = COM acceleration. The ZMP output is y = Cx + D(t)u eq. 2 eq. 3 eq. 4. For a COM of constant height z, the paper's expression eq. 3 gives D = z/g, which is linear in the state and can be used directly. For a known time-varying COM height, the dynamics are time-varying and linear, so no linearisation is needed §2.

The tracking cost is J = ȳᵀ(t_f)Q_f ȳ(t_f) + ∫ ȳᵀQ ȳ dt, where ȳ = y − y_d is the ZMP error, Q ≻ 0 and Q_f ≻ 0 eq. 1 §2. Solving the Riccati equation gives the cost-to-go J*(x̄, t) = x̄ᵀS(t)x̄ + s₁(t)ᵀx̄ + s₀(t) and the feedback ū* = −K(t)x̄ §2, unnumbered cost-to-go eq. 5.

Stage 2: surrogate value function §2. Because ū* may be unachievable with actuator limits, friction and contacts, the controller minimises the surrogate V = ȳᵀQȳ + (∂J*/∂x̄)·ẋ̄ instead eq. 6 §2. This is a Lyapunov-style descent term. The paper places no explicit constraint on its derivative §6.

Stage 3: full-robot QP §3. The floating-base equations are split into an unactuated part and an actuated part eq. 8 §3:

• unactuated: H_f q̈ + C_f = Φ_fᵀλ eq. 8;
• actuated: H_a q̈ + C_a = B_a τ + Φ_aᵀλ §3, unnumbered;
• torques are eliminated: τ = B_a⁻¹(H_a q̈ + C_a − Φ_aᵀλ) §3, unnumbered.

Each contact's friction cone is a polyhedral approximation with generators v_ij = n_j + μ_j d_ij and β_ij ≥ 0 eq. 9 §3. The QP at each step is:

• objective: V + w_q̈ ‖q̈_des − q̈‖² + ε Σ β_ij² + ‖η‖² eq. 10;
• subject to: unactuated dynamics eq. 11; no-slip acceleration J q̈ + J̇ q̇ = −αJq̇ + η eq. 12; torque bounds on B_a⁻¹(H_a q̈ + C_a − Φ_aᵀλ) eq. 13; λ_j = Σ β_ij v_ij eq. 14; β_ij ≥ 0 eq. 15; η within bounds eq. 16 §3.

Joint limits add a bound on q̈_i: q̈_i ≥ 0 at the lower limit and q̈_i ≤ 0 at the upper limit §3. The desired acceleration q̈_des comes from a PD rule q̈_des = K_p(q_des − q) − K_d q̇, with the same scalar gains for all joints §5.

Stage 4: active-set solver §4.1. The QP is written in standard form eq. 17 with inequalities P z ≤ f. The active set A is assumed equal to the set of inequalities active at optimality at time t−1 §4.1. The equality-constrained KKT system is solved with that set eq. 18; the candidate is optimal if P z ≤ f and γ_i ≥ 0 for i in A eq. 19 §4.1. If not, add a violated index, or drop an index with a negative multiplier, and solve again §4.1 alg. 1. The loop has an iteration limit alg. 1.

# Walking QP controller: one control step  [§3] [§4]
# Input:  robot state q, dq; ZMP plan point at time t
# Output: joint torques tau

def control_step(q, dq, zmp_plan_point, active_set_prev):
    # Value-function surrogate from the ZMP LQR  [§2] [eq. 6]
    surrogate = zmp_surrogate_cost(q, dq, zmp_plan_point)
    # Full QP: cost [eq. 10], constraints [eq. 11]-[eq. 16]
    qp = build_walking_qp(q, dq, surrogate)
    # W^-1 is computed once per step, not per iteration  [§4.2] [eq. 22]
    w_inverse = invert_structured_w(qp.W)
    # Start from the active set at optimality of the previous step  [§4.1]
    active_set = active_set_prev
    # Active-set loop; the iteration limit is not stated  [alg. 1]
    for iteration in range(iteration_limit):
        # Candidate from the KKT system on the active set  [eq. 20] [eq. 21]
        candidate = solve_kkt(qp, active_set, w_inverse)
        # Add a violated inequality to the active set  [alg. 1] [§4.1]
        active_set = add_violated(active_set, candidate)
        # Drop an index whose multiplier is negative  [alg. 1] [§4.1]
        active_set = drop_negative_multiplier(active_set, candidate)
        # Stop once the candidate satisfies all optimality conditions  [eq. 19]
        if is_optimal(candidate, active_set):
            return torques_from(candidate)  # [§3] torque expression
    # Rare failure: fall back to a slower interior-point solver  [§4]
    return torques_from(interior_point_solve(qp))

Efficient linear solve §4.2. Let P_act and f_act be the rows of P and f indexed by A, R = [A; P_act] (stacked, the paper writes it as [Aᵀ P_actᵀ]ᵀ), and e = [b; f_act]. Then the multipliers (α, γ) solve −RW⁻¹Rᵀ[α; γ] = e + RW⁻¹g eq. 20, and z = −W⁻¹(g + Rᵀ[α; γ]) eq. 21 §4.2. The matrix W is block-diagonal, with W22 diagonal and W11 = wI + UᵀQU, where U = D(t)J and J is the COM (x, y) Jacobian eq. 22 §4.2. Its inverse uses the matrix inversion lemma, so only 2×2 matrices are inverted §4.2, unnumbered:

W_{11}^{-1} = \frac{1}{w}I - \frac{1}{w^2}U^T\left(Q^{-1} + \frac{1}{w}UU^T\right)^{-1}U

W⁻¹ does not depend on A, so it is computed once per control step even when several solver iterations are needed §4.2.

Balancing and walking §5. For balancing, COM height is held constant, so the LQR is solved once. The cost is J = ∫ yᵀQy dt with Q = I, regulating the ZMP to (0, 0), and J* = x̄ᵀSx̄ with S from the algebraic Riccati equation §5, unnumbered. For walking, a ZMP plan y_d(t) is made by linear interpolation between step locations. The TVLQR uses Q_f = S as its final cost, and the COM trajectory is found by simulating the closed loop §5. Desired configurations come from an inverse-kinematics QP that minimises a weighted distance to a nominal posture §5. Each foot has four contact points. A contact is passed to the QP only when the footstep plan and the estimated foot-to-terrain distance both indicate contact §5.

Control rate. QP setup (dynamics, contact normals, Jacobians) takes about 0.8 ms for the 34-DOF Atlas model, giving a total control step of about 1 ms §5. The paper reports an average control rate of 1 kHz §1 §5.

Hyperparameters

Results

Author-reported. Setup: simulated 34-DOF Atlas humanoid; flat-ground walking pattern; i7 2.1 GHz quad-core laptop; Gurobi (barrier and dual simplex, with warm start) and CVXGEN as baselines; the CVXGEN formulation omits the torque-limit inequalities §5.1.

The paper states a speed-up "by a factor of 5 or more" over the best off-the-shelf solver §1. The table ratio against Gurobi (DS) is 5.0 tab. 1. Qualitatively, the paper reports stable walking on rough ground, in simulated mud about knee deep, and with a hose the controller model omits, all with noisy state and terrain estimates §5.

Paper vs code

No official code.

The paper's footnote points to example simulation code on the first author's personal page §5. That page links no code repository and does not mention the paper. It redirects to another academic site. GitHub searches for "fastQP" (unrelated results), "fast_qp org:RobotLocomotion", "quadratic program stabilizing locomotion org:RobotLocomotion" and "active set atlas humanoid org:RobotLocomotion" returned no repository belonging to the authors.

Pitfalls

• Numbering. Equation numbers above were counted in source order from the LaTeX (fastQPcontrol.tex), not from a compiled PDF, so they may be off by one. Figures and tables are numbered in source order too. Where unsure, the section is the reliable reference.
• Algorithm 1 is ambiguous alg. 1. It lists the add check and the drop check as bare If lines without a loop over constraint indices, and it does not say whether they apply one index at a time or to all violated indices together. The order matters for the number of iterations.
• Warm start at the first step or after a failure. The initial active set is not stated, nor is what to do with the active set after the fallback solver runs §4.1 §4.
• Termination. The paper says finite termination cannot be guaranteed, which is why the fallback exists §4. The fallback solver is not named and its cost is reported only as one-step input delays of about 3 ms in their experiments §4.
• W₂₂ is not defined. The paper says W₂₂ is diagonal but does not list which variables z contains, so the block split cannot be checked against the QP §4.2 eq. 22. Infer the layout from the constraint matrices before implementing.
• R is written awkwardly. The source gives R as [Aᵀ P_actᵀ]ᵀ. Read it as the stack of A and P_act, which is what makes Rᵀ[α; γ] = Aᵀα + P_actᵀγ match the stationarity row of the KKT system §4.2 eq. 18.
• Matrix inversion lemma. The W₁₁⁻¹ form inverts Q⁻¹ + UUᵀ/w, so it needs Q invertible (stated Q ≻ 0) and it contains 1/w and 1/w² terms. The paper gives no conditioning analysis, so check it when w is small §4.2.
• Balancing cost. The balancing cost in §5 includes a cross term 2xᵀCᵀDu. Keep it when you form the QP cost, since dropping it changes the optimum §5.
• Constant height is an approximation. The paper says constant COM height has "minimal practical effect" on balancing, but gives no measurement for walking §5.
• Contact switching. Contacts enter the QP only when both perception and the plan agree, which the paper calls essential for breaking contact §5. Contact changes alter the constraint set, so they are where warm starts fail. The paper does not measure this separately.
• Unstated values. Motion weight, bounds, friction coefficient, PD gains, iteration limit and the walking Q are all not stated. Do not fill them from other work without marking them as your own choice.
• Quantitative tracking results are missing. The source has commented-out drafts of tracking-error experiments §5 (not part of the paper). Do not expect ZMP or footstep error numbers in the text.
• Timing and hardware. The 0.2 ms and 1 ms numbers were measured on one laptop and one simulator model, with an implementation language not stated for the solver. The ~¼ s TVLQR time uses an unoptimised MATLAB code path and is not an online cost §5 §5.1.
• Baseline strength. The 50% figure for Stewart–Trinkle comes from one experiment on flat ground §5.1.

Lineage

• Builds on: the ZMP Riccati solution of Tedrake et al., cited in the source under the key Tedrake_etalTBD (no year or arXiv id given) §5; the constant-height assumption of Kajita et al. §2; the friction polyhedron of Pollard and Reitsma §3; the Stewart–Trinkle parameterisation, used as the comparison §5.1.
• Compared against: Gurobi and CVXGEN as QP solvers §5.1. Related QP walking controllers: Herzog et al. (floating-base split, 1 kHz on a 14-DOF biped, as cited by the authors) §6; Ames et al. (CLF-based QPs minimising the input norm) §6; active-set and MPC warm-start work by Bartlett et al., Wang and Boyd, and Ferreau et al. §6.
• Superseded by: not known from content on this wiki.