Motion planning for constrained systems is a version of the motion planning problem in which the motion of a robot is limited by constraints. For example, one can require that a humanoid robot such as a PR2 remain upright by constraining its torso to be above its base or require that an object such as a bucket of water remain upright by constraining the vertices of the object to be parallel to the robot’s base. Such problems are particularly difficult because the constraints form a manifold in C-space, and planning must be restricted to this manifold.

We introduce a new concept, reachable volumes, that are a geometric representation of the regions the joints and end effectors of a robot can reach, and use it to define a new planning space called RV-space where all points automatically satisfy a problem’s constraints. Samples and paths generated in RV-space naturally conform to constraints, making planning for constrained systems no more difficult than planning for unconstrained systems. Consequently, constrained motion planning problems that were previously difficult or unsolvable become manageable and in many cases trivial. We introduce tools and techniques to extend the state of the art sampling based motion planning algorithms to RV-space. We define a reachable volume sampler, a reachable volume local planner and a reachable volume distance metric. We show that RV-based planners are more efficient than existing methods, particularly for higher dimensional problems, solving problems with 1000+ degrees of freedom for multi-loop, and tree-like linkages.