Grasping Trajectory Optimization with Point Clouds

TL;DR

This paper tackles the intertwined problems of motion and grasp planning by introducing a unified trajectory optimization framework that uses a point-cloud representation for both the robot and the scene. It formulates goal-reaching as a point-matching cost over end-effector surface points and collision avoidance via a precomputed signed distance field, solving a constrained nonlinear program with Ipopt (via CasADi) to jointly optimize joint trajectories and grasp poses. The approach demonstrates improved grasp success and collision avoidance in both simulated (PyBullet) tabletop and shelf scenes and real-world, model-free scenarios, showing generality across Fetch and Panda arms and varying environments. While effective, the method incurs nontrivial optimization times, motivating future work in GPU acceleration and model predictive control to further enhance practical deployment.

Abstract

We introduce a new trajectory optimization method for robotic grasping based on a point-cloud representation of robots and task spaces. In our method, robots are represented by 3D points on their link surfaces. The task space of a robot is represented by a point cloud that can be obtained from depth sensors. Using the point-cloud representation, goal reaching in grasping can be formulated as point matching, while collision avoidance can be efficiently achieved by querying the signed distance values of the robot points in the signed distance field of the scene points. Consequently, a constrained nonlinear optimization problem is formulated to solve the joint motion and grasp planning problem. The advantage of our method is that the point-cloud representation is general to be used with any robot in any environment. We demonstrate the effectiveness of our method by performing experiments on a tabletop scene and a shelf scene for grasping with a Fetch mobile manipulator and a Franka Panda arm. The project page is available at \url{https://irvlutd.github.io/GraspTrajOpt}

Figures (7)

• Figure 1: We represent robots and the task space with point clouds, and solve a trajectory optimization problem for joint motion and grasp planning.
• Figure 2: Surface points (red points in the figure) are sampled as a representation for robots.
• Figure 3: (a) A tabletop scene for grasping with a Fetch robot. (b) A 3D point cloud of the scene computed using a depth image from the camera on the robot. (c) Reaching a grasping goal can be formulated as matching 3D points on the robot gripper. (d) Visualization of the signed distance field of the task space. Cyan points are with negative distances, and yellow points are with positive distances.
• Figure 4: Illustration of the grasping pose and the standoff pose for grasping.
• Figure 5: Examples of (a) tabletop scenes and (b) shelf scenes for grasping in PyBullet.