Simultaneous Localization and Grasping as a Belief Space Control Problem

Most approaches to grasp planning assume that the configurations of the object to be grasped and any potential obstacles are known perfectly. As a result, implementations of these “perfect information” approaches to grasp synthesis are necessarily preceded by a perception stage where the the state of the object and obstacles are estimated. Unfortunately, small perceptual errors during the perception stage can cause even good grasp plans to fail. A potentially more robust approach would be to combine perception and grasp synthesis in a single process. We formalize the problem of achieving this as simultaneous localization and grasping (SLAG). The objective of SLAG is to reach what is very likely to be a good grasp configuration given initial uncertainty by actively perceiving the environment. Given this formalization, SLAG can be viewed as an instance of the belief space control problem. This paper applies a new approach to belief space control that is effective in non-Gaussian belief spaces [17]. The approach is demonstrated in a SLAG problem where a robot must locate a cardboard box using a wrist-mounted laser scanner and grasp it. If a second box occludes the box to be grasped, the robot must either move the laser scanner or push the occluding box out of the way. We empirically demonstrate that our approach is capable of solving this problem and present results over many trials.