PaperlayerView exact revision on arXiv

Source-linked AI summary

BiConMP: A Nonlinear Model Predictive Control Framework for Whole Body Motion Planning

Avadesh Meduri, Paarth Shah, Julian Viereck, Majid Khadiv, Ioannis Havoutis, Ludovic Righetti

arXiv:2201.07601v2cs.RO

TL;DR

Online whole-body planning for legged robots is difficult because robot dynamics are nonlinear and planners must adapt in real time. BiConMP uses biconvex centroidal optimization with ADMM and DDP-based kinematic optimization; experiments report dynamic motions across real and simulated robots, including gait transitions and disturbance robustness.

  • Problem

    Whole-body trajectory planning for legged robots must handle nonlinear dynamics while running fast enough for online adaptation to environmental changes and disturbances.

    Source: §I · p. 1

  • Method

    BiConMP combines an ADMM solver exploiting biconvex centroidal dynamics with second-order DDP kinematics optimization for whole-body nonlinear MPC.

    Source: §I · p. 2

  • Results

    The framework generated cyclic and acyclic motions on real Solo12 and dynamic motions in simulation on Talos and Anymal, with reported velocity tracking, disturbance robustness, uneven-terrain operation, and online gait transitions.

    Source: §I · p. 2, C. Cyclic Gaits · p. 9, 2) Gait Transitions · p. 11

  • Takeaways & Limitations

    BiConMP demonstrates real-time whole-body motion generation across different legged robots and includes empirical analysis of horizon and replanning-frequency effects.

    Source: §I · p. 2, CONCLUSION · p. 16

Abstract

from arXiv · show

Online planning of whole-body motions for legged robots is challenging due to the inherent nonlinearity in the robot dynamics. In this work, we propose a nonlinear MPC framework, the BiConMP which can generate whole body trajectories online by efficiently exploiting the structure of the robot dynamics. BiConMP is used to generate various cyclic gaits on a real quadruped robot and its performance is evaluated on different terrain, countering unforeseen pushes and transitioning online between different gaits. Further, the ability of BiConMP to generate non-trivial acyclic whole-body dynamic motions on the robot is presented. The same approach is also used to generate various dynamic motions in MPC on a humanoid robot (Talos) and another quadruped robot (AnYmal) in simulation. Finally, an extensive empirical analysis on the effects of planning horizon and frequency on the nonlinear MPC framework is reported and discussed.

I. INTRODUCTION

Legged-robot motion planning must handle nonlinear dynamics while generating trajectories fast enough for online adaptation. BiConMP combines a biconvex centroidal-dynamics solver with whole-body kinematic optimization and is evaluated across robots and motions.

I. INTRODUCTION

  • Prior approaches either limit motion scope through simplified models or predefined contacts, or remain too computationally expensive for real-time complex contact planning.The cited quadruped nonlinear-MPC demonstration was limited to low-angular-momentum motions such as trotting and jumping in place.Source: §I · p. 1
  • BiConMP exploits centroidal-dynamics biconvexity with ADMM, splitting the dynamics differently to reduce optimization variables and improve efficiency and reliability.The framework targets real-time closed-loop MPC for whole-body motions.Source: §I · p. 2
  • A second-order DDP kinematics optimizer generates smooth joint trajectories that track centroidal momentum, avoiding heuristic swing-foot paths and a QP-based whole-body controller.The resulting joint acceleration profiles support direct torque computation with inverse dynamics.Source: §I · p. 2
  • The framework is demonstrated in closed-loop MPC on Solo12 at 20 Hz for trotting, jumping, bounding, disturbance recovery, uneven terrain, and a high-five motion.Dynamic motions are also generated in simulation on Talos and Anymal.Source: §I · p. 2
  • The paper empirically analyzes how planning horizon and replanning frequency affect nonlinear MPC performance on the real Solo12 robot.The authors describe this as the first reported empirical analysis of closed-loop nonlinear-MPC performance.Source: §I · p. 2

A. Centroidal Dynamics

The paper separates floating-base dynamics into actuated and unactuated parts, then uses centroidal motion and whole-body kinematics in an iterative kino-dynamic planning process. The resulting force and joint trajectories feed inverse dynamics for torque computation.

