Planner chỉ tốt bằng world model
IK có thể chạy chỉ với robot model. Motion planning cần world model. Với robot arm trên bàn, bạn có thể bắt đầu bằng cuboid cho table, bin, fixture. Khi bài toán có người, vật lạ hoặc scene thay đổi nhanh, phải đưa depth camera vào. cuRoboV2 có hướng GPU-native perception và mapping, nhưng deploy tốt vẫn cần discipline ở tầng dữ liệu: timestamp, frame transform, filter và snapshot.
1. Bắt đầu từ obstacle thủ công
Trước khi nối RealSense/ZED, tạo scene YAML tĩnh:
world:
cuboid:
table:
dims: [1.4, 0.8, 0.06]
pose: [0.55, 0.0, -0.03, 1.0, 0.0, 0.0, 0.0]
safety_wall:
dims: [0.04, 1.2, 0.8]
pose: [0.85, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0]
Nếu planner chưa ổn với scene tĩnh, depth sẽ chỉ làm debug khó hơn.
2. Depth pipeline
camera depth
-> depth filter
-> point cloud in camera frame
-> TF to robot base
-> voxel / TSDF / ESDF
-> planning snapshot
Mỗi frame phải có timestamp. Nếu TF lookup dùng transform mới hơn depth frame 100 ms, obstacle sẽ bị lệch khi robot/camera chuyển động.
3. Snapshot thay vì stream trực tiếp
Planner nên nhận immutable snapshot:
scene = scene_buffer.get_latest(max_age=0.10)
result = planner.plan_pose(start, goal, scene)
Không để mapping thread sửa scene object trong lúc optimizer đang đọc. Dùng copy-on-write hoặc double buffer.
4. Filter thực dụng
| Filter | Lý do |
|---|---|
| crop workspace | bỏ điểm ngoài vùng robot với tới |
| floor/table removal | giảm false collision |
| temporal median | bớt noise depth |
| inflation margin | bù sai số calibration |
| unknown zone | không plan xuyên vùng camera không thấy |
Với Jetson, ưu tiên filter nhẹ và deterministic. Đừng chạy pipeline perception quá nặng khiến planner mất GPU budget.
5. Calibration checklist
camera_linktớibase_linkđược đo và lưu version.- Depth scale đúng mét.
- Point cloud table khớp cuboid table trong viewer.
- Khi đặt box chuẩn 10 cm, obstacle trong planner cũng khoảng 10 cm.
- Scene age được publish ra diagnostics.
6. Khi nào dùng ESDF/TSDF?
Cuboid/mesh đủ cho fixture cố định. ESDF/TSDF hữu ích khi scene thay đổi và obstacle không có shape đơn giản. Nhưng mapping 3D cũng tăng latency, memory và tuning. Lộ trình an toàn:
- Static cuboids.
- Dynamic cuboids từ perception.
- Point cloud voxel collision.
- TSDF/ESDF cho workspace phức tạp.
Kết luận
Scene mapping là nơi nhiều hệ thống sim chạy tốt nhưng real fail. Giữ snapshot rõ ràng, timestamp chặt và bắt đầu bằng obstacle đơn giản. Bài tiếp theo chuyển sang Unitree G1 arm-only: vẫn là planner, nhưng base và balance là ràng buộc mới.
