Stage 08 · Lesson 03/15

Kinematics & Inverse Kinematics · Lesson 03

Position, Orientation & Pose

Est. 45–60 min

Why

Why this lesson matters

In Lessons 1–2 you described *where* a point is, in a named frame. But a tool is not a point. A pen, a gripper or a screwdriver also has a direction. Kinematics describes both: the position of the tool frame’s origin and the orientation of its axes. Together they are the tool’s pose.

By the end of this lesson you will be able to: separate position and orientation; describe a planar pose as (x, y, φ); describe a 3D orientation with roll, pitch and yaw or with a rotation matrix R; check that a matrix really is a rotation (unit, perpendicular columns, det = +1); reach the same point with different tool angles using a 3-link arm; and measure roll and pitch with an MPU-6050 sensor.

A dark robotic hand with many finger joints holding a white light bulb between its fingertips, against a blue background
The Shadow Dexterous Hand holding a light bulb. To hold it like this, the fingertips need the right position *and* the right direction — the right pose.Photo: Richard Greenhill and Hugo Elias, Shadow Robot Company · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

Position, orientation and pose

Three words

WordAnswersIn 2D (top view)In 3D
Position pWhere is the tool frame’s origin?(x, y) — 2 numbers(x, y, z) — 3 numbers
Orientation RWhich way do its axes point?one angle φ3 independent numbers (e.g. roll, pitch, yaw), often stored as a 3 × 3 matrix R
Pose (p, R)Both together(x, y, φ) — 3 numbers6 numbers — this is why a general-purpose arm has 6 joints

A pose is always the pose of one frame in another frame, like “the tool frame {T} in the base frame {B}”. The position is where {T}’s origin is, measured in {B}; the orientation is how {T}’s axes are turned compared with {B}’s axes.

Orientation in 3D: roll, pitch, yaw

A common way to describe orientation is three turns: roll about the forward (x) axis, pitch about the side (y) axis and yaw about the vertical (z) axis. Pilots use the same words for an aeroplane. They are easy to imagine, but the order of the turns matters — roll then yaw is not the same as yaw then roll.

A cartoon aeroplane with three arrows through its centre of mass and curved arrows labelled roll, pitch and yaw
Roll, pitch and yaw of an aeroplane. A robot tool frame uses the same three ideas: turning about its forward, side and vertical axes.Photo: NASA; labelling modified by Brews ohare · Public domain · Source: Wikimedia Commons · resized, converted to WebP

Orientation as a rotation matrix R

Robots usually store orientation as a 3 × 3 rotation matrix. Its trick is simple: each column is one of the tool’s axes, written in the base frame. Column 1 is where the tool’s x axis points, column 2 its y axis, column 3 its z axis. A real rotation matrix must pass three checks:

  • Every column has length 1 (axes are unit length).
  • Every pair of columns is at right angles (dot product 0).
  • The determinant is +1 (a right-handed frame; −1 would be a mirror image).

Visual

Visualise it: position and yaw together

Position is where the tool frame’s origin is; orientation is which way its axes point. Change one at a time and read the full planar pose (x, y, yaw).

Interactive · Coordinate-frame explorer

Base, joint, and end-effector frames
XbYbTool {T}
p_B = R(θ)·p_T + o → (92.0, 87.3)

Code lab

Code lab: same place, different pose

pose.py uses a 3-link planar arm: the 2-link arm from Lesson 1 plus a 4 cm tool. Its tool pose is (x, y, φ), where φ = θ₁ + θ₂ + θ₃ is the direction the tool points. ik3() walks back 4 cm along the tool to find the wrist, then reuses the 2-link IK. Part 3 builds a 3D rotation matrix and runs the three checks.

pose.py · Python
# pose.py - position + orientation = pose.
# Part 1-2: a 3-link planar arm (top view). Its tool pose is (x, y, phi): where the tip is AND
#           which way the tool points. The same tip position can have different tool angles.
# Part 3:   a 3D orientation as a rotation matrix R, and the checks every real R must pass.
import math

L1, L2, L3 = 10.0, 8.0, 4.0     # cm; link 3 is the tool (e.g. a pen or a small gripper)


def fk3(t1, t2, t3):
    """Joint angles in degrees -> tool pose (x, y, phi_deg)."""
    a1, a12, a123 = map(math.radians, (t1, t1 + t2, t1 + t2 + t3))
    x = L1 * math.cos(a1) + L2 * math.cos(a12) + L3 * math.cos(a123)
    y = L1 * math.sin(a1) + L2 * math.sin(a12) + L3 * math.sin(a123)
    return x, y, t1 + t2 + t3