A. Centroidal Dynamics

  • The rigid-body dynamics express generalized forces through mass, nonlinear terms, joint torques, and end-effector forces and torques.The generalized configuration includes joint positions and base pose, while generalized velocity includes the floating-base degrees of freedom.Source: A. Centroidal Dynamics · p. 3
  • The unactuated dynamics are equivalent to Newton-Euler equations for the center of mass and describe linear and angular momentum under contact forces.Linear momentum is related to center-of-mass velocity by l = m˙c.Source: A. Centroidal Dynamics · p. 3

B. Kino-Dynamic Motion Generation

  • Kino-dynamic planning iterates between centroidal optimization and whole-body kinematics, using the generated momentum trajectory as a soft constraint until the subproblems converge.The kinematic optimizer tracks centroidal momentum while also handling whole-body tasks such as swing-foot motion.Source: B. Kino-Dynamic Motion Generation · p. 3
  • The framework uses plain inverse dynamics on desired joint positions, velocities, accelerations, and contact forces to compute actuated joint torques.The framework overview places this torque computation after the kino-dynamic trajectory iteration.Source: B. Kino-Dynamic Motion Generation · p. 3, Figure 1 · p. 4

C. Fast Iterative Shrinkage Thresolding Algorithm (FISTA)

FISTA is presented as an accelerated proximal-gradient method for convex-cost problems with constraints enforceable through a proximal operator. Its low-cost iterations make it useful for solving the convex subproblems in the centroidal optimization.

C. Fast Iterative Shrinkage Thresolding Algorithm (FISTA)

  • A proximal-gradient iteration takes a descent step and applies a proximal operator to keep the next optimization variable within the feasible domain.The operator is usable for constraints when its closed-form solution can be computed; arbitrary inequality constraints cannot generally be enforced this way.Source: C. Fast Iterative Shrinkage… · p. 4
  • FISTA accelerates first-order gradient optimization for convex costs and achieves quadratic convergence through its auxiliary variable and step-length update.The step length is chosen using a sufficient-decrease condition.Source: C. Fast Iterative Shrinkage… · p. 4
  • The paper uses FISTA to solve convex centroidal-OCP subproblems, enforcing kinematic box constraints and friction-cone constraints through the indicator function.The proximal step is described as computationally inexpensive and does not require Hessian inversion.Source: C. Fast Iterative Shrinkage… · pp. 4–5

III. APPROACH

BiConMP exploits the biconvex structure of centroidal dynamics by alternating between force and state subproblems in an ADMM scheme. This supports early termination when a feasible trajectory is needed within the MPC time budget.

III. APPROACH

  • The approach combines a biconvex dynamics solver, DDP-based second-order kinematics optimization, and nonlinear MPC for real-time whole-body motion generation.The framework overview introduces these as the main solver components.Source: §III · p. 5

A. Biconvexity in Centroidal Dynamics

  • Centroidal dynamics constraints are affine in either centroidal states or contact forces when the other variable group is fixed, exposing their biconvex structure.The formulation writes the constraints in affine form with matrices depending on the fixed variable group.Source: A. Biconvexity in Centroidal Dynamics · p. 5

B. Biconvex optimization with ADMM

  • ADMM alternates between convex force and state subproblems subject to centroidal dynamics, kinematic, unilaterality, and friction-cone constraints.The objective and constraints are formulated to exploit biconvexity in the dynamics.Source: B. Biconvex optimization with ADMM · pp. 5–6
  • ADMM can terminate before full convergence when the dynamics-violation tolerance is met, making a realizable trajectory available within the required MPC time.The paper contrasts this with block-coordinate descent, whose sublinear convergence is not guaranteed.Source: B. Biconvex optimization with ADMM · p. 5
  • For cyclic gaits, the state cost tracks a nominal centroidal trajectory, including nominal angular momentum when used to target base orientation.The desired orientation is represented by a quaternion-based angular-momentum target; the paper notes that unit base inertia is assumed only for this nominal target.Source: B. Biconvex optimization with ADMM · pp. 5–6

C. Convex Sub-problems

The centroidal optimization is split into state and force sub-problems, with FISTA used to impose kinematic and friction-cone constraints efficiently. Its force projection enforces second-order friction cones directly, avoiding the conservative trajectories associated with linearized cones.

