PaperlayerView exact revision on arXiv

Source-linked AI summary

An Efficiently Solvable Quadratic Program for Stabilizing Dynamic Locomotion

Scott Kuindersma, Frank Permenter, Russ Tedrake

arXiv:1311.1839v2cs.RO

TL;DR

High-dimensional humanoid locomotion requires constrained optimization over dynamics, actuation, and contacts, but conventional MPC can be computationally difficult. The paper embeds a simple-model LQR value function in a full-robot convex QP and uses a temporally consistent active-set solver. The resulting controller outperforms general-purpose solvers and reaches 1kHz control for a 34-DOF humanoid.

  • Problem

    High-dimensional humanoid locomotion requires optimization that simultaneously handles nonlinear dynamics, actuation limits, and contact constraints, while MPC complexity hinders such applications.

  • Method

    The controller embeds an optimal cost-to-go from a simple walking model into a constrained full-robot QP and solves repeated instances with a custom active-set method.

  • Results

    1kHz control rates: achieved for a 34-DOF humanoid, while the custom solver outperformed CVXGEN and Gurobi by a factor of 5 or more.

  • Takeaways & Limitations

    The approach efficiently computes stabilizing control inputs for simulated Atlas balancing and walking tasks while respecting full-robot constraints.

Abstract

from arXiv · show

We describe a whole-body dynamic walking controller implemented as a convex quadratic program. The controller solves an optimal control problem using an approximate value function derived from a simple walking model while respecting the dynamic, input, and contact constraints of the full robot dynamics. By exploiting sparsity and temporal structure in the optimization with a custom active-set algorithm, we surpass the performance of the best available off-the-shelf solvers and achieve 1kHz control rates for a 34-DOF humanoid. We describe applications to balancing and walking tasks using the simulated Atlas robot in the DARPA Virtual Robotics Challenge.

I. INTRODUCTION

The paper develops a convex QP controller for dynamically stable humanoid locomotion, combining a simple walking model with full-robot constraints. A custom active-set solver exploits temporal consistency to achieve real-time performance.

  • I. INTRODUCTION: High-dimensional humanoid locomotion requires simultaneous handling of nonlinear dynamics, actuation limits, and contact constraints.MPC is hindered by computational complexity and walking's hybrid dynamics, motivating simplified models or relaxed constraints in prior work.
  • I. INTRODUCTION: QP approaches exploit linear instantaneous dynamics and contact constraints, while this paper explicitly uses the infrequent changes in active inequalities between control steps.The formulation is designed for balancing and locomotion tasks.
  • I. INTRODUCTION: The controller uses a time-varying LQR cost-to-go from a simple unconstrained walking model inside a constrained optimization for the full robot.The resulting QP also has low-dimensional structure that can reduce solution time.
  • I. INTRODUCTION: 5 or more: the custom active-set solver outperforms CVXGEN and Gurobi, while active sets remain constant approximately 97% of consecutive control steps.This usually requires only one linear-system solve per step and supports average 1kHz control for a 34-DOF humanoid.

II. LQR DESIGN FOR ZMP DYNAMICS

The paper uses ZMP dynamics and time-varying LQR to construct a stabilizing surrogate value function. The resulting control objective tracks a desired ZMP trajectory while accommodating full-system constraints later in the controller.

  • II. LQR DESIGN FOR ZMP DYNAMICS: The planar COM and ZMP dynamics are represented in state-space form using COM position, velocity, acceleration, gravity, and COM height.The ZMP is the ground-plane point where inertial and gravitational moments do not cause tipping.
  • II. LQR DESIGN FOR ZMP DYNAMICS: Maintaining the ZMP inside the contact support polygon provides an effective strategy for dynamic stability in legged locomotion.Dynamic balance is described as contact forces opposing gravitational and inertial forces.
  • II. LQR DESIGN FOR ZMP DYNAMICS: The controller seeks optimal tracking of a desired ZMP trajectory while accounting for time- and state-varying constraints from full-system dynamics, inputs, and contacts.Directly solving the corresponding nonlinear constrained optimal-control problem is computationally prohibitive at this scale.
  • II. LQR DESIGN FOR ZMP DYNAMICS: An unconstrained time-varying LQR problem computes an optimal cost-to-go that serves as a control-Lyapunov function for the ZMP dynamics.Each iteration selects full-system inputs that descend this ZMP-based function while respecting instantaneous constraints.
  • II. LQR DESIGN FOR ZMP DYNAMICS: Assuming constant COM height makes the ZMP dynamics linear, while a known time-varying height yields time-varying linear dynamics suitable for TVLQR without explicit linearization.The Riccati equation supplies the optimal cost-to-go for the time-varying linear system.

III. QP FORMULATION

The QP computes full-robot control inputs by minimizing the LQR-derived ZMP cost and motion cost under dynamics, contact, friction, input, and joint-limit constraints.

  • III. QP FORMULATION: The QP minimizes the stabilizing ZMP cost and a quadratic motion cost subject to instantaneous full-robot constraints.The optimization is solved from the current robot state at every control step.
  • III. QP FORMULATION: Rigid-body dynamics use inertia, gravitational and Coriolis terms, the input map, and contact forces transformed into generalized forces.For floating-base humanoids, actuated and unactuated degrees of freedom are partitioned so actuator torques can be removed as decision variables.
  • III. QP FORMULATION: Contact forces are modeled at the active ground-contact points and constrained within conservative polyhedral friction-cone approximations.The cone generators combine contact-surface normals, tangent directions, and Coulomb friction coefficients.
  • III. QP FORMULATION: The constraints enforce dynamics and input limits, impose no-slip foot-contact behavior, and keep contact forces inside the approximated friction cones.Slack variables permit bounded no-slip violations to reduce infeasibility risk, while epsilon regularizes the objective.
  • III. QP FORMULATION: Joint-limit bounds are added for joints at their minimum or maximum positions.These bounds restrict acceleration direction at the corresponding limits.

