Motion planning for closed-chain systems is particularly difficult due to additional constraints, so-called closure constraints, placed on the system. In fact, the probability of randomly selecting a set of joint angles that satisfy the closure constraints is zero. We overcome this challenge by considering a representation of the chain as a hierarchy of sub-chains, each with its own reachable distance range, and instead of randomly sampling in the joint angle space, we recursively sample in the reachable distance space. This provides two distinct advantages over traditional approaches: (1) joint angles are quickly and easily calculated using basic trigonometry relationships instead of using more expensive inverse kinematics solvers, and (2) configurations are guaranteed to satisfy the closure constraints.

In this paper, we describe this hierarchical chain representation and give a sampling algorithm with complexity linear in the number of links in the chain. Our method can be used to significantly improve sampling-based planners for closed-chain systems by overcoming the difficulty of sampling and satisfying closure constraints. We provide the necessary motion planning primitives (namely sampling and local planning) to implement most sampling-based motion planners. Our experimental results show that our method is fast and efficient in practice making sampling configurations with closure constraints comparable to sampling open chain configurations that ignore closure constraints entirely. It is easy to implement and general. It is also extendible to more distance-related constraints besides the ones demonstrated here.