C. Convex Sub-problems

  • FISTA solves the constrained sub-problems with low computational cost while retaining quadratic convergence, using indicator functions and inexpensive proximal operators.The state sub-problem is a QP, while the force sub-problem requires a quadratically constrained QP because of its second-order friction-cone constraints.Source: C. Convex Sub-problems · p. 6
  • The state sub-problem constrains the CoM to a contact-dependent box, while velocity and angular-momentum components are left unbounded.The CoM bounds are enforced by a box-projection proximal operator.Source: 1) State sub-problem (optimizing for X) · p. 6
  • The force sub-problem projects each contact-time force group onto a second-order friction cone, which implicitly enforces f_z ≥ 0.FISTA permits direct second-order cone projection while retaining quadratic convergence; linearized cones usually produce conservative trajectories.Source: 2) Force sub-problem (optimizing for F) · p. 6

3) FISTA implementation:

The FISTA implementation reduces solve time through analytic gradients and reuse of sparse computations. It also warm-starts line-search steps, while the authors distinguish convergence of each sub-problem from that of the full trajectory optimization.

3) FISTA implementation:

  • Analytic cost gradients, sparse matrix operations, and cached matrix products reduce computation within each FISTA iteration.The cached products are reused within each convex sub-problem.Source: 3) FISTA implementation · p. 6
  • Warm-starting line-search steps avoids runtime searches after the solver has settled on nearly identical step lengths across iterations.The authors conjecture that the empirically determined value satisfies Wolfe’s condition; they note that this warm start is not convenient with other QP solvers.Source: 3) FISTA implementation · pp. 6–7
  • FISTA gives quadratic convergence for each ADMM sub-problem, while the full centroidal trajectory optimization is expected to converge only super-linearly.The authors attribute improved solve times to FISTA’s convergence, inexpensive proximal operators, and implementation details.Source: 3) FISTA implementation · p. 7

D. Kinematics Solver

BiConMP uses DDP for nonlinear whole-body kinematics, tracking centroidal motion and end-effector targets while penalizing control. Its MPC pipeline updates trajectories from feedback and sends inverse-dynamics torques to the robot without an additional constrained inverse-dynamics filter.

D. Kinematics Solver

  • The nonlinear whole-body kinematics problem is solved with DDP, which exploits block-diagonal structure and has quadratic convergence.The implementation uses Crocoddyl.Source: D. Kinematics Solver · p. 7
  • The kinematics cost tracks centroidal momentum, CoM motion, and end-effector locations and velocities, while penalizing control to encourage smooth contact transitions.During jump landings, the solver retracts legs in the air, reducing the torque needed at contact and the impact of the legs.Source: D. Kinematics Solver · p. 7

E. The Model Predictive Control Pipeline

  • BiConMP computes whole-horizon forces and joint trajectories from the current state, then updates the MPC plan every 50 ms and computes inverse-dynamics torques at 1 kHz.The robot tracks the desired torques onboard at 10 kHz.Source: E. The Model Predictive Control Pipeline · p. 7
  • The moving-horizon pipeline shifts longer acyclic contact plans to the current horizon and updates gait phases and desired velocity as time or user input changes.The horizon remains constant after the start of gaited motion.Source: E. The Model Predictive Control Pipeline · p. 7
  • The pipeline combines biconvex centroidal planning, FISTA, whole-body kinematics, and torque computation without solving constrained whole-body inverse dynamics.The authors state that torque references enforcing friction-cone constraints can be applied directly, without further filtering for physical consistency.Source: E. The Model Predictive Control Pipeline · pp. 7–8

IV. EXPERIMENTS

The experiments evaluate optimizer scaling and termination, then demonstrate BiConMP on Solo12 and in simulation on AnYmal and Talos. Solve times scale approximately linearly with problem size, and the framework remains faster than the planned horizon in the reported tests.

IV. EXPERIMENTS

  • The experiments cover cyclic and acyclic motions on Solo12, robustness scenarios, and simulated motions on AnYmal and Talos.The study also analyzes solve times and termination behavior as the optimization problem changes.Source: §IV · p. 8

A. Implementation details

  • The experiments run on a Dell Precision 5820 with a 3.7 GHz Intel Xeon processor, using ROS 2 to connect the 20 Hz MPC and 1 kHz inverse-dynamics loops.The implementation uses C++ and a custom FISTA-based biconvex dynamics optimizer.Source: A. Implementation details · p. 8

