Efficient motion planning of highly articulated chains using physics-based sampling

We present a novel motion planning algorithm that efficiently generates physics-based samples in a kinematically and dynamically constrained space of a highly articulated chain. Similar to prior kinodynamic planning methods, the sampled nodes in our roadmaps are generated based on dynamic simulation. Moreover, we bias these samples by using constraint forces designed to avoid collisions while moving toward the goal configuration. We adaptively reduce the complexity of the state space by determining a subset of joints that contribute most towards the motion and only simulate these joints. Based on these configurations, we compute a valid path that satisfies non-penetration, kinematic, and dynamics constraints. Our approach can be easily combined with a variety of motion planning algorithms including probabilistic roadmaps (PRMs) and rapidly-exploring random trees (RRTs) and applied to articulated robots with hundreds of joints. We demonstrate the performance of our algorithm on several challenging benchmarks.