Vì sao phải kiểm tra FK/IK trước motion planning?
Motion planner thất bại thường bị đổ lỗi cho optimizer, nhưng nguyên nhân nhiều khi là frame sai, quaternion sai thứ tự, hoặc joint order từ driver không trùng config. cuRobo dùng convention quaternion wxyz trong nhiều API. ROS message thường hiển thị x y z w. Nếu bạn đưa thẳng quaternion ROS vào solver mà không đổi thứ tự, robot có thể xoay cổ tay theo hướng không mong muốn.
1. Tạo bộ pose chuẩn
Lưu 5 posture:
home: posture boot an toàn.ready: cách bàn đủ xa.pre_grasp_center: trước vật ở giữa workspace.left_boundary: gần biên trái.right_boundary: gần biên phải.
Mỗi posture gồm joint vector, end-effector pose từ TF, và ảnh viewer. Đây là dữ liệu regression test rẻ nhất cho robot arm.
home:
joints:
joint1: 0.0
joint2: -0.8
joint3: 1.2
ee_pose_ros_xyzw:
position: [0.45, 0.0, 0.32]
orientation: [0.0, 0.707, 0.0, 0.707]
2. Kiểm tra FK
Pseudo-code:
import torch
joint_order = robot_cfg.kinematics.joint_names
q = torch.tensor([[joint_map[name] for name in joint_order]], device="cuda")
state = kin_model.get_state(q)
ee = state.link_pose["tool0"]
print("position", ee.position)
print("quaternion wxyz", ee.quaternion)
So sánh với TF từ ROS 2:
ros2 run tf2_ros tf2_echo base_link tool0
Sai số vị trí nên ở mức milimet đến vài milimet tùy mesh/URDF. Nếu sai hàng centimet, kiểm tra base_link, tool0, fixed joint và unit meter/millimeter.
3. Kiểm tra IK
IK test nên có ba lớp:
| Test | Mục tiêu |
|---|---|
| Reconstruct FK pose | IK phải tìm lại posture ban đầu |
| Nearby pose | Dịch end-effector 2-5 cm, xem solver mượt không |
| Boundary pose | Biên workspace, xem failure có rõ ràng không |
goal_position = torch.tensor([[0.45, 0.00, 0.32]], device="cuda")
goal_quat_wxyz = torch.tensor([[0.707, 0.0, 0.707, 0.0]], device="cuda")
Nếu pose đến từ ROS:
def ros_xyzw_to_wxyz(q):
x, y, z, w = q
return [w, x, y, z]
4. Joint order là lỗi nguy hiểm nhất
Driver robot có thể publish:
["shoulder_pan", "shoulder_lift", "elbow", "wrist_1", "wrist_2", "wrist_3"]
Nhưng cuRobo config có thể dùng tên khác hoặc thứ tự khác. Luôn reorder bằng name, không dùng index thẳng:
def reorder_joint_state(msg, expected_names):
by_name = dict(zip(msg.name, msg.position))
return [by_name[name] for name in expected_names]
5. IK không phải controller
IK trả về joint target hợp lệ về hình học. Nó không đảm bảo velocity, acceleration, torque và contact force. Trước khi gửi robot thật:
- clamp delta joint mỗi tick;
- validate joint limit lần nữa;
- chạy trajectory time parameterization;
- có watchdog nếu solver timeout;
- không gửi target mới khi perception timestamp quá cũ.
Kết luận
Khi FK/IK pass, motion planning mới có nền tin cậy. Nếu planner fail sau bước này, bạn có thể tập trung vào obstacle, seed, collision hoặc optimizer thay vì săn lỗi frame. Bài tiếp theo sẽ chạy pose-to-pose planning, obstacle và grasp.