1) Solve Times:

  • The horizon-scaling analysis holds motion weights, step time, discretization, and tolerances constant while increasing the horizon and requiring termination criteria to be met.It compares dynamics, DDP kinematics, and total solve times for trot, jump, and bound motions.Source: 1) Solve Times · p. 8
  • The total BiConMP solve time grows linearly with problem size and remains faster than the planning horizon; a 7-second jump takes less than 0.85 seconds to generate.The biconvex dynamics solver takes the most time, while the kinematics solver maintains strong near-linear scaling for the 6–12 collocation points important to MPC.Source: 1) Solve Times · p. 8

2) Termination Criteria:

  • ADMM iterations rapidly reduce dynamic violation, but the solver can hit its iteration limit before reaching the 0.001 threshold; 50 iterations sufficed for the required robot motions.The authors report that trajectories failing the threshold were empirically still of sufficient quality for execution.Source: 2) Termination Criteria · p. 8

C. Cyclic Gaits

On Solo12, BiConMP generates trot, jump, and bound gaits, tracks commanded velocities, and remains robust to pushes and uneven terrain. Its solve times remain within the 50 ms replanning period, while shorter horizons can destabilize gait execution.

C. Cyclic Gaits

  • BiConMP generates trot, jump, and bound gaits on Solo12, with bounding involving considerable changes in angular momentum and pitch.Fig. 2 shows the demonstrated motions; Fig. 5 presents base pitch-axis angular velocity and desired forces during bounding.Source: C. Cyclic Gaits · pp. 8–9, Figure 2 · p. 9, Figure 5 · p. 10
  • Smaller gait horizons often led to solver divergence after a few cycles; bounding required a two-cycle planning horizon for stability.The authors hypothesize that the instability reflects the absence of a terminal cost or constraint ensuring gait viability.Source: C. Cyclic Gaits · p. 9
  • The robot tracks desired linear and angular velocities accurately across gaits, while tolerating unforeseen pushes and unaccounted uneven terrain.The reported tracking plots compare trot and bounding; the velocity-tracking figure uses elapsed time in seconds on its x-axis.Source: C. Cyclic Gaits · p. 9, Figure 6 · p. 10

1) Solve times in real experiments:

  • Worst-case solve time stays within the 50 ms replanning period across flat, disturbed, and uneven-terrain runs; mean solve times remain similar across scenarios.Without sensor noise, solve times average about 5–6 ms less, suggesting clean sensor data matters for higher replanning frequencies.Source: 1) Solve times in real experiments · pp. 9–10

2) Gait Transitions:

BiConMP supports unplanned transitions among trot, jump, and bound, and generates a non-trivial Solo12 high-five motion. Simulation tests also demonstrate its use on other robot morphologies, including dynamic Talos motions.

2) Gait Transitions:

  • User-triggered transitions between trot, jump, and bound require no pre-planning, remain stable, and keep solve times below 50 ms.Solve times remain similar to those without transitions.Source: 2) Gait Transitions · p. 11

D. Acylic Motions

  • For Solo12’s high-five, the planner crouches and builds angular momentum before lifting the front legs to reach the user’s goal position.After the high-five, the kinematics solver finds a rotating front-leg motion to track the momentum profile, rather than relying on a predesigned swing-foot trajectory.Source: D. Acylic Motions · p. 11

E. Simulations with other robots

  • In simulation, the unchanged framework was tested in MPC on two other robots with different morphology, complexity, and mass distribution.The resulting motions and solve times are reported for these experiments.Source: E. Simulations with other robots · p. 11

1) Talos:

  • Talos walking and jumping are generated in MPC using a 10 Hz loop because the mean solve time slightly exceeds 50 ms for its 35-joint full-body optimization.For the jump, the planner receives only a contact plan specifying 0.4-second flight and stance times and generates the force profile and whole-body motions.Source: 1) Talos · p. 11

2) AnYmal:

Anymal simulations demonstrate low-frequency trot and cyclic jumping with stable unplanned gait transitions, while the controller analysis examines torque contributions and direct simulation-to-robot transfer. The evaluation also studies MPC robustness and tuning parameters.

2) AnYmal:

  • Anymal ran a 0.4-second trot and 0.3-second-airtime cyclic jumps, with stable unplanned gait transitions and no QP-based inverse-dynamics controller.The framework also remained unaffected by low stepping frequencies and Talos arm-swinging motions.Source: 2) AnYmal · pp. 11–12

