Kinodynamics-based Pose Optimization for Humanoid Loco-manipulation

This paper presents a novel approach for controlling humanoid robots pushing heavy objects using kinodynamics-based pose optimization and loco-manipulation MPC. The proposed pose optimization plans the optimal pushing pose for the robot while accounting for the unified object-robot dynamics model in steady state, robot kinematic constraints, and object parameters. The approach is combined with loco-manipulation MPC to track the optimal pose. Coordinating pushing reaction forces and ground reaction forces, the MPC allows accurate tracking in manipulation while maintaining stable locomotion. In numerical validation, the framework enables the humanoid robot to effectively push objects with a variety of parameter setups. The pose optimization generates different pushing poses for each setup and can be efficiently solved as a nonlinear programming (NLP) problem, averaging 250 ms. The proposed control scheme enables the humanoid robot to push object with a mass of up to 20 kg (118$\%$ of the robot's mass). Additionally, the MPC can recover the system when a 120 N force disturbance is applied to the object.