A Nonlinear MPC Framework for Loco-Manipulation of Quadrupedal Robots with Non-Negligible Manipulator Dynamics

A Nonlinear MPC Framework for Loco-Manipulation of Quadrupedal Robots with Non-Negligible Manipulator Dynamics

Paper: A Nonlinear MPC Framework for Loco-Manipulation of Quadrupedal Robots with Non-Negligible Manipulator Dynamics
Authors: Ruturaj Sambhus, Kapi Ketan Mehta, Ali Mir Mohammad Sadeghi, Basit Muhammad Imran, Jeeseop Kim, Taizoon Chunawala, Vittorio Pastore, Sujith Vijayan, Kaveh Akbari Hamed
Affiliation: Department of Mechanical Engineering, Virginia Tech; California Institute of Technology; School of Neuroscience, Virginia Tech
Link: arXiv:2507.22042

One-Sentence Summary

This paper proposes a computationally efficient layered nonlinear MPC (NMPC) framework that couples a single rigid body (SRB) locomotion template with a full-order floating-base manipulator dynamics model via rigid holonomic constraints, solving trajectory optimization at 60 Hz while a low-level whole-body controller (WBC) tracks locomotion at 500 Hz and arm torques are applied directly. Validated on a 15-kg Unitree Go2 with a 4.4-kg 4-DOF Kinova arm (29% arm-to-base mass ratio, 216% height ratio), it achieves robust door-opening, heavy payload pushing (125% mass uncertainty), pick-and-place, and disturbance rejection, with 58% higher success rate than baseline kino-centroidal NMPC across 200 random terrains.

Research Background and Motivation

Model predictive control (MPC) combined with reduced-order template models has emerged as a powerful tool for trajectory optimization in dynamic legged locomotion. Reduced-order models like LIP, SLIP, and SRB provide low-dimensional abstractions of complex nonlinear locomotion systems. However, loco-manipulation tasks performed by legged robots with articulated manipulators introduce new complexity — requiring MPC algorithms that are not only computationally efficient but also capable of coordinating high-DOF motions under dynamic constraints.

Existing whole-body planning algorithms typically leverage centroidal dynamics and full-body kinematics, but many approximate or neglect the full-order dynamics of the manipulator itself. This simplification becomes limiting when arm dynamics are non-negligible — e.g., integrating a 4.4-kg 4-DOF Kinova arm with a 15-kg Unitree Go2, where arm-to-base mass and height ratios reach 29% and 216%. The Go2's nominal 8-kg static payload capacity does not account for dynamic, tall manipulators like the Kinova arm. To the authors' knowledge, no model-based method has addressed dynamic loco-manipulation with full-order, torque-controllable, heavier arms on small quadrupeds.

This paper addresses a fundamental question: how can full-order manipulator dynamics be systematically integrated with locomotion template models to enable real-time, efficient, and robust NMPC for loco-manipulation? The core idea is a decomposition strategy — preserving SRB template effectiveness for dynamic quadrupedal locomotion while systematically integrating arm dynamics for manipulation, coupled through rigid holonomic constraints.

Figure 1: Layered control framework overview. High-level NMPC based on decomposition optimizes both the locomotion template and full-order arm model; low-level nonlinear WBC tracks the full-order locomotion model.

Dynamic Model

Locomotion Template — SRB Model. The simplified SRB dynamics serve as the locomotion template, with states including CoM position, Euler angles, and velocities. Net force and torque comprise stance-foot ground reaction forces (GRFs), the arm-base interaction wrench $\lambda^{int} = \text{col}(f^{int}, \tau^{int}) \in \mathbb{R}^6$, and its induced torque:

where $\mathcal{C}$ is the stance foot set, $f^\ell$ is the GRF at foot $\ell$, $r^\ell$ is the vector from foot to CoM, and $\mathbb{S}(\cdot)$ is the skew-symmetric matrix operator. GRFs serve as SRB control inputs $u^{SRB}$ subject to friction cone constraints. The discretized nonlinear state-space equation is:

Manipulation Model — Floating-Base Arm Dynamics. For torque-level control within NMPC, the arm's floating-base dynamics are incorporated. Floating-base coordinates $q^{arm} = \text{col}(q^b, q^s) \in \mathbb{R}^{10}$, where $q^b \in \mathbb{R}^6$ are base coordinates and $q^s \in \mathbb{R}^4$ are shape variables. The arm equations of motion are derived via Euler-Lagrange formalism:

