PaperlayerView exact revision on arXiv

Source-linked AI summary

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

James Foster, Beomyeong Park, Luigi Penco, Robert Griffin

arXiv:2610.22176v1cs.RO

TL;DR

Humanoid teleoperation under substantial payloads requires adapting to inertial changes that cannot be left to feedback control alone. The paper combines online physically consistent inertial estimation with full-body WB-MPC and VR task references. Simulation and hardware experiments validate the framework, including teleoperation while handling an asymmetric load.

  • Problem

    Humanoid tasks can involve payloads whose inertia is a significant fraction of the robot’s, while VR operators typically lack force feedback indicating the load.

    Source: §I · p. 1

  • Method

    The framework couples an EKF that estimates physically consistent inertial parameters online to full-rigid-body WB-MPC, with a VR interface supplying task references.

    Source: §I · p. 2

  • Results

    Simulation showed online inertial estimation was necessary for WB-MPC trajectory tracking with significant added payload, and hardware testing demonstrated VR teleoperation while manipulating a large asymmetric load.

    Source: B. Hardware · p. 6, §V · p. 7

  • Takeaways & Limitations

    The framework was demonstrated for practical VR teleoperation of unknown payloads using WB-MPC and physically consistent inertial filtering on a humanoid robot.

    Source: §V · p. 7

Abstract

from arXiv · show

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.

I. INTRODUCTION

Humanoid manipulation often involves payloads whose inertia is significant relative to the robot, while existing WB-MPC and online identification work has addressed this setting only sparsely. The paper couples physically consistent online inertial estimation, full-body WB-MPC, and a VR interface for teleoperation under load.

I. INTRODUCTION

  • Payload inertia can be a significant fraction of the robot’s own, while prior WB-MPC manipulation work rarely addresses objects with inertia significant relative to the robot.The closest cited work remains in simulation and estimates a modest payload only after task execution.Source: §I · p. 1
  • A full rigid-body WB-MPC can command feedforward effort for added payload only if its dynamics model accurately captures the manipulated inertia.Prior online control-oriented identification often estimates gross quantities or payloads from previously identified dynamics or a momentum observer; downstream estimates must remain physically consistent.Source: §I · p. 1
  • The proposed framework couples online physically consistent inertial EKF estimates to WB-MPC dynamics and provides VR task references while delegating dynamic balance to the controller.The EKF regularizes weakly excited estimates around a nominal body and uses a generalized log-Cholesky parameterization; the framework is validated in simulation and on a 23-degree-of-freedom humanoid in hardware.Source: §I · p. 2

II. BACKGROUND

The WB-MPC and inertial EKF use a floating-base rigid-body model of the robot. Its equations relate system dynamics to joint torques and external contact wrenches.

II. BACKGROUND

  • The model assumes the robot can be represented by the equations of motion of a floating-base rigid-body system.The same assumption underlies both the WB-MPC and inertial-parameter EKF.Source: §II · p. 2
  • The equations relate configuration and velocity, joint torques, and contact Jacobians and wrenches through the mass matrix and the centripetal, Coriolis, and gravitational terms.The selection matrix identifies the actuated degrees of freedom, and the contact terms sum over external contacts.Source: §II · p. 2

A. Regressor Formulation of Inverse Dynamics

The inverse-dynamics formulation is linear in rigid-body inertial parameters, enabling their estimation through a state-dependent regressor. A log-Cholesky parameterization represents those parameters while enforcing physical consistency.

A. Regressor Formulation of Inverse Dynamics

  • Inverse dynamics can be written linearly in the inertial parameters, with a state-dependent regressor mapping stacked body parameters into torque.The stacked vector contains N bodies’ parameter vectors; each body’s ten parameters comprise mass, first moment of mass, and the unique elements of its symmetric rotational inertia matrix.Source: A. Regressor Formulation of Inverse… · p. 2
  • Positive-definite pseudo-inertia is equivalent to positive mass, positive-definite rotational inertia, and the triangle inequality on principal moments.These physical-consistency conditions are the reason the parameterization is suitable for estimates passed to the WB-MPC.Source: A. Regressor Formulation of Inverse… · pp. 2–3
  • The log-Cholesky parameterization describes each rigid body with ten parameters for density scaling, scaling, shearing, and translation.Its pseudo-inertia remains positive definite for any parameter vector, making every estimate physically consistent by construction.Source: A. Regressor Formulation of Inverse… · p. 3
  • The smooth, invertible map between log-Cholesky parameters and inertial parameters has a closed-form Jacobian.The paper describes deviations from a nominal body using density, scale, shear, and translation changes.Source: A. Regressor Formulation of Inverse… · p. 3

B. Whole Body Model Predictive Control

At each control instant, the WB-MPC solves a finite-horizon optimal control problem from the current estimated state. Its forward-dynamics model incorporates online inertial-parameter estimates.