def ik3(x, y, phi_deg, elbow=+1):
    """Tool pose -> joint angles. Step back along the tool to the wrist, then 2-link IK."""
    phi = math.radians(phi_deg)
    wx, wy = x - L3 * math.cos(phi), y - L3 * math.sin(phi)
    c2 = (wx * wx + wy * wy - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    if abs(c2) > 1:
        return None
    t2 = elbow * math.acos(c2)
    t1 = math.atan2(wy, wx) - math.atan2(L2 * math.sin(t2), L1 + L2 * math.cos(t2))
    t1d, t2d = math.degrees(t1), math.degrees(t2)
    return t1d, t2d, phi_deg - t1d - t2d


print("1) Same tip position (14, 6), three different tool angles")
for phi in (0.0, 45.0, 90.0):
    q = ik3(14.0, 6.0, phi)
    x, y, p = fk3(*q)
    print(f"   phi {phi:5.1f}° -> joints ({q[0]:7.2f}°, {q[1]:7.2f}°, {q[2]:7.2f}°)"
          f"  check: x {x:5.2f}, y {y:5.2f}, phi {p:5.1f}°")

print("\n2) Position only is not enough: the tool direction (unit vector) for each phi")
for phi in (0.0, 45.0, 90.0):
    print(f"   phi {phi:5.1f}° -> tool points along ({math.cos(math.radians(phi)):5.2f},"
          f" {math.sin(math.radians(phi)):5.2f})")


def rz(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 ry(deg):
    c, s = math.cos(math.radians(deg)), math.sin(math.radians(deg))
    return [[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]]


def matmul(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 det(m):
    return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
            - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
            + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]))


print("\n3) A 3D orientation: yaw 30° about z, then pitch 90° about the new y")
R = matmul(rz(30), ry(90))
for row in R:
    print("   [" + "  ".join(f"{v:6.3f}" for v in row) + " ]")
cols = [[R[i][j] for i in range(3)] for j in range(3)]
lengths = [math.sqrt(sum(v * v for v in c)) for c in cols]
dot01 = sum(cols[0][i] * cols[1][i] for i in range(3))
print(f"   column lengths {lengths[0]:.3f} {lengths[1]:.3f} {lengths[2]:.3f}  (each must be 1)")
print(f"   x·y = {abs(dot01):.3f}  (axes at right angles -> 0)   det(R) = {det(R):.3f}  (right-handed -> +1)")
for name, c in zip("xyz", cols):
    print(f"   tool {name} axis (column {'xyz'.index(name) + 1}) points along ({c[0]:6.3f}, {c[1]:6.3f}, {c[2]:6.3f})")
print("   -> the tool's x axis points straight down; its z axis lies flat, 30° round from base x")

Output (run with python3 pose.py)

1) Same tip position (14, 6), three different tool angles
   phi   0.0° -> joints ( -11.52°,  100.08°,  -88.56°)  check: x 14.00, y  6.00, phi   0.0°
   phi  45.0° -> joints ( -26.79°,  100.49°,  -28.70°)  check: x 14.00, y  6.00, phi  45.0°
   phi  90.0° -> joints ( -25.32°,   77.00°,   38.32°)  check: x 14.00, y  6.00, phi  90.0°

2) Position only is not enough: the tool direction (unit vector) for each phi
   phi   0.0° -> tool points along ( 1.00,  0.00)
   phi  45.0° -> tool points along ( 0.71,  0.71)
   phi  90.0° -> tool points along ( 0.00,  1.00)

3) A 3D orientation: yaw 30° about z, then pitch 90° about the new y
   [ 0.000  -0.500   0.866 ]
   [ 0.000   0.866   0.500 ]
   [-1.000   0.000   0.000 ]
   column lengths 1.000 1.000 1.000  (each must be 1)
   x·y = 0.000  (axes at right angles -> 0)   det(R) = 1.000  (right-handed -> +1)
   tool x axis (column 1) points along ( 0.000,  0.000, -1.000)
   tool y axis (column 2) points along (-0.500,  0.866,  0.000)
   tool z axis (column 3) points along ( 0.866,  0.500,  0.000)
   -> the tool's x axis points straight down; its z axis lies flat, 30° round from base x

Try it: change only the origin (position), then only the yaw (orientation). Watch which numbers change each time.

Interactive · Coordinate-frame explorer

Base, joint, and end-effector frames
XbYbTool {T}
p_B = R(θ)·p_T + o → (92.0, 87.3)

