Stage 08 · Lesson 10/15

Kinematics & Inverse Kinematics · Lesson 10

End-Effector Pose Calculation

Est. 60–80 min

Why

Why this lesson matters

Multiplying the DH rows (Lessons 8–9) gives the full end-effector pose: a rotation R (3 × 3) and a position p (3 × 1). The position says *where* the gripper is; the rotation says *which way* it points and how it is turned. Pick-and-place, drawing, welding, pouring — every real job needs both.

By the end of this lesson you will be able to: read p and the three axis columns of R from a computed pose; find the gripper’s approach direction and its tilt below the horizontal; convert R to yaw–pitch–roll angles and back; find the position of a tool point (like a pen tip) held by the gripper; express an object in the gripper’s own frame using the inverse transform; and recognise gimbal lock when angle sets stop working.

A dark mechanical five-finger robot hand with metal joints, seen against a bright window
A five-finger robot gripping hand shown at a trade fair. Before any gripper can grasp, the arm must place it at the right position *and* the right orientation.Photo: NearEMPTiness · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

Reading T = [R p; 0 1]

The four columns of T

ColumnWhat it isEveryday meaning
1st (x₅)Gripper’s x axis, in base coordinatesThe direction the fingers open across
2nd (y₅)Gripper’s y axisThe third direction (right-hand rule)
3rd (z₅)Gripper’s z axis — the approach directionThe way the gripper is pointing, like your index finger
4th (p)Tip positionWhere the fingertips are, in cm from the base

Three things you will do with a pose

  • Describe the direction with three angles. Yaw–pitch–roll (Tait–Bryan z-y′-x″) is common: R = Rz(yaw) · Ry(pitch) · Rx(roll). From R: pitch = atan2(−r₃₁, √(r₁₁² + r₂₁²)), yaw = atan2(r₂₁, r₁₁), roll = atan2(r₃₂, r₃₃).
  • Find a tool point. A pen sticking 3 cm out along the approach axis is at T · (0, 0, 3, 1). The same works for a camera or a suction cup.
  • See the world from the gripper. T⁻¹ = [Rᵀ, −Rᵀp; 0 1]. Multiply an object’s base-frame position by T⁻¹ and you get ‘how far ahead and how far to the side’ of the gripper it is.

Visual

Visualise it: position and pointing direction

In the arm’s vertical plane, the tool position is the chain of links and the tool direction is the sum of the joint angles — the planar view of p and the approach column.

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

ee_pose.py uses the classic DH table from Lesson 9 to compute T for one pose and then reads it: position, the three axes, the approach tilt and heading, yaw–pitch–roll (and a rebuild check), a pen tip, a cup seen from the gripper, and a gimbal-lock case.

ee_pose.py · Python
# ee_pose.py - read the end-effector pose T = [R p; 0 1] from the DH chain (Lessons 8-9).
# Where is the gripper (p)? Which way does it point (columns of R)? What are its yaw/pitch/roll?
# Where is a pen held in the gripper? Where is a cup, as seen from the gripper?
# EXAMPLE arm in cm, classic DH, same table as build_dh.py.
import math

D1, L2, L3, L4 = 9.0, 10.5, 9.8, 14.0
ARM = [(0, D1, 0, 90), (0, 0, L2, 0), (0, 0, L3, 0), (90, 0, 0, 90), (0, L4, 0, 0)]  # (offset, d, a, alpha) deg


def dh(th, d, a, al):
    ct, st = math.cos(math.radians(th)), math.sin(math.radians(th))
    ca, sa = math.cos(math.radians(al)), math.sin(math.radians(al))
    return [[ct, -st * ca, st * sa, a * ct], [st, ct * ca, -ct * sa, a * st], [0, sa, ca, d], [0, 0, 0, 1]]


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


def fk(q):
    T = [[float(i == j) for j in range(4)] for i in range(4)]
    for qi, (off, d, a, al) in zip(q, ARM):
        T = mul(T, dh(qi + off, d, a, al))
    return T


