Inertially Significant Humanoid Teleoperation via Whole Body Model Predictive Control and Online Parameter Estimation

James Foster; Beomyeong Park; Luigi Penco; Robert Griffin

cs.ROOct 15, 2026

Inertially Significant Humanoid Teleoperation via Whole Body Model Predictive Control and Online Parameter Estimation

Authors: James Foster, Beomyeong Park, Luigi Penco, Robert Griffin

Organizations: Robotics Lab, Florida Institute for Human & Machine Cognition, USA · Intelligent Systems & Robotics, University of West Florida, USA

Abstract

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

Panel of Figure 1 — 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 $\mathbf{r}$	Type	Activation	Standard reference
Upper body jointspace tracking	$\mathbf{K}_{d}(\mathbf{v}^{\text{upper}}-\mathbf{v}^{\text{upper}}_{\text{des}})+\mathbf{K}_{p}(\mathbf{q}^{\text{upper}}-\mathbf{q}^{\text{upper}}_{\text{des}})$	$2^{\text{nd}}$ order	Weighted Quad.	Home config., zero velocity
Center of mass tracking	$\mathbf{K}_{d}(\dot{\mathbf{c}}-\dot{\mathbf{c}}_{\text{des}})+\mathbf{K}_{p}(\mathbf{c}-\mathbf{c}_{\text{des}})$	$2^{\text{nd}}$ order	Weighted Quad.	$(0,0,\mathbf{c}_{\text{home}}^{z})$
Pelvis orientation	$\mathbf{K}_{d}(\boldsymbol{\omega}-\boldsymbol{\omega}_{\text{des}})+\mathbf{K}_{p}\log(\mathbf{R}_{\text{des}}^{\top}\mathbf{R})$	$2^{\text{nd}}$ order	Weighted Quad.	$(0,0,0,1)$ (quaternion)
Foot placement	$\mathbf{K}_{d}(\dot{\mathbf{p}}-\dot{\mathbf{p}}_{\text{des}})+\mathbf{K}_{p}(\mathbf{p}-\mathbf{p}_{\text{des}})$	$2^{\text{nd}}$ order	Weighted Quad.	Home config. equivalent
Angular momentum	$(\dot{\mathbf{l}}-\dot{\mathbf{l}}_{\text{des}})+\boldsymbol{\alpha}(\mathbf{l}-\mathbf{l}_{\text{des}})$	$1^{\text{st}}$ order	Weighted Quad.	$\mathbf{0}$
Lower body state regularization	$\mathbf{x}^{\text{legs}}\ominus\mathbf{x}^{\text{legs}}_{\text{des}}$	Standard	Weighted Quad.	Home config., zero velocity

TABLE I : Cost terms comprising the WB-MPC. Without input, desired quantities

*_{\text{des}}

are the “Standard reference” column.

WB-MPC cost term	VR operator input
Arm jointspace $\mathbf{q}^{\text{arms}}_{\text{user}}$	Handheld controllers
Spine jointspace $\mathbf{q}^{\text{spine}}_{\text{user}}$	Right thumbstick, horizontal
Pelvis pitch $\mathbf{R}^{\text{pitch}}_{\text{user}}$	Right thumbstick, vertical
Center of mass height $\mathbf{c}^{\text{z}}_{\text{user}}$	Left thumbstick, vertical

TABLE II : VR inputs for streaming to WB-MPC.

Parameter	Value
Process $\mathbf{Q}$ : $\alpha$	$10^{-3}$
Process $\mathbf{Q}$ : $d$ , $s$ , $t$	$10^{-2}$
Prior $\mathbf{R}^{\text{reg}}$ : $d$ , $s$	$10^{-2}$
Prior $\mathbf{R}^{\text{reg}}$ : $\alpha$ , $t$	—
Measurement $\mathbf{R}$ : floating base, spine	$10^{3}$
Measurement $\mathbf{R}$ : arms, legs	$10^{2}$

TABLE III : EKF configuration used for experiments. The mass (

\alpha

) 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.

Prior regime	[deg]	$\alpha$	$d$	$s$	$t$
	Elbow RMSE	RMS deviation from nominal
No prior	1.27	1.11	0.11	0.09	0.28
Inertia-only	1.27	1.11	$4{\times}10^{-5}$	$1{\times}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:

|\alpha|

||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$	$\phantom{-}0.862$	$\phantom{-}0.859$	$\phantom{-}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.

Condition	$y$	$z$	$y$	$z$
	RMSE [mm]		Bias [mm]
Unloaded	14.2	13.2	$-11.5$	$\phantom{-}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 Robot Control

Humanoid robot control develops methods for coordinating bipedal balance, locomotion, and whole-body interaction with people and objects.

See all 201 papers

Adaptive Whole-Body MPC with Optimal Posture Planning for Humanoid Robots under Unknown Payloads
Oct 9, 2026Manh Cuong Nguyen, Duc Cuong Vu, Viet Anh Vu +5Humanoid Robot Control Adaptive Control
VinRobotics, Vingroup, Hanoi 10000, Vietnam · Center for AI Research (CAIR), VinUniversity, Hanoi 10000, Vietnam
Contact-Consistent Interaction Dynamics Normalization for Predictive Physical Human--Robot Interaction
Jun 12, 2026Yongyan CaoRobotic Control Robust Control
Voryx Robotics, San Jose, CA 95136, USA
RGB: RL Guided Whole-Body MPPI for Humanoid Control
Jun 23, 2026Yunsoo Seo, Sol Choi, Euncheol Im +2Robot Trajectory Tracking Humanoid Robot Control
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

Oct 9, 2026J/K move · Enter open · S save

Adaptive Whole-Body MPC with Optimal Posture Planning for Humanoid Robots under Unknown Payloads

Manh Cuong Nguyen, Duc Cuong Vu, Viet Anh Vu, Duc Tuan Pham+4

VinRobotics, Vingroup, Hanoi 10000, Vietnam · Center for AI Research (CAIR), VinUniversity, Hanoi 10000, Vietnam

Humanoid Robot Control Adaptive Control Whole-Body Robotic Manipulation Robust Control Model Predictive Control

Open paper AskPDF arXiv

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.

Abstract

Humanoid Robot Control

Explore similar work

Adaptive Whole-Body MPC with Optimal Posture Planning for Humanoid Robots under Unknown Payloads

Contact-Consistent Interaction Dynamics Normalization for Predictive Physical Human--Robot Interaction

RGB: RL Guided Whole-Body MPPI for Humanoid Control