What cuRobo motion planning does
cuRobo formulates motion planning as trajectory optimization: start from a joint state, reach a goal pose, and optimize smoothness, joint limits, and collision avoidance. Multiple trajectory seeds are optimized in parallel on the GPU, and the best collision-free solution is returned. This is why Jetson AGX Orin 64GB is relevant: the GPU can search multiple planning seeds, not only run neural inference.
1. Warm up before serving requests
Do not let the first real robot request pay compile and warmup cost:
planner = MotionPlanner(planner_cfg)
planner.warmup()
In a ROS 2 lifecycle node, run warmup in on_configure() or on_activate(), not inside the goal callback.
2. Start with simple obstacles
Begin with cuboids before complex meshes:
world:
cuboid:
table:
dims: [1.2, 0.8, 0.05]
pose: [0.55, 0.0, -0.025, 1.0, 0.0, 0.0, 0.0]
bin:
dims: [0.25, 0.18, 0.12]
pose: [0.45, 0.2, 0.06, 1.0, 0.0, 0.0, 0.0]
Obstacle poses use [x, y, z, qw, qx, qy, qz]. This is another place quaternion order bugs show up.
3. Pose-to-pose planning
Basic flow:
start = JointState.from_position(start_q, joint_names=joint_names)
goal = Pose(
position=goal_xyz,
quaternion=goal_quat_wxyz,
)
result = planner.plan_pose(start, goal)
if result.success.item():
traj = result.interpolated_plan
else:
raise RuntimeError(result.status)
Do not send traj directly to hardware without time-scaling it to real limits. On Jetson, publish a ROS 2 JointTrajectory and let the controller enforce velocity and acceleration limits.
4. Grasp planning: approach, grasp, lift
cuRobo supports three-phase grasp planning: approach the pre-grasp pose, move into grasp pose with appropriate finger collision settings, then lift away from the surface. In production, keep three explicit state-machine states:
APPROACH: gripper open, avoid obstacles.CLOSE: stop or run low-speed servo.LIFT: lift the object and update attached-object collision.
The currently open issue #663 reports a case where plan_grasp may require plan_pose to run twice for subsequent steps to succeed. For deployment, do not treat plan_grasp as an opaque atomic call. Log each segment and keep a fallback that runs three manual plan_pose stages.
5. Tune seeds and timeout
| Parameter | Increase when | Decrease when |
|---|---|---|
| IK seeds | Goal reachability is hard | Latency matters |
| trajectory seeds | Local minima are common | GPU budget is tight |
| collision margin | You need more safety | False collisions dominate |
| timeout | The problem is difficult | Online response matters |
On Jetson, benchmark percentiles, not just mean latency. A planner with 40 ms average but 600 ms p99 is not suitable for online control.
6. Logs to keep
- start joint state;
- goal pose in
wxyz; - world obstacle snapshot;
- planner status;
- latency p50/p95/p99;
- successful segment count;
- max velocity/acceleration after retiming.
Conclusion
Motion planning works best when the scene is simple, frames are correct, and warmup is complete. Grasp planning deserves extra audit because a current open issue touches plan_grasp directly. Next we will connect the planner to ROS 2 and the controller.