B. Whole Body Model Predictive Control

  • The WB-MPC repeatedly solves a discrete-time optimal control problem from the current estimated state.The problem spans K stages and includes running and terminal costs and path constraints.Source: B. Whole Body Model Predictive Control · p. 3
  • The controller obtains its dynamics model by manipulating the equations of motion into forward dynamics.The forward dynamics are integrated to form the discrete state transition.Source: B. Whole Body Model Predictive Control · p. 3
  • Online inertial-parameter estimates enter the controller through the discrete dynamics transition.The transition depends on the mass matrix and bias terms, which depend on the inertial parameters.Source: B. Whole Body Model Predictive Control · p. 3

III. METHOD

The framework combines WB-MPC, online inertial-parameter estimation, and VR teleoperation to support humanoid manipulation of significant loads. The estimator updates the dynamics used by the controller, while the interface supplies task references.

III. METHOD

  • The framework couples an EKF that estimates physically consistent inertial parameters online with a WB-MPC that plans using dynamics updated by those estimates.The third component is a VR interface that supplies task references to the controller.Source: §III · p. 3
  • The WB-MPC plans against full rigid-body dynamics updated by the EKF.This is one of the three components identified in the framework.Source: §III · p. 3
  • A VR teleoperation interface supplies task references to the WB-MPC for humanoid manipulation of significant loads.The framework’s stated purpose is intuitive VR teleoperation of humanoids manipulating significant loads.Source: §III · p. 3

A. Extended Kalman Filter

The estimator adapts a physically consistent inertial EKF by separating parameters to estimate from nominal parameters. It derives torque measurements for an EKF update and uses a nonlinear parameterization to preserve physical consistency.

A. Extended Kalman Filter

  • The estimator assumes offline identification resolves most robot inertial parameters and uses task knowledge to identify which links vary.It splits the regressor into estimated and nominal contributions; the nominal contribution is obtained with an inverse-dynamics call with estimated parameters zeroed.Source: A. Extended Kalman Filter · p. 3
  • The measurement model isolates the estimated contribution as a residual torque using observed joint torques and measured contact wrenches.Source: A. Extended Kalman Filter · p. 3
  • A nonlinear parameterization of θ guarantees fully physically consistent estimates at every iteration, requiring an EKF rather than a regular Kalman filter.The formulation gives up the linear parameterization of π.Source: A. Extended Kalman Filter · p. 3
  • The measurement Jacobian is formed by applying the chain rule through the invertible mapping from θ_est to π_est.The process model is assumed constant and driven by noise.Source: A. Extended Kalman Filter · p. 3

2) Regularizing weakly-observable parameters:

Weakly observable inertial parameters are regularized toward nominal values, while response-based WB-MPC costs support stable behavior with uniform VR references. The VR interface streams operator references to the controller’s upper body.

2) Regularizing weakly-observable parameters:

  • A fictitious zero-deviation measurement regularizes only selected weakly observable parameter channels toward the nominal body.This is equivalent to a maximum a posteriori update with a Gaussian prior; under teleoperated motion, forearm mass and center of mass are well excited, but rotational inertia can drift if left free.Source: 2) Regularizing weakly-observable… · p. 4

B. Whole Body Model Predictive Control

  • The WB-MPC uses FDDP with forward dynamics and analytical derivatives at each stage.Its running cost is primarily a weighted sum of least-squares terms, with regularization on tracking error and response-based residuals.Source: B. Whole Body Model Predictive Control · p. 4
  • Response-based residuals decouple task priority from desired closed-loop bandwidth, easing tuning and producing more horizon-invariant behavior.These properties are useful for tracking VR references that are uniform over the prediction horizon.Source: B. Whole Body Model Predictive Control · p. 4
  • Standard regularization costs combined with response-based costs on VR input suffice to induce horizon-invariant, stable behavior without modifying the terminal cost.Source: B. Whole Body Model Predictive Control · p. 4

C. Virtual Reality Teleoperation

  • The VR operator’s tracked device poses are resolved into joint-space targets by an inverse-kinematics routine running on a separate computer.The WB-MPC consumes only the upper-body portion of this solution.Source: C. Virtual Reality Teleoperation · pp. 4–5
  • The interface specifies joystick bindings for streaming pelvis pitch, center-of-mass height, and spine yaw references to the WB-MPC.The operator commands the base pose of a bimanual manipulation system via joystick and then teleoperates the arms; Table II lists the streamed inputs.Source: C. Virtual Reality Teleoperation · p. 5, Table II · p. 5

D. Practical Implementation Details

The experiments couple online inertial estimates to WB-MPC dynamics and test tracking, balance, and runtime loading on a humanoid. Results include improved loaded-arm tracking, posture adjustment under load, and close mass-estimate tracking during incremental hardware loading.

