Stage 08 · Lesson 07/15

Kinematics & Inverse Kinematics · Lesson 07

Forward Kinematics — 6DOF Arm

Est. 60–80 min

Why

Why this lesson matters

In Lesson 6 you built FK for a planar arm. A hobby ‘6DOF’ arm like the one from Stage 6 is the same idea in 3D: a base yaw that turns the whole arm about the vertical, three pitch joints (shoulder, elbow, wrist pitch) that work in one vertical plane, a wrist roll, and a gripper. With 4 × 4 transforms (Lesson 4) you can chain them all and get the gripper tip in 3D.

By the end of this lesson you will be able to: write the FK chain for a base-yaw + shoulder + elbow + wrist-pitch + wrist-roll arm; compute the wrist centre and the gripper tip in 3D; explain why wrist roll does not move the tip and why the wrist centre depends only on J1–J3; use FK as a safety check before sending angles; and measure your own arm’s link lengths for the model.

A small transparent acrylic robot arm with blue micro servos, a two-finger gripper and jumper wires, on a white desk
A small Arduino robot arm with five degrees of freedom in clear acrylic. Base, shoulder, elbow, wrist and gripper — the same layout as most Stage 6 hobby builds.Photo: Nicolò Bertuccioli · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

FK for your Stage 6 arm

The joints (same names as Stage 6)

JointTurns aboutWhat it does to the tip
J1 base yawThe vertical axis (z)Swings the whole arm round; distance from the base axis stays the same
J2 shoulder pitchA horizontal axisLifts the upper arm
J3 elbow pitchA horizontal axisBends the forearm
J4 wrist pitchA horizontal axisTilts the gripper up or down
J5 wrist rollThe gripper’s own long axisSpins the gripper — the tip does not move
Gripper (J6)—Opens/closes; treated as a tool, not part of the position chain

The chain

FK of the example arm (4 × 4 transforms) · Text
T_B_wrist = Rz(J1) · Tz(D1) · Ry(−J2) · Tx(L2) · Ry(−J3) · Tx(L3)
T_B_tip   = T_B_wrist · Ry(−J4) · Rx(J5) · Tx(L4)

D1 = table to shoulder axis, L2 = shoulder to elbow, L3 = elbow to wrist pitch,
L4 = wrist pitch to gripper tip.  Ry(−J) is used so that +J lifts the arm.

Read it like Lesson 6: turn the base, go up to the shoulder, pitch, go along the upper arm, pitch, go along the forearm — that is the wrist centre. Then pitch the wrist, roll, and go out to the gripper tip. Because J2, J3 and J4 all turn about parallel horizontal axes, inside the arm’s vertical plane their angles simply add up, exactly like the planar arm.

Two useful facts

  • The wrist centre depends only on J1, J2, J3. J4 and J5 sit *at* the wrist, so they change where the gripper points, not where the wrist is. Lesson 13–14’s IK uses this to split the problem in two.
  • Wrist roll does not move the tip when the tip lies on the roll axis — it only turns the gripper fingers.

Visual

Visualise it: the position chain

Simplified planar view of the shoulder → elbow → wrist chain inside the arm’s vertical plane. Base yaw (J1) then swings this whole plane round.

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: fk6.py

fk6.py builds the chain above with plain-Python 4 × 4 matrices. For each set of joint angles it prints the wrist centre, the gripper tip and the direction the gripper points (the tool’s x axis). The ‘What to notice’ part checks the two useful facts and does a safety check.

fk6.py · Python
# fk6.py - forward kinematics of a typical hobby "6DOF" arm in 3D, with 4x4 transforms.
# Joints (same names as the Stage 6 course): J1 base yaw, J2 shoulder pitch, J3 elbow pitch,
# J4 wrist pitch, J5 wrist roll. The gripper (J6) opens/closes and does not move the tip frame.
# EXAMPLE link sizes in cm - measure YOUR arm (shaft centre to shaft centre) and change them.
import math

D1 = 9.0     # table -> shoulder axis height
L2 = 10.5    # shoulder axis -> elbow axis
L3 = 9.8     # elbow axis -> wrist-pitch axis
L4 = 14.0    # wrist-pitch axis -> gripper tip (tool)


