PaperlayerView exact revision on arXiv

Source-linked AI summary

Whole-Body Nonlinear Model Predictive Control Through Contacts for Quadrupeds

Michael Neunert, Markus Stäuble, Markus Giftthaler, Carmine D. Bellicoso, Jan Carius, Christian Gehring, Marco Hutter, Jonas Buchli

arXiv:1712.02889v1cs.RO

TL;DR

Quadruped locomotion control must handle contacts while producing dynamically consistent motions at control rates suitable for hardware. This paper presents whole-body NMPC with explicit contact dynamics and efficient optimization, demonstrating robust, transferable hardware performance across two quadrupeds and dynamic tasks.

  • Problem

    Existing planning and control frameworks may produce dynamically inconsistent plans, while tracking controllers lack authority to modify footholds or contact timings; generic nonlinear-programming solvers are often too slow for MPC.

  • Method

    The approach uses whole-body NMPC with a full rigid-body dynamics model, explicit auto-differentiable contact modeling, and continuous high-frequency re-optimization of state and control trajectories.

  • Results

    The method runs on two quadrupeds and supports periodic gaits, dynamic contact-switching motions, disturbances, and repeated squat jumps, with execution rates exceeding the state of the art by an order of magnitude.

  • Takeaways & Limitations

    Whole-body NMPC through contacts can be deployed across quadrupeds with different actuation principles without major adjustments while maintaining robust performance and replanning capabilities.

  • Takeaways & Limitations

    Extending the NMPC controller to a larger time horizon remains future work, with its effects on performance and robustness still to be evaluated.

Abstract

from arXiv · show

In this work we present a whole-body Nonlinear Model Predictive Control approach for Rigid Body Systems subject to contacts. We use a full dynamic system model which also includes explicit contact dynamics. Therefore, contact locations, sequences and timings are not prespecified but optimized by the solver. Yet, thorough numerical and software engineering allows for running the nonlinear Optimal Control solver at rates up to 190 Hz on a quadruped for a time horizon of half a second. This outperforms the state of the art by at least one order of magnitude. Hardware experiments in form of periodic and non-periodic tasks are applied to two quadrupeds with different actuation systems. The obtained results underline the performance, transferability and robustness of the approach.

I. INTRODUCTION

The paper presents whole-body NMPC that explicitly reasons through contacts and continuously re-optimizes trajectories for quadruped hardware. It addresses feasibility, runtime, transferability, and robustness across different robots and tasks.

  • I. INTRODUCTION: Whole-body NMPC incorporates explicit contact dynamics, allowing contact timings, sequences, and locations to emerge from optimization.The approach is applied to periodic gait patterns and highly dynamic motions, including squat jumps.
  • I. INTRODUCTION: Multi-stage frameworks can produce dynamically infeasible plans, while their tracking controllers cannot sufficiently modify footholds or contact timings.Centroidal-dynamics methods capture core dynamics but often still prespecify or optimize contacts separately.
  • I. INTRODUCTION: The solver exceeds the state of the art in robotics NMPC applications by at least one order of magnitude through Auto-Differentiation, code generation, and software engineering.The implementation is also released as open-source software, and experimental cost-function weights are published for reproducibility.
  • I. INTRODUCTION: High-frequency continuous re-optimization improves robustness to model mismatches and enables hardware transfer of periodic gaits and dynamic motions.This work extends earlier offline whole-body motion optimization, whose plans were less robust for hardware transfer.

II. NMPC FOR RIGID BODY SYSTEMS

The NMPC formulation repeatedly solves finite-horizon nonlinear optimal-control problems over rigid-body states and controls. Its dynamics include inertia, nonlinear forces, actuation, and external contact forces for floating-base robots.

  • II. NMPC FOR RIGID BODY SYSTEMS: NMPC recurrently solves finite-horizon optimal-control problems defined by a running cost, terminal cost, nonlinear dynamics, and state-control trajectories.The trajectories are denoted x(t) and u(t).
  • A. System Modelling: Rigid-body dynamics combine inertia, Coriolis and centripetal forces, gravity, joint torques, and external forces transmitted through a contact Jacobian.The model uses an 18-dimensional generalized coordinate space and 12 actuated joint torques.
  • A. System Modelling: For floating-base robots, generalized positions and velocities include base pose and twist, representing an unactuated six-degree-of-freedom joint.The orientation is parameterized with Euler angles.
  • A. System Modelling: The state uses pose in an inertial world frame and twist in a local body frame, requiring a coordinate transform between them.The transform combines a rotation for linear velocities with a mapping for angular velocities.

B. Contact Model

