Multi-modal motion planning in non-expansive spaces

Motion planning problems encountered in manipulation and legged locomotion have a distinctive multi-modal structure, where the space of feasible configurations consists of intersecting submanifolds, often of different dimensionalities. Such a feasible space does not possess expansiveness, a property that characterizes whether planning queries can be solved efficiently with traditional probabilistic roadmap (PRM) planners. In this paper we present a new PRM-based multi-modal planning algorithm for problems where the number of intersecting manifolds is finite. We also analyze the completeness properties of this algorithm. More specifically, we show that the algorithm converges quickly when each submanifold is individually expansive and establish a bound on the expected running time in that case. We also present an incremental variant of the algorithm that has the same convergence properties, but works better for problems with a large number of submanifolds by considering subsets of submanifolds likely to contain a solution path. These algorithms are demonstrated in geometric examples and in a legged locomotion planner.