Stage 08 · Lesson 06/15

Kinematics & Inverse Kinematics · Lesson 06

Forward Kinematics — Concept

Est. 45–60 min

Why

Why this lesson matters

Forward kinematics (FK) answers one question: *given the joint values, where is the tool, and which way does it point?* You have all the pieces already — frames (Lesson 2), pose (Lesson 3), transforms (Lesson 4) and ‘rotate at the joint, then go along the link’ (Lesson 5). FK is just chaining them from the base to the tool.

By the end of this lesson you will be able to: write FK as a product of one transform per link; compute every joint position and the tool pose of a planar arm; check a chain against the closed-form formula; explain why FK always has exactly one answer; use FK to check an IK answer; and run FK live on a two-servo arm.

A yellow JCB backhoe loader on a muddy site among banana and coconut trees, with its rear digging arm and bucket lowered
A JCB 3DX backhoe loader in Kerala. The boom, arm and bucket are a chain of links and joints — the tool position comes from all three angles together.Photo: Ramesh NG · CC BY-SA 2.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

FK as a chain

FK is a function

Write the joint values as a list q = (q₁, q₂, …, qₙ). FK is a function T_B_tool = FK(q) that returns the tool pose in the base frame. For a normal (serial) arm there is exactly one answer for each q — you just multiply things out. That is why FK is easy and IK (pose → q) is hard.

Build it link by link

FK of an n-link planar arm · Text
T_B_tool = [R(q₁) · Tr(L₁)] · [R(q₂) · Tr(L₂)] · … · [R(qₙ) · Tr(Lₙ)]

After each bracket, the last column of the running product is the next joint (or the tip).

Each bracket is one link: rotate by its joint angle, then go along its length. Multiply from the base outwards. Because every bracket is measured in the frame of the link before it, joint 2’s angle is automatically added to joint 1’s — exactly the θ₁ + θ₂ you saw in Lesson 1.

An old lithograph showing two human arms with muscles and bones, and close-ups of the elbow and wrist joints
Muscles and bones of the shoulder, arm and hand (lithograph, c. 1839). Your own arm is a kinematic chain: shoulder, elbow and wrist angles together decide where your fingertips are.Photo: Wellcome Collection (lithograph by Battistelli after C. Squanquerillo, c. 1839) · CC BY 4.0 · Source: Wikimedia Commons · resized, converted to WebP
What FK gives you
OutputUsed for
Tool pose (x, y, φ) or T_B_toolKnowing where the tool is right now, from the encoders
Every joint positionDrawing the arm on screen; checking for collisions with the table or itself
A way to check IKPlug the IK answer into FK; the error should be ~0
The workspaceRun FK over many q to see all reachable points (Lesson 11)

Visual

Visualise it: the FK chain

Slide the joints and read the tool pose and the planar T matrix. Every set of joint values gives exactly one pose.

Interactive · FK chain visualizer (3R planar educational)

FK transform chain Base→J1…→EE6DOF FK schematic

Simplified 3R planar stand-in for Base→J1→J2→J3→EE. Full 6DOF adds wrist RPY on top of this position chain.

EE x

153.4

EE y

35.8

ᵇTₑ (planar) calculator

0.966-0.2590.000157.02
0.2590.9660.00020.55
0.0000.0001.0000.00
0.0000.0000.0001.000

Code lab

Code lab: a general FK chain

fk_chain.py has one function, fk(lengths, joints_deg), that works for any number of links. It multiplies one ‘rotate, then go along the link’ per joint and records every joint position on the way. It uses the 3-link arm (10, 8, 4 cm) from Lesson 3.

fk_chain.py · Python
# fk_chain.py - forward kinematics as a CHAIN of "rotate at the joint, then go along the link".
# Works for any number of links in a plane. It also returns where every joint is,
# which you need for drawing the arm or checking it does not hit something.
import math


def R(deg):
    c, s = math.cos(math.radians(deg)), math.sin(math.radians(deg))
    return [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]


def Tr(x, y):
    return [[1.0, 0.0, x], [0.0, 1.0, y], [0.0, 0.0, 1.0]]


def mul(A, B):
    return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)]


def fk(lengths, joints_deg):
    """Return T_base_tool and the (x, y) of every joint and of the tip."""
    T = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
    points = [(0.0, 0.0)]
    for L, q in zip(lengths, joints_deg):
        T = mul(mul(T, R(q)), Tr(L, 0.0))          # one link: rotate at the joint, go along it
        points.append((T[0][2], T[1][2]))
    return T, points


