Hybrid iLQR Model Predictive Control for Contact Implicit Stabilization on Legged Robots

Model predictive control (MPC) is a popular strategy for controlling robots but is difficult for systems with contact due to the complex nature of hybrid dynamics. To implement MPC for systems with contact, dynamic models are often simplified or contact sequences fixed in time in order to plan trajectories efficiently. In this work, we propose the hybrid iterative linear quadratic regulator (iLQR) (HiLQR), which extends iLQR to a class of piecewisesmooth hybrid dynamical systems with state jumps. This is accomplished by, first, allowing for changing hybrid modes in the forward pass, second, using the saltation matrix to update the gradient information in the backwards pass, and third, using a reference extension to account for mode mismatch. We demonstrate these changes on a variety of hybrid systems and compare the different strategies for computing the gradients. We further show how HiLQR can work in an MPC fashion (HiLQR MPC) by, first, modifying how the cost function is computed when contact modes do not align, second, utilizing parallelizations when simulating rigid body dynamics, and third, using efficient analytical derivative computations of the rigid body dynamics. The result is a system that can modify the contact sequence of the reference behavior and plan whole body motions cohesively-which is crucial when dealing with large perturbations. HiLQR MPC is tested on two systems: first, the hybrid cost modification is validated on a simple actuated bouncing ball hybrid system. Then, HiLQR MPC is compared against methods that utilize centroidal dynamic assumptions on a quadruped robot (Unitree A1). HiLQR MPC outperforms the centroidal methods in both simulation and hardware tests.