def Rz(d):
    c, s = math.cos(math.radians(d)), math.sin(math.radians(d))
    return [[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]


def Ry(d):
    c, s = math.cos(math.radians(d)), math.sin(math.radians(d))
    return [[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]


def Rx(d):
    c, s = math.cos(math.radians(d)), math.sin(math.radians(d))
    return [[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]


def Tx(a):
    return [[1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]


def Tz(a):
    return [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, a], [0, 0, 0, 1]]


def mul(*Ms):
    out = [[float(i == j) for j in range(4)] for i in range(4)]
    for M in Ms:
        out = [[sum(out[i][k] * M[k][j] for k in range(4)) for j in range(4)] for i in range(4)]
    return out


def fk(q):
    """q = (J1..J5) in degrees. Pitch joints use Ry(-q) so that +q LIFTS the arm."""
    j1, j2, j3, j4, j5 = q
    T_wrist = mul(Rz(j1), Tz(D1), Ry(-j2), Tx(L2), Ry(-j3), Tx(L3))   # wrist-pitch axis
    T_tool = mul(T_wrist, Ry(-j4), Rx(j5), Tx(L4))                     # gripper tip
    return T_wrist, T_tool


def p(T):
    return f"({T[0][3] + 0.0:6.2f}, {T[1][3] + 0.0:6.2f}, {T[2][3] + 0.0:6.2f})"


def x_axis(T):                                   # which way the gripper points
    return f"({T[0][0] + 0.0:5.2f}, {T[1][0] + 0.0:5.2f}, {T[2][0] + 0.0:5.2f})"


print(f"Example arm: D1 {D1}, L2 {L2}, L3 {L3}, L4 {L4} cm\n")
print(f"{'J1..J5 (deg)':<26} {'wrist centre':>24} {'gripper tip':>24}  tip points along")
for q in ((0, 0, 0, 0, 0), (0, 90, 0, 0, 0), (0, 45, -45, -45, 0), (90, 45, -45, -45, 0),
          (30, 70, -80, -80, 0), (30, 70, -80, -80, 90)):
    Tw, Tt = fk(q)
    print(f"{str(q):<26} {p(Tw):>24} {p(Tt):>24}  {x_axis(Tt)}")

print("\nWhat to notice")
Tw1, Tt1 = fk((30, 70, -80, -80, 0))
Tw2, Tt2 = fk((30, 70, -80, -80, 90))
same = all(abs(Tt1[i][3] - Tt2[i][3]) < 1e-9 for i in range(3))
print(f"- wrist roll J5 from 0° to 90°: tip position unchanged? {same}  (roll spins the tool about its own axis)")
moved = any(abs(fk((30, 70, -80, a, 0))[0][i][3] - Tw1[i][3]) > 1e-9 for a in (-30, 0, 30) for i in range(3))
print(f"- wrist pitch J4 moves the wrist centre? {moved}  (wrist centre depends on J1-J3 only)")
rs = {round(math.hypot(fk((j1, 70, -80, -80, 0))[1][0][3], fk((j1, 70, -80, -80, 0))[1][1][3]), 2) for j1 in (-60, 0, 30, 90)}
print(f"- J1 only swings the arm about the vertical: tip distance from the base axis for J1 = -60, 0, 30, 90 -> {sorted(rs)} cm")
z = fk((30, 60, -90, -60, 0))[1][2][3]
print(f"- safety check before moving: q = (30, 60, -90, -60, 0) puts the tip at z = {z:.2f} cm -> {'BELOW THE TABLE, do not send' if z < 0 else 'ok'}")

Output (run with python3 fk6.py)

Example arm: D1 9.0, L2 10.5, L3 9.8, L4 14.0 cm

J1..J5 (deg)                           wrist centre              gripper tip  tip points along
(0, 0, 0, 0, 0)            ( 20.30,   0.00,   9.00) ( 34.30,   0.00,   9.00)  ( 1.00,  0.00,  0.00)
(0, 90, 0, 0, 0)           (  0.00,   0.00,  29.30) (  0.00,   0.00,  43.30)  ( 0.00,  0.00,  1.00)
(0, 45, -45, -45, 0)       ( 17.22,   0.00,  16.42) ( 27.12,   0.00,   6.53)  ( 0.71,  0.00, -0.71)
(90, 45, -45, -45, 0)      (  0.00,  17.22,  16.42) (  0.00,  27.12,   6.53)  ( 0.00,  0.71, -0.71)
(30, 70, -80, -80, 0)      ( 11.47,   6.62,  17.17) ( 11.47,   6.62,   3.17)  ( 0.00,  0.00, -1.00)
(30, 70, -80, -80, 90)     ( 11.47,   6.62,  17.17) ( 11.47,   6.62,   3.17)  ( 0.00,  0.00, -1.00)

What to notice
- wrist roll J5 from 0° to 90°: tip position unchanged? True  (roll spins the tool about its own axis)
- wrist pitch J4 moves the wrist centre? False  (wrist centre depends on J1-J3 only)
- J1 only swings the arm about the vertical: tip distance from the base axis for J1 = -60, 0, 30, 90 -> [13.24] cm
- safety check before moving: q = (30, 60, -90, -60, 0) puts the tip at z = -0.81 cm -> BELOW THE TABLE, do not send

Try it: treat the three links as shoulder → elbow → wrist in the arm’s vertical plane (simplified). Find settings where the tool points straight down.

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

Real world

6DOF FK in the real world

  • Industrial arms: controllers use 6-joint FK to show the tool pose and check limits.
  • Many industrial arms have a ‘spherical wrist’: the last three joint axes meet at one point, so the wrist centre depends only on the first three joints — the same split you saw in fk6.py.
  • Space and planetary arms: a rover’s arm must know exactly where its tools and cameras are before touching a rock.
  • Simulation: digital twins of arms in ROS and simulators draw every link from FK.
NASA’s Curiosity rover on reddish Martian ground, seen from slightly above, with its wheels, mast and instruments visible
Curiosity’s self-portrait at the ‘John Klein’ site, stitched from images taken by the Mars Hand Lens Imager (MAHLI), a camera on the end of the rover’s robotic arm. Pointing that camera well needs the arm’s forward kinematics.Photo: NASA/JPL-Caltech/MSSS · Public domain · Source: Wikimedia Commons · resized, converted to WebP

Bench build

Bench build: measure your arm and check FK

This bench build does not add new wiring. You measure your Stage 6 arm, put the numbers into the model, pose the arm with its own firmware, and check the FK answer with a ruler. arm_fk_calculator.ino runs the same FK on an Arduino (it drives nothing) so you can check numbers at the bench without a laptop running Python.

  1. Measure D1

    Table to the shoulder servo’s shaft centre, straight up.

  2. Measure L2 and L3

    Shaft centre to shaft centre: shoulder → elbow, elbow → wrist pitch. Use a vernier caliper or a steel ruler.

  3. Measure L4

    Wrist-pitch shaft centre to the point between the gripper fingertips.

  4. Check signs and zeros

    Command each joint to 0°, then +20°. Write down which way it moves. Fix signs in the model if needed.

  5. Pose and compare

    Send (0, 45, −45, −45, 0) and (30, 70, −80, −80, 0) with your Stage 6 firmware. Measure the tip with a ruler from the base axis and the table. Compare with fk6.py or the calculator sketch.

arm_fk_calculator.ino · Arduino C++
// arm_fk_calculator.ino - a forward-kinematics calculator that runs ON the Arduino.
// It does NOT move any servo. Pose your Stage 6 arm with its own firmware, type the same five
// joint angles here (e.g. "30 70 -80 -80 0"), and compare the printed tip with a ruler.
// Same model as fk6.py: J1 base yaw, J2 shoulder pitch, J3 elbow pitch, J4 wrist pitch, J5 roll.
// Safety: this sketch drives nothing. Your arm's servos stay on their own high-current supply
// (Stage 6 power rules); never power servos from the Arduino 5 V pin.
const float D1 = 9.0, L2 = 10.5, L3 = 9.8, L4 = 14.0;   // cm - MEASURE YOUR ARM

void setup() {
  Serial.begin(115200);
  Serial.println(F("Type J1 J2 J3 J4 J5 in degrees, e.g. 30 70 -80 -80 0"));
}

void loop() {
  if (!Serial.available()) return;
  float j[5];
  for (int i = 0; i < 5; i++) j[i] = Serial.parseFloat();
  while (Serial.available()) Serial.read();
  // In the vertical plane of the arm, the pitch angles add up (like the planar FK in Lesson 6)
  float a2 = radians(j[1]), a23 = radians(j[1] + j[2]), a234 = radians(j[1] + j[2] + j[3]);
  float rWrist = L2 * cos(a2) + L3 * cos(a23);          // horizontal distance from the base axis
  float zWrist = D1 + L2 * sin(a2) + L3 * sin(a23);
  float r = rWrist + L4 * cos(a234), z = zWrist + L4 * sin(a234);
  // J1 swings that plane about the vertical axis. J5 (roll) does not move the tip.
  float yaw = radians(j[0]);
  Serial.print(F("wrist (")); Serial.print(rWrist * cos(yaw), 2); Serial.print(F(", "));
  Serial.print(rWrist * sin(yaw), 2); Serial.print(F(", ")); Serial.print(zWrist, 2);
  Serial.print(F(")  tip (")); Serial.print(r * cos(yaw), 2); Serial.print(F(", "));
  Serial.print(r * sin(yaw), 2); Serial.print(F(", ")); Serial.print(z, 2); Serial.print(F(")"));
  Serial.println(z < 0 ? F("  WARNING: below the table!") : F(""));
}
A steel vernier caliper with its jaws open, lying on a dark surface
A vernier caliper measures shaft-to-shaft distances far better than a plastic ruler. Your FK can only be as good as your link lengths.Photo: Jeremyida002 · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Debugging 6DOF FK

SymptomLikely causeFix
Tip height wrong by the same amount everywhereD1 wrongRe-measure table to shoulder axis
Tip goes up in the model, down on the armPitch sign flippedUse Ry(+J) instead of Ry(−J) for that joint
Error grows with reachA joint zero is off by a few degreesRe-zero with a set square
Error only when J1 ≠ 0Shoulder not on the base axis (sideways or forward offset)Add a Tx/Ty offset after Rz(J1)
Changing J5 moves the tipTip is not on the roll axis (gripper offset)Add the offset after Rx(J5) and re-measure L4
Model fine, real arm sags under loadServo deflection, not kinematicsLighter payload or stronger servos (Stage 7)

Think

Think about it

Think

Fatima says, “Your arm has five joints but the tip only has three position numbers (x, y, z). So two joints are useless.” Is she right?

Experiment

Try it in the lab

About 45 minutes with fk6.py, a caliper or ruler, and your Stage 6 arm (or the photos and example numbers if your arm is not built yet).

Experiment

FK for your own arm

You will need

  • `fk6.py` and Python 3
  • Your Stage 6 arm and its firmware (optional)
  • Vernier caliper or steel ruler, set square
  • Optional: Arduino UNO for `arm_fk_calculator.ino`

Steps

  1. Run `fk6.py` with the example sizes and check the all-zeros and straight-up rows by hand.
  2. Measure your arm and put your D1, L2, L3, L4 into the file. Run again.
  3. Add a pose of your own that points the gripper straight down 5 cm above the table. Use FK to find it by trying values.
  4. Add a check that prints a warning if the **elbow** (not just the tip) goes below the table.
  5. If your arm is built: pose it at two of your rows and measure the tip. Record the error.
  6. In the visualiser, set the three links so the tool points straight down, and compare with your `fk6.py` 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

The 3D chain behaves like the planar chain inside the arm’s vertical plane, with J1 swinging that plane. J4 and J5 change where the gripper points but not where the wrist is. FK makes ‘will this pose hit the table?’ a quick calculation.

Knowledge check

Check your understanding

Knowledge check

1. In the example arm, which joint swings the whole arm about the vertical axis?

2. With all joints at 0°, the example tip is at (34.30, 0, 9.00). Where does 34.30 come from?

3. Changing wrist roll J5 from 0° to 90° in fk6.py…

4. The wrist centre depends on…

5. For (30, 70, −80, −80, 0) the gripper points straight down. Why?

6. fk6.py says a pose puts the tip at z = −0.81 cm. You should…

7. Your real arm’s error grows the further it reaches. The most likely cause is…

Mini assignment

Hands-on assignment

Mini assignment

FK model card for your arm

Make a one-page FK model card for your Stage 6 arm that someone else could use.

Deliverables

  • A side-view and a top-view sketch with D1, L2, L3, L4, any offsets and every joint’s positive direction.
  • The FK chain written in Rz/Ry/Rx/Tx/Tz steps for your arm.
  • A table of measured link lengths and how you measured them.
  • Three poses: FK prediction vs measured tip, with the error.
  • A list of ‘forbidden’ poses your FK check would block (tip or elbow below the table).

Do this, step by step

  1. Measure D1, L2, L3 and L4 on your Stage 6 arm (axis to axis), and write each joint’s positive direction on a side-view sketch.
  2. Put your numbers into fk6.py and run the all-zeros and straight-up poses first. Check those by hand — they are just sums of lengths.
  3. Choose three more poses, run FK, then set the real arm to them slowly and measure the tip with a ruler.
  4. Record prediction vs measured tip and the error for each pose.
  5. List at least two ‘forbidden’ poses where FK puts the tip or elbow below the table, and add a check to fk6.py that prints a warning for them.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. Start with the all-zeros pose — the tip should be a simple sum of lengths.
  2. J1 only swings the arm round the base, so changing it must not change the tip height.
  3. If one pose is badly off, check that joint’s positive direction first.

Challenges (pick by level)

  • 🟢 Easy: Compute the all-zeros and straight-up tips for your own link lengths by hand.
  • 🟢 Easy: Change J1 to 45° in one row and check the tip is only swung round.
  • 🟡 Medium: Make fk6.py also print the elbow position and warn if it is below the table.
  • 🟠 Hard-ish: Add a 2 cm sideways wrist offset (Ty after the roll) and show that J5 now moves the tip.
  • 🔴 Hard: Sweep J2, J3 in 10° steps (J1 = 0, J4 = −J2 − J3) and plot every tip point: the side-view workspace.
  • ⚫ Research: Look up what a ‘spherical wrist’ is and find one industrial arm that has it.

Next step

Writing chains by hand works, but people use a standard recipe so everyone gets the same frames. In Lesson 8 — Denavit–Hartenberg Parameters you learn that recipe.

Look back: Lesson 6: Forward Kinematics — Concept. Next: Lesson 8: Denavit–Hartenberg Parameters. Related: Stage 6 — 6DOF Arm and Stage 12 — Simulation & Digital Twin.

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