A new three-level control architecture gives floating-base robots—humanoids and quadrupeds—the ability to reject physical human pushes and sustained contact forces with zero steady-state error, without sacrificing balance. By combining centroidal model predictive control (MPC) for locomotion with a separate impedance MPC that predicts and cancels human interaction forces at 1 kHz, the system occupies a structural niche that prior work has left unfilled.
What the Researchers Built
Yongyan Cao developed a control stack that runs three nested loops on a floating-base robot (humanoid or quadruped). The outermost loop—centroidal MPC—runs at 40–100 Hz and plans the center-of-mass trajectory and ground reaction forces over a 500-millisecond horizon. The middle loop—a whole-body control (WBC) hierarchy—runs at 500 Hz and resolves those ground forces into joint torques for contact maintenance and balance. Crucially, the WBC hierarchy leaves the arm end-effector task slot open.
That open slot is filled by the innermost loop: an impedance MPC that runs at approximately 1 kHz. This layer predicts and rejects physical human-robot interaction (pHRI) forces using a contact-consistent dynamic model of the arm’s residual dynamics. Instead of a standard PD impedance law, the impedance MPC uses a receding-horizon quadratic program (QP) that considers both the predicted disturbance and the robot’s torque limits. A Kalman filter estimates the external force disturbance, and a contact-mode switch protocol ensures the filter re-converges quickly when the robot transitions between stance phases (e.g., double support to single support).
Key Results
The paper reports simulation experiments on two platforms: a generic humanoid model and the official Unitree G1 MJCF model (29 DOF, 33.3 kg). Three main scenarios are tested:
- Scenario A (sustained push): An 8 N constant force is applied to the arm end-effector while the robot stands in double support. The impedance MPC achieves zero steady-state tracking error, whereas a conventional PD impedance law drifts by 2–3 cm and cannot fully reject the disturbance.
- Scenario B (push + periodic spikes): An 8 N sustained force is combined with 6 N spikes at 1 Hz (0.1 s duration). The impedance MPC keeps end-effector position error below 0.5 cm during spikes and returns to zero afterward; the PD law suffers peak errors exceeding 4 cm.
- Scenario C (Unitree G1 real model): The same Scenario A protocol run on the official G1 model confirms the architecture’s transferability, with comparable zero steady-state error and 80% reduction in peak transient error compared to PD.
A convergence bound across contact transitions shows that the Kalman filter re-converges within approximately 10 samples (10 ms at 1 kHz) after covariance inflation, ensuring that force estimation remains accurate even when feet lift or land.
How It Works
The robot’s state is described by generalized coordinates: a 6-DOF floating base plus joint angles. The control hierarchy leverages the contact-consistent mass inverse, which projects task forces into the null space of rigid contact constraints. This ensures that commands generated by the impedance MPC do not violate kinematic contact constraints or interfere with balance.
Architecture breakdown:
| Level | Component | Rate | Objective |
|---|---|---|---|
| 1 | Centroidal MPC | 40–100 Hz | Plan CoM trajectory and ground reaction forces (500 ms horizon) using the single-rigid-body dynamics (SRBD) model |
| 2 | WBC Hierarchy | 500 Hz | Resolve Level 1 forces into joint torques for contact tasks and balance, leaving the arm end-effector task slot open |
| 3 | Impedance MPC | ~1 kHz | Fill the arm slot with a receding-horizon QP that predicts and rejects pHRI disturbances |
The impedance MPC uses a residual plant model: after Level 2 commits balance torques, the remaining arm end-effector dynamics are linearized and augmented with an external force disturbance state. A Kalman filter estimates the disturbance online. At each time step, the MPC solves a QP that minimizes tracking error and torque effort while respecting joint torque limits. The disturbance estimate is fed into the model to cancel the human force.
For contact transitions (e.g., lifting a foot), a protocol inflates the Kalman filter’s covariance and holds the current disturbance estimate constant until the filter re-converges (about 10 samples). This avoids large transient spikes when the contact set changes.
Why This Matters for Robotics
Floating-base robots—humanoids and quadrupeds—are increasingly moving from research labs into warehouses, factories, and homes. They need to work safely alongside people, not just walk and balance. A human might accidentally bump an arm, push a shoulder, or grab a limb during collaborative tasks. Prior control stacks optimized for locomotion treat the arms as passive masses or use high-gain position control that resists interaction forces, making the robot stiff and dangerous.
This architecture directly addresses that gap. By reserving a computational “slot” for impedance MPC, the robot can actively comply with human forces while still maintaining balance—and even reject steady pushes to stay on task. The 1 kHz update rate matches the speed of typical force-torque sensor loops, so the system is ready for real-world deployment on platforms like the Unitree G1 or R1.
For warehouse and manufacturing applications, this means humanoid robots can assist with part handling, assembly, or tool operation without requiring safety cages. For browse humanoid robots on Robot Overflow that support torque-controlled arms, this control scheme could be integrated to enable safe physical collaboration.
Limitations and Open Questions
The results are currently validated only in simulation (on generic humanoid and Unitree G1 models). Hardware validation on a real robot is listed as future work. The architecture assumes the disturbance force is applied only to the arm end-effector; it does not yet handle multiple simultaneous contact points on the torso or legs. Additionally, the contact-mode switch protocol currently handles only transitions between a fixed set of predefined contact states; dynamic walking with variable gait sequences would require extending the centroidal MPC to generate foothold locations online. Finally, the Kalman filter relies on accurate joint torque measurements, which may be noisy on low-cost servos.
Frequently Asked Questions
What problem does whole-body impedance MPC solve? It solves the problem of a floating-base robot maintaining both balance and accurate arm positioning when a human applies sustained or impulsive forces to the arm.
How does this differ from standard impedance control? Standard PD impedance control cannot achieve zero steady-state error under a constant disturbance; the impedance MPC uses a predictive model and disturbance estimation to cancel the human force completely.
Does this work for quadrupeds with arms as well as humanoids? Yes—the control framework is contact-consistent and works for any floating-base robot with actuated joints and an arm end-effector, including legged manipulators.
What sensors are required? The system needs joint encoders, torque sensors (or torque estimates from current sensing), and optionally a force-torque sensor at the wrist for improved Kalman convergence. No external motion capture is required.
Conclusion
The three-level architecture proposed by Yongyan Cao fills a critical gap in floating-base robot control: it adds a fast, predictive compliance layer for physical human interaction without compromising balance. Simulation results on a generic humanoid and the Unitree G1 show zero steady-state error and significantly lower peak errors compared to conventional PD impedance. The path to hardware deployment is clear, and the approach could make humanoid robots safer and more practical in human-inhabited environments.