Stage 13 · Lesson 03/16

Robot Perception, Planning & Manipulation · Lesson 03

Objects, Poses & Coordinate Frames

Est. 60–75 min

Why

Why this lesson matters

In Lesson 2 each object in the world model had a pose. Now you will learn what a pose really is — a position and an orientation of one frame, written in another frame — and how to move a pose from the camera to the base to the gripper without mistakes. This is the everyday maths of manipulation.

You will chain 4 × 4 transforms in plain Python, compute a grasp and pre-grasp, compare four ways of writing orientation, and break things on purpose: wrong order, wrong inverse, degrees vs radians, and offsets in the wrong frame. On the bench, an Arduino turns an ultrasonic reading on a pan servo into a point in the base frame.

Concept

Position + orientation, always in a named frame

1. Frames: a ruler and a compass glued to something

A coordinate frame is an origin point plus three perpendicular axes x, y, z. We glue frames to things: base_link to the arm base, camera_optical to the camera lens, tool0 or gripper to the hand, and one frame to each object. ROS’s REP 103 sets common rules: SI units (metres, radians), right-handed frames, and for a body x forward, y left, z up. Camera “optical” frames are different: z forward, x right, y down.

A drawing of a right hand with the index finger, middle finger and thumb pointing along three perpendicular arrows labelled as vectors
The right-hand rule: x along the index finger, y along the middle finger, z along the thumb. All ROS frames are right-handed. A rotation matrix with determinant −1 is a mirror, not a rotation — a common bug.Photo: Sepitropova · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

2. A pose is a transform

The pose of frame B in frame A is written T_A_B (read: “B as seen from A”). It is a 4 × 4 matrix: a 3 × 3 rotation R and a position p, with a last row of 0 0 0 1. Multiplying it by a point written in B gives the same point written in A: p_A = T_A_B · p_B.

Homogeneous transform · Text
        | r11 r12 r13  px |
T_A_B = | r21 r22 r23  py |     columns of R = B's x, y, z axes written in A
        | r31 r32 r33  pz |     p            = B's origin written in A
        |  0   0   0    1 |
  • Chain: T_base_block = T_base_cam · T_cam_block. The inner names must match (cam … cam), like dominoes.
  • Invert: T_cam_base = inverse(T_base_cam). For a transform, the inverse is [Rᵀ, −Rᵀp] — no general matrix inverse needed.
  • Order matters: A · B is not B · A for rotations. The script shows a 13 cm error from swapping them.
  • Offsets in the tool frame: “back off 8 cm along the gripper” is T_base_grasp · Trans(0, 0, −0.08) — multiply on the right. Multiply on the left and you move along the base axes instead.

3. Four ways to write orientation

REP 103 prefers quaternions first, then rotation matrices, then fixed-axis roll–pitch–yaw. Euler angles are discouraged because there are 24 “valid” conventions.
FormatNumbersGoodWatch out
Rotation matrix9 (3 × 3)Easy to chain; no singularitiesMust stay orthonormal with det +1
Quaternion (w, x, y, z)4, length 1Compact; no singularities; ROS messages use itq and −q are the same rotation; normalise after maths
Axis–angle3 + 1Easy to picture: “turn 30° about z”Angle 0 has no defined axis
Roll–pitch–yaw / Euler3Easy for humans to readMany conventions; gimbal lock at pitch ±90°

A rotation of θ about a unit axis (x, y, z) is the quaternion (cos θ/2, x·sin θ/2, y·sin θ/2, z·sin θ/2). So “−30° about z” is (0.9659, 0, 0, −0.2588). The script checks this.

A stone plaque on a bridge wall with carved text and the formula i squared equals j squared equals k squared equals ijk equals minus one
The plaque on Broom Bridge, Dublin, marking where William Rowan Hamilton discovered quaternions and carved the rule i² = j² = k² = ijk = −1. Today the same maths stores the orientation of almost every robot gripper.Photo: Brendan Ward · CC0 · Source: Wikimedia Commons · resized, converted to WebP

4. Gimbal lock — why robots avoid Euler angles inside