def clean(v, nd=3):
    return round(v, nd) + 0.0


def vec(v):
    return "(" + ", ".join(f"{clean(x):6.3f}" for x in v) + ")"


def ypr_zyx(R):
    """Yaw-pitch-roll (Tait-Bryan z-y'-x'') from R, in degrees. R = Rz(yaw) Ry(pitch) Rx(roll)."""
    pitch = math.atan2(-R[2][0], math.hypot(R[0][0], R[1][0]))
    if abs(math.cos(pitch)) < 1e-6:                      # gimbal lock: yaw and roll not separable
        return None, math.degrees(pitch), None
    yaw = math.atan2(R[1][0], R[0][0])
    roll = math.atan2(R[2][1], R[2][2])
    return math.degrees(yaw), math.degrees(pitch), math.degrees(roll)


def apply(T, pt):
    return [sum(T[i][k] * v for k, v in enumerate(list(pt) + [1])) for i in range(3)]


def inverse(T):          # [R p]^-1 = [R^T  -R^T p]
    Rt = [[T[j][i] for j in range(3)] for i in range(3)]
    p = [T[i][3] for i in range(3)]
    mp = [-sum(Rt[i][k] * p[k] for k in range(3)) for i in range(3)]
    return [Rt[0] + [mp[0]], Rt[1] + [mp[1]], Rt[2] + [mp[2]], [0, 0, 0, 1]]


q = (30, 60, -60, -30, 0)
T = fk(q)
R = [row[:3] for row in T[:3]]
print(f"Pose q = {q} deg\n")
print("T (rotation | position, cm):")
for row in T:
    print("  [" + "  ".join(f"{clean(v):7.3f}" for v in row) + " ]")
print("\nReading it")
print("  p  = tip position          ", vec([T[i][3] for i in range(3)]), "cm")
print("  x5 = 'fingers open' axis   ", vec([R[i][0] for i in range(3)]))
print("  y5 = third axis            ", vec([R[i][1] for i in range(3)]))
print("  z5 = approach (gripper)    ", vec([R[i][2] for i in range(3)]))
tilt = math.degrees(math.asin(-R[2][2]))
print(f"  approach points {abs(tilt):.1f} deg {'below' if tilt > 0 else 'above'} the horizontal, "
      f"heading {math.degrees(math.atan2(R[1][2], R[0][2])):.1f} deg from base x")