The contact model embeds smooth spring-damper contact physics directly into the rigid-body dynamics. This lets the gradient-based NMPC solver reason about contacts without prespecified sequences, locations, or timings.

  • B. Contact Model: The method avoids prespecified contact sequences, locations, and timings by adding explicit contact physics to the dynamic model.This avoids complementarity constraints that violate LICQ assumptions used by many nonlinear optimal-control and programming solvers.
  • B. Contact Model: Each end-effector uses linear springs and dampers acting perpendicular and parallel to the contact surface.The contact model is computed in a specialized contact frame.
  • B. Contact Model: Sigmoid and exponential smoothing functions produce differentiable contact behavior controlled by spring and damper sharpness parameters.The sigmoid scales the damper term using the normal contact-surface penetration.
  • B. Contact Model: Smoothed contact forces are transformed into the robot body frame and passed to forward dynamics on the corresponding link.Forces never completely vanish, allowing the solver to reason about contacts before they are established.
  • B. Contact Model: The simplified contact model is less physically accurate than alternatives but supports efficient Auto-Diff derivatives needed for fast NMPC.The authors report that this modeling choice does not hinder good hardware performance.

III. NMPC APPROACH

The approach solves whole-body NMPC by locally approximating nonlinear optimal control with LQOC problems and iteratively updating feedback policies and trajectories online.

  • NMPC Approach: The solver formulates NMPC as an unconstrained optimal control problem solved by iterative Gauss-Newton NLOC algorithms.Each iteration locally approximates the problem using a Gauss-Newton Hessian approximation.
  • NMPC Approach: Riccati-based LQOC solves provide linear complexity in the time horizon, improving efficiency for longer horizons.
  • NMPC Approach: The resulting control policy combines feedforward action with linear feedback that regulates deviations from the reference state trajectory.
  • NMPC Approach: iLQR and GNMS use the same local LQOC formulation, while GNMS designs the state reference trajectory together with the control policy.
  • NMPC Approach: The iLQR-NMPC loop measures the state, forward-integrates dynamics, computes sensitivities, solves LQOC, updates the feedback policy, and performs line search.
  • NMPC Approach: The GNMS-NMPC algorithm separates feedback and preparation phases, updating the policy and trajectories before completing multiple-shooting interval computations.

IV. SOFTWARE IMPLEMENTATION

Real-time NMPC remains computationally demanding because processor performance increasingly depends on parallel cores and vectorization, which standard compilers do not automatically exploit.

  • Software Implementation: Real-time NMPC for high-dimensional systems remains challenging despite increasingly powerful consumer computers.
  • Software Implementation: Modern processing power increasingly comes from higher computation core counts and vectorization rather than faster clock speeds.
  • Software Implementation: Standard compilers cannot automatically leverage parallel execution and vectorization for many computational routines.

A. Modelling Framework

The modelling framework uses code-generated rigid-body dynamics and kinematics with automatic differentiation and contact-force mappings to produce fast, hard-real-time-capable code.

  • Modelling Framework: The controller relies on RobCoGen for efficient code generation of rigid-body dynamics and kinematics.
  • Modelling Framework: RobCoGen is integrated with Control Toolbox automatic differentiation and contact models to provide contact-force mappings.
  • Modelling Framework: The resulting framework is lean compared with sophisticated physics engines and produces fast-to-evaluate, hard-real-time-capable code.

B. Integration and Sensitivity Computation

The implementation addresses stiff contact dynamics and sensitivity computation through specialized numerical methods, while parallelism and AVX vectorization accelerate execution across hardware platforms.

  • Integration and Sensitivity Computation: Stiff contact models require small integration time-steps, so the implementation uses symplectic or semi-implicit integrators instead of standard explicit schemes.
  • Integration and Sensitivity Computation: Sensitivities are computed for the integrated state with respect to the initial state and control action to support local linearization.
  • Multithreading and Vectorization: Multiple shooting parallelizes forward simulation across intervals, reducing computation time linearly with the number of cores.
  • Multithreading and Vectorization: Switching from SSE to AVX produced an additional speedup of almost a factor of two for matrix and vector computations.
  • Platform Descriptions: The hardware experiments use HyQ, an 80 kg hydraulic quadruped, and ANYmal, a roughly 34 kg quadruped with electric series-elastic actuators.

1) HyQ:

The experiments use two quadruped platforms with different actuation systems and a tracking framework that combines optimized torques with feedback for swing-leg tracking.

  • 1) HyQ:: HyQ is a fully torque-controlled hydraulic quadruped with three joints per leg: HAA, HFE, and KFE.Joint torques are measured by load cells, while absolute and relative encoders measure joint motion.
  • 1) HyQ:: ANYmal has the same joint configuration as HyQ but uses fully torque-controlled series-elastic actuators.Encoder measurements across the elastic elements provide joint deflection for computing internal torque.
  • 1) HyQ:: A 10–15 ms measurement-to-execution delay prevents robust swing-leg position and velocity control from optimized torques alone.The framework therefore adds a PD loop around optimized joint trajectories.
  • 1) HyQ:: Both robots estimate base pose and twist by fusing IMU and leg-encoder measurements, while joint velocities are obtained by numerical differentiation.Ground estimation uses foot stance checks and plane fitting.
  • 1) HyQ:: The tracking controller runs at 250 Hz on HyQ and 400 Hz on ANYmal.A dedicated computer runs the NMPC loop and communicates with the midlevel controller through ROS.

