Active exploration using trajectory optimization for robotic grasping in the presence of occlusions

We consider the task of actively exploring unstructured environments to facilitate robotic grasping of occluded objects. Typically, the geometry and locations of these objects are not known a priori. We mount an RGB-D sensor on the robot gripper to maintain a 3D voxel map of the environment during exploration. The objective is to plan the motion of the sensor in order to search for feasible grasp handles that lie within occluded regions of the map. In contrast to prior work that generates exploration trajectories by sampling, we directly optimize the exploration trajectory to find grasp handles. Since it is challenging to optimize over the discrete voxel map, we encode the uncertainty of the positions of the occluded grasp handles as a mixture of Gaussians, one per occluded region. Our trajectory optimization approach encourages exploration by penalizing a measure of the uncertainty. We then plan a collision-free trajectory for the robot arm to the detected grasp handle. We evaluated our approach by actively exploring and attempting 300 grasps. Our experiments suggest that compared to the baseline method of sampling 10 trajectories, which successfully grasped 58% of the objects, our active exploration formulation with trajectory optimization successfully grasped 93% of the objects, was 1.3× faster, and had 3.2× fewer failed grasp attempts.