ROS 2 Lyrical Luth · Lesson 38 of 86

Inverse kinematics

From a target position to joint angles, multiple solutions and unreachable targets.

Estimated time · 60–90 min

Learning goals

What you will understand

Explain inverse kinematics in your own words
Use it in a small robotics example
Verify the result instead of assuming it worked

Understand the idea first

The mental model

Inverse kinematics (IK) finds joint values that put the tool at a target. For a 2-link planar arm it has a closed form: q2 = acos((x² + y² − l1² − l2²) / (2 l1 l2)) and q1 = atan2(y, x) − atan2(l2 sin q2, l1 + l2 cos q2). The sign of q2 gives the two solutions, elbow-up and elbow-down.

IK can fail: if the target is farther than l1 + l2 or closer than |l1 − l2| there is no answer, so check that the acos argument is between −1 and 1. Real arms use numerical solvers (MoveIt, KDL, Pinocchio) but the same rules apply: check reachability and joint limits.

Guided lesson

Work through these steps

1.
Source the ROS 2 Lyrical environment in a fresh terminal.
2.
Read the inverse kinematics example and identify what each line does.
3.
Build only the affected package, source the workspace overlay, and run the example.
4.
Inspect the result from a second terminal with the ros2 CLI or RViz2 and compare it with the expected result.

Worked example

Read it before you run it

double c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2);
// Cosine of the elbow angle from the law of cosines.
if (c2 < -1.0 || c2 > 1.0) return false;
// Outside [-1, 1] the target is unreachable.
double q2 = std::acos(c2);
// Elbow angle (one of two solutions; use -q2 for the other).
double q1 = std::atan2(y, x) - std::atan2(l2 * std::sin(q2), l1 + l2 * std::cos(q2));
// Shoulder angle: direction to the target minus the angle the elbow adds.
return true;
// A valid solution was found.

What the example is doing

1.The ros2 commands query the running graph; they do not prove that a node's internal behavior is correct.
2.Build from the workspace root, then source install/setup.bash so the shell can discover the new package.
3.Use a second sourced terminal to inspect nodes and interfaces while the example runs.

Watch it run · Robot arm

See what this code does

The terminal types this lesson's practice solution and runs it. The scene shows the kind of robot behaviour that code drives. It plays by itself and loops.

Practice code

Now type it yourself

Copy the starter into practice.cpp and replace every TODO with your own code. Build and run it: g++ -std=c++17 -Wall -Wextra -Wpedantic practice.cpp -o practice -pthread && ./practice

#include <cmath>
#include <cstdio>

// Returns false when the target cannot be reached
bool ik(double x, double y, double l1, double l2, double& q1, double& q2) {
  // TODO: use the law of cosines: reject |c2| > 1, then q2 = acos(c2) and q1 = atan2(y,x) - atan2(l2*sin(q2), l1 + l2*cos(q2))
}

int main() {
  double q1{}, q2{};
  if (ik(1.0, 1.0, 1.0, 1.0, q1, q2)) {
    const double d1 = std::round(q1 * 180.0 / M_PI * 10) / 10 + 0.0;
    const double d2 = std::round(q2 * 180.0 / M_PI * 10) / 10 + 0.0;
    std::printf("q1=%.1f q2=%.1f\n", d1, d2);
  }
  return 0;
}

Expected output

q1=0.0 q2=90.0

Stuck? Show the full solution (try it yourself first)

#include <cmath>
#include <cstdio>

// Returns false when the target cannot be reached
bool ik(double x, double y, double l1, double l2, double& q1, double& q2) {
  const double c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2);
  if (c2 < -1.0 || c2 > 1.0) return false;
  q2 = std::acos(c2);
  q1 = std::atan2(y, x) - std::atan2(l2 * std::sin(q2), l1 + l2 * std::cos(q2));
  return true;
}

int main() {
  double q1{}, q2{};
  if (ik(1.0, 1.0, 1.0, 1.0, q1, q2)) {
    const double d1 = std::round(q1 * 180.0 / M_PI * 10) / 10 + 0.0;
    const double d2 = std::round(q2 * 180.0 / M_PI * 10) / 10 + 0.0;
    std::printf("q1=%.1f q2=%.1f\n", d1, d2);
  }
  return 0;
}

Expected result

How you know it worked

ROS_DISTRO is lyrical in every terminal used for the exercise.
The complete package from the exercise builds and the expected graph entity appears in a second terminal.

Practical exercise

Apply the lesson

Implement 2-link IK, test it by feeding its result into your FK from the previous lesson, and try targets at distance 0.5, 1.4, 2.0 and 2.1. Report which are unreachable and why.

Before you continue

ROS_DISTRO reports lyrical in every terminal you used
The example builds and runs without errors
You checked the result with a ros2 command, RViz2 or a test, not just by reading the code

If something goes wrong

ros2 cannot find a package or executable.

Source /opt/ros/lyrical/setup.bash, build from the workspace root, then source install/setup.bash in that terminal.

Nodes run but cannot discover each other.

Confirm both terminals use Lyrical, the same ROS_DOMAIN_ID, and compatible network/discovery settings.