Tangent Space RRT: A Randomized Planning Algorithm on Constraint Manifolds

Motion planning for robots subject to holonomic constraints typically involves planning on constraint manifolds. In this paper we present the Tangent Space Rapidly Exploring Random Tree (TS-RRT) algorithm for planning on constraint manifolds. The key idea is to construct random trees not on the constraint manifold itself, but rather on tangent space approximations to the constraint manifold. Curvature-based methods are developed for constructing bounded tangent space approximations, as well as procedures for random node generation and bidirectional tree extension. Extensive numerical experiments suggest that the TS-RRT algorithm, despite its increased preprocessing and bookkeeping, outperforms existing constrained planning algorithms for a wide range of benchmark planning problems.