y, pch, r = ypr_zyx(R)
print(f"\nYaw-pitch-roll (z-y'-x''): yaw {y:.1f}, pitch {pch:.1f}, roll {r:.1f} deg")
c = lambda d: math.cos(math.radians(d))
s = lambda d: math.sin(math.radians(d))
Rz = [[c(y), -s(y), 0], [s(y), c(y), 0], [0, 0, 1]]
Ry = [[c(pch), 0, s(pch)], [0, 1, 0], [-s(pch), 0, c(pch)]]
Rx = [[1, 0, 0], [0, c(r), -s(r)], [0, s(r), c(r)]]
m3 = lambda A, B: [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
Rb = m3(m3(Rz, Ry), Rx)
print("  rebuilding R from the three angles matches:", all(abs(Rb[i][j] - R[i][j]) < 1e-9 for i in range(3) for j in range(3)))

pen = apply(T, (0, 0, 3.0))
print("\nA pen sticking 3 cm out of the gripper along z5 ->  pen tip at", vec(pen), "cm (base frame)")
cup_base = (22.0, 14.0, 4.0)
cup_in_gripper = apply(inverse(T), cup_base)
print("A cup at", vec(cup_base), "cm (base) is at", vec(cup_in_gripper), "cm in the GRIPPER frame")
print("  -> z5 component = how far ahead along the approach; x5/y5 = how far off to the side")

print("\nGimbal-lock check: arm stretched out level, q = (0, 0, 0, 0, 0)")
Rd = [row[:3] for row in fk((0, 0, 0, 0, 0))[:3]]
yd, pd, rd = ypr_zyx(Rd)
print("  x5 =", vec([Rd[i][0] for i in range(3)]), " (points straight up)  z5 =", vec([Rd[i][2] for i in range(3)]))
if yd is None:
    print(f"  pitch = {pd:.1f} deg -> yaw and roll cannot be separated (gimbal lock); keep using R itself")
else:
    print(f"  yaw {yd:.1f}, pitch {pd:.1f}, roll {rd:.1f}")

Output (run with python3 ee_pose.py)

Pose q = (30, 60, -60, -30, 0) deg

T (rotation | position, cm):
  [  0.433    0.500    0.750   23.534 ]
  [  0.250   -0.866    0.433   13.587 ]
  [  0.866    0.000   -0.500   11.093 ]
  [  0.000    0.000    0.000    1.000 ]

Reading it
  p  = tip position           (23.534, 13.587, 11.093) cm
  x5 = 'fingers open' axis    ( 0.433,  0.250,  0.866)
  y5 = third axis             ( 0.500, -0.866,  0.000)
  z5 = approach (gripper)     ( 0.750,  0.433, -0.500)
  approach points 30.0 deg below the horizontal, heading 30.0 deg from base x

Yaw-pitch-roll (z-y'-x''): yaw 30.0, pitch -60.0, roll 180.0 deg
  rebuilding R from the three angles matches: True

A pen sticking 3 cm out of the gripper along z5 ->  pen tip at (25.784, 14.886,  9.593) cm (base frame)
A cup at (22.000, 14.000,  4.000) cm (base) is at (-6.704, -1.124,  2.575) cm in the GRIPPER frame
  -> z5 component = how far ahead along the approach; x5/y5 = how far off to the side

Gimbal-lock check: arm stretched out level, q = (0, 0, 0, 0, 0)
  x5 = ( 0.000,  0.000,  1.000)  (points straight up)  z5 = ( 1.000,  0.000,  0.000)
  pitch = -90.0 deg -> yaw and roll cannot be separated (gimbal lock); keep using R itself

Try it: in the arm’s vertical plane, the tool direction is the sum of the joint angles. Set the three links so the tool points 30° below the horizontal, like the pose in ee_pose.py.

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

End-effector poses in the real world

  • Pick and place: the gripper must approach from above (approach = −z) at a known position; both parts of the pose matter.
  • Welding and painting: the torch or spray gun must keep a set angle to the surface as it moves.
  • Cameras on arms: to know what a wrist camera sees, you need its full pose, not just where it is.
  • Planetary arms: a rover’s arm carries instruments that must be pressed against a rock face at the right angle.
Two people in white clean-room suits watching the robotic arm of the Curiosity Mars rover, which holds a turret of instruments, during a test in a large clean room
Engineers at NASA JPL watch some of the first motions of Curiosity’s robotic arm (2010). The arm’s turret holds several tools; each one must be placed at the right position *and* orientation.Photo: NASA/JPL-Caltech · Public domain · Source: Wikimedia Commons · resized, converted to WebP

Bench build

Bench build: check the gripper’s tilt with an IMU

Position is easy to check with a ruler. Orientation is harder — so use a sensor. Tape an MPU-6050 board flat on the gripper with its x arrow along the approach direction. Pose the arm with your Stage 6 firmware, type the same five angles, and gripper_tilt_check.ino compares FK’s approach elevation (J2 + J3 + J4) with the elevation the accelerometer measures from gravity.

  1. Wire the IMU

    GND → GND, SDA → A4, SCL → A5. VCC to 5 V only if your GY-521 board has an on-board 3.3 V regulator; otherwise 3.3 V. Use a light ribbon cable with slack.

  2. Mount it

    Double-sided tape, flat on the gripper body, x arrow pointing out along the fingers.

  3. Test three poses

    Level (approach horizontal), 30° down, and pointing straight down. Hold still for a second before typing the angles.

  4. Read the difference

    Within about 5° is good for hobby servos. A constant difference at every pose means the IMU is mounted at a slight angle; a difference that changes means a joint zero or sign is off.

gripper_tilt_check.ino · Arduino C++
// gripper_tilt_check.ino - check the ORIENTATION part of your FK with a real sensor.
// Tape an MPU-6050 (GY-521 board) flat on the gripper with its x arrow pointing along the
// gripper (the approach direction, z5 in ee_pose.py). Pose the arm with your Stage 6 firmware,
// type the same five joint angles here ("30 60 -60 -30 0"), and compare:
//   FK elevation of the approach = J2 + J3 + J4  (pitch angles add up in the arm's plane)
//   IMU elevation of its x axis  = atan2(ax, sqrt(ay^2 + az^2))   (gravity only; hold still)
// Wiring: VCC -> 5 V ONLY if your board has an on-board 3.3 V regulator (check its listing),
//         otherwise 3.3 V. GND -> GND, SDA -> A4, SCL -> A5. Use a long, light ribbon cable
//         and leave slack so it never pulls on the moving arm.
// This sketch drives no servo. Servos stay on their own 5-6 V supply with a common ground -
// never powered from the Arduino 5 V pin.
#include <Wire.h>
const int MPU = 0x68;

int16_t read16() { int16_t hi = Wire.read(); return (hi << 8) | Wire.read(); }

float imuElevation() {
  float sx = 0, sy = 0, sz = 0;
  for (int i = 0; i < 20; i++) {             // average 20 readings to calm the noise
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);                        // ACCEL_XOUT_H
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    sx += read16() / 16384.0; sy += read16() / 16384.0; sz += read16() / 16384.0;
    delay(10);
  }
  return degrees(atan2(sx, sqrt(sy * sy + sz * sz)));
}

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);                          // PWR_MGMT_1: wake up
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(115200);
  Serial.println(F("Pose the arm, keep it still, then type J1 J2 J3 J4 J5 (deg)"));
}

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();
  float fkElev = j[1] + j[2] + j[3];
  float imu = imuElevation();
  Serial.print(F("FK approach elevation ")); Serial.print(fkElev, 1);
  Serial.print(F(" deg   IMU ")); Serial.print(imu, 1);
  Serial.print(F(" deg   difference ")); Serial.print(imu - fkElev, 1);
  Serial.println(abs(imu - fkElev) <= 5 ? F(" deg  OK") : F(" deg  CHECK zero offsets / signs"));
}
A grey hand-held robot teach pendant with a colour screen, a joystick and a red emergency stop button, standing on a desk
An industrial teach pendant (for an ABB IRB 140), the hand-held unit operators use to jog and program the arm. When people work with tool poses, they read numbers and angles; the controller underneath works with matrices like your T.Photo: Auledas · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Debugging pose calculations

