Skip to content
LumenaryAcademy
Motion 60 min

Frames and rigid transforms

Where is that, and according to whom? Points, frames and the 3×3 and 4×4 matrices that carry coordinates from a sensor to a robot to the world.

Code tested · 5AI-draftedAwaiting review by a roboticist

Your guides: Chloe · The Builder, Margaret · The Experimenter, Gideon · The Safety Engineer

MotivationH

Where is that cup?

A delivery robot rolls down a hospital ward in Nairobi. Its lidar, bolted to the right side of its body, reports an echo 2.0 m away and 10° to the left of the lidar's own forward direction. The ward's map, drawn by a surveyor, has its origin in the corner by the nurses' station. The robot's planner wants to know: where on the map is the thing that reflected?

Three different observers are describing the same physical point. The lidar knows the point relative to itself. The robot's software knows where the lidar is mounted on the body. The localisation system knows where the body is in the ward. None of them alone can answer the question; chained together, they can, in two matrix multiplications.

That chain is the backbone of every robot's software. A humanoid has dozens of links, each with its own frame; a self-driving car has a dozen cameras, lidars and radars, each calibrated against the vehicle body; an arm picking parcels must turn "the parcel's corner, as the wrist camera sees it" into "joint angles that put the gripper there". Get the chain right and everything downstream works. Get one multiplication in the wrong order and the robot reaches for a point five metres from the cup, as you will see below.