D. Practical Implementation Details

  • WB-MPC, EKF, and state estimation ran onboard; state estimation and low-level control used a 1 kHz thread, while EKF and WB-MPC ran at 100 Hz.The VR toolbox ran on a separate desktop and streamed references over ROS2 at a best-effort 50 Hz; the low-level controller interpolated planned setpoints and supplemented feedforward torque with PD tracking.Source: D. Practical Implementation Details · p. 5

IV. RESULTS

1) Pre-recorded trajectory tracking with/without EKF:

  • Mass and center-of-mass deviations recovered the payload, while a fictitious inertia measurement kept weakly observable rotational-inertia channels near nominal.The mass and CoM channels were recovered regardless; without the fictitious measurement, rotational-inertia channels wandered. Table V reports parameter-group deviations from nominal.Source: 1) Pre-recorded trajectory tracking… · p. 5, Table V · p. 6

2) Posture adjustment under load with/without EKF:

  • With EKF disabled, a 10 kg two-forearm load drove the robot toward instability; with EKF enabled, WB-MPC moved the pelvis backward to keep CoM within the support polygon.The controller also maintained an upright pelvis; the counterbalance emerged from the WB-MPC costs.Source: 2) Posture adjustment under load… · pp. 5–6, Figure 5 · p. 6

B. Hardware

  • The hardware setup used adjustable dumbbells mounted at runtime and, because of recurring left-arm issues, loaded only the right arm to test asymmetric-load handling.The experiments used a 23-degree-of-freedom humanoid; its neck was PD-controlled outside the EKF/WB-MPC scheme.Source: §IV · p. 5, B. Hardware · p. 6

1) Pre-recorded trajectory tracking with/without EKF:

  • 389.2 mm vertical RMSE without adaptation was over 25× the adapted error; horizontal RMSE fell from 67.8 mm to 6.4 mm with EKF adaptation.Estimated mass and center of mass tracked the added payload within two seconds. The paths are shown in the forearm taskspace figure, and the error statistics table covers the frontal y–z plane.Source: 1) Pre-recorded trajectory tracking… · p. 5, Table IV · p. 6, Figure 3 · p. 6

1) Runtime loading:

  • 89%, 88%, 91%, 94%, and 96% of cumulative loads were recovered at 3.4, 4.5, 5.7, 6.8, and 7.9 kg, respectively, during right-forearm hardware loading.The estimate had a 0.6 kg positive bias relative to the CAD nominal; forearm height dropped 94 mm at full load while robot CoM stayed regulated within 32 mm horizontally and 7 mm vertically.Source: 1) Runtime loading · p. 6

2) Teleoperated pick up of asymmetric load:

The teleoperated pickup experiment tested a 4.5 kg forearm load during squat-and-stand motion and subsequent window-washing commands. Tracking remained generally good, while loaded-arm vertical error was modestly higher than unloaded error.

2) Teleoperated pick up of asymmetric load:

  • The operator first commanded a squat and forward pitch, then the robot returned to standing for operator-commanded circular window-washing motions with the load.Figure 8 shows chest-frame forearm paths in the frontal y–z plane.Source: 2) Teleoperated pick up of asymmetric… · p. 7, Figure 8 · p. 7
  • During the load interval, the estimate recovered 95% of the added 4.5 kg after initially undershooting due to manual handling.CoM-height and pelvis-pitch tracking were generally good, aside from a persistent 4 cm height offset hypothesized to reflect competing costs.Source: 2) Teleoperated pick up of asymmetric… · p. 7, Figure 7 · p. 7
  • The loaded forearm had 22.4 mm vertical RMSE versus 13.2 mm unloaded, while horizontal RMSE increased by 5 mm under the 4.5 kg payload.The loaded forearm sagged 15.5 mm vertically from its reference; the authors describe the vertical degradation as modest and attribute dominant tracking error under these teleoperation conditions to gravity.Source: 2) Teleoperated pick up of asymmetric… · p. 7

V. CONCLUSIONS AND FUTURE WORK

The framework was validated in simulation and hardware for teleoperation with unknown payloads. Future work targets reducing estimation transients, testing robustness in dynamic maneuvers with grasped objects, and integrating large behavior models.

V. CONCLUSIONS AND FUTURE WORK

  • Simulation showed online inertial estimation was necessary for trajectory tracking, while closed-loop EKF–WB-MPC coupling adjusted posture under load to maintain balance; hardware testing demonstrated manipulation of a large asymmetric load.The hardware operator test included squatting to pick up the load.Source: §V · p. 7
  • Future work will investigate robustness limits during more dynamic maneuvers with a grasped object.Source: §V · pp. 7–8
  • Object-inertial priors from perception are proposed as a way to reduce estimation transient response.Source: §V · p. 7
  • The authors identify integration with state-of-the-art large behavior models as a future direction, noting that taskspace-pose outputs can be consumed by WB-MPC.Source: §V · p. 8
Loading 2610.22176v1…