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.
Introduction
In traditional robotics, motion planning ("getting to the object") and grasp planning ("picking up the object") are often treated as decoupled problems. This isolation is frequently inefficient; for instance, a grasp planner might suggest an optimal top-down pick that is physically unreachable for the robot arm due to environmental constraints like a low shelf.
Joint motion and grasp planning attempts to address these issues simultaneously, but it is computationally demanding. Many existing methods rely on simplified geometric primitives (spheres or bounding boxes) that lack precision, or 3D meshes that are difficult to generate for novel objects in real-time. This research proposes a method that is precise, robot-agnostic, and robust enough for cluttered, real-world environments.
Methods
The core innovation of this work is a unified point-cloud representation for both the robot and its environment.
Robot and Task Space Representation
The system samples approximately 100 points on the surface of each link of the robot arm to create a digital twin in point-cloud form. Simultaneously, the environment is captured via an RGB-D camera and converted into a 3D point cloud.
Optimization Framework
- Point-Matching for Grasping: Rather than matching a 6D pose—which requires complex weighting to balance rotation and translation—the goal is formulated as a point-matching problem. The objective is to minimize the distance between points on the robot's gripper and the target grasp points.
- Collision Avoidance via SDF: The system pre-computes a Signed Distance Field (SDF) of the scene. Collision checking is performed by querying the SDF for the coordinates of the robot's surface points. A negative distance value indicates a collision.
- Trajectory Optimization: These constraints are integrated into a nonlinear optimization problem solved by the Ipopt interior-point optimizer. To ensure safety, the system utilizes a "standoff pose" waypoint to prevent the robot from colliding with the target object during the final approach.
Key Insights
- Points as a Universal Language: By utilizing point clouds, the method becomes entirely robot-agnostic. It eliminates the need for complex CAD models of the environment, relying solely on live depth data.
- Superiority of Point Matching: Point matching proved more effective for Inverse Kinematics (IK) than traditional methods using Quaternions or Euler angles, as it naturally accounts for gripper geometry without manual hyperparameter tuning.
- Smart Initialization: To overcome the sensitivity of nonlinear optimization to starting conditions, the team implemented an initialization strategy that filters out unreachable or colliding grasps before the optimization begins.
Results and Evaluation
The researchers evaluated the framework using the YCB Object Set in both the PyBullet simulator and real-world benchmarks (SceneReplica).
- Higher Success Rates: In simulation, the proposed method achieved higher grasping success and significantly lower collision rates compared to the state-of-the-art OMG-Planner.
- Real-World Performance: In the SceneReplica benchmark, the method achieved a 65% pick-and-place success rate, outperforming a standard sampling-based baseline (OMPL) at 57%.
- Model-Free Capability: The system successfully grasped objects in real-world settings without prior 3D models, demonstrating its robustness to unknown geometry.
Limitations and Future Work
The primary limitation is computational latency, with optimization taking an average of 15.4 seconds per grasp. While effective, this is slower than the 3.2 seconds achieved by the OMG-Planner, making it less ideal for dynamic environments. Future research will focus on GPU acceleration for SDF queries and exploring Model Predictive Control (MPC) to enable real-time reactive adjustments.
Summary updated