This lesson builds the tools: frames, rotation matrices, homogeneous transforms, and the rules for composing and inverting them. They are linear algebra you already know (Lumenary Math's Linear algebra and Vectors and coordinate geometry), used with a discipline about which frame every number belongs to.

IntuitionH

Points, vectors and frames

A point is a place: the tip of the gripper, a corner of the doorway, the centre of the robot's wheel axle. A vector, in the sense we need, is a displacement or a direction: "0.3 m forward", the robot's velocity, the direction gravity pulls. The difference matters the moment we change viewpoint. If you walk to the other side of the room, every point has new coordinates relative to you, but "the direction gravity pulls" has not moved at all, and "1 m to the north" is still 1 m to the north. Points care about where the observer stands; free vectors care only about which way the observer faces.

A frame is an observer made precise: an origin, plus a set of mutually perpendicular unit axes. In the plane that is two axes, xx and yy; in space, three. Coordinates are meaningless without a frame. "The cup is at (0.8,0.1)(0.8, 0.1)" says nothing until we know whether that is measured from the camera, the robot or the corner of the room.

Analogy, not a formal construction

Giving directions in a city is giving coordinates in a frame. "Two blocks ahead, then left" is in your frame: it depends on where you are standing and which way you face. "Corner of Kenyatta Avenue and Moi Avenue" is in the city's frame and means the same to everyone. A taxi app does exactly what a robot does: it converts between the two, using your phone's position and heading.

x_Wy_WWx_By_BBtp30°
The world frame W and the robot's body frame B. B's origin sits at (2, 1) in W and its axes are turned 30° anticlockwise. The point p is 1 m ahead of the robot and 0.5 m to its left; in W it is at about (2.62, 1.93). The arrow t is the translation part of the transform: B's origin written in W's coordinates.
DefinitionsH

Notation and conventions

Robotics has no single notation, and mismatched conventions cause real bugs. This course fixes one set and keeps it in every lesson.

Note

Conventions used throughout Lumenary Robotics.

  • Coordinates are column vectors, and matrices act on the left: y=Mx\mathbf{y} = M\mathbf{x}.
  • Ap{}^{A}\mathbf{p} means the coordinates of the point pp in frame AA.
  • ARB{}^{A}R_{B} is the orientation of frame B expressed in frame A: its columns are B's axes written in A's coordinates.
  • ATB{}^{A}T_{B} is the pose of frame B expressed in frame A. It maps B-coordinates to A-coordinates: Ap=ATB Bp{}^{A}\mathbf{p} = {}^{A}T_{B}\,{}^{B}\mathbf{p}.
  • Frames are right-handed; angles are in radians in code and positive anticlockwise (about +z+z); lengths are in metres.
  • For a mobile robot, the body frame has xx forward, yy to the left and zz up, as in ROS's REP 103.
  • Quaternions (next lesson) are in Hamilton's convention, written (w,x,y,z)(w, x, y, z); ROS and several libraries store them as (x,y,z,w)(x, y, z, w).

The superscript-subscript placement is the whole trick. In ATB Bp{}^{A}T_{B}\,{}^{B}\mathbf{p} the inner labels match (BB next to BB) and "cancel", leaving Ap{}^{A}\mathbf{p}. You will lean on that rule constantly.

Definition— 2-D rotation matrix

The rotation by angle θ\theta in the plane is

R(θ)=(cos⁡θ−sin⁡θsin⁡θcos⁡θ).R(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix}.

If frame B is turned by θ\theta relative to frame A (with the same origin), then ARB=R(θ){}^{A}R_{B} = R(\theta). Its first column, (cos⁡θ,sin⁡θ)(\cos\theta, \sin\theta), is B's xx axis written in A; its second column, (−sin⁡θ,cos⁡θ)(-\sin\theta, \cos\theta), is B's yy axis.

Why does this matrix convert B-coordinates to A-coordinates? A point with B-coordinates (u,v)(u, v) is uu steps along B's xx axis plus vv steps along B's yy axis. Writing those axes in A's coordinates and adding gives u (cos⁡θ,sin⁡θ)+v (−sin⁡θ,cos⁡θ)u\,(\cos\theta, \sin\theta) + v\,(-\sin\theta, \cos\theta), which is exactly R(θ) (u,v)TR(\theta)\,(u, v)^{\mathsf T}. A matrix times a vector is a weighted sum of its columns, and the columns are the new axes.

Three properties follow directly and are used every day:

  • Orthonormal columns, so RTR=IR^{\mathsf T}R = I and therefore R−1=RTR^{-1} = R^{\mathsf T}. Inverting a rotation is free: transpose it.
  • det⁡R=cos⁡2θ+sin⁡2θ=1\det R = \cos^2\theta + \sin^2\theta = 1. A determinant of −1-1 would be a reflection, which no motion of a rigid body can produce.
  • R(α)R(β)=R(α+β)R(\alpha)R(\beta) = R(\alpha + \beta): in the plane, rotations add (and so commute). This is the angle-addition formula of trigonometry, in matrix form. In 3-D the commuting stops, as the next lesson shows.

The same matrix has two readings. Passively, ARB{}^{A}R_{B} re-expresses a fixed point in a different frame. Actively, R(θ)R(\theta) takes a vector and turns it by θ\theta within one frame. Both are correct; trouble starts only when one person thinks actively and the code was written passively. In this course, a matrix with frame labels on it is always the passive reading.

Definition— Homogeneous transform

A rigid transform in the plane is a rotation followed by a translation: Ap=ARB Bp+AtB{}^{A}\mathbf{p} = {}^{A}R_{B}\,{}^{B}\mathbf{p} + {}^{A}\mathbf{t}_{B}, where AtB{}^{A}\mathbf{t}_{B} is the position of B's origin in A. Appending a 1 to every point lets one matrix do both:

(Ap1)=(ARBAtB0T1)⏟ATB(Bp1).\begin{pmatrix} {}^{A}\mathbf{p} \\ 1 \end{pmatrix} = \underbrace{\begin{pmatrix} {}^{A}R_{B} & {}^{A}\mathbf{t}_{B} \\ \mathbf{0}^{\mathsf T} & 1 \end{pmatrix}}_{{}^{A}T_{B}} \begin{pmatrix} {}^{B}\mathbf{p} \\ 1 \end{pmatrix}.

ATB{}^{A}T_{B} is 3×33\times3 in the plane and 4×44\times4 in space. Points get a final coordinate w=1w = 1; free vectors get w=0w = 0, so the translation column is multiplied by zero and ignored, exactly as a displacement should be.

The extra coordinate comes from projective geometry (Lumenary Math's Transformation geometry shows it as the trick that turns the affine maps into matrices). For robotics it buys two things: a whole chain of rotations and translations becomes a single matrix product, and the same code handles points and directions, told apart by one number.

ComputationC

Rotations and transforms in NumPy

Here are the planar tools as a few lines of NumPy. The inverse uses the transpose, and the code checks it against a general matrix inverse.

python
import numpy as np

def rot2(theta):
    """2-D rotation matrix for an angle theta in radians (anticlockwise positive)."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s],
                     [s,  c]])

def se2(x, y, theta):
    """3x3 homogeneous transform: rotate by theta, then translate by (x, y)."""
    T = np.eye(3)
    T[:2, :2] = rot2(theta)
    T[:2, 2] = [x, y]
    return T

def inv_se2(T):
    """Inverse of a rigid transform, without a general matrix inverse."""
    R, t = T[:2, :2], T[:2, 2]
    Ti = np.eye(3)
    Ti[:2, :2] = R.T
    Ti[:2, 2] = -R.T @ t
    return Ti

def point(x, y):  return np.array([x, y, 1.0])    # w = 1: translations apply
def vector(x, y): return np.array([x, y, 0.0])    # w = 0: translations ignored

R = rot2(np.radians(30))
print("R(30 deg) =\n", R.round(4))
print("R^T R = I:", np.allclose(R.T @ R, np.eye(2)), "  det R =", round(float(np.linalg.det(R)), 12))
print("R(30) R(45) = R(75):", np.allclose(rot2(np.radians(30)) @ rot2(np.radians(45)), rot2(np.radians(75))))

W_T_B = se2(2.0, 1.0, np.radians(30))     # pose of the robot's body frame B in the world frame W
p_B = point(1.0, 0.5)                      # a point 1 m ahead and 0.5 m to the left of the robot
v_B = vector(1.0, 0.5)                     # the displacement with the same coordinates
print("the point  in W:", (W_T_B @ p_B).round(4))
print("the vector in W:", (W_T_B @ v_B).round(4))
print("back again in B:", (inv_se2(W_T_B) @ W_T_B @ p_B).round(12))
print("inv_se2 agrees with np.linalg.inv:", np.allclose(inv_se2(W_T_B), np.linalg.inv(W_T_B)))
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The point lands at (2.616,1.933)(2.616, 1.933), the spot marked pp in the figure. The vector with the same coordinates becomes (0.616,0.933)(0.616, 0.933): it was turned by 30°, but not moved, because a displacement has no position to move.

Composing and inverting

Suppose we know ATB{}^{A}T_{B} (where B is, seen from A) and BTC{}^{B}T_{C} (where C is, seen from B). A point known in C goes to B and then to A:

Ap=ATB (BTC Cp)=(ATB BTC) Cp,soATC=ATB BTC.{}^{A}\mathbf{p} = {}^{A}T_{B}\,\big({}^{B}T_{C}\,{}^{C}\mathbf{p}\big) = \big({}^{A}T_{B}\,{}^{B}T_{C}\big)\,{}^{C}\mathbf{p}, \qquad\text{so}\qquad {}^{A}T_{C} = {}^{A}T_{B}\,{}^{B}T_{C}.

The labels chain like dominoes: A←BA \leftarrow B, then B←CB \leftarrow C. Reading the product from left to right walks outwards from the world towards the sensor; the point travels the other way, from the right-hand end to the left.

Theorem— Inverse of a rigid transform

If ATB=(Rt0T1){}^{A}T_{B} = \begin{pmatrix} R & \mathbf{t} \\ \mathbf{0}^{\mathsf T} & 1 \end{pmatrix} with RR a rotation, then

BTA=(ATB)−1=(RT−RTt0T1).{}^{B}T_{A} = \big({}^{A}T_{B}\big)^{-1} = \begin{pmatrix} R^{\mathsf T} & -R^{\mathsf T}\mathbf{t} \\ \mathbf{0}^{\mathsf T} & 1 \end{pmatrix}.

Proof

Solve Ap=R Bp+t{}^{A}\mathbf{p} = R\,{}^{B}\mathbf{p} + \mathbf{t} for Bp{}^{B}\mathbf{p}. Subtract t\mathbf{t} and multiply on the left by RTR^{\mathsf T}, using RTR=IR^{\mathsf T}R = I: Bp=RT Ap−RTt{}^{B}\mathbf{p} = R^{\mathsf T}\,{}^{A}\mathbf{p} - R^{\mathsf T}\mathbf{t}. That is the stated matrix acting on (Ap,1)({}^{A}\mathbf{p}, 1). Multiplying the two block matrices confirms the product is II: the top-left block is RRT=IR R^{\mathsf T} = I and the top-right is R(−RTt)+t=0R(-R^{\mathsf T}\mathbf{t}) + \mathbf{t} = \mathbf{0}.

∎

The translation of the inverse is not −t-\mathbf{t}. It is −t-\mathbf{t} rotated into B's axes. In words: "where is A's origin, seen from B?" The answer is "back along the vector t\mathbf{t}", and that vector must be expressed in B's own axes.

Worked examplesH

A robot, its sensor and the world

Back to the ward. Put numbers on the three frames:

  • W, the world (map) frame, in the corner of the ward.
  • B, the robot's body frame, at (3.0,2.0)(3.0, 2.0) in W and facing W's +y+y direction (heading 90°90°).
  • L, the lidar frame. The lidar sits 0.150.15 m to the robot's right and looks rightwards, so BTL{}^{B}T_{L} has translation (0,−0.15)(0, -0.15) and rotation −90°-90°.

The lidar reports range r=2.0r = 2.0 m at bearing β=10°\beta = 10°, which in its own frame is the point Lp=(rcos⁡β, rsin⁡β){}^{L}\mathbf{p} = (r\cos\beta,\ r\sin\beta). The world position is Wp=WTB BTL Lp{}^{W}\mathbf{p} = {}^{W}T_{B}\,{}^{B}T_{L}\,{}^{L}\mathbf{p}. The code also tries the two classic mistakes.

python
import numpy as np

def se2(x, y, theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, x],
                     [s,  c, y],
                     [0,  0, 1.0]])

def inv_se2(T):
    R, t = T[:2, :2], T[:2, 2]
    Ti = np.eye(3)
    Ti[:2, :2], Ti[:2, 2] = R.T, -R.T @ t
    return Ti

deg = np.radians
W_T_B = se2(3.0, 2.0, deg(90))       # the robot: at (3, 2) in the ward, facing the world's +y
B_T_L = se2(0.0, -0.15, deg(-90))    # a lidar on the robot's right side, looking right

# The lidar reports a wall reflection 2.0 m away, 10 degrees anticlockwise of its own x axis.
r, bearing = 2.0, deg(10)
p_L = np.array([r * np.cos(bearing), r * np.sin(bearing), 1.0])

right = W_T_B @ B_T_L @ p_L          # inner labels match: W<-B, B<-L
wrong_order = B_T_L @ W_T_B @ p_L    # the classic mistake
no_mount = W_T_B @ p_L               # forgetting the sensor's mounting
for name, p in [("W_T_B @ B_T_L @ p", right), ("B_T_L @ W_T_B @ p", wrong_order), ("W_T_B @ p (no mount)", no_mount)]:
    err = np.linalg.norm(p[:2] - right[:2])
    print(f"{name:22} -> ({p[0]:6.3f}, {p[1]:6.3f})   off by {err:.3f} m")

W_T_L = W_T_B @ B_T_L
print("pose of the lidar in W: x = %.3f, y = %.3f, heading = %.1f deg"
      % (W_T_L[0, 2], W_T_L[1, 2], np.degrees(np.arctan2(W_T_L[1, 0], W_T_L[0, 0]))))

# The other direction: the charging dock is at (5, 4) in W. Where is it for the robot?
dock_W = np.array([5.0, 4.0, 1.0])
dock_B = inv_se2(W_T_B) @ dock_W
print("dock in the robot's frame:", dock_B[:2].round(3), "(x ahead, y to the left)")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The correct chain puts the echo at (5.120,2.347)(5.120, 2.347) on the map. Check it by eye: the robot faces +y+y, so its right-hand side faces +x+x; the lidar sits at (3.15,2.0)(3.15, 2.0) looking along +x+x, and an echo 2 m away, slightly anticlockwise of straight ahead, is at about (3.15+1.97, 2.0+0.35)(3.15 + 1.97,\ 2.0 + 0.35).

Swapping the two matrices gives (3.970,−2.803)(3.970, -2.803), more than five metres away and outside the ward. Forgetting the mounting gives an error of almost three metres, because it treats the lidar as if it looked forward instead of sideways. Neither mistake raises an error: the matrices have the right shapes, the program runs, and the robot confidently maps a wall that is not there. This is why careful teams name their variables W_T_B, B_T_L and check that adjacent labels match before they multiply.

The last line runs the chain backwards. The dock at (5,4)(5, 4) is 2 m ahead and 2 m to the robot's right, so Bp=(2,−2){}^{B}\mathbf{p} = (2, -2): the inverse transform answers "where is that, from where I stand?", which is what a controller steering towards the dock needs.

ComputationC

Order matters: moving in my frame or the world's

Matrix products do not commute, and the two orders have two physical meanings. Let a robot's pose be WTB{}^{W}T_{B} and let Δ\Delta be a small motion.

  • WTB Δ{}^{W}T_{B}\,\Delta (multiply on the right) applies Δ\Delta in the robot's own frame: "drive 1 m forward, then turn left". This is how odometry updates a pose, as you will see in Wheeled robots and odometry.
  • Δ WTB\Delta\,{}^{W}T_{B} (multiply on the left) applies Δ\Delta in the world frame: the whole pose is rotated about the world's origin and shifted along the world's axes.

The program below gives two robots the same step four times, one composed on each side, and then tries the same bookkeeping in 3-D with a camera.

python
import numpy as np

def se2(x, y, theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def pose(T):
    x, y = round(T[0, 2], 2) + 0.0, round(T[1, 2], 2) + 0.0      # + 0.0 turns -0.0 into 0.0
    heading = round(np.degrees(np.arctan2(T[1, 0], T[0, 0])), 1) + 0.0
    return f"x={x:6.2f}  y={y:6.2f}  heading={heading:6.1f}"

step = se2(1.0, 0.0, np.radians(90))       # drive 1 m forward, then turn 90 degrees left
body = world = se2(2.0, 0.0, 0.0)          # both robots start at (2, 0) facing +x
for k in range(1, 5):
    body = body @ step                     # right-multiply: the step is in the robot's own frame
    world = step @ world                   # left-multiply: the step is applied in the world frame
    print(f"step {k}:  body frame  {pose(body)}  |  world frame  {pose(world)}")

# In 3-D the same rules hold with 4x4 matrices. A camera's "optical" frame has z forward,
# x right and y down; the robot body frame has x forward, y left and z up.
B_R_C = np.array([[0, 0, 1],               # columns: the camera's x, y, z axes written in B
                  [-1, 0, 0],
                  [0, -1, 0]], dtype=float)
B_T_C = np.eye(4)
B_T_C[:3, :3], B_T_C[:3, 3] = B_R_C, [0.20, 0.0, 0.45]   # camera 20 cm ahead, 45 cm up
p_C = np.array([0.1, -0.05, 2.0, 1.0])     # seen 2 m in front, a little right and up
print("point in the body frame:", (B_T_C @ p_C)[:3].round(3))
print("det of B_R_C:", round(float(np.linalg.det(B_R_C)), 6), " B_R_C^T B_R_C = I:", np.allclose(B_R_C.T @ B_R_C, np.eye(3)))
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The body-frame robot traces a 1 m square, (3,0)→(3,1)→(2,1)→(2,0)(3,0) \to (3,1) \to (2,1) \to (2,0), as a robot obeying "forward, turn left" should. The world-frame robot visits (1,2)(1, 2), (−1,1)(-1, 1) and (0,−1)(0, -1): each step swung its whole pose about a point near the world's origin. Both return home after four steps, because Δ4=I\Delta^4 = I, but only one of them drove the route you meant.

The camera example is the most common 3-D trap. Cameras follow the image convention (zz out of the lens, xx to the right of the image, yy down), while robot bodies follow xx forward, yy left, zz up. The rotation between them has no angle you would guess by eye, but its columns are easy to write down: the camera's xx (right) is the body's −y-y; its yy (down) is the body's −z-z; its zz (forward) is the body's +x+x. A point 2 m in front of the lens, 0.1 m right and 0.05 m up, ends up at (2.2,−0.1,0.5)(2.2, -0.1, 0.5) in the body frame. In 3-D, ATB{}^{A}T_{B} is 4×44\times4 with a 3×33\times3 rotation block, and everything else in this lesson carries over unchanged. The next lesson is about that 3×33\times3 block.

DefinitionsH

SE(2) and SE(3)

The set of all rigid transforms has the structure that mathematicians call a group: composing two rigid transforms gives a rigid transform, the identity matrix does nothing, every transform has an inverse (the theorem above), and composition is associative because matrix multiplication is.

Definition— SO(n) and SE(n)

The special orthogonal group SO(n)SO(n) is the set of n×nn\times n real matrices RR with RTR=IR^{\mathsf T}R = I and det⁡R=1\det R = 1: the rotations of nn-dimensional space. The special Euclidean group SE(n)SE(n) is the set of (n+1)×(n+1)(n+1)\times(n+1) matrices (Rt0T1)\begin{pmatrix} R & \mathbf{t} \\ \mathbf{0}^{\mathsf T} & 1 \end{pmatrix} with R∈SO(n)R \in SO(n) and t∈Rn\mathbf{t} \in \mathbb{R}^n: the rigid motions, or poses.

SE(2)SE(2) has three degrees of freedom (xx, yy, heading): the pose of a robot on a floor. SE(3)SE(3) has six (three of position, three of orientation): the pose of a drone, a gripper or a camera. A 4×44\times4 transform stores 16 numbers, but the bottom row is fixed and the nine rotation entries obey six constraints (three unit-length columns, three perpendicular pairs), leaving 12−6=612 - 6 = 6 free.

Neither group is commutative: ATB BTC{}^{A}T_{B}\,{}^{B}T_{C} and BTC ATB{}^{B}T_{C}\,{}^{A}T_{B} differ, as the ward example showed by more than five metres. They are also smooth, curved spaces rather than flat ones, which is why you cannot average two poses by averaging their matrices. Both facts make SE(3)SE(3) a Lie group, and modern estimation and optimisation in robotics (the subject of later lessons on Kalman filters and SLAM) is built on exploiting that structure. Lumenary Math's Matrix groups and continuous symmetry develops it properly.

ExplanationH

The professional view: a tree of frames

Real robots have many frames, and they change at different rates. A mobile manipulator might have a map frame, an odometry frame, the base, a lidar, two cameras (each with a mounting frame and an optical frame), an IMU, and one frame per arm link. Some transforms never change (a camera bolted to the base); some change hundreds of times a second (the arm's joints); some jump occasionally (localisation correcting the robot's position on the map).

ROS 2, the open-source framework most research robots and many commercial ones use, handles this with tf2. Every frame has exactly one parent, so the frames form a tree. Each edge stores the transform from parent to child, stamped with a time. Nodes broadcast edges as they learn them; any node can ask a buffer for the transform between any two frames at any time, and tf2 walks up from both frames to their common ancestor, multiplies the edges along the way, inverts where needed, and interpolates between timestamps.

mapodombase_linklaserimu_linkcamera_linkcamera_opticallocalisation: may jumpodometry: smooth, driftsedges below base_link: fixed by the design (URDF)
A typical tf2 tree for a wheeled robot, following ROS's REP 105. Each arrow is one parent-to-child transform. The map-to-odom edge comes from localisation and can jump; odom-to-base_link comes from odometry and is smooth but drifts; the edges below base_link are fixed by the robot's mechanical design.

ROS's REP 105 explains why there are two frames above the base. Odometry is smooth but drifts; localisation against a map is accurate in the long run but corrects itself in jumps. A controller following a corridor wants the smooth odom frame; a planner going to a room on the map wants map. Rather than give base_link two parents, which a tree forbids, the localiser publishes the correction map → odom, computed as mapTodom=mapTbase (odomTbase)−1{}^{map}T_{odom} = {}^{map}T_{base}\,\big({}^{odom}T_{base}\big)^{-1} — exactly the composition and inversion of this lesson.

Asking tf2 a question looks like this. The method name reads naturally in our notation: lookup_transform(target, source, time) returns targetTsource{}^{target}T_{source}, the pose of the source frame expressed in the target frame.

python
# ROS 2 (rclpy) code: shown for reading, not run here (it needs a ROS 2 installation).
import rclpy
from rclpy.node import Node
from rclpy.time import Time
from tf2_ros import Buffer, TransformListener

class WhereIsTheCamera(Node):
    def __init__(self):
        super().__init__("where_is_the_camera")
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)   # fills the buffer from /tf
        self.create_timer(1.0, self.report)

    def report(self):
        try:
            # The pose of camera_link expressed in map: map_T_camera_link.
            t = self.buffer.lookup_transform("map", "camera_link", Time())   # Time() = latest
        except Exception as err:          # frames not yet known, or the tree is broken
            self.get_logger().warn(str(err))
            return
        p, q = t.transform.translation, t.transform.rotation   # q is stored as x, y, z, w
        self.get_logger().info(f"camera at ({p.x:.2f}, {p.y:.2f}, {p.z:.2f}) in map")

rclpy.init()
rclpy.spin(WhereIsTheCamera())

The fixed edges usually come from the robot's description file (URDF, met in Forward kinematics), published by robot_state_publisher; a one-off fixed edge can be published from the command line with ros2 run tf2_ros static_transform_publisher, and ros2 run tf2_tools view_frames draws the current tree. The tf2 tutorials walk through all of this, and Tully Foote's paper in the references explains the design. Other toolkits use the same ideas with different spellings: Drake writes the pose of B in A as X_AB, and Lynch and Park's Modern Robotics writes TsbT_{sb} for the body in the space frame. Learn to translate notations; never mix them in one file.

Common misconceptionsH

Common misconceptions

Misconception

"To invert a transform, negate the translation." That works only when the rotation is the identity. The inverse of (R,t)(R, \mathbf{t}) is (RT,−RTt)(R^{\mathsf T}, -R^{\mathsf T}\mathbf{t}); the translation must be re-expressed in the other frame's axes. In the ward example, the world origin seen from the robot is (−2,3)(-2, 3), not (−3,−2)(-3, -2).

Misconception

"Points and vectors transform the same way." A velocity, a force direction or a surface normal must not be translated. In homogeneous coordinates give them w=0w = 0, or apply only the rotation block. Translating a velocity vector is a quiet bug: it changes the vector's length and direction by an amount that depends on where the robot is.

Misconception

"The transform from A to B" is unambiguous. It is not. Some authors mean the map that takes A-coordinates to B-coordinates; others mean the pose of B in A, which does the opposite. These are inverses of each other. Avoid the phrase; say "the pose of B in A", write ATB{}^{A}T_{B}, and check that adjacent labels match.

Misconception

"If the code runs, the chain is right." Every 3×33\times3 (or 4×44\times4) product has the right shape, whatever the order. Only the labels, or a test against a hand-checked case like the one above, catch an order error.

History & global context

A short history

Coordinates go back to Descartes and Fermat in the 17th century. Homogeneous coordinates were introduced around 1827–1830 by August Möbius and Julius Plücker for projective geometry, a century before anyone needed them for machines. In 1955 Jacques Denavit and Richard Hartenberg used 4×44\times4 matrices to describe the links of a mechanism, the convention you will meet in Forward kinematics, and through the 1970s and 1980s robotics textbooks made homogeneous transforms the standard language of manipulators. Software caught up later: the ROS tf library, developed at Willow Garage from about 2008 and later described by its lead developer Tully Foote, made "ask for any frame in any other frame, at any time" a standard service, and its successor tf2 is part of every ROS 2 installation.

Exercises

Exercises

1. Frames and rigid transforms

Warm-upHuman

In homogeneous coordinates, which of these quantities should be written with last coordinate w=1w = 1, so that the translation part of a transform applies to them? Select all that apply.

Choose all that apply

2. Frames and rigid transforms

CoreHuman

A robot's body frame B has pose WTB{}^{W}T_{B} with heading 90°90° and translation (4,1)(4, 1) in the world frame W. A point has body coordinates Bp=(2,3){}^{B}\mathbf{p} = (2, 3): 2 m ahead and 3 m to the robot's left. What are its world coordinates? Answer as x, y.

3. Frames and rigid transforms

CoreHuman

You know WTB{}^{W}T_{B}, the robot's pose in the world, and BTC{}^{B}T_{C}, the camera's pose on the robot. Which expression is WTC{}^{W}T_{C}, the camera's pose in the world?

Choose one

4. Frames and rigid transforms

CoreHuman

A robot's pose in the world is WTB{}^{W}T_{B} with heading 90°90° and translation (3,2)(3, 2), as in the lesson's ward example. Compute BTW=(WTB)−1{}^{B}T_{W} = \big({}^{W}T_{B}\big)^{-1}. What is the yy component of its translation, that is, the yy coordinate of the world origin as seen from the robot?

5. Frames and rigid transforms

Warm-upHuman

A 4×44\times4 homogeneous transform in SE(3)SE(3) has 16 entries. How many of them can be chosen independently? (That is, how many degrees of freedom does a rigid body in space have?)

6. Frames and rigid transforms

CoreComputational

Write scan_to_world(W_T_B, B_T_L, ranges, bearings) that converts a lidar scan to world coordinates. W_T_B and B_T_L are 3×33\times3 homogeneous transforms (the robot in the world, the lidar on the robot); ranges and bearings are equal-length lists or arrays, bearings in radians measured anticlockwise from the lidar's xx axis. Return an array of shape (N, 2) with one world point (x, y) per reading. The helper se2(x, y, theta) is provided. Try to do it without a Python loop.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

7. Frames and rigid transforms

StretchComputational

Two delivery robots, Amani (frame A) and Bao (frame B), meet in a corridor to hand over a parcel. Both are localised on the same map, so you know WTA{}^{W}T_{A} and WTB{}^{W}T_{B}. Write relative_pose(W_T_A, W_T_B) that returns Bao's pose as seen from Amani, as a tuple (x, y, theta) with theta in (−π,π](-\pi, \pi]. Use the rigid-transform inverse from the lesson rather than np.linalg.inv.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Further reading and resources