def pose(T):
    return T[0][2], T[1][2], math.degrees(math.atan2(T[1][0], T[0][0]))


LINKS = [10.0, 8.0, 4.0]                           # same 3-link arm as Lesson 3

print("1) Walk along the chain for joints (30°, 45°, -30°)")
T, pts = fk(LINKS, [30, 45, -30])
for name, p in zip(["base", "joint 2", "joint 3", "tool tip"], pts):
    print(f"   {name:<8} at ({p[0] + 0.0:6.2f}, {p[1] + 0.0:6.2f})")
x, y, phi = pose(T)
print(f"   tool pose (x, y, phi) = ({x:.2f}, {y:.2f}, {phi:.1f}°)")

print("\n2) Check against the closed-form formula (sum of link vectors)")
a1, a12, a123 = math.radians(30), math.radians(75), math.radians(45)
xc = 10 * math.cos(a1) + 8 * math.cos(a12) + 4 * math.cos(a123)
yc = 10 * math.sin(a1) + 8 * math.sin(a12) + 4 * math.sin(a123)
print(f"   formula gives ({xc:.2f}, {yc:.2f})  same answer: {math.dist((x, y), (xc, yc)) < 1e-9}")

print("\n3) FK has exactly one answer, and it is fast: a quick sweep of joint 1")
for q1 in (0, 30, 60, 90):
    x, y, phi = pose(fk(LINKS, [q1, 45, -30])[0])
    print(f"   q = ({q1:2d}°, 45°, -30°) -> ({x:6.2f}, {y:6.2f}, {phi:5.1f}°)")

print("\n4) FK as the judge of an IK answer (always check IK with FK)")
target = (14.0, 6.0)
guesses = {"from Lesson 3 IK": [-11.52, 100.08, -88.56], "a wrong guess": [-11.52, 90.0, -88.56]}
for name, q in guesses.items():
    x, y, _ = pose(fk(LINKS, q)[0])
    print(f"   {name:<17} -> tip ({x:6.2f}, {y:6.2f}), error {math.dist((x, y), target):.2f} cm")

Output (run with python3 fk_chain.py)

1) Walk along the chain for joints (30°, 45°, -30°)
   base     at (  0.00,   0.00)
   joint 2  at (  8.66,   5.00)
   joint 3  at ( 10.73,  12.73)
   tool tip at ( 13.56,  15.56)
   tool pose (x, y, phi) = (13.56, 15.56, 45.0°)

2) Check against the closed-form formula (sum of link vectors)
   formula gives (13.56, 15.56)  same answer: True

3) FK has exactly one answer, and it is fast: a quick sweep of joint 1
   q = ( 0°, 45°, -30°) -> ( 19.52,   6.69,  15.0°)
   q = (30°, 45°, -30°) -> ( 13.56,  15.56,  45.0°)
   q = (60°, 45°, -30°) -> (  3.96,  20.25,  75.0°)
   q = (90°, 45°, -30°) -> ( -6.69,  19.52, 105.0°)

4) FK as the judge of an IK answer (always check IK with FK)
   from Lesson 3 IK  -> tip ( 14.00,   6.00), error 0.00 cm
   a wrong guess     -> tip ( 15.33,   5.14), error 1.59 cm

Try it: slide θ₁–θ₃ and read the tip and the planar T matrix. Move only θ₁ and watch every link after it move.

Interactive · FK chain visualizer (3R planar educational)

FK transform chain Base→J1…→EE6DOF FK schematic

Simplified 3R planar stand-in for Base→J1→J2→J3→EE. Full 6DOF adds wrist RPY on top of this position chain.

EE x

153.4

EE y

35.8

ᵇTₑ (planar) calculator

0.966-0.2590.000157.02
0.2590.9660.00020.55
0.0000.0001.0000.00
0.0000.0000.0001.000

Simulator

Try it in the simulator

Real world

Forward kinematics in the real world

  • Robot controllers: read the encoders, run FK, and show the tool position on the teach pendant — many times a second.
  • Digital twins and simulators: draw the robot on screen by running FK for every link.
  • Safety systems: check that no link enters a forbidden zone, using FK joint positions.

Bench build

Bench build: a two-servo arm that knows where its tip is

Mount one servo as the shoulder on a heavy base, glue a 10 cm card or ice-cream-stick link to its horn, and mount a second servo (the elbow) at the end, with an 8 cm link and a pointer. two_servo_fk.ino takes two angles from the Serial Monitor, moves both servos and prints the FK elbow and tip positions. Put graph paper underneath and compare the printed tip with the real one.