where $D(q^{arm}) \in \mathbb{R}^{10 \times 10}$ is the positive-definite mass-inertia matrix, $H$ includes Coriolis, centrifugal, and gravitational effects, $B \in \mathbb{R}^{10 \times 4}$ is the input distribution matrix, $u^{arm} \in \mathbb{R}^4$ are joint torques, and $J$ is the constraint Jacobian. The interaction wrench $\lambda^{int}$ acts on the arm via $-J^\top \lambda^{int}$ — Newton's third law ensures equal and opposite wrenches on SRB and arm.

Holonomic Constraints and Interaction Wrench

The SRB and arm are coupled through rigid holonomic constraints — the arm base position and orientation must match the corresponding point on the SRB:

where $d$ is the fixed offset vector from SRB CoM to the interaction point. Compact form: $\varphi(p, \theta, q^{arm}) = 0$. Differentiating twice and substituting SRB and arm dynamics yields the dynamic consistency condition:

This serves as an NMPC equality constraint, enabling the optimizer to implicitly solve for the interaction wrench while SRB and arm dynamics remain defined in their own local states and control variables, coupled only through the holonomic constraint. This strategy avoids the additional nonlinearity from analytically eliminating $\lambda^{int}$, keeping the optimal control problem tractable.

Unified NMPC Formulation

The real-time unified NMPC optimizes decision variables $(\xi^{SRB}(\cdot), \xi^{arm}(\cdot), \lambda^{int}(\cdot))$ with the objective:

subject to SRB and arm discrete dynamics, holonomic constraint $\ddot{\varphi} = 0$, GRF availability constraints, and friction cone / torque admissibility inequalities. Each subsystem cost is standard quadratic:

The interaction wrench cost $J^{int} = \sum_{k=0}^{N-1} \|\lambda^{int}_{t+k|t}\|_{R^{int}}^2$ penalizes excessive arm-base interaction forces. NMPC is solved at 60 Hz in a receding horizon fashion.

