CRISP: Constraint-Resolved Interactions between Subsystems for Predictive Whole-Body Cooperation

Dario Valenzuela, Jeeseop Kim

November 2026

TL;DR Model a humanoid holding a box with both hands as a torso and two arms tied together by holonomic constraints, so one NMPC optimizes arm torques, foot forces and the interaction wrenches at shoulders and hands. It runs online on a Unitree G1 (8 ms average solve) and keeps the robot balanced while the held box is pushed.

What they do

When a humanoid grips one object with both hands, the arms and the object form a closed kinematic chain. The hand forces then include an internal squeeze that moves nothing, and they feed back into balance. CRISP treats the robot as separate subsystems joined by constraints. The forces those constraints carry become decision variables of one controller, and their optimal values double as estimates of the interaction wrenches.

How it works

Decomposition. The torso is a single rigid body. Each 7-DoF arm is a floating-base manipulator. Holonomic constraints fix each arm base to the shoulder and tie the two end-effectors through the box.
Wrenches as multipliers. The Lagrange multipliers of these constraints are the shoulder and hand interaction wrenches. The NMPC optimizes them together with the states, arm torques and ground reaction forces, with slack variables on the constraints.
Layers. Optimal arm torques go straight to the arms. A quasi-static lower-body controller turns the prescribed ground reaction forces into leg joint torques.

Results

On a 29-DoF Unitree G1, the NMPC (460 variables, 60 ms horizon, IPOPT) solves in 8 ms on average, and 99% of solves take under 14 ms.
In MuJoCo, the optimized hand and shoulder wrenches track the measured contact wrenches during grasp and lift, including payload steps from 0.1 to 0.5 kg.
On hardware, end-effector tracking errors stay bounded until the grasp, then grow as the hands squeeze the box. Under vertical pushes on the box, torso pitch does not diverge and the center of pressure stays inside the support polygon.

Limitations

The model assumes a rigid, non-slipping grasp and a quasi-static lower body, and the robot does not walk while manipulating. Hardware results are shown as time-series plots, with no comparison against another controller. The authors list slip-aware contact models and manipulation during locomotion as future work.

Figures

From Figure 1: The humanoid split into subsystems: rigid arm-torso connections at the shoulders, a rigid torso-lower-... — From Figure 1: The humanoid split into subsystems: rigid arm-torso connections at the shoulders, a rigid torso-lower-body connection, and a closed-chain constraint through the box held by both hands.

From Figure 2: Control architecture. An NMPC over the torso and arm dynamics with interaction wrenches sends arm torq... — From Figure 2: Control architecture. An NMPC over the torso and arm dynamics with interaction wrenches sends arm torques to the robot and ground reaction forces to a lower-body controller for whole-body coordination.

From Figure 8: Hardware tests with pushes on the held box along x, y and z, and the optimal end-effector and shoulder... — From Figure 8: Hardware tests with pushes on the held box along x, y and z, and the optimal end-effector and shoulder wrenches while each push is applied (red shading).

Abstract

This paper presents a layered control framework that recasts high-dimensional system dynamics as a composition of dynamically interconnected subsystems, with dynamic dual-arm manipulation on the humanoid serving as a representative application. The humanoid is decomposed into a single rigid body (SRB) representing the torso and floating-base dynamic models representing the articulated arms and lower-body. The rigid connections between arm and torso and the closed kinematic chain formed between the end-effectors during cooperative manipulation are represented as holonomic constraints. The corresponding Lagrange multipliers parametrize the interaction wrenches transmitted at the shoulders and between the end-effectors. The high-level nonlinear model predictive control (NMPC) jointly optimizes the subsystem state trajectories, arm torques, ground reaction forces (GRFs), and interaction wrenches, enabling online wrench design and optimization while preserving the dynamic coupling among the independently modeled subsystems. The optimized arm torques are applied directly for cooperative dual-arm manipulation, whereas the prescribed GRFs are mapped to lower-body joint torque for whole-body coordination. We validate the proposed framework through numerical simulations and hardware experiments on a G1 humanoid robot. The results demonstrate optimal interaction wrenches consistent with simulated contact measurement, cooperative dual-arm trajectory tracking, and stable whole-body coordination while executing the NMPC in online manner.

Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.

Matt Strong