The effective deployment of humanoid robots in practical tasks of interest will require a capability of sensing and adapting to variations in inertial parameters, as occurs during heavy tool use or when carrying significant payload. Such payloads constitute a significant proportion of the combined payload-robot system, the effects of which cannot be ignored by delegating to feedback control. This paper showcases a general solution to this problem by integrating a whole body model predictive controller (WB-MPC) with an extended Kalman filter (EKF) for online estimation of fully physically consistent time-varying inertial parameters. These estimates are fed through to the WB-MPC's full rigid body dynamics model, resulting in more accurate feedforward torques for task demands under external load. Additionally, a virtual reality (VR) interface is provided that enables an operator to provide task-specific references without regard to payload while delegating dynamic balance to the WB-MPC. The framework is validated with simulation and hardware experiments on a humanoid system.
Figures & tables
Fig. 1 : Humanoid teleoperation tasks with a 4.5 kg ( 10 lb) payload fixed to the right forearm link.
Fig. 2 : Block diagram of the framework, consisting of the physically consistent inertial EKF (Sec. III-A ) coupled to the WB-MPC (Sec. III-B ) and the VR operator interface (Sec. III-C ). The VR operator interface runs on a separate machine.
Cost
Residual r
Type
Activation
Standard reference
Upper body jointspace tracking
Kd(vupper−vdesupper)+Kp(qupper−qdesupper)
2nd order
Weighted Quad.
Home config., zero velocity
Center of mass tracking
Kd(c˙−c˙des)+Kp(c−cdes)
2nd order
Weighted Quad.
(0,0,chomez)
Pelvis orientation
Kd(ω−ωdes)+Kplog(Rdes⊤R)
2nd order
Weighted Quad.
(0,0,0,1) (quaternion)
Foot placement
Kd(p˙−p˙des)+Kp(p−pdes)
2nd order
Weighted Quad.
Home config. equivalent
Angular momentum
(l˙−l˙des)+α(l−ldes)
1st order
Weighted Quad.
0
Lower body state regularization
xlegs⊖xdeslegs
Standard
Weighted Quad.
Home config., zero velocity
TABLE I : Cost terms comprising the WB-MPC. Without input, desired quantities ∗des are the “Standard reference” column.
WB-MPC cost term
VR operator input
Arm jointspace quserarms
Handheld controllers
Spine jointspace quserspine
Right thumbstick, horizontal
Pelvis pitch Ruserpitch
Right thumbstick, vertical
Center of mass height cuserz
Left thumbstick, vertical
TABLE II : VR inputs for streaming to WB-MPC.
Parameter
Value
Process Q : α
10−3
Process Q : d , s , t
10−2
Prior Rreg : d , s
10−2
Prior Rreg : α , t
—
Measurement R : floating base, spine
103
Measurement R : arms, legs
102
TABLE III : EKF configuration used for experiments. The mass ( α ) and center of mass ( t ) channels are unregularized (—); only the rotational inertia channels ( d , s ) carry a prior.
Fig. 3 : Forearm taskspace paths (chest frame, frontal y – z plane) for pre-recorded trajectory.
Fig. 4 : Estimated forearm mass and CoM vs. ground truth.
Condition
RMSE y [mm]
RMSE z [mm]
No estimator
67.8
389.2
With estimator
6.4
14.2
TABLE IV : Taskspace error statistics (frontal y – z plane) for pre-recorded trajectory, loaded vs. unloaded baseline.
Elbow RMSE
RMS deviation from nominal
Prior regime
[deg]
α
d
s
t
No prior
1.27
1.11
0.11
0.09
0.28
Inertia-only
1.27
1.11
4×10−5
1×10−5
0.28
TABLE V : Effects of parameter prior. Each parameter group entry is the RMS of the Euclidean norm of that group’s deviation from nominal: ∣α∣ , ∣∣d∣∣2 , ∣∣s∣∣2 , ∣∣t∣∣2 .
Fig. 5 : Overhead ( x – y plane) view of robot balance quantities during static hold.
Fig. 6 : Mass estimate, end-effector taskspace height, and whole-robot electrical power draw for the incremental hardware loading of the right forearm held horizontally in front of the robot. The shaded interval marks a state estimator artifact.
Dimension
Mean [m]
Min [m]
Max [m]
Spread [m]
x
−0.042
−0.059
−0.032
0.027
y
−0.023
−0.045
−0.013
0.032
z
−0.862
−0.859
−0.865
0.007
TABLE VI : Robot CoM in the z -up mid-foot frame over the loading run of Fig. 6 .
Fig. 7 : Top: Robot CoM height and pelvis pitch tracking through the squat/stand. Bottom: estimated forearm mass across the loading interval.
Fig. 8 : Taskspace paths (chest frame, frontal y – z plane) for unloaded vs. loaded right forearm.
RMSE [mm]
Bias [mm]
Condition
y
z
y
z
Unloaded
14.2
13.2
−11.5
−0.8
Loaded (4.5 kg)
19.3
22.4
−15.6
−15.5
TABLE VII : Right forearm taskspace error statistics (chest frame, frontal y – z plane), loaded vs. unloaded baseline.
Humanoid robots performing lifting and material-handling tasks must maintain balance despite unknown and varying payloads. However, payload uncertainty significantly affects whole-body dynamics and may destabilize conventional controllers. We propose an adaptive whole-body control architecture that integrates three components: (i) a centroidal model predictive controller (MPC) that generates dynamically feasible reference motions, (ii) an adaptive fuzzy balance controller that compensates for payload-induced model mismatch by generating corrective torques distributed across the supporting-leg joints, and (iii) a waypoint-based optimal posture planner built on a triple-integrator model that produces smooth sit-to-stand and stand-to-sit trajectories satisfying center-of-mass (CoM) stability constraints, joint limits, and whole-body feasibility requirements. Together, these components enable stable lowering, grasping, lifting, carrying, and placing behaviors under significant payload uncertainty. Extensive simulation and hardware experiments demonstrate that the proposed framework improves balance robustness and reduces tracking degradation during posture transitions and lifting tasks compared to non-adaptive baselines.
Manh Cuong Nguyen, Duc Cuong Vu, Viet Anh Vu +5
VinRobotics, Vingroup, Hanoi 10000, Vietnam · Center for AI Research (CAIR), VinUniversity, Hanoi 10000, Vietnam
Safe physical human--robot interaction on floating-base robots requires interaction regulation under changing contact constraints. We develop a contact-consistent normalization in which the residual end-effector channel is represented as a linear double integrator in acceleration coordinates. Both discrete prediction matrices are independent of configuration and support mode; posture and contact enter only through task-inertia force recovery and constraints. The controller combines a constant-Hessian receding-horizon QP, an acceleration-disturbance observer, and a priority-consistent realization. Classical operational-space impedance is shown to be the unconstrained infinite-horizon limit. MuJoCo experiments on a 17-DOF biped and a Menagerie-derived Unitree G1 model evaluate sustained forces, transmitted shocks, and scheduled contact-model changes. The observer sustains near-offset-free tracking across a genuine contact-set switch and a scheduled support-mode transition, while disturbance estimation---not contact consistency alone---is the dominant source of fixed-stance accuracy; covariance inflation gives only scenario-dependent transient benefit.
Humanoid robots require whole-body controllers that are both robust and precise in contact-rich environments. While deep reinforcement learning (RL) achieves robust stability, its behavior is tightly coupled to the training objective and command interface, making it difficult to add new feedback objectives without retraining. In this study, we propose an RL guided whole-body model predictive path integral (MPPI) framework that acts as an add-on feedback controller on top of a pretrained RL policy. Instead of using RL policy as the final controller, we use it as a sampling prior that biases MPPI rollouts toward dynamically feasible behaviors. Task objectives are specified through modular MPPI cost terms, and MPPI closes the loop by continuously correcting the RL prior online to satisfy these objectives without retraining the policy. Simulations on a 29-DoF Unitree G1 humanoid in MuJoCo demonstrate stable high-rate control (average 280~Hz). The proposed method improves task-level precision over a pure RL baseline under the same command interface. This is achieved by correcting systematic drift during straight walking and tracking additional whole-body reference signals imposed through the cost.
Yunsoo Seo, Sol Choi, Euncheol Im +2
Center for Humanoid Research, Korea Institute of Science and Technology (KIST), 02792 Seoul, South Korea · University of Texas at Austin, Austin, Texas 78712, USA · Department of Mechanical Engineering, Yonsei University, 03722, Seoul, South Korea +1