Asymptotically-optimal Path Planning on Manifolds

This paper presents an approach for optimal path planning on implicitly-defined configuration spaces such as those arising, for instance, when manipulating an object with two arms or with a multifingered hand. In this kind of situations, the kinematic and contact constraints induce configuration spaces that are manifolds embedded in higher dimensional ambient spaces. Existing sampling-based approaches for path planning on manifolds focus on finding a feasible solution, but they do not optimize the quality of the path in any sense. Thus, the returned paths are usually not suitable for direct execution. Recently, RRT* and other similar asymptotically-optimal path planners have been proposed to generate high-quality paths in the case of globally parametrizable configuration spaces. In this paper, we propose to use higher dimensional continuation tools to extend RRT* to the case of implicitly-defined configuration spaces. Experiments in different problems validate the proposed approach.