Balancing and Reaching with Model Predictive Control
HRP-2 balances while trying to reach a moving target (i.e. a simulated yellow ball moved by the user through a 3D mouse). The motion is generated online through MuJoCo, a fast trajectory optimization software based on the optimal-control algorithm iLQR and a smooth approximation of the contact dynamics. The control objectives are specified with a cost function, which was designed to make the robot i) balance, ii) reach the ball, iii) not take a step and iv) minimize joint torques and velocities.