SymptomLikely causeFix
Approach direction looks sidewaysRead a row of R instead of a columnGripper axes = columns of R
Yaw/roll jump wildly near one poseGimbal lock (pitch near ±90°)Use R (or quaternions) for maths; show angles only
Rebuilt R from angles doesn’t matchDifferent angle order (XYZ vs ZYX) or degrees/radians mixWrite the convention next to every angle triple
Tool point wrong by the tool lengthOffset applied along the wrong axisApply along z₅ for an approach-axis tool: T · (0, 0, L, 1)
Object ‘behind’ the gripper in gripper frameUsed T instead of T⁻¹Base → gripper needs T⁻¹ = [Rᵀ, −Rᵀp]
IMU and FK differ by a constantSensor taped at an angleSubtract the constant once (calibration)

Think

Think about it

Think

Sameer says, “If the tip position is correct, the pose is correct.” Give one job where that is false.

Experiment

Try it in the lab

About 45 minutes with ee_pose.py, and optionally your Stage 6 arm with an MPU-6050.

Experiment

Read a pose, then check it

You will need

  • `ee_pose.py` and Python 3
  • Paper and pencil
  • Optional: Stage 6 arm, Arduino UNO, MPU-6050 (GY-521) for `gripper_tilt_check.ino`

Steps

  1. Run `ee_pose.py`. Check that the approach tilt equals −(J2 + J3 + J4).
  2. Change the pose to (0, 45, −45, −45, 0) and predict the approach direction before running.
  3. Change the pen length to 5 cm and check the pen tip moved exactly 2 cm further along z₅.
  4. Move the cup until it is straight ahead of the gripper (x₅ and y₅ near 0). Write down its base position.
  5. Find another pose where yaw–pitch–roll hits gimbal lock.
  6. If you have the IMU, run the three bench poses and record FK vs IMU tilt.

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

One matrix holds both answers: p for where, R for which way. Angles are handy for people but can lock; the matrix always works. T⁻¹ turns ‘where is it in the room’ into ‘where is it from my gripper’, which is what grasping needs.

Knowledge check

Check your understanding

Knowledge check

1. In T = [R p; 0 1], the position of the gripper tip is…

2. The gripper’s approach axis (z₅) in base coordinates is…

3. In ee_pose.py, J2 + J3 + J4 = −30°. The approach points…

4. A pen sticks out L cm along the approach axis. Its tip is at…

5. To express a cup’s base-frame position in the gripper frame you use…

6. Gimbal lock in yaw–pitch–roll happens when…

7. Your IMU tilt and FK tilt differ by the same 4° at every pose. Most likely…

Mini assignment

Hands-on assignment

Mini assignment

Pose report for a pick-and-place

Plan one pick: the gripper must approach a 3 cm cube from above. Write the pose report.

Deliverables

  • The cube position in base coordinates and the target pose (p and the approach direction (0, 0, −1)).
  • Joint angles (found by trying values with `ee_pose.py`) that give that pose, and the resulting T.
  • The cube’s position in the gripper frame for a pose 5 cm above it (using T⁻¹).
  • Yaw–pitch–roll of the gripper frame for your pose, and whether it is near gimbal lock.
  • If you have the IMU: FK vs measured tilt for your pose.

Do this, step by step

  1. Place a 3 cm cube on paper in front of the arm and measure its centre in base coordinates.
  2. Run python3 ee_pose.py. Change the joint angles in q until the approach column of R is close to (0, 0, −1) and p is 5 cm above the cube.
  3. Use the inverse() function to find the cube’s position in the gripper frame. It should be about 5 cm straight ahead along the approach axis.
  4. Print yaw–pitch–roll for your pose with ypr_zyx() and write whether pitch is near ±90° (gimbal lock).
  5. Optional: fit the MPU-6050 on the gripper, run gripper_tilt_check.ino, and compare FK tilt with the measured tilt.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. The third column of R is the approach direction; straight down is (0, 0, −1).
  2. The inverse of [R p] is [Rᵀ −Rᵀp], not just the transpose of the whole matrix.
  3. Near pitch = ±90°, yaw and roll mix together — that is gimbal lock, not a bug.

Challenges (pick by level)

  • 🟢 Easy: For q = (0, 0, 0, 0, 0), write down p and the approach column without running code.
  • 🟢 Easy: Check that every column of R in the output has length 1.
  • 🟡 Medium: Add a function that prints the angle between the approach axis and the table (straight down = 90°).
  • 🟠 Hard-ish: Write a function that returns R from yaw, pitch, roll in X-Y-Z order and show it differs from z-y′-x″ for the same three numbers.
  • 🔴 Hard: Convert R to a quaternion and back, and show the quaternion has no trouble at the gimbal-lock pose.
  • ⚫ Research: Find which orientation format ROS uses in its pose messages and why.

Next step

You can now compute any pose the arm *can* reach. But which poses *can* it reach at all? In Lesson 11 — Robot Workspace you will map the space your arm can reach, and the smaller space where it can reach with any orientation.

Look back: Lesson 9: Building a DH Table. Next: Lesson 11: Robot Workspace. Related: Stage 1: Rotation and Stage 10 — ROS 2.

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