Fast C-obstacle query computation for motion planning

The configuration space of a robot is partitioned into free space and C-obstacle space. Most of the prior work in collision detection and motion planning algorithms is targeted towards checking whether a configuration or a 1D path lies in the free space. In this paper, we address the problem of checking whether a C-space primitive or a spatial cell lies completely inside C-obstacle space, without explicitly computing the boundary of C-obstacle. We refer to the problem as the C-obstacle query. We present a fast and conservative algorithm to perform this C-obstacle query. Our algorithm uses the notion of generalized penetration depth that takes into account both translational and rotational motion. We compute the generalized penetration depth for polyhedral objects and compare it with the extent of the motion that the polyhedral robot can undergo. Our approach is general and useful for designing practical algorithms for complete motion planning of rigid robots. We have integrated our query computation algorithm with star-shaped roadmaps [1] - a deterministic sampling approach for complete motion planning. We have applied our modified planning algorithm to planar robots undergoing translational and rotational motion in complex 2D environments. Our algorithm is able to perform the C-obstacle query in milliseconds and improves the performance of the complete motion planning algorithm.