flowchart TD
  subgraph High["High-Level NMPC 60Hz"]
    SRB["SRB Template Model
States: CoM pos/Euler/vel
Inputs: GRFs"]
    ARM["Full-Order Arm Dynamics
States: floating-base coords
Inputs: joint torques"]
    SRB <-->|Interaction wrench λ_int| ARM
    HC["Holonomic Constraint φ̈=0"]
    SRB --> HC
    ARM --> HC
  end
  SRB -->|Optimal states + GRFs| WBC["Low-Level WBC 500Hz
QP Virtual Constraints"]
  WBC -->|Joint Torques| LEGS["Legs Execution"]
  ARM -->|Optimal Torques| ARMEXE["Arm Direct Execution"]

Low-Level Nonlinear WBC

The SRB optimal state and GRF trajectories are relayed to a low-level nonlinear WBC (500 Hz) based on QP and virtual constraints. The WBC optimization objective is:

subject to output dynamics $\ddot{y} + K_D \dot{y} + K_P y = \delta$ (virtual constraints encoding position tracking), no-slip condition $\ddot{r}^{st} = 0$, and feasibility constraints. $f^{des}(t)$ is provided by the high-level NMPC, $\delta$ is a defect variable. Swing foot trajectories use Bezier polynomials interpolating between footholds (Raibert heuristics). Arm optimal torque commands are applied directly without low-level WBC.

Experimental Setup

Hardware: Unitree Go2 (15.0 kg, 0.28m standing height, 12 actuated joints) with Kinova Gen2 4-DOF arm (4.4 kg, torque-controlled, 3-finger gripper). A Sony DualShock 4 controller provides real-time reference commands. NMPC runs offboard on an i9-12900F CPU with 64GB RAM, communicating via Ethernet LAN. Simulations use the RaiSim physics engine. CasADi and IPOPT solve the NMPC; qpSWIFT solves the WBC QP.

Experimental Results

Extensive simulations and hardware experiments validate diverse loco-manipulation tasks:

Figure 2: Experiment snapshots across diverse loco-manipulation tasks: opening fridge door, blocks walking with payload, pushing wagon, pulling suitcase, pick-and-place, door traversal, push disturbance recovery, drawer opening, gravel/grass walking.

Quantitative Comparison. On 200 randomly generated terrains, the proposed method achieves 58% higher robust locomotion success rate than the baseline kino-centroidal NMPC (from Sleiman et al.). The baseline ignores arm full-order dynamics, leading to significantly reduced robustness and stability at high arm-to-base mass ratios.

Figure 3: Go2 + Kinova arm robust loco-manipulation. (a) Trotting over wooden blocks carrying 1kg payload, (b) withstanding external push disturbance.

Technical Analysis

Core advantage of decomposition. The decomposition strategy keeps SRB and arm in their own local states and control variables, coupled only through the holonomic constraint $\ddot{\varphi} = 0$. This allows NMPC to separately optimize locomotion and manipulation sub-objectives while explicitly regulating their force interaction via the wrench penalty $J^{int}$. Compared to eliminating arm dynamics analytically, decomposition avoids additional system nonlinearity, making the optimal control problem more tractable for 60 Hz real-time performance.

Significance of direct torque application. Arm optimal torque commands are applied directly without low-level WBC because the arm's floating-base dynamics are already fully modeled in NMPC — torque-level control can be directly realized. Legs require WBC to distribute SRB-level GRFs to joint torques because the SRB model lacks joint-level information. This differentiated treatment reflects precise layered control design.

Computational efficiency. The 60 Hz real-time NMPC is enabled by the decomposition strategy reducing problem scale. Decision variables include SRB trajectories $\xi^{SRB} \in \mathbb{R}^{24N}$, arm trajectories $\xi^{arm} \in \mathbb{R}^{24N}$, and interaction wrench $\lambda^{int} \in \mathbb{R}^{6N}$. The decomposition preserves local linear structure in each subsystem's dynamics, and CasADi's automatic differentiation with IPOPT efficiently handles this sparse structure. The low-level WBC QP (qpSWIFT) is even smaller.

125% mass uncertainty significance. The wagon cart with payload totals 24.4 kg (125% of Go2 base mass). This large mass uncertainty severely tests controller robustness — arm pushing forces create reaction torques that could flip the robot. The NMPC's explicit interaction wrench modeling enables preemptive compensation of pushing force effects on base balance, while SRB GRF distribution dynamically adjusts to maintain stability — precisely the value of full-order arm dynamics modeling.

Comparison with learning methods. Compared to RL-based methods (DeepWBC, RoboDuet), this model-based approach offers interpretability and safety guarantees — NMPC explicitly handles constraints (friction cone, torque limits) with provable physical feasibility. RL methods have faster inference but lack formal safety guarantees and face sim-to-real uncertainty. However, model-based methods are sensitive to model accuracy — arm dynamics parameter errors affect performance. This work minimizes uncertainty through full-order modeling, maintaining robustness under 125% mass uncertainty.

Limitations and Future Directions

Limitation 1: Offboard computation dependency. NMPC runs offboard on a desktop PC via Ethernet. Real deployment may face communication latency and bandwidth limitations; onboard computational resources may be insufficient for 60 Hz NMPC solving.

Limitation 2: Limited arm DOF. The Kinova Gen2 has only 4 DOF, limiting manipulation dexterity. Higher-DOF arms would increase NMPC decision variable dimension, posing greater real-time solving challenges.

Limitation 3: Manual reference commands. Currently requires a human operator to provide real-time reference commands via gamepad, lacking autonomous task planning and perception.

Future work includes: onboard computation, integrating perception for autonomous operation, extending to higher-DOF arms, and combining learning methods for robustness.

Conclusion

This paper presents the first model-based control algorithm for dynamic loco-manipulation on small quadrupeds with non-negligible dynamics manipulator arms. The core innovation is the decomposition strategy — coupling the SRB locomotion template with full-order arm floating-base dynamics through rigid holonomic constraints, enabling NMPC to simultaneously optimize locomotion and manipulation sub-objectives while explicitly regulating interaction wrenches, solved at 60 Hz real-time. The low-level WBC tracks locomotion at 500 Hz, with arm torques applied directly. On the Go2 + Kinova platform (29% arm-to-base mass ratio), diverse tasks including door opening, heavy payload pushing (125% mass uncertainty), pick-and-place, and disturbance recovery are validated, with 58% higher success rate than the baseline ignoring arm full-order dynamics. This work enables dynamic loco-manipulation near the physical limits of small quadruped platforms, laying a model-based foundation for robust control of armed quadruped robots.