VI. RESULTS

The experiments evaluate periodic trotting and additional dynamic motions, using a 500 ms horizon and 4 ms control discretization. The controller adapts gait and contact behavior under disturbances and returns ANYmal to periodic trotting afterward.

  • VI. RESULTS: 500 ms horizons, 4 ms control discretization, and 1 ms integration rates are used in all experiments.The solver uses iLQR and GNMS with warm starts and applies the trajectory after one real-time iteration.
  • VI. RESULTS: Periodic trotting is encouraged through periodically activated costs on swing-leg apex joint-angle deviations.These costs influence gait frequency while leaving exact contact timings free to optimization.
  • VI. RESULTS: During strong lateral pushes, the algorithm can keep feet on the ground, modify gait timing, and stabilize the base before resuming the gait pattern.The feedforward-dominant controller plans ahead rather than reacting aggressively to disturbances.
  • VI. RESULTS: HyQ’s trotting plots show larger orientation and position deviations than ANYmal, with orientation penalized more strongly than position in the cost function.The difference in deviation magnitude is partly associated with the robots’ different sizes.
  • VI. RESULTS: At t = 2.8s, a wooden plank disturbance causes ANYmal to leave its periodic trot temporarily, shift and rotate its base, then return to periodic trotting.The experiment also reports smaller base-orientation and position deviations than HyQ, partly attributed to robot size and weight differences.

2) Squat Jumps:

The controller executes repeated and forward squat jumps while optimizing contact timing, and does so with high-rate NMPC suitable for hardware experiments.

  • Squat jumps: 40 Nm torque limit is not reached; torques drop after takeoff and peak during landing.No additional torque constraints were imposed in the experiment.
  • Squat jumps: Repeated jumps use periodic vertical-velocity costs, while lift-off and landing times remain optimized rather than pre-specified.The robot performs several jumps without resetting to its nominal configuration.
  • Squat jumps: 1.0 m/s vertical takeoff velocity produces a 3 cm apex, with a 0.1 m/s execution overshoot and slight x–y drift.The controller maintains a constant apex height during repeated jumping.
  • Timings: 175 Hz GNMS-MPC exceeds iLQR’s roughly 80 Hz update rate, although the higher rate does not produce significantly better performance.GNMS benefits from parallel simulation, no linesearch for this problem, and finer control discretization.
  • Squat jumps: The controller first lowers the base by bending the knees, accumulating energy before takeoff in a behavior unavailable to a pure feedback controller.This preparatory motion emerges from trajectory optimization.
  • Timings: Below 30 Hz, hardware performance degrades significantly, whereas GNMS provides headroom for longer horizons or more complex systems.At 80 Hz, each computation takes less than 12 ms.
  • Timings: The solver is about 300% faster than the compared implementation at the same horizon and similar degrees of freedom, and could be 10–15 times faster under matched conditions.The gains are attributed to solver implementation, vectorization, faster dynamics, and Auto-Diff code generation.

VII. SUMMARY AND OUTLOOK

The work demonstrates hardware whole-body NMPC through contacts for dynamic motions, with transfer across quadrupeds and contact-aware behavior emerging from one controller. Future work targets longer horizons, explicit constraints, and additional robustness mechanisms.

  • Summary: Whole-body NMPC through contacts is demonstrated on hardware for dynamic motions, periodic gaits, and dynamic contact switches.The paper identifies this as the first such application to dynamic motions and hardware tasks.
  • Summary: Applying the same framework to HyQ and ANYmal demonstrates transfer across robots with different actuation principles without major adjustments.The experiments use hydraulic HyQ and electrically actuated, series-elastic ANYmal.
  • Summary: Disturbance-dependent gait changes, base stabilization, and reactive sidestepping emerge naturally from a single NMPC algorithm.A classical approach would potentially require several planning and control modules.
  • Outlook: Future work will use GNMS speed and warm-starting to extend the NMPC horizon and study effects on performance, robustness, and recovery.The authors expect longer horizons to provide more predictive flexibility for disturbance rejection.
  • Outlook: GNMS could incorporate torque limitations as explicit inequality constraints, potentially using soft constraints to manage added computational complexity.Most reported tasks were designed to remain within platform limits.
  • Outlook: Planned additions include adaptation or disturbance estimation and tracking motions from high-level planners for non-convex terrain.These extensions are intended to further robustify the approach and bring new motions to hardware.
Loading 1712.02889v1…