Learning the metric of task constraint manifolds for constrained motion planning

Finding feasible motion for robots with high-dimensional configuration space is a fundamental problem in robotics. Sampling-based motion planning algorithms have been shown to be effective for these high-dimensional systems. However, robots are often subject to task constraints (e.g., keeping a glass of water upright, opening doors and coordinating operation with dual manipulators), which introduce significant challenges to sampling-based motion planners. In this work, we introduce a method to establish approximate model for constraint manifolds, and to compute an approximate metric for constraint manifolds. The manifold metric is combined with motion planning methods based on projection operations, which greatly improves the efficiency and success rate of motion planning tasks under constraints. The proposed method Approximate Graph-based Constrained Bi-direction Rapidly Exploring Tree (AG-CBiRRT), which improves upon CBiRRT, and CBiRRT were tested on several task constraints, highlighting the benefits of our approach for constrained motion planning tasks.