ROS 2 Lyrical Luth · Lesson 37 of 86

Forward kinematics

From joint angles to the tool position with trigonometry for a planar arm.

Estimated time · 60–90 min

Learning goals

What you will understand

Explain forward 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

Forward kinematics (FK) computes the tool pose from the joint values. For a planar arm, add each link as a vector: after joint 1 the link points at angle q1; after joint 2 it points at q1 + q2. The tool position is the sum of these vectors: x = l1 cos q1 + l2 cos(q1 + q2), y = l1 sin q1 + l2 sin(q1 + q2).

FK always has exactly one answer, so it is easy to test. In ROS 2, robot_state_publisher performs FK for the whole URDF and publishes the result as TF frames. Check your maths against tf2_echo.

Guided lesson

Work through these steps

1.
Source the ROS 2 Lyrical environment in a fresh terminal.
2.
Read the forward 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 l1 = 1.0, l2 = 1.0;
// Link lengths in metres.
double q1 = M_PI / 2, q2 = -M_PI / 2;
// Joint angles: shoulder points up, elbow bends back by 90 degrees.
double x = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
// Sum the X components; the second link's angle is q1 + q2.
double y = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);
// Sum the Y components the same way.
std::printf("x=%.3f y=%.3f\n", x, y);
// Print the tool position: x=1.000 y=1.000.

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>

int main() {
  const double l1 = 1.0, l2 = 1.0;
  const double q1 = M_PI / 2, q2 = -M_PI / 2;
  // TODO: compute x and y of the tool: l1*cos(q1) + l2*cos(q1+q2), and the same with sin
  std::printf("x=%.3f y=%.3f\n", x, y);
  return 0;
}

Expected output

x=1.000 y=1.000

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

#include <cmath>
#include <cstdio>

int main() {
  const double l1 = 1.0, l2 = 1.0;
  const double q1 = M_PI / 2, q2 = -M_PI / 2;
  const double x = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
  const double y = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);
  std::printf("x=%.3f y=%.3f\n", x, y);
  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 FK for a 2-link planar arm and test five joint pairs, including both angles 0 (fully stretched) and q2 = 180° (folded). Compare one result with tf2_echo from a matching URDF.

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.