Full-body navigation code for PR2, from http://wiki.ros.org/sbpl_3dnav_planner