Whole-Body Inverse Dynamics MPC for Legged Loco-Manipulation

2025-11-24

AI Summary

To address the challenge of jointly ensuring motion stability and manipulation force control in legged mobile manipulation, this paper proposes a full-order inverse-dynamics-based whole-body model predictive control (MPC) framework. The method directly optimizes joint torques within a single prediction horizon, unifying whole-body motion planning and contact force generation to achieve dynamically consistent and constraint-complete natural coupling behavior. It integrates Pinocchio for rigid-body dynamics modeling, CasADi for automatic differentiation, and Fatrop for efficient interior-point optimization, enabling real-time control at 80 Hz on a Unitree B2 quadrupedal platform equipped with a Z1 manipulator. Experimental validation demonstrates robust performance across diverse dynamic manipulation tasks—including dragging heavy objects, pushing boxes, and wiping whiteboards—significantly enhancing both robustness and generalization capability of legged mobile manipulation.

Technology Category

• Intelligent Robots: Manipulation
• Computer Vision: Motion & Tracking
• Humans and AI: Human-Aware Planning and Behavior Prediction

Application Category

• Systems and Infrastructure for Web, Mobile and WoT: Applied ML and AI for Web-based mobile applications
• Responsible Web: Machine-in-the-loop, human agency and autonomy
• User Modeling, Personalization and Recommendation: User modeling and simulation for interactive and conversational systems

Abstract

Loco-manipulation demands coordinated whole-body motion to manipulate objects effectively while maintaining locomotion stability, presenting significant challenges for both planning and control. In this work, we propose a whole-body model predictive control (MPC) framework that directly optimizes joint torques through full-order inverse dynamics, enabling unified motion and force planning and execution within a single predictive layer. This approach allows emergent, physically consistent whole-body behaviors that account for the system's dynamics and physical constraints. We implement our MPC formulation using open software frameworks (Pinocchio and CasADi), along with the state-of-the-art interior-point solver Fatrop. In real-world experiments on a Unitree B2 quadruped equipped with a Unitree Z1 manipulator arm, our MPC formulation achieves real-time performance at 80 Hz. We demonstrate loco-manipulation tasks that demand fine control over the end-effector's position and force to perform real-world interactions like pulling heavy loads, pushing boxes, and wiping whiteboards.

Problem

• Developing whole-body MPC for legged robots to manipulate objects while maintaining locomotion stability
• Optimizing joint torques through inverse dynamics for unified motion and force planning
• Achieving real-time performance for complex loco-manipulation tasks like pulling loads and pushing boxes

Innovation

• Optimizes joint torques via inverse dynamics MPC
• Enables unified motion and force planning execution
• Achieves real-time performance at 80 Hz