Optimal Motion Planning for Humanoid Robots

This paper aims at combining state of the art developments of path planning and optimal control and to create the algorithmic foundations to tackle optimal control problems in cluttered environments. Our contribution is threefold: first, we describe a simple method to automatically generate minimum bounding capsules around exact robot body geometries represented by meshes. Second, we use the bounding capsules to implement distance constraints for an optimal control problem solver and achieve (self-)collision avoidance. Finally, we propose a complete two-stage framework for optimal motion planning on complex robots. This framework is successfully applied to generate optimal collision-free trajectories both in simulation and on the humanoid robot HRP-2.