With three angles applied one after another, there are special positions where two rotation axes line up. Then one degree of freedom is lost: changing roll or yaw does the same thing. This is gimbal lock. It happened to the Apollo 11 guidance platform, whose computer warned near the locked position and froze the platform at 85° — Michael Collins joked about wanting “a fourth gimbal for Christmas”. In robot arms the same thing is called wrist flip, when two wrist axes line up. Inside software, store orientation as a quaternion or a matrix, and use roll–pitch–yaw only for showing numbers to people.

A drawing of a small aeroplane inside three nested rings, where two of the rings lie in the same plane
Gimbal lock: two of the three gimbal rings have become coplanar, so the set can no longer rotate freely about one axis.Photo: MathsPoetry · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP

5. Poses you need for one pick

PoseWritten inComes from
T_base_cambase_linkcamera mount + calibration (fixed)
T_cam_blockcamera_opticaldetector, every frame
T_base_blockbase_linkchain of the two above — goes into the world model
T_block_graspblockgrasp choice: where the gripper sits relative to the object
T_base_graspbase_linkT_base_block · T_block_grasp — the IK target
T_base_pregraspbase_linkT_base_grasp · Trans(0, 0, −d) — back off along the approach axis

Visual

Visualise it: one object, many frames

The same block has a different pose in every frame — camera, base and gripper. Move things around and watch the numbers change while the block itself stays put. (Educational explorer, not a calibration tool.)

Interactive · Object pose & frame lab · Educational

Object poses and frames
Pose in {camera}: x=120.0 y=70.0 yaw=25°
cam

Educational frame change stub — not industrial 6D pose / not live TF2 tree.

Code lab

Code lab: chain, grasp, break, convert

pose_frames_lab.py uses plain Python lists for 4 × 4 matrices (no numpy). The overhead camera is 60 cm above the base plane at x = 15 cm, looking straight down (example values). The detector reports a 4 cm block 57 cm in front of the lens, turned +30° about the camera z axis. The script moves this into base_link, builds the grasp and pre-grasp poses, checks the round trip, makes five classic mistakes, prints the orientation four ways and shows gimbal lock. Run python3 pose_frames_lab.py.

pose_frames_lab.py · Python
"""pose_frames_lab.py — Stage 13 L03 (AIHUB STATION lab)

Poses and coordinate frames for a desk arm, with plain Python (no numpy).
A pose = position + orientation of one frame, written in another frame.
We store it as a 4x4 homogeneous transform  T_parent_child.

Frames (example values, metres):
  base_link          : arm base. x forward, y left, z up (ROS REP 103 body frame)
  camera_optical     : overhead camera 60 cm above the base plane, at x = 0.15.
                       Optical convention: z forward (down at the desk), x right, y down.
  block              : detected block, 4 cm cube
  grasp / pregrasp   : where the gripper frame must go (gripper z = approach axis)
Run:  python3 pose_frames_lab.py
"""
import math