IV. OPTIMIZATION

A custom active-set method accelerates repeated QP solves by exploiting stable active constraints between control steps. It usually solves a structured linear system directly, with a slower fallback for rare failures.

  • IV. OPTIMIZATION: The active-set solver assumes consecutive QPs share the same active inequality set and tests a candidate solution against the full optimality conditions.If the conditions fail, it updates the active set and repeats until success or an iteration limit.
  • IV. OPTIMIZATION: Rare failures trigger a more reliable but slower interior-point solver, causing infrequent single-step delays of approximately 3ms without significant walking-performance effects.Finite termination is not guaranteed for the active-set method.

A. Active-set method

The method solves a quadratic program by carrying forward the previous step’s active constraints and iteratively checking the resulting KKT solution.

  • The QP is written in standard quadratic form with cost matrix W, linear term g, inequality matrix P, and bounds f.
  • At optimality, the active set A contains inequalities satisfied as equalities, while inactive constraints have zero associated multipliers.
  • The algorithm initializes A from the previous control step’s active inequalities, exploiting temporal consistency between consecutive solutions.
  • It solves the KKT equations, then adds violated constraints or removes constraints with negative multipliers until all inequalities are satisfied or the iteration limit is reached.

B. Efficiently computing a candidate solution

The solver efficiently computes candidate QP solutions by reducing the KKT system and exploiting the cost matrix’s block structure and reusable inverse components.

  • The active-set method reduces the KKT solve to a smaller system involving active-constraint multipliers, then recovers the primal solution z.
  • Algorithm 1 receives a QP with structured W and an assumed active set, returning an optimal solution or a failure indication after the iteration limit.
  • The cost matrix W has block-diagonal structure, with diagonal block W22 and W11 = w_q¨I + U^TQU.
  • For the ZMP dynamics, U = D(t)J, and the matrix inversion lemma reduces the required inversions to 2 × 2 matrices.
  • W^-1 is independent of the active set, so it and related submatrices need to be computed only once per control step.

V. APPLICATION

The controller is applied to balancing and locomotion with a simulated Atlas humanoid, using LQR-derived costs, ZMP planning, and constrained whole-body control.

  • The controller was evaluated on the 34-DOF Atlas model in Drake and Gazebo across balancing and locomotion tasks.
  • The system walked over uneven terrain, through simulated knee-deep mud, and while carrying an unmodeled multi-link hose using imperfect state and terrain estimation.
  • For balancing, an infinite-horizon LQR regulates the ZMP at (0, 0), assuming constant COM height so the ZMP dynamics remain linear.
  • The QP combines the LQR cost-to-go with tracking and motion costs, including desired-acceleration regulation and a small regularization term.
  • Walking plans are generated from desired foot trajectories, with ZMP trajectories interpolated between steps and COM trajectories obtained from the linear dynamics.
  • Desired configurations use inverse kinematics and can be computed offline or reactively with a second small QP respecting foot-pose, COM, and joint-limit constraints.

A. Solver Performance

The custom active-set solver substantially reduces QP solve time by exploiting stable active sets, supporting fast control for the Atlas humanoid while remaining sensitive to friction-cone parameterization.

  • The evaluation compares the active-set solver with Gurobi and CVXGEN, using warm starts for Gurobi and omitting input saturation inequalities from CVXGEN.
  • The custom active-set method outperforms the next-best solver by a factor of 5 while walking on a fixed flat-ground pattern.
  • Average QP setup takes approximately 0.8ms for the 34-DOF Atlas model, producing a total control-step time of 1ms.
  • The solver’s performance depends on problem formulation because some parameterizations allow active inequalities to change when contact-force directions change.
  • Approximately 97% of walking control steps require one solver iteration because the active set usually remains unchanged.
  • A friction-cone parameterization can cause approximately 50% more control steps to require at least two iterations than the paper’s alternative parameterization.

VI. RELATED WORK

The controller is situated among horizon-1 MPC and QP approaches for locomotion, while differing in its treatment of CLFs and friction-cone parameterizations. Prior work also exploits temporal relationships between successive MPC problems to accelerate active-set solution methods.

  • Prior QP locomotion controllers use linear instantaneous dynamics and contact constraints, effectively solving horizon-1 MPC problems.
  • Different friction-cone parameterizations affect solver behavior, with one parameterization producing fewer active-set changes than the commonly used Stewart and Trinkle formulation.The paper connects this parameterization choice to solver performance.
  • Unlike CLF-based approaches that constrain ˙V_clf to be negative, this controller minimizes ℓ(x, u) + ˙V_clf without imposing that constraint.The authors report greater practical robustness and fewer QP infeasibilities from this choice.
  • Earlier MPC active-set methods exploit temporal relationships between successive QPs through Schur-complement updates, explicit active-set enumeration, or solution-path tracking.

VII. CONCLUSION

The paper presents a stabilizing QP controller and active-set solution technique for dynamic walking that exploits consistency between consecutive active inequality constraints. In simulated Atlas experiments, the approach efficiently computes walking inputs and extends to broader floating-base systems and MPC formulations.

  • The controller exploits consistency between consecutive active inequality constraints to solve dynamic-walking QPs efficiently.The formulation is demonstrated on a simulated Atlas robot using ZMP dynamics.
Loading 1311.1839v2…