Wiring
FromToNote
Both servos red (+)Separate 5–6 V supply + (2 A or more)NEVER the Arduino 5 V pin
Both servos brown/black (−)Supply − AND Arduino GNDGrounds joined
Shoulder signalPin 5—
Elbow signalPin 6—
  1. Measure the links

    Measure shaft-to-shaft (L1) and elbow shaft to pointer (L2). Type them into the sketch.

  2. Find the zeros

    Type 0 0. Adjust SH_ZERO and EL_ZERO until both links lie straight along the paper’s +x axis.

  3. Test FK

    Type 30 45, 0 60 and -30 30. Mark the real tip each time and write the printed tip beside it.

  4. Measure the error

    How far apart are printed and real? Which pose is worst, and why might that be (servo play, link bending, zero offset)?

two_servo_fk.ino · Arduino C++
// two_servo_fk.ino - a 2-link servo arm that reports its own tip position with forward kinematics.
// Type two angles, e.g. "30 60", in Serial Monitor. The servos move, and FK prints where the tip
// should be. Mark the real tip on graph paper and compare.
// SERVOS run from their OWN 5-6 V supply (2 A or more), NEVER from the Arduino 5 V pin.
// Join the servo supply GND to Arduino GND. Signal wires: shoulder -> pin 5, elbow -> pin 6.
#include <Servo.h>
const float L1 = 10.0, L2 = 8.0;                  // shaft-to-shaft and shaft-to-tip, in cm - MEASURE
const int SH_ZERO = 90, EL_ZERO = 90;             // servo command that gives 0 deg for each joint
Servo shoulder, elbow;

void setup() {
  shoulder.attach(5); elbow.attach(6);
  shoulder.write(SH_ZERO); elbow.write(EL_ZERO);
  Serial.begin(115200);
  Serial.println(F("Type: shoulder_deg elbow_deg   (each -70..70)"));
}

void loop() {
  if (!Serial.available()) return;
  float q1 = constrain(Serial.parseFloat(), -70, 70);   // stay inside safe servo travel
  float q2 = constrain(Serial.parseFloat(), -70, 70);
  while (Serial.available()) Serial.read();
  shoulder.write(SH_ZERO + (int)q1);
  elbow.write(EL_ZERO + (int)q2);
  // forward kinematics: rotate at joint 1, go L1; rotate at joint 2, go L2
  float a1 = radians(q1), a12 = radians(q1 + q2);
  float ex = L1 * cos(a1), ey = L1 * sin(a1);            // elbow position
  float tx = ex + L2 * cos(a12), ty = ey + L2 * sin(a12); // tip position
  Serial.print(F("q = (")); Serial.print(q1, 0); Serial.print(F(", ")); Serial.print(q2, 0);
  Serial.print(F(")  elbow (")); Serial.print(ex, 1); Serial.print(F(", ")); Serial.print(ey, 1);
  Serial.print(F(")  tip (")); Serial.print(tx, 1); Serial.print(F(", ")); Serial.print(ty, 1);
  Serial.println(F(")"));
}
A black ink drawing of an architect-style desk lamp with a base, two arm segments joined by hinges, and a lamp head
An architect-style desk lamp (drawing) is a 2-link arm you already own: the two hinge angles decide where the light lands. Try FK on it with a protractor and a ruler.Photo: Rsperberg · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Debugging FK

SymptomLikely causeFix
Tip right only when joint 1 is 0°Link angles not summed (θ₂ used instead of θ₁ + θ₂)Chain transforms, or sum the angles
Tip mirroredJoint angle sign flipped (servo turns the other way)Negate that joint in the code
Constant offset at every poseWrong link lengthMeasure shaft-centre to shaft-centre
Error depends on the poseJoint zero wrongRe-set SH_ZERO / EL_ZERO with the links straight
Small random errors (a few mm)Servo backlash or bending linksStiffer links; approach each pose from the same side
Arduino resets when servos moveServos on Arduino powerSeparate supply, joined grounds

Think

Think about it

Think

Your two-servo arm prints a tip that is always about 4 mm off, in a direction that changes with the pose. You measured L1 and L2 very carefully. What is the most likely cause, and how would you test it?

Experiment

Try it in the lab

About 35 minutes with the Python file, the FK visualiser and (optionally) the two-servo arm.

Experiment

FK by hand, by code and by servo

You will need

  • `fk_chain.py` and Python 3
  • FK chain visualiser (in your browser)
  • Graph paper, ruler, protractor
  • Optional: Arduino UNO, two micro servos, separate 5–6 V supply, card links, jumper wires

