Source-linked AI summary
A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation
Jean-Pierre Sleiman, Farbod Farshidian, Maria Vittoria Minniti, Marco Hutter
TL;DR
Whole-body locomotion and manipulation require coordinated planning for hybrid, nonlinear, and coupled robot-environment dynamics. The paper formulates both tasks as one constrained switched-system optimal-control problem with augmented robot-object dynamics and solves it through real-time MPC. Hardware experiments demonstrate free-motion and heavy-door manipulation tasks, while also verifying robustness to model mismatch and external disturbances.
Problem
Hybrid, underactuated, and nonlinear dynamics make it challenging to coordinate whole-body locomotion and manipulation with environmental interaction.
Method
The framework uses a switched multi-contact optimal-control formulation with shared gait or manipulation constraints and an augmented model combining robot centroidal, full-kinematic, and object dynamics.
Results
The framework resolves a wide variety of free-motion and manipulation tasks in real time through receding-horizon MPC deployed on hardware.
Takeaways & Limitations
Whole-body MPC can plan physically tractable motion and force trajectories for combined dynamic locomotion and manipulation while respecting operational limits and supporting hardware deployment.
Abstract
from arXiv · showhide
In this paper, we propose a whole-body planning framework that unifies dynamic locomotion and manipulation tasks by formulating a single multi-contact optimal control problem. We model the hybrid nature of a generic multi-limbed mobile manipulator as a switched system, and introduce a set of constraints that can encode any pre-defined gait sequence or manipulation schedule in the formulation. Since the system is designed to actively manipulate its environment, the equations of motion are composed by augmenting the robot's centroidal dynamics with the manipulated-object dynamics. This allows us to describe any high-level task in the same cost/constraint function. The resulting planning framework could be solved on the robot's onboard computer in real-time within a model predictive control scheme. This is demonstrated in a set of real hardware experiments done in free-motion, such as base or end-effector pose tracking, and while pushing/pulling a heavy resistive door. Robustness against model mismatches and external disturbances is also verified during these test cases.
I. INTRODUCTION
The paper addresses the difficulty of coordinating hybrid, underactuated, nonlinear whole-body dynamics by unifying locomotion and manipulation in one optimal-control framework. It combines contact scheduling, robot-object dynamics, and real-time MPC, with demonstrations on hardware.
- Motivation: Whole-body robotic motion must coordinate complex movements and environmental interaction across locomotion and manipulation tasks.Such systems have hybrid, underactuated, and highly nonlinear dynamics, making controller design challenging.
- Relation to prior work: The approach targets limitations of prior methods that separated manipulation from locomotion, specified reactive gripper forces, or could not support real-time replanning.The cited prior formulations included offline planning and non-real-time approaches.
- Unified formulation: The framework formulates dynamic locomotion and manipulation as a unified multi-contact optimal control problem for switched systems.A single constraint set describes predefined gait sequences or manipulation contact schedules.
- Unified formulation: An augmented model combines manipulated-object dynamics, robot centroidal dynamics, and full kinematics in one cost/constraint function.This encodes robot-centric and object-centric tasks within the same formulation.
- Results and deployment: The resulting receding-horizon optimal-control framework resolves free-motion and manipulation tasks in real time on hardware with limited onboard computational power.Experiments include whole-body maneuvers and manipulation problems, including heavy resistive-door interaction.
- Results and deployment: The work reports one of the first real-hardware applications of whole-body MPC unifying dynamic locomotion and manipulation.The claim concerns a unified whole-body MPC framework demonstrated on real hardware.
II. PROBLEM FORMULATION
The problem formulation treats whole-body planning as constrained optimal control for a switched system. It uses SLQ-MPC to solve the formulation in receding horizon while incorporating equality and inequality constraints.
- A. Whole-Body Planner: The nonlinear-MPC framework models hybrid dynamics as switched systems with predefined modes and can optimize switching times alongside state and input trajectories.In this work, mode dependence and switching times are omitted from notation because they are assumed fixed.
- A. Whole-Body Planner: Sequential-Linear-Quadratic (SLQ) is a continuous-time iterative-LQR method within the Differential Dynamic Programming family.It performs forward rollout, local approximations, and a backward Riccati pass to produce feedforward and feedback control terms.
- A. Whole-Body Planner: One SLQ iteration has complexity linear in the time horizon, unlike direct trajectory-optimization methods with cubic complexity.The complexity distinction is stated for the full sequence of SLQ computations.
- A. Whole-Body Planner: The constrained-SLQ extension handles state-input and state-only equality constraints using projections and inequality path constraints using penalty and barrier functions.These mechanisms extend unconstrained SLQ to the constraints required by the optimal-control problem.
- A. Whole-Body Planner: The optimal-control problem uses state x(t), input u(t), stage cost L(x,u,t), terminal cost Φ(x(T)), dynamics, equality constraints, inequality constraints, and initial condition x(0)=x0.The formulation defines x(t) and u(t) as state and input vectors, respectively.
- A. Whole-Body Planner: The SLQ-MPC framework solves the constrained optimal-control problem in closed loop by repeatedly applying constrained SLQ over a receding horizon.This is implemented within a real-time iteration scheme.
1) System Modeling:
The framework models a floating-base robot using centroidal dynamics, full kinematics, and manipulated-object dynamics in one augmented state-space description. This retains coupling between the base, limbs, and object while avoiding the standard massless-limbs assumption.
- The robot is modeled as an unactuated 3D floating base with fully actuated limbs.
- The generalized model uses q and ν for coordinates and velocities, M for mass, b for nonlinear effects, and τa for actuation torques.
- Centroidal momentum hcom combines linear and angular momentum, while contact forces and torques determine its evolution.
- The centroidal momentum matrix maps generalized-coordinate rates to centroidal momentum using the full kinematic configuration and multibody inertias.
- The robot state is xr = (hcom, qb, qj), and the input contains contact forces and joint velocities; the demonstrated system has 28 states and 31 inputs.
- Manipulated-object coordinates and velocities are appended to the robot state, requiring known object dynamics, known parameters, and continuous object-state feedback.
2) Equality Constraints:
Equality constraints impose contact behavior according to a predefined mode schedule. They enforce stance contact, swing-foot motion, and optional grasped-object contact while supporting non-prehensile manipulation.
- Potential contact points are open or closed, and equality constraints depend on a predefined contact mode sequence and switching times.
- Open contacts have zero force, stance feet remain fixed relative to the ground, and grasped objects remain in contact with the end-effector.
- Swing legs track a reference velocity along the surface normal while retaining freedom in the orthogonal directions that determine stride length.
- Allowing the arm contact to open extends the framework from continuous grasping to non-prehensile tasks requiring intermittent object contact.
3) Inequality Constraints:
The planner adds inequality constraints for operational limits and contact feasibility, while constrained SLQ uses dual-variable updates and converges to a potential KKT optimum. Torque feasibility is enforced strictly downstream in the tracking controller.
- Inequality constraints are incorporated through a relaxed log-barrier extension of SLQ because Riccati-based DDP solvers do not naturally handle them.
- The Lagrange dual function is maximized along the dual variables, and the algorithm converges to a potential primal-dual KKT point.
- The constraint set limits arm joint velocities and torques and keeps foot contact forces inside the friction cone with coefficient µs.
- The planner’s torque constraint includes object dynamic effects but omits arm inertial and velocity-dependent terms to preserve MPC frequency.
- Strict feasibility with respect to real torque limits is imposed by the QP-based tracking controller rather than directly by the MPC layer.
4) Complementary Remarks:
The implementation uses Pinocchio for rigid-body quantities and computes the gradients required by SLQ, including a comparison with automatic differentiation based on CppAD.
- Pinocchio provides fast rigid-body algorithms for the robot’s kinematic transforms, Jacobians, and centroidal momentum matrix.
- Because SLQ requires dynamics and constraint gradients, the implementation compares automatic differentiation with CppAD against another implementation.
B. Whole-Body Controller
The WBC tracks whole-body planner references through prioritized quadratic-programming tasks, converting planned states, forces, and motions into physically consistent control commands.
- B. Whole-Body Controller: The WBC uses a hierarchical quadratic program to optimize generalized accelerations and contact forces, then retrieves joint torques by inverting the desired dynamics.Its task priorities are represented in the high-level controller architecture.
- B. Whole-Body Controller: Rather than directly tracking planned ground-reaction forces, the WBC tracks the induced base motion and adjusts forces when higher-priority objectives require it.The WBC uses a more realistic model and imposes stricter physical-correctness conditions than the planner.
- B. Whole-Body Controller: Arm reference positions, velocities, and contact forces come directly from the MPC solution, while joint accelerations are approximated by finite differences and swing-foot trajectories are derived separately.The MPC output is converted into the quantities required by the WBC tracking tasks.
- B. Whole-Body Controller: The controller architecture combines a whole-body planner with the WBC, whose high-priority tasks are identified in the architecture diagram.The high-level control module contains this planner–WBC interaction.
- B. Whole-Body Controller: Base pose is taken from the MPC state, while base velocities and accelerations are obtained through mappings and differentiated dynamics expressions.The angular-velocity mapping uses a transformation from ZYX-Euler-angle derivatives.
III. SYSTEM DESCRIPTION
The framework is demonstrated on an ANYmal C quadrupedal mobile manipulator equipped with a torque-controllable DynaArm, with onboard computation supporting real-time MPC updates.
- III. SYSTEM DESCRIPTION: The experiments use an ANYmal C quadrupedal platform equipped with a custom-made four-degree-of-freedom DynaArm.The framework itself is described as robot-agnostic and capable of encompassing multiple multi-limbed systems.
- III. SYSTEM DESCRIPTION: The DynaArm has four powerful actuators and a 7 kg payload capability, supporting highly dynamic maneuvers.Its elbow actuator is mounted at the shoulder and driven through a belt transmission.
- III. SYSTEM DESCRIPTION: With a 1 s horizon, the onboard MPC computes free-motion feedforward trajectories at an average update rate of 70 Hz.The onboard computer is an Intel Core i7-8850H hexacore processor; feedback fuses encoder and IMU measurements.
IV. EXPERIMENTAL RESULTS
The experiments use a generic cost function whose task objectives are selected through switch variables and weighted tracking terms for robot and end-effector states.
- IV. EXPERIMENTAL RESULTS: The experiments include simulation and real-hardware tests divided into free-motion and object-manipulation categories.The paper introduces a generic cost function before presenting the test cases.
- IV. EXPERIMENTAL RESULTS: The cost function uses positive-definite control weighting and positive-semidefinite tracking matrices for end-effector position and orientation errors.Orientation error is represented with exponential coordinates.
- IV. EXPERIMENTAL RESULTS: Binary parameters α1, α2, and α3 switch between different objectives according to the task being performed.These parameters are selected differently for the experimental scenarios.
A. Free-Motion
Free-motion experiments show that weighting choices let the planner either constrain the arm as a lumped mass or exploit it as a balancing tail, while full-body planning supports tracking under an unmodeled load.
- A. Free-Motion: Dynamic trot and flying-trot experiments impose high arm-joint penalties to keep the arm near a nominal configuration and treat it as a lumped mass.In this setting, the arm does not contribute to balancing.
- A. Free-Motion: Reducing joint weights lets the planner exploit base–limb coupling, moving the arm to redistribute contact forces and counteract rolling angular momentum.The arm swings opposite the robot’s lateral acceleration during sideways trotting.
- A. Free-Motion: The lateral-trot comparison evaluates base roll when the arm is modeled as a rigidly attached mass versus when it acts as a balancing tail.The two cases correspond to high and low joint weights, respectively.
- A. Free-Motion: Including the full kinematic model enables motion plans that exploit redundancy for end-effector tracking, with the employed degrees of freedom determined by the relative Qr weights.The framework also tracks base and gripper references while the robot carries an unmodeled 2 kg load.
- A. Free-Motion: Object-centered manipulation simulations include pulling a 10 kg load, throwing a 3 kg load, and pushing a 10 kg load toward target positions.These tasks use the object state in the task specification.
- A. Free-Motion: Real hardware door-opening experiments produce physically consistent pushing and pulling plans that reach the desired door angle despite model mismatches.Collision-avoidance behavior is shaped through cost penalties, such as penalizing lateral base motion during pushing.
C. Comparative Study
The comparative study evaluates template models for dynamic object lifting and reports that richer dynamics improve fast-motion performance, despite a lower MPC update rate.
- Door experiments: Door experiments plot estimated door angle and applied normal force for pushing and pulling, with a disturbance introduced during pulling.
- Model comparison: The comparison evaluates centroidal and SRBD template models with and without object dynamics during a 1.25 m dynamic lifting task.The cases use varying desired lifting times and report failure thresholds, settling times, and average MPC iteration times.
- Model comparison: Including arm and object dynamics in the planner allows a wider range of fast lifting motions.
- Performance trade-off: The richest model achieves the best performance, although excluding object state or using SRBD slightly reduces average computational time.
- Unified formulation: The framework augments robot centroidal dynamics with manipulated-object dynamics to account for coupling between the base, limbs, and object.This extended model is part of the constrained, switched MPC formulation evaluated across free-motion and object-manipulation tasks.