V. ANALYSIS OF THE MODEL PREDICTIVE CONTROLLER

  • The controller analysis evaluates inverse-dynamics contributions, closed-loop MPC versus fixed-plan tracking, and the effects of planning horizon and replanning frequency.It also examines motion robustness and qualitative advantages that are difficult to quantify.Source: §V · p. 12

A. Inverse Dynamics controller

  • During contact, planned torques—especially at the knee—are close to the total torque, while PD gains contribute more during air-to-contact transitions.The authors attribute the transition contribution in part to optimized forces being interpolated from zero to match the 1 kHz torque loop from a 20 Hz MPC loop.Source: A. Inverse Dynamics controller · pp. 12–13, Figure 11 · p. 13

B. Sim-to-Real Transfer

  • Motions stable in simulation transferred directly to Solo12 without retuning, using the same Kp and Ky gains across the presented motions.The authors identify reduced real-robot gain tuning as a benefit of closed-loop MPC.Source: B. Sim-to-Real Transfer · p. 13

C. Impact of re-planning frequency on performance

The experiments examine how replanning frequency and planning horizon affect MPC performance and how replanning frequency affects disturbance rejection. Performance levels off beyond a frequency threshold, while robustness gains are observed at higher frequencies in simulation.

C. Impact of re-planning frequency on performance

  • MPC performance stopped changing above an approximately 7 Hz replanning threshold, including under terrain uncertainty, and simulation and real-robot performance were similar beyond it.The experiment used mean optimal cost while holding the trajectory, horizon, weights, and controller gains constant.Source: C. Impact of re-planning frequency on… · p. 14

D. Impact of planning horizon on performance

  • Below 7 Hz, increasing planning horizon improved observed performance or stability until a threshold, with no visible further benefit beyond it.These horizon results were qualitative because the cost was not comparable across problem sizes; the threshold was unchanged by terrain uncertainty.Source: D. Impact of planning horizon on… · p. 14

E. Impact of re-planning frequency on robustness

  • Disturbance rejection improved from 20 Hz to 100 Hz, with relative gains then decaying; this robustness experiment was conducted in simulation.The authors say comparable real-system results could be expected if the solver were 4–5 times faster.Source: E. Impact of re-planning frequency on… · p. 14

1) Algorithms for closed-loop whole-Body MPC:

The discussion reports how BiConMP’s real-time whole-body MPC performs across analyzed settings and explains the solver choices and assumptions behind it. It also compares the approach with DDP-based methods and deep reinforcement learning.

1) Algorithms for closed-loop whole-Body MPC:

  • BiConMP demonstrates closed-loop whole-body MPC at competitive rates using ADMM and first-order proximal methods, unlike existing approaches that seldom show diverse dynamic motions across robot anatomies.The authors describe it as the first algorithm using ADMM and first-order proximal methods to demonstrate closed-loop MPC at competitive rates.Source: 1) Algorithms for closed-loop… · p. 15

2) Advantages of proximal methods for MPC:

  • FISTA and proximal operators offer numerical robustness and straightforward constraint satisfaction, while the paper reports stable closed-loop MPC behavior.The authors suggest these methods may suit MPC applications where early termination matters more than very high precision.Source: 2) Advantages of proximal methods for… · p. 15

3) Enforcing torque constraints:

  • The kino-dynamic decomposition assumes sufficient torque authority; experiments did not violate this assumption, and additional kino-dynamic iterations or torque constraints are possible if limits are approached.Adding torque constraints in the kinematic optimizer carries slightly higher solve times.Source: 3) Enforcing torque constraints · p. 15

4) Insights from Nonlinear MPC Implementation:

  • Robustness improves as replanning frequency rises from 20 Hz to 100 Hz, with gains tapering afterward; above 10 Hz was unnecessary for direct sim-to-real transfer on Solo12.Higher frequencies are needed for greater robustness to external disturbances.Source: 4) Insights from Nonlinear MPC… · pp. 15–16

5) Comparison to Deep Reinforcement Learning (DRL):

  • BiConMP’s replanning frequency is comparable to DRL methods, while generating new motions does not require retraining a policy and sim-to-real transfer is described as simple and instantaneous.The conclusion reports dynamic behaviors generated in real time across various legged robots and an analysis of MPC frequency, cost, and horizon.Source: 5) Comparison to Deep Reinforcement… · p. 16, CONCLUSION · p. 16

Related papers

Loading 2201.07601v2…