Steps

  1. Compute FK for (30°, 45°, −30°) by hand with the link-vector sum. Check against Part 1.
  2. Change `LINKS` to a 4-link arm (10, 8, 6, 3) and add a fourth angle. The function needs no other changes — why?
  3. In Part 4, change the wrong guess by 1° instead of 10°. How big is the error now?
  4. Use FK in a loop: print the tip for q₂ from −90° to 90° in 30° steps (q₁ = 0, q₃ = 0) and sketch the path.
  5. In the FK visualiser, find two different joint settings that put the tip at almost the same place. What does that tell you about IK?
  6. Optional: test three poses on the two-servo arm and record the error.

Interactive · FK chain visualizer (3R planar educational)

FK transform chain Base→J1…→EE6DOF FK schematic

Simplified 3R planar stand-in for Base→J1→J2→J3→EE. Full 6DOF adds wrist RPY on top of this position chain.

EE x

153.4

EE y

35.8

ᵇTₑ (planar) calculator

0.966-0.2590.000157.02
0.2590.9660.00020.55
0.0000.0001.0000.00
0.0000.0000.0001.000

FK always gives one answer and works for any number of links with the same loop. Small joint errors give tip errors that grow with reach. Different q can give the same tip — FK is many-to-one, which is why IK can have several answers.

Knowledge check

Check your understanding

Knowledge check

1. Forward kinematics answers…

2. For a normal serial arm, how many FK answers does one set of joint values have?

3. One link in the planar FK chain is…

4. In fk_chain.py Part 1, the tool points at 45°. Where does that come from?

5. Why should you always check an IK answer with FK?

6. Your FK tip is right only when joint 1 is at 0°. The bug is probably…

7. Two servos make your Arduino reset when they move. The fix is…

Mini assignment

Hands-on assignment

Mini assignment

FK worksheet for a real arm

Write and test FK for a 2- or 3-link arm you can measure: the two-servo arm, a desk lamp or your Stage 6 arm’s planar view.

Deliverables

  • A labelled sketch: links L1…Ln, joint angles, base frame.
  • The FK chain written as a product of R(q) · Tr(L) brackets.
  • `fk_chain.py` run for three poses, with every joint position listed.
  • The same three poses measured on the real arm, and a table of errors.
  • Two sentences on where the errors come from (zeros, lengths, play).

Do this, step by step

  1. Choose your arm: the two-servo arm, a desk lamp or the planar view of your Stage 6 arm. Measure each link from joint axis to joint axis.
  2. Put your lengths in LINKS in fk_chain.py and run it for three poses. Copy every joint position, not only the tip.
  3. Set the real arm (or lamp) to the same three poses using a protractor, and measure the tip with a ruler.
  4. Make an error table: predicted tip, measured tip, difference. Circle the worst pose.
  5. In the FK chain visualiser, set the same angles and check the drawing matches your real arm’s shape.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. Measure lengths from joint axis to joint axis, not along the outside of the parts.
  2. If the errors are the same size in every pose, suspect a zero offset; if they grow with reach, suspect a length.
  3. Write each joint’s 0° and positive direction on the arm with tape before you measure.

Challenges (pick by level)

  • 🟢 Easy: FK by hand for a 2-link arm (10, 8) at (90°, −90°).
  • 🟢 Easy: Add a fourth link to LINKS and run Part 1.
  • 🟡 Medium: Make fk_chain.py print an ASCII drawing or save a CSV of joint positions you can plot.
  • 🟠 Hard-ish: Add a prismatic (sliding) joint: a link whose length is a joint variable. Change fk() to support it.
  • 🔴 Hard: Use FK to find, by brute force, a set of three joint angles that puts the tip within 0.1 cm of (5, 15) with φ = 90°.
  • ⚫ Research: Find how a 6-joint industrial arm’s FK is written (one 4 × 4 transform per joint) in a robotics textbook or the Modern Robotics book.

Next step

In Lesson 7 — Forward Kinematics: 6DOF Arm you take the same idea into 3D and apply it to the arm you built in Stage 6.

Look back: Lesson 5: Rotation & Translation. Next: Lesson 7: Forward Kinematics — 6DOF Arm. Related: Stage 1: Math → Robot Connection and Stage 6 — 6DOF Arm.

Sources

Self-attested for now — keep artifacts in your lab notebook.

Complete

Today's goal

0 of 1 done today

0-day streak
Daily goal:lessons, quizzes or assignments a day

Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).

Stuck somewhere?

Ask a doubt about this lesson

Saved with this lesson's id and your email so we can reply. Privacy · or email contact@aihubstation.in