def matmul(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 T(R=None, p=(0, 0, 0)):
    R = R or [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
    return [R[0] + [p[0]], R[1] + [p[1]], R[2] + [p[2]], [0, 0, 0, 1]]
def Rz(a):
    c, s = math.cos(a), math.sin(a); return [[c, -s, 0], [s, c, 0], [0, 0, 1]]
def Ry(a):
    c, s = math.cos(a), math.sin(a); return [[c, 0, s], [0, 1, 0], [-s, 0, c]]
def Rx(a):
    c, s = math.cos(a), math.sin(a); return [[1, 0, 0], [0, c, -s], [0, s, c]]
def rot(Tm): return [row[:3] for row in Tm[:3]]
def pos(Tm): return [Tm[i][3] for i in range(3)]
def inv(Tm):
    R = rot(Tm); p = pos(Tm)
    Rt = [[R[j][i] for j in range(3)] for i in range(3)]
    return T(Rt, [-sum(Rt[i][k]*p[k] for k in range(3)) for i in range(3)])
def det3(R):
    return (R[0][0]*(R[1][1]*R[2][2]-R[1][2]*R[2][1]) - R[0][1]*(R[1][0]*R[2][2]-R[1][2]*R[2][0])
            + R[0][2]*(R[1][0]*R[2][1]-R[1][1]*R[2][0]))
def yaw_of(Tm): return math.degrees(math.atan2(Tm[1][0], Tm[0][0]))
def cm(p): return "(" + ", ".join(f"{v*100:6.1f}" for v in p) + ") cm"
def quat(R):
    """rotation matrix -> unit quaternion (w, x, y, z)"""
    w = math.sqrt(max(0, 1 + R[0][0] + R[1][1] + R[2][2])) / 2
    x = math.copysign(math.sqrt(max(0, 1 + R[0][0] - R[1][1] - R[2][2])) / 2, R[2][1] - R[1][2])
    y = math.copysign(math.sqrt(max(0, 1 - R[0][0] + R[1][1] - R[2][2])) / 2, R[0][2] - R[2][0])
    z = math.copysign(math.sqrt(max(0, 1 - R[0][0] - R[1][1] + R[2][2])) / 2, R[1][0] - R[0][1])
    return (w, x, y, z)
def apply(Tm, p):
    return [sum(Tm[i][k]*p[k] for k in range(3)) + Tm[i][3] for i in range(3)]

# ---- 1. the camera mount: T_base_cam ----
# optical x (right in image) = base +x, optical y (down in image) = base -y, optical z = base -z
R_base_cam = [[1, 0, 0], [0, -1, 0], [0, 0, -1]]
T_base_cam = T(R_base_cam, (0.15, 0.0, 0.60))
print("1. Camera mount  T_base_cam")
print(f"   det(R) = {det3(R_base_cam):+.0f}  (must be +1: a right-handed frame, not a mirror)")
print("   camera z axis in base =", [R_base_cam[i][2] for i in range(3)], "-> looking straight down")

# ---- 2. detector output: block pose in the camera frame ----
# detector: block centre 57 cm in front of the lens, turned +30 deg about the camera z axis.
# The block's own z axis points UP out of its top face, i.e. towards the camera (= camera -z),
# so we add a half turn about x:  R_cam_block = Rz(30) * Rx(180)
R_cb = [[sum(Rz(math.radians(30))[i][k]*Rx(math.pi)[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
T_cam_block = T(R_cb, (0.05, 0.02, 0.57))
T_base_block = matmul(T_base_cam, T_cam_block)
print("\n2. Block from detector (camera frame): pos", cm(pos(T_cam_block)), " yaw +30.0 deg about camera z")
print("   Block in base_link: T_base_block = T_base_cam * T_cam_block")
print("   pos", cm(pos(T_base_block)), f" yaw {yaw_of(T_base_block):+.1f} deg about base z")
print("   (camera z points down, so +30 deg about camera z is -30 deg about base z)")

# ---- 3. grasp and pre-grasp ----
# gripper frame: z = approach (points down into the block), fingers close along gripper y
T_block_grasp = T(Rx(math.pi), (0, 0, 0.0))           # flip: gripper z = -block z (down), at the block centre
T_base_grasp = matmul(T_base_block, T_block_grasp)
T_base_pre = matmul(T_base_grasp, T(p=(0, 0, -0.08)))   # 8 cm back along the gripper's own approach axis
print("\n3. Grasp pose in base:    ", cm(pos(T_base_grasp)), "gripper z =", [round(rot(T_base_grasp)[i][2], 3) + 0 for i in range(3)])
print("   Pre-grasp (8 cm back): ", cm(pos(T_base_pre)), "-> straight above the block")
corner = apply(T_base_block, (0.02, 0.02, 0.02))
print("   One top corner of the 4 cm block in base:", cm(corner))

# ---- 4. check: round trip ----
I = matmul(inv(T_base_block), T_base_block)
err = max(abs(I[i][j] - (1 if i == j else 0)) for i in range(4) for j in range(4))
print(f"\n4. inv(T_base_block) * T_base_block = identity? max error {err:.1e}")
back = apply(inv(T_base_cam), pos(T_base_block))
print("   base -> camera again:", cm(back), "(matches the detector)")

# ---- 5. classic mistakes ----
truth = pos(T_base_block)
def err_mm(p): return math.dist(p, truth) * 1000
print("\n5. Classic mistakes (error of the block position in base_link)")
m1 = pos(matmul(T_cam_block, T_base_cam))
print(f"   a) wrong order T_cam_block*T_base_cam    : {cm(m1)}  error {err_mm(m1):6.1f} mm")
m2 = pos(matmul(inv(T_base_cam), T_cam_block))
print(f"   b) used T_cam_base instead of T_base_cam: {cm(m2)}  error {err_mm(m2):6.1f} mm")
m3 = pos(matmul(T(None, (0.15, 0.0, 0.60)), T_cam_block))
print(f"   c) forgot the camera rotation           : {cm(m3)}  error {err_mm(m3):6.1f} mm")
yaw_bad = math.degrees(math.atan2(math.sin(30), math.cos(30)))
print(f"   d) degrees given to sin/cos as radians  : yaw 30 -> {yaw_bad:+.1f} deg (wanted +30.0)")
# e) pre-grasp offset in the wrong frame, for a gripper tilted 45 deg about base y
T_tilt = matmul(T_base_grasp, T(Ry(math.radians(45))))
good = pos(matmul(T_tilt, T(p=(0, 0, -0.08))))
bad = [pos(T_tilt)[0], pos(T_tilt)[1], pos(T_tilt)[2] + 0.08]
print(f"   e) tilted 45 deg gripper, pre-grasp 8 cm 'up in base' instead of 'back along tool z':")
print(f"      tool-frame pre-grasp {cm(good)}  vs base-frame {cm(bad)}  -> {math.dist(good, bad)*1000:.1f} mm apart")

# ---- 6. orientation formats ----
print("\n6. Orientation of the block in base_link, four ways")
Rb = rot(T_base_block)
print("   rotation matrix rows:", [[round(v, 3) + 0 for v in r] for r in Rb])
q = quat(Rb)
print("   quaternion (w,x,y,z):", tuple(round(v, 4) + 0 for v in q),
      f"  expected (cos(-15), 0, 0, sin(-15)) = ({math.cos(math.radians(-15)):.4f}, 0, 0, {math.sin(math.radians(-15)):.4f})")
print("   -q is the SAME rotation:", tuple(round(-v, 4) + 0 for v in q))
print(f"   axis-angle: about +z by {yaw_of(T_base_block):+.1f} deg;  yaw (RPY) = {yaw_of(T_base_block):+.1f} deg")
qd = (q[0]*1.003, q[1]*1.003, q[2], q[3]*1.003)
n = math.sqrt(sum(v*v for v in qd))
print(f"   after drift |q| = {n:.4f} -> normalise to 1 before use")

# ---- 7. gimbal lock ----
print("\n7. Gimbal lock: R = Rz(yaw) * Ry(pitch) * Rx(roll) with pitch = 90 deg")
def rpy(roll, pitch, yaw):
    A = [[sum(Rz(yaw)[i][k]*Ry(pitch)[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
    return [[sum(A[i][k]*Rx(roll)[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
for r, y in ((10, 0), (0, -10), (40, 30)):
    R = rpy(math.radians(r), math.radians(90), math.radians(y))
    print(f"   roll {r:+3d}, yaw {y:+3d} -> first row {[round(v, 3) + 0 for v in R[0]]}, quat {tuple(round(v, 3) + 0 for v in quat(R))}")
print("   All three give the SAME rotation: at pitch 90 deg only (roll - yaw) matters. One degree of freedom is lost.")

Output (run with python3 pose_frames_lab.py)

1. Camera mount  T_base_cam
   det(R) = +1  (must be +1: a right-handed frame, not a mirror)
   camera z axis in base = [0, 0, -1] -> looking straight down

2. Block from detector (camera frame): pos (   5.0,    2.0,   57.0) cm  yaw +30.0 deg about camera z
   Block in base_link: T_base_block = T_base_cam * T_cam_block
   pos (  20.0,   -2.0,    3.0) cm  yaw -30.0 deg about base z
   (camera z points down, so +30 deg about camera z is -30 deg about base z)

3. Grasp pose in base:     (  20.0,   -2.0,    3.0) cm gripper z = [0.0, 0.0, -1.0]
   Pre-grasp (8 cm back):  (  20.0,   -2.0,   11.0) cm -> straight above the block
   One top corner of the 4 cm block in base: (  22.7,   -1.3,    5.0) cm

4. inv(T_base_block) * T_base_block = identity? max error 1.0e-17
   base -> camera again: (   5.0,    2.0,   57.0) cm (matches the detector)

5. Classic mistakes (error of the block position in base_link)
   a) wrong order T_cam_block*T_base_cam    : (  18.0,    9.5,   -3.0) cm  error  131.3 mm
   b) used T_cam_base instead of T_base_cam: ( -10.0,   -2.0,    3.0) cm  error  300.0 mm
   c) forgot the camera rotation           : (  20.0,    2.0,  117.0) cm  error 1140.7 mm
   d) degrees given to sin/cos as radians  : yaw 30 -> -81.1 deg (wanted +30.0)
   e) tilted 45 deg gripper, pre-grasp 8 cm 'up in base' instead of 'back along tool z':
      tool-frame pre-grasp (  15.1,    0.8,    8.7) cm  vs base-frame (  20.0,   -2.0,   11.0) cm  -> 61.2 mm apart

6. Orientation of the block in base_link, four ways
   rotation matrix rows: [[0.866, 0.5, 0.0], [-0.5, 0.866, 0.0], [0.0, 0.0, 1.0]]
   quaternion (w,x,y,z): (0.9659, 0.0, 0.0, -0.2588)   expected (cos(-15), 0, 0, sin(-15)) = (0.9659, 0, 0, -0.2588)
   -q is the SAME rotation: (-0.9659, 0.0, 0.0, 0.2588)
   axis-angle: about +z by -30.0 deg;  yaw (RPY) = -30.0 deg
   after drift |q| = 1.0030 -> normalise to 1 before use

7. Gimbal lock: R = Rz(yaw) * Ry(pitch) * Rx(roll) with pitch = 90 deg
   roll +10, yaw  +0 -> first row [0.0, 0.174, 0.985], quat (0.704, 0.062, 0.704, -0.062)
   roll  +0, yaw -10 -> first row [0.0, 0.174, 0.985], quat (0.704, 0.062, 0.704, -0.062)
   roll +40, yaw +30 -> first row [0.0, 0.174, 0.985], quat (0.704, 0.062, 0.704, -0.062)
   All three give the SAME rotation: at pitch 90 deg only (roll - yaw) matters. One degree of freedom is lost.

Try it: move and rotate the object and the camera, and watch its pose change in each frame. (Educational explorer, not a calibration tool.)

Interactive · Object pose & frame lab · Educational

Object poses and frames
Pose in {camera}: x=120.0 y=70.0 yaw=25°
cam

Educational frame change stub — not industrial 6D pose / not live TF2 tree.

A 3D diagram of a coordinate frame being turned three times about its own axes, with coloured arcs showing each turn
Euler angles as three turns about the body’s own axes (z, then x, then z). Change the order or the axes and you get a different orientation — one reason robot software prefers quaternions and matrices.Photo: PaulSch · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Bench build: sensor reading → base frame on the UNO

sensor_to_base.ino mounts an HC-SR04 on a small pan servo. A potentiometer sets the pan angle from −60° to +60°. The sensor measures distance d along its own x axis, so the object is at (d, 0, 0) in the sensor frame. The sketch chains T_base_sensor = Trans(5, 0, 10 cm) · Rz(yaw) (example mount) and prints the object position in base_link. Notice the line yaw = yawDeg * PI / 180.0 — the sin/cos functions need radians.

Safety: never power a servo or motor from a GPIO pin or the UNO 5 V pin — a stalled servo can draw far more current than they can give. Keep the sensor cable loose enough for the full ±60° swing, keep fingers away from the horn, and unplug the servo supply before re-wiring.
PartConnectionNote
Potentiometer 10 kΩends → 5V and GND, wiper → A0sets the pan angle
HC-SR04VCC → 5V, GND → GND, TRIG → D7, ECHO → D8mounted on the servo horn, pointing along the horn
Pan servo (SG90-class)signal → D9power from a separate 5–6 V supply
Servo supplyred → +5–6 V, brown → supply GNDsupply GND joined to UNO GND
sensor_to_base.ino · Arduino C++
// sensor_to_base.ino — Stage 13 L03 (AIHUB STATION lab)
// Turn a sensor reading into a point in the robot's BASE frame.
// A potentiometer sets a pan servo angle; an HC-SR04 sits on the servo horn and
// measures distance d along its own x axis. We chain two transforms:
//   T_base_sensor = Trans(MOUNT_X, 0, MOUNT_Z) * Rz(yaw)
//   p_base        = T_base_sensor * (d, 0, 0)
// and print the object position in base_link (x forward, y left, z up), in cm.
//
// Wiring (example): pot ends to 5V/GND, wiper to A0. HC-SR04 TRIG D7, ECHO D8.
//   Servo signal D9; servo power from a SEPARATE 5-6 V supply (never GPIO or the
//   UNO 5V pin), grounds joined.
#include <Servo.h>

const int TRIG = 7, ECHO = 8, SERVO_PIN = 9, POT = A0;
const float MOUNT_X = 5.0, MOUNT_Z = 10.0;     // servo axis: 5 cm in front of base, 10 cm up (example)
const int YAW_MIN = -60, YAW_MAX = 60;          // degrees about base z (left = +)
Servo pan;

float readCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 30000UL);
  return us == 0 ? -1 : us / 58.0;
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  pan.attach(SERVO_PIN);
  Serial.println(F("yaw_deg  d_sensor_cm  ->  base x  y  z (cm)"));
}

void loop() {
  int yawDeg = map(analogRead(POT), 0, 1023, YAW_MIN, YAW_MAX);
  pan.write(90 + yawDeg);                 // servo 90 = straight ahead (calibrate yours)
  delay(150);
  float d = readCm();
  if (d < 2 || d > 200) {                 // trust 2..200 cm only
    Serial.print(yawDeg); Serial.println(F("   no trusted echo"));
    return;
  }
  float yaw = yawDeg * PI / 180.0;        // sin/cos need RADIANS
  // p_sensor = (d, 0, 0)  ->  rotate by Rz(yaw), then translate by the mount offset
  float x = MOUNT_X + cos(yaw) * d;
  float y = 0.0     + sin(yaw) * d;
  float z = MOUNT_Z;
  Serial.print(yawDeg); Serial.print(F("      ")); Serial.print(d, 1);
  Serial.print(F("        ->  ")); Serial.print(x, 1); Serial.print(F("  "));
  Serial.print(y, 1); Serial.print(F("  ")); Serial.println(z, 1);
}
Serial Monitor (115200) — example readings, your values will differ · Text
yaw_deg  d_sensor_cm  ->  base x  y  z (cm)
0      20.0        ->  25.0  0.0  10.0
30      20.0        ->  22.3  10.0  10.0
-45      15.0        ->  15.6  -10.6  10.0
60   no trusted echo

Debugging

SymptomLikely causeFix
Object appears mirrored (left/right swapped)a left-handed frame or a wrong axis signCheck det(R) = +1; test with one known point
Error grows with distance from the camerarotation part of T_base_cam slightly wrongRe-calibrate; check with points at the image edges
Position is right, gripper is turned wrongorientation chained in the wrong order, or yaw sign flipped by the camera frameWrite T_A_B names; check inner names match
Approach comes from the side, not from abovepre-grasp offset applied in the base frameMultiply the offset on the right (tool frame)
Angles jump by huge amountsdegrees passed to sin/cosConvert to radians; keep radians inside the code
Orientation slowly drifts after many updatesquaternion not normalisedNormalise after every multiplication
Bench: base x, y change when you only turn the potmount offset or servo zero wrongMeasure MOUNT_X; set the servo so 90 = straight ahead

Real world

Frames in the real world

  • ROS robots: tf2 keeps all frames of a robot in a tree, buffered in time, so any node can ask “where was the block in base_link at time t?” without knowing the chain in between.
  • Industrial arms: you program the tool centre point (TCP) as a frame on the gripper, and teach work-object frames on fixtures, so a program still works when the fixture is moved.
  • Phones and drones: inertial sensors integrate rotation with quaternions instead of gimbals — the strapdown approach that avoids gimbal lock.
  • Your lab: write down base_link, camera_optical, gripper and block for your desk arm, with real measured offsets, before you write any picking code.

Think

Think about it

Think

Your camera is on the robot’s wrist instead of above the desk. Which transforms in the pick chain now change every time the arm moves, and what extra information do you need at the exact moment the image was taken?

Experiment

Try it in the lab

About 60 minutes. Python 3 for the script; Arduino UNO, HC-SR04, a small servo with its own 5–6 V supply and a 10 kΩ potentiometer for the bench; a ruler.

Experiment

Move one block through every frame

You will need

  • `pose_frames_lab.py` (Python 3, no libraries)
  • Object-pose frame lab (in your browser)
  • Arduino UNO, HC-SR04, SG90-class servo + separate 5–6 V supply, 10 kΩ potentiometer, jumper wires, a box, a ruler

Steps

  1. Run `pose_frames_lab.py`. Check the block position in base by hand: x = 15 + 5, y = 0 − 2, z = 60 − 57.
  2. Change the detector yaw from +30° to +90°. What is the yaw in base? Explain the sign.
  3. Change the pre-grasp distance to 5 cm and 12 cm. Where does each pre-grasp land?
  4. In mistake 5e, try tilts of 15°, 30° and 60°. Plot how far apart the two pre-grasps are.
  5. Write a function that turns a quaternion back into a rotation matrix and check it on the block orientation.
  6. Bench: put a box at a measured spot (for example 25 cm ahead). Read base x, y at yaw −30°, 0°, +30° of the pot. How much do they vary?
  7. Bench: deliberately set MOUNT_X wrong by 3 cm. Which printed number changes, and by how much?

Interactive · Object pose & frame lab · Educational

Object poses and frames
Pose in {camera}: x=120.0 y=70.0 yaw=25°
cam

Educational frame change stub — not industrial 6D pose / not live TF2 tree.

A transform is just a careful way of saying “this point, seen from there”. When the frame names are written down and the inner names match, the maths almost writes itself.

Knowledge check

Check your understanding

Knowledge check

1. A pose is…

2. In REP 103, a body frame uses…

3. Which chain gives the block in the base frame?

4. A rotation matrix has determinant −1. This means…

5. The quaternion for −30° about z is about…

6. To back off 8 cm along the gripper’s own approach axis you compute…

7. Gimbal lock means…

8. In the bench sketch, why multiply yawDeg by PI / 180?

Mini assignment

Hands-on assignment

Mini assignment

A frame map for your desk arm

Draw, measure and test every frame your desk arm needs for one pick.

Deliverables

  • A labelled drawing of `base_link`, `camera_optical`, `gripper` and `block` frames with axis directions, following REP 103.
  • Your measured T_base_cam (position in cm and orientation as a quaternion), clearly marked as measured or estimated.
  • A table of three real points: ruler position vs the position your code computes, with the error in mm.
  • A bench log from `sensor_to_base.ino` for one fixed box seen at three different pan angles.

Do this, step by step

  1. Run pose_frames_lab.py and read the error lines for each mistake it shows.
  2. Draw base_link, camera_optical, gripper and block with axes as in REP 103 (x forward, y left, z up).
  3. Measure your T_base_cam with a ruler and write it as position (cm) plus quaternion.
  4. Check three ruler points against your computed points and write the error in mm.
  5. Bench: set MOUNT_X and MOUNT_Z to your servo mount, upload sensor_to_base.ino and log one fixed box at three pan angles.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. If the error looks like the point was mirrored, you probably multiplied in the wrong order. Moving along the gripper’s own axis means multiplying on the right.
  2. Check det3(R) is 1. If it is −1, one of your axes is flipped (left-handed frame).
  3. In the sketch, sin() and cos() want radians, so multiply yawDeg by PI / 180 before using it.

Challenges (pick by level)

  • 🟢 Beginner: By hand, move the point (2, 0, 0) cm through Rz(90°) and then Trans(10, 0, 0) cm. Then do it in the other order. Explain the difference.
  • 🟡 Intermediate: Add a camera_link → camera_optical step to the script (body frame to optical frame) and check you get the same block pose.
  • 🟠 Advanced: Add a tilt servo to the bench build, chain Rz(yaw) · Ry(pitch), and print full 3D positions.
  • 🔴 Expert: With an OpenCV ArUco marker, measure T_cam_marker from a real webcam and compute the marker in your arm’s base frame.
  • ⚫ Research: Read about hand–eye calibration (AX = XB). Write one page on how you would calibrate a wrist camera on your desk arm.

Next step

Now every object has a proper pose. But the planner also needs to know the space objects take up. In Lesson 4 — Occupancy & Collision Representation you will turn objects into boxes, spheres and voxels that a planner can check collisions against.

Look back: Lesson 2: Robot World Models. Next: Lesson 4: Occupancy & Collision Representation. Related: Stage 8 — Kinematics & IK.

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