Efficient whole-body trajectory optimization using contact constraint relaxation

In this work we present a Trajectory Optimization framework for whole-body motion planning for floating base robots. We demonstrate how the proposed approach can optimize whole body trajectories for most common gaits found in bipeds and quadrupeds. The underlying optimal control problem is solved efficiently using Sequential Linear Quadratic control. In contrast to most previous methods, the proposed approach is fast while still using a full dynamic model. Additionally, the approach is contact model free. Instead contact forces are added to the Optimal Control formulation as additional control inputs and contact constraints are handled using a relaxation approach. In our experiments we demonstrate that, despite this relaxation, our solver resolves constraint violations in only a few iterations. Hardware experiments underline the transferability from simulation to hardware.