3D modeling, distance and gradient computation for motion planning: A direct GPGPU approach

The Kinect sensor and KinectFusion algorithm have revolutionized environment modeling. We bring these advances to optimization-based motion planning by computing the obstacle and self-collision avoidance objective functions and their gradients directly from the KinectFusion model on the GPU without ever transferring any model to the CPU. Based on this, we implement a proof-of-concept motion planner which we validate in an experiment with a 19-DOF humanoid robot using real data from a tabletop work space. The summed-up time from taking the first look at the scene until the planned path avoiding an obstacle on the table is executed is only three seconds.