Practical local planning in the contact space

Proximity query is an integral part of any motion planning algorithm and takes up the majority of planning time. Due to performance issues, most existing planners perform queries at fixed sampled configurations, sometimes resulting in missed collisions. Moreover, randomly determining collision-free configurations makes it difficult to obtain samples close to, or on, the surface of C-obstacles in the configuration space. In this paper, we present an efficient and practical local planning method in contact space which uses "continuous collision detection" (CCD). We show how, using the precise contact information provided by a CCD algorithm, a randomized planner can be enhanced by efficiently sampling the contact space, as well as by constraining the sampling when the roadmap is expanded. We have included our contact-space planning methods in a freely available state-of-the-art planning library - the Stanford MPK library. We have been able to observe that in complex scenarios involving cluttered and narrow passages, which are typically difficult for randomized planners, the enhanced planner offers up to 70 times performance improvement when our contact-space sampling and constrained sampling methods are enabled.