We study supported reaching to extend a humanoid robot's workspace by bracing against the environment for stabilization using a straightforward yet capable whole-body model predictive control (MPC) approach, exclusively with local sensing and fiducial markers. While typical solutions for whole body humanoid control aim at avoiding or accommodating contact, contact is explicitly required to reach objects placed on surfaces far out of normal reach. We employ sampling-based MPC for whole-body leaning, bracing, reaching, and recovery on a Unitree H1-2 humanoid. The controller runs in real-time at 34 Hz on a laptop, transfers sim-to-real, extends reach by about 50 cm beyond the unbraced standing workspace, and sustains the loaded posture for over four minutes, before pose estimation drifts. The controller reaches the commanded target in 89% of trials and completes the full reach-and-recovery cycle in 81%, across 54 trials spanning six targets and three table heights. We further characterize why such a small real-time sampling budget is sufficient, finding that temporal persistence of sampled actions and exploration scale strongly govern success, while CEM provides the widest operating region over planning rate and model mismatch. These results establish a real-world baseline for online, contact-rich humanoid manipulation. Code, videos, and full hardware details are available at https://h12leaner.github.io/
Figures & tables
Fig. 1 : Three stages of leaning and reaching Unitree H1-2 in real (top) and simulation (MuJoCo, bottom). With sampling-based model predictive control, the robot can hold the 1.3m reaching stage (frame 3 ), 50cm past the unbraced standing reach of ≈80 cm for over four minutes with EE accuracy of 2.7±0.9 cm.
Planner
Center
Spread
Fold
N
Predictive sampling (PS) [ 6 ]
θ
σ2I
θ(k⋆)
20
Model predictive path integral (MPPI) [ 20 ]
θ
σ2I
∑kwkθ(k)
20
Cross-entropy method (CEM) [ 21 ]
μE
ΣE
μE
20
Improved CEM (iCEM) † [ 22 ]
μE
ΣE , AR(1)
μE
20
Table I. How planners instantiate Alg. III-A . E is the elite set; k⋆=argminkJ(k) ; wk∝e−J(k)/λ . All ablations use N=20 rollouts.
Group
Representative residual
Brace
min(dˉ,[diseat−dsat]+),i∈B
Keep-out
[dsafe−dikeep]+,i∈/B
Trunk clear
[dsafe−dtrunk]+
Reach
phand−p⋆
Balance
∣ccp−ΠS(ccp)∣
Feet & stance
foot rise/tilt, capture, knees, hip posture
TABLE II : Sampling-controller cost terms, grouped by role. The deployed task defines 39 residuals, grouped here into nine functional families. [x]+=max(0,x) ; diseat and dikeep are clearances to the usable and physical table surfaces; B is the selected brace set.
Fig. 2 : Estimator drift during an active stand. While the robot actively holds a stand, Z-axis drift is 0.3mm/min, X-axis drift of the Internal + AprilTag Fiducial estimator is 4 mm oscillation.
Fig. 3 : Estimated balance states during standing. Base-attitude jitter, height drift, inter-foot wander, and reported velocity noise are equivalent across all three estimators.
Fig. 4 : Reach, stability, hand jitter, and brace loading in successful runs. Each panel reports two tasks: reaching a commanded target (left pair) and reaching to maximum extent (right pair), in simulation (orange) and on hardware (green). Bars show medians over successful trials; whiskers show the half-ranges. Reach is the gripper-tip distance from the ankle midpoint. Support margin is the minimum margin to the active support-region boundary; hand jitter is high-pass RMS motion above 1 Hz; and bracing-arm force is the measured normal load.
Fig. 5 : CoM support margin and reconstructed brace-contact force over hardware trials. Time is normalized to the commanded phase boundaries for each trial. Top: minimum distance from the horizontal CoM projection to the foot-only support polygon ; negative values lie outside the foot support region. Bottom: normal contact force estimated quasi-statically from measured bracing-arm joint torques through the contact Jacobian transpose. The reconstruction gives 4.3 N RMSE against “ground-truth” force in simulation.
Fig. 6 : Gripper tip tracking. Wrist-camera measured tip error relative to the aim point through the 5s hold, mean ± 1std over 48 hardware holds across six targets and three table heights. Depth carries a small bias as the hand drives in along the approach ( −1.4±1.3 cm), while the grasp-critical lateral and height axes stay well centered at +0.2±1.3 and +1.1±1.3 cm within the jaw aperture.
Fig. 7 : Commanded reaching across table heights and targets. The same controller is evaluated at three table heights and six target positions, with three trials per condition. Dots show wrist-camera measured hold positions in the table plane; crosses denote commanded positions, dotted circles show a 4cm radius, and × marks conditions where the brace failed before reaching. All targets are completed at the lower and nominal table heights; at the tallest table, the two deeper lateral targets exceed the current bracing workspace.
Fig. 8 : Brace-cost ablation. Brace-hold success rate versus the number of active brace cost terms (Wilson 95% intervals; per-point trials n annotated below).
Fig. 9 : Sampling-based planner ablation: sensitivity to configuration and to model error. Hold spline, σ=0.01 rad, N=20 , k=6 and λ=1 unless varied.
Contact-rich robotic manipulation studies how robots plan and control physical interactions such as grasping, insertion, pushing, and dexterous object handling.
Push recovery during locomotion will facilitate the deployment of humanoid robots in human-centered environments. In this paper, we present a unified framework for walking control and push recovery for humanoid robots, leveraging the arms for push recovery while dynamically walking. The key innovation is to use the environment, such as walls, to facilitate push recovery by combining Single Rigid Body model predictive control (SRB-MPC) with Hybrid Linear Inverted Pendulum (HLIP) dynamics to enable robust locomotion, push detection, and recovery by utilizing the robot's arms to brace against such walls and dynamically adjusting the desired contact forces and stepping patterns. Extensive simulation results on a humanoid robot demonstrate improved perturbation rejection and tracking performance compared to HLIP alone, with the robot able to recover from pushes up to 100N for 0.2s while walking at commanded speeds up to 0.5m/s. Robustness is further validated in scenarios with angled walls and multi-directional pushes.
Lizhi Yang, Blake Werner, Adrian B. Ghansah +1
AMBER Lab at the Department of Mechanical and Civil Engineering, California Institute of Technology, Pasadena, USA
Virtual Model Control (VMC) is an approach to design a controller for force-controlled robots in complex uncertain environments. While this method was primarily investigated for legged robot locomotion in the past, it can be more generally applicable to other types of robotic systems. This paper investigates the VMC framework for reaching tasks in a force-controlled robotic arm. We propose six different approaches to designing virtual models in order to achieve reaching tasks in environments with obstacles and uncertainties. A force-controlled 8 degree-of-freedom humanoid robot was used to validate the proposed approach in the real world. We conducted three experiments to test the performance of VMC controllers in terms of predictability, sensitivity to external force, and adaptability against known and unknown obstacles. Experimental analyses show that, even though the proposed approach needs to sacrifice accuracy and trajectory optimality, it enables us to design complex reaching motions under uncertainties, in an intuitive and extendable manner.
Yi Zhang, Daniel Larby, Fumiya Iida +1
Department of Engineering, University of Cambridge, UK.
Humanoid locomotion requires deciding where to step and how to distribute contact forces. However, a disturbance can invalidate the nominal plan, requiring coordinated revision under constraints that switch with every footfall. To solve this online, the proposed approach employs the single-rigid-body model, which retains attitude dynamics and the full contact wrench. An iterative linear-quadratic-regulator MPC (iLQR-MPC) then jointly optimizes contact forces and footstep locations in a single receding-horizon problem, without a separate step planner. Specifically, contact and motion constraints are incorporated into the backward pass through augmented-Lagrangian and relaxed-barrier penalties. A whole-body controller subsequently maps the plan to joint torques at 500 Hz while preserving contact feasibility. The model-based formulation makes disturbance responses interpretable through explicit costs and constraints, and requires no platform-specific policy training. In MuJoCo simulations, the iLQR-MPC replans at 100 Hz (mean solve time 8.9 ms); the Unitree G1 sustains steady-state walking and recovers from impulsive 245 N forward and 175 N lateral pushes.
Xiang Li, Linlin Ou, Ruihan Wu +1
Zhejiang University of Technology, Hangzhou, China