Real world

Pose in the real world

  • Assembly: inserting a peg, key or screw needs the tool’s axis lined up with the hole — orientation matters as much as position.
  • Grasping: picking a cup from the side and from the top uses the same position but different gripper orientations.
  • Cameras: in computer vision, a camera’s pose is its position and orientation, and it is found before images can be turned into robot targets.
  • Drones and phones: IMUs like the MPU-6050 measure tilt; a compass or other sensor is added to get heading (yaw).
A hand holding a round silver compass with a magnetic needle above a dusty ground
A compass measures heading — the yaw part of orientation that an accelerometer cannot see.Photo: Sneha G Gupta · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Bench build

Bench build: measure orientation with an MPU-6050

An MPU-6050 accelerometer feels gravity. When it is held still, the direction of gravity in the sensor’s own frame tells you its roll and pitch. tilt_orientation.ino reads the raw accelerometer over I²C and prints both angles. Tape the board flat to a ruler — the ruler is your ‘tool’ — and tilt it.

Wiring
MPU-6050 boardArduino UNONote
VCC5 V or 3.3 V5 V only if your board has an on-board 3.3 V regulator; otherwise 3.3 V
GNDGND—
SDAA4I²C data
SCLA5I²C clock
  1. Wire and upload

    Open Serial Monitor at 115200. Lying flat, you should see az ≈ 1.00 g and roll ≈ 0°, pitch ≈ 0°.

  2. Roll

    Tilt the ruler sideways about its long axis. Roll changes; pitch stays near 0.

  3. Pitch

    Tilt the front end up. Pitch changes; roll stays near 0.

  4. Try yaw

    Keep it flat and turn it on the table like a compass. Roll and pitch don’t change — gravity cannot see yaw.

  5. Check with a protractor

    Hold the ruler at 30° pitch against a protractor. How close is the reading?

tilt_orientation.ino · Arduino C++
// tilt_orientation.ino - measure part of an ORIENTATION with an MPU-6050 (GY-521 board).
// When the board is still, the accelerometer feels only gravity. From that we get
// roll (tilt about x) and pitch (tilt about y). Yaw (turning about the vertical) cannot be
// seen from gravity - a compass or a camera is needed for that.
// Wiring: VCC -> 5 V ONLY if your board has an on-board 3.3 V regulator (check its listing);
//         a bare MPU-6050 board with no regulator must use 3.3 V. GND -> GND,
//         SDA -> A4, SCL -> A5. No motors in this build.
#include <Wire.h>
const int MPU = 0x68;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);            // PWR_MGMT_1 register
  Wire.write(0);               // wake the sensor up
  Wire.endTransmission(true);
  Serial.begin(115200);
  Serial.println(F("ax_g, ay_g, az_g,  roll_deg, pitch_deg"));
}

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

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);            // ACCEL_XOUT_H: first of six accelerometer bytes
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  float ax = read16() / 16384.0, ay = read16() / 16384.0, az = read16() / 16384.0;  // +-2 g range
  float roll = degrees(atan2(ay, az));
  float pitch = degrees(atan2(-ax, sqrt(ay * ay + az * az)));
  Serial.print(ax, 2); Serial.print(F(", ")); Serial.print(ay, 2); Serial.print(F(", "));
  Serial.print(az, 2); Serial.print(F(",  ")); Serial.print(roll, 1); Serial.print(F(", "));
  Serial.println(pitch, 1);
  delay(200);
}
Close-up of the green liquid vial of a spirit level on an aluminium bar, with an air bubble between two lines
A spirit level measures the same thing as the accelerometer: tilt relative to gravity. Neither can tell which way you are facing.Photo: Santeri Viinamäki · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Debugging pose

SymptomLikely causeFix
Tool reaches the point but at a wrong angleOnly position was givenGive a full pose (x, y, φ) or (p, R)
det(R) = −1One axis flipped — a mirror, not a rotationCheck the frame with the right-hand rule
Columns not length 1Rounding or a wrongly typed matrixRebuild R from angles, or re-normalise
Roll/pitch swap or wrong signBoard mounted turnedCheck the x, y arrows printed on the board
Roll and pitch jump while movingAccelerometer feels motion, not just gravityRead only when still, or filter (Stage 7, Lesson 12)
Yaw never changesGravity can’t measure yawUse a compass, a gyro or a camera for yaw

Think

Think about it

Think

Your 3-link planar arm must reach (14, 6). Imran says, “Why do we even need the third joint? The 2-link arm already reaches (14, 6).” What would you answer?

