Tutorial 6: Inverse and Forward Kinematics Services

🟢 Difficulty: Beginner ⏱️ Estimated Time: 20-30 minutes

Overview

This tutorial covers how to use the Inverse Kinematics (IK) and Forward Kinematics (FK) services on the unified planner node.

  • IK: Compute joint configurations that achieve a desired end-effector pose

  • FK: Compute end-effector pose from a set of joint positions

Both solvers are lazy-initialized — they are not created at startup. You must call the warmup service before using IK or FK. This allows the node to start quickly and avoids allocating GPU memory for solvers you don’t need.

For detailed information about cuRobo’s kinematics capabilities and underlying algorithms, visit curobo.org.

When to Use IK/FK vs Motion Planning

Use Case

FK

IK

Motion Planning

Get end-effector pose from joint state

✅

❌

❌

Check if a pose is reachable

❌

✅

❌

Find joint config for a target pose

❌

✅

❌

Generate collision-free trajectory

❌

❌

✅

Key rule: Use IK/FK for kinematic queries. Use generate_trajectory for actual robot motion.

Prerequisites

Section 1: Forward Kinematics (FK)

1.1 What is Forward Kinematics?

Forward kinematics computes the end-effector pose from a given set of joint positions. It is a purely geometric computation — no collision checking, no obstacle dependency.

Input: Joint positions [j1, j2, j3, j4, j5, j6] Output: End-effector pose (x, y, z, qx, qy, qz, qw) in the robot base frame

1.2 Warmup FK

Before calling fk, initialize the FK model:

ros2 service call /unified_planner/warmup_fk curobo_msgs/srv/WarmupFK "{batch_size: 1}"

The batch_size parameter tells the model how many joint states to expect per call, allowing CUDA kernels to be pre-compiled for that size. Unlike IK, FK does not require reinitialization when the batch size changes.

Expected response:

success: true
message: 'FK model initialized with batch_size=1'

1.3 FK Service Examples

Example 1: Single joint configuration

ros2 service call /unified_planner/fk curobo_msgs/srv/Fk \
  "{joint_states: [{position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}]}"

Example 2: Multiple joint configurations in one call

Warmup with the batch size you intend to use, then call fk with that many joint states:

ros2 service call /unified_planner/warmup_fk curobo_msgs/srv/WarmupFK "{batch_size: 3}"

ros2 service call /unified_planner/fk curobo_msgs/srv/Fk \
  "{joint_states: [
    {position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]},
    {position: [0.0, -0.785, 1.57, 0.0, 1.57, 0.0]},
    {position: [0.5, -0.5, 0.5, 0.0, 1.0, 0.0]}
  ]}"

Response:

poses:
  - position: {x: 0.796, y: 0.0, z: 0.413}
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
  - position: {x: 0.452, y: 0.0, z: 0.627}
    orientation: {x: 0.0, y: 0.707, z: 0.0, w: 0.707}
  - position: {x: ..., y: ..., z: ...}
    orientation: {...}

All poses are in the robot’s base frame.

Section 2: Inverse Kinematics (IK)

2.1 What is Inverse Kinematics?

Inverse kinematics computes a joint configuration that places the end-effector at a given pose. The IK solver in curobo_ros uses cuRobo’s IkSolver, which:

  • Performs collision checking (self-collision + obstacle avoidance)

  • Shares the same obstacle world as the trajectory planners — obstacle changes are propagated automatically

Input: Target pose (x, y, z, qx, qy, qz, qw) Output: Joint configuration or failure if no valid solution exists

2.2 Warmup IK

Before calling ik or ik_batch, initialize the IK solver:

ros2 service call /unified_planner/warmup_ik curobo_msgs/srv/WarmupIK "{batch_size: 1}"

The batch_size parameter pre-allocates GPU memory for that number of poses. If you later call ik_batch with a different number of poses, the solver reinitializes automatically (the first call will be slower). For best performance, warmup with the batch size you intend to use.

Expected response:

success: true
message: 'IK solver initialized with batch_size=1'

2.3 Single Pose IK

Use the ik service for a single target pose:

ros2 service call /unified_planner/ik curobo_msgs/srv/Ik \
  "{pose: {position: {x: 0.5, y: 0.3, z: 0.4}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}"

Response (success):

success: true
joint_states:
  position: [0.523, -0.234, 1.245, -0.523, 1.456, 0.234]
joint_states_valid:
  data: true
error_msg:
  data: ''

Response (failure — pose unreachable):

success: false
joint_states:
  position: []
joint_states_valid:
  data: false
error_msg:
  data: 'IK failed: no valid solution found'

2.4 Batch IK

Use ik_batch to solve multiple poses in one call. Warmup with the matching batch size first:

ros2 service call /unified_planner/warmup_ik curobo_msgs/srv/WarmupIK "{batch_size: 2}"

ros2 service call /unified_planner/ik_batch curobo_msgs/srv/IkBatch \
  "{poses: [
    {position: {x: 0.5, y: 0.3, z: 0.4}, orientation: {w: 1.0}},
    {position: {x: 0.4, y: 0.2, z: 0.5}, orientation: {w: 1.0}}
  ]}"

Response:

success: true
joint_states:
  - position: [0.523, -0.234, 1.245, -0.523, 1.456, 0.234]
  - position: [0.312, -0.445, 1.102, -0.234, 1.234, 0.456]
joint_states_valid:
  - data: true
  - data: true

2.5 IK with Obstacles

Because the IK solver shares the obstacle world with the trajectory planners, any obstacle you add via add_object is automatically reflected in IK solutions. No manual synchronization is needed.

For example, after adding a box obstacle that blocks a target pose, IK will return success: false for poses that collide with that obstacle.

Section 3: Troubleshooting

IK returns no solution

  1. Pose outside workspace — check that the target is within the robot’s reach (typically 0.3–1.0 m from base for a 6-DOF arm)

  2. Collision — the IK solver checks collisions; an obstacle at the target pose will cause failure

  3. Invalid quaternion — ensure the quaternion is normalized (x²+y²+z²+w² = 1)

Verify with a known reachable pose:

ros2 service call /unified_planner/ik curobo_msgs/srv/Ik \
  "{pose: {position: {x: 0.5, y: 0.0, z: 0.4}, orientation: {w: 1.0}}}"

Service not available

IK and FK solvers are not initialized until warmup is called. If you see Service not available, check that the node is running and that you have called the warmup service:

ros2 node list | grep unified_planner
ros2 service list | grep unified_planner

Batch size mismatch (slower first call)

If you call ik_batch with a different number of poses than the warmup batch size, the solver reinitializes automatically. The first call will be slower. To avoid this, warmup with the batch size you intend to use most often.

Section 4: Comparison

Feature

FK

IK

Motion Planning

Speed

< 5 ms

< 10 ms

50–500 ms

Output

End-effector pose

Joint configuration

Full trajectory

Collision checking

No

Yes (self + obstacles)

Yes

Path planning

No

No

Yes

Suitable for execution

No

No

Yes

Rule of thumb: Use IK/FK for validation and workspace checks. Use generate_trajectory for motion execution.

Next Steps