Full-body motion planning and control for the car egress task of the DARPA robotics challenge

We present a motion planning and control method for contact-rich full-body behaviors, particularly, the car egress task of the DARPA Robotics Challenge (DRC). We take advantage of human experience by manually specifying multiple task phases, each of which has a specific contact mode. Then, we optimize a sequence of static robot poses as well as contact locations that satisfy all constraints, such as geometric constraints, joint limit constraints, and equilibrium constraints. To accommodate model errors, uncertainties, and satisfy continuous contact constraints, we use online robot pose optimization to generate smooth trajectories based on sensor feedback and user inputs. We demonstrate with experiments that our motion planning method is capable of generating a rough plan for the challenging car egress task of the DRC. Combined with online robot pose optimization, the rough plan could be applied with excellent performance.