Why validate FK/IK before motion planning?
Planner failures are often blamed on the optimizer, but the root cause is frequently a wrong frame, wrong quaternion order, or mismatched joint order. cuRobo commonly uses wxyz quaternions. ROS messages expose x y z w. Passing ROS quaternions directly into a solver can rotate the wrist in an unexpected direction.
1. Create reference poses
Save five postures:
home: safe boot posture.ready: away from the table.pre_grasp_center: in front of a centered object.left_boundary: near the left workspace edge.right_boundary: near the right workspace edge.
Each posture should include a joint vector, end-effector pose from TF, and a viewer screenshot. This becomes your cheapest regression test.
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. Check 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)
Compare it with ROS 2 TF:
ros2 run tf2_ros tf2_echo base_link tool0
Position error should be millimeters to a few millimeters depending on URDF quality. Centimeter-level error means you should inspect base_link, tool0, fixed joints, and meter versus millimeter units.
3. Check IK
Run three test classes:
| Test | Purpose |
|---|---|
| Reconstruct FK pose | IK should recover the original posture |
| Nearby pose | Move the end effector 2-5 cm and check smoothness |
| Boundary pose | Confirm failures are explicit near workspace limits |
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")
If the pose comes from ROS:
def ros_xyzw_to_wxyz(q):
x, y, z, w = q
return [w, x, y, z]
4. Joint order is the dangerous failure
A driver may publish:
["shoulder_pan", "shoulder_lift", "elbow", "wrist_1", "wrist_2", "wrist_3"]
Your cuRobo config may use different names or order. Always reorder by name:
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 is not a controller
IK returns a geometrically valid joint target. It does not guarantee velocity, acceleration, torque, or contact safety. Before sending commands to hardware:
- clamp joint deltas per tick;
- validate limits again;
- run trajectory time parameterization;
- add a watchdog for solver timeouts;
- reject targets when perception timestamps are stale.
Conclusion
Once FK/IK passes, motion planning has a trustworthy base. If planning fails after this point, focus on obstacles, seeds, collision, or optimizer settings rather than frame bugs. Next we will run pose-to-pose planning, obstacle avoidance, and grasp planning.
