Humanoid Arm Motion Planning for Improved Disturbance Recovery Using Model Hierarchy Predictive Control

Humans noticeably swing their arms for balancing and locomotion. Although the underlying biomechanical mechanisms have been studied, it is unclear how robots can fully take advantage of these appendages. Most controllers that exploit arms for balance and locomotion rely on feedback and cannot anticipate incoming disturbances and future states. Model predictive controllers readily address these drawbacks but are computationally expensive. Here, we leverage recent work on model hierarchy predictive control (MHPC). We develop an MHPC formulation that plans arm motions in reaction to expected or unexpected disturbances. We tested multiple model compositions using simulated balance experiments with the MIT Humanoid undergoing various disturbances. We found that an MHPC formulation that plans over a full-body kino-dynamic model for a 0.3 s horizon followed by a single rigid body model for 0.5 s horizon runs at 40 Hz and increases the set of disturbances that the robot can withstand. Arms allow the robot to dissipate momentum quickly and move the center of mass independently from the lower body. This kinematic advantage helps generate ground wrenches while avoiding kinematic singularities and keeping the center of mass and center pressure within the support polygon. We note similar advantages when allowing the MHPC to anticipate incoming disturbances.