Experiment

Try it in the lab

About 30 minutes with the Python file, the frame explorer and (optionally) the MPU-6050 bench.

Experiment

Position vs orientation

You will need

  • `pose.py` and Python 3
  • Frame explorer (in your browser)
  • A pen and a ruler
  • Optional: Arduino UNO, MPU-6050 board, jumper wires, protractor

Steps

  1. Hold a pen above a dot on paper. Keep the tip on the dot and change only the direction. Write the pose in words.
  2. Run `pose.py`. Add φ = 135° to Part 1. Does it work? What happens with φ = 180°?
  3. Try a target near full reach, e.g. (20, 0). Which tool angles still work, and why do some return None?
  4. In Part 3, change the order to `matmul(ry(90), rz(30))`. Does R change? Do the three checks still pass?
  5. In the frame explorer, keep the origin fixed and turn only the yaw. Which numbers change?
  6. Optional: with the MPU-6050, measure roll and pitch at 0°, 30° and 60° against a protractor.

Interactive · Coordinate-frame explorer

Base, joint, and end-effector frames
XbYbTool {T}
p_B = R(θ)·p_T + o → (92.0, 87.3)

The same position can have many orientations, and each needs different joint angles. Near the edge of the reach, fewer orientations are possible. Changing the order of rotations changes R — but every real R still passes the three checks.

Knowledge check

Check your understanding

Knowledge check

1. A pose combines…

2. How many numbers describe a planar (top-view) pose?

3. In a rotation matrix R, column 3 is…

4. A matrix has unit, perpendicular columns but det = −1. It is…

5. In pose.py, the tip reaches (14, 6) with φ = 0°, 45° and 90°. This shows that…

6. Why can’t an accelerometer lying flat measure yaw?

7. A general-purpose arm usually has six joints because…

Mini assignment

Hands-on assignment

Mini assignment

Pose cards

Describe three everyday tool tasks as poses and check one with code.

Deliverables

  • Three task cards (e.g. pouring water, writing, turning a screw), each with a sketch of the tool frame and its position and orientation in words.
  • For one card, a planar pose (x, y, φ) and the joint angles from `ik3()` with the FK check.
  • A rotation matrix of your own from `rz()` and `ry()` with the three checks passing, and one sentence on what each column means.
  • One sentence on why order matters for roll, pitch and yaw, with your Part 3 result as evidence.
  • Optional: an MPU-6050 table of protractor angle vs measured roll and pitch.

Do this, step by step

  1. Run python3 pose.py. For Part 1, choose one target (x, y, φ) and check that fk3() of the ik3() answer lands back on it.
  2. Write three task cards (pouring, writing, turning a screw). On each, draw the tool frame and say in words where it is and which way its x axis points.
  3. For one card, put its planar pose into ik3() with elbow=+1 and then elbow=-1. Record both sets of joint angles.
  4. Build your own R with matmul(rz(…), ry(…)), then check: columns of length 1, columns at right angles, det = +1.
  5. Swap the order to matmul(ry(…), rz(…)) and write one sentence on why the result is different.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. φ is the direction the tool points, measured from the x axis, and in this planar arm it is θ₁ + θ₂ + θ₃.
  2. If ik3() returns nothing, the wrist point is out of reach — try a different φ for the same (x, y).
  3. Each column of R is one tool axis written in base coordinates.

Challenges (pick by level)

  • 🟢 Easy: Write the pose of your pen in the frame of your notebook when it lies along the bottom edge.
  • 🟢 Easy: Compute (cos φ, sin φ) for φ = 30° and 120° by hand.
  • 🟡 Medium: Make pose.py print, for (14, 6), every φ from 0° to 350° in 10° steps that has an IK answer.
  • 🟠 Hard-ish: Add an rx() function and build R for roll 90°, then check the three conditions.
  • 🔴 Hard: Show with numbers that rz(30)·ry(90) and ry(90)·rz(30) are different matrices, and explain the difference in words.
  • ⚫ Research: Find out what a quaternion is and why many robot programs store orientation that way.

Next step

You now have position p and orientation R as separate pieces. In Lesson 4 — Homogeneous Transformation Matrices you will pack them into one 4 × 4 matrix T, so moving between frames becomes a single multiplication.

Look back: Lesson 2: Coordinate Frames & Reference Frames. Next: Lesson 4: Homogeneous Transformation Matrices. Related: Stage 1: Rotation and Stage 1: 3D Vectors.

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