Motion planning trong cuRobo làm gì?
Theo tài liệu, cuRobo giải motion planning như một bài toán trajectory optimization: từ start joint state tới goal pose, tối ưu smoothness, joint limits và collision avoidance. Nhiều seed trajectory chạy song song trên GPU, sau đó chọn nghiệm collision-free tốt nhất. Đây là lý do cuRobo hợp với Jetson AGX Orin 64GB: GPU không chỉ để inference AI, mà còn để search nhiều nghiệm planner cùng lúc.
1. Warmup trước khi nhận request
Không để request đầu tiên từ robot thật chịu chi phí compile/warmup:
planner = MotionPlanner(planner_cfg)
planner.warmup()
Trong ROS 2 node, gọi warmup ở lifecycle on_configure() hoặc on_activate(), không gọi trong callback nhận goal.
2. Scene obstacle tối thiểu
Bắt đầu bằng cuboid thay vì mesh phức tạp:
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]
Pose obstacle dùng [x, y, z, qw, qx, qy, qz]. Đây là nơi lỗi quaternion lại xuất hiện.
3. Pose-to-pose planning
Luồng cơ bản:
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)
Không gửi traj thẳng vào robot nếu chưa time-scale theo giới hạn thật. Với Jetson, serialize trajectory ra ROS 2 JointTrajectory rồi để controller enforce velocity/acceleration.
4. Grasp planning: approach, grasp, lift
cuRobo hỗ trợ grasp planning kiểu ba đoạn: đi tới pre-grasp, tiến vào grasp pose với collision setting phù hợp cho gripper, rồi lift khỏi bề mặt. Với production, tách rõ ba trạng thái trong state machine:
APPROACH: chưa đóng gripper, tránh obstacle.CLOSE: dừng hoặc chạy low-speed servo.LIFT: nâng vật, cập nhật collision model nếu vật đi theo gripper.
Issue mở hiện tại #663 báo plan_grasp có trường hợp cần gọi plan_pose hai lần để các bước sau thành công. Vì vậy trong deploy, đừng coi plan_grasp là atomic black box. Log từng segment và có fallback sang ba lần plan_pose thủ công nếu segment fail.
5. Tune seed và timeout
| Tham số | Khi tăng | Khi giảm |
|---|---|---|
| số seed IK | nhiều nghiệm goal hơn | latency thấp hơn |
| số seed trajectory | tránh local minima | tiết kiệm GPU |
| collision margin | an toàn hơn | bớt false negative |
| timeout | bài toán khó hơn | realtime hơn |
Với Jetson, hãy benchmark theo percentile, không chỉ trung bình. Planner 40 ms trung bình nhưng p99 600 ms là không ổn cho online control.
6. Log cần lưu
- start joint state;
- goal pose
wxyz; - world obstacle snapshot;
- planner status;
- latency p50/p95/p99;
- số segment thành công;
- trajectory max velocity/acceleration sau retiming.
Kết luận
Motion planning chạy tốt khi scene đơn giản, frame đúng và warmup đầy đủ. Grasp planning cần audit kỹ vì issue hiện tại đang liên quan trực tiếp tới plan_grasp. Bài tiếp theo sẽ nối planner này vào ROS 2 và controller.
