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.
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, and ; in space, three. Coordinates are meaningless without a frame. "The cup is at " 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.
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: .
- means the coordinates of the point in frame .
- is the orientation of frame B expressed in frame A: its columns are B's axes written in A's coordinates.
- is the pose of frame B expressed in frame A. It maps B-coordinates to A-coordinates: .
- Frames are right-handed; angles are in radians in code and positive anticlockwise (about ); lengths are in metres.
- For a mobile robot, the body frame has forward, to the left and up, as in ROS's REP 103.
- Quaternions (next lesson) are in Hamilton's convention, written ; ROS and several libraries store them as .
The superscript-subscript placement is the whole trick. In the inner labels match ( next to ) and "cancel", leaving . You will lean on that rule constantly.
Definition— 2-D rotation matrix
The rotation by angle in the plane is
If frame B is turned by relative to frame A (with the same origin), then . Its first column, , is B's axis written in A; its second column, , is B's axis.
Why does this matrix convert B-coordinates to A-coordinates? A point with B-coordinates is steps along B's axis plus steps along B's axis. Writing those axes in A's coordinates and adding gives , which is exactly . 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 and therefore . Inverting a rotation is free: transpose it.
- . A determinant of would be a reflection, which no motion of a rigid body can produce.
- : 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, re-expresses a fixed point in a different frame. Actively, takes a vector and turns it by 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: , where is the position of B's origin in A. Appending a 1 to every point lets one matrix do both:
is in the plane and in space. Points get a final coordinate ; free vectors get , 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.
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.
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)))The point lands at , the spot marked in the figure. The vector with the same coordinates becomes : it was turned by 30°, but not moved, because a displacement has no position to move.
Composing and inverting
Suppose we know (where B is, seen from A) and (where C is, seen from B). A point known in C goes to B and then to A:
The labels chain like dominoes: , then . 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 with a rotation, then
Proof
Solve for . Subtract and multiply on the left by , using : . That is the stated matrix acting on . Multiplying the two block matrices confirms the product is : the top-left block is and the top-right is .
∎
The translation of the inverse is not . It is rotated into B's axes. In words: "where is A's origin, seen from B?" The answer is "back along the vector ", and that vector must be expressed in B's own axes.
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 in W and facing W's direction (heading ).
- L, the lidar frame. The lidar sits m to the robot's right and looks rightwards, so has translation and rotation .
The lidar reports range m at bearing , which in its own frame is the point . The world position is . The code also tries the two classic mistakes.
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)")The correct chain puts the echo at on the map. Check it by eye: the robot faces , so its right-hand side faces ; the lidar sits at looking along , and an echo 2 m away, slightly anticlockwise of straight ahead, is at about .
Swapping the two matrices gives , 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 is 2 m ahead and 2 m to the robot's right, so : the inverse transform answers "where is that, from where I stand?", which is what a controller steering towards the dock needs.
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 and let be a small motion.
- (multiply on the right) applies 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.
- (multiply on the left) applies 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.
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)))The body-frame robot traces a 1 m square, , as a robot obeying "forward, turn left" should. The world-frame robot visits , and : each step swung its whole pose about a point near the world's origin. Both return home after four steps, because , 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 ( out of the lens, to the right of the image, down), while robot bodies follow forward, left, up. The rotation between them has no angle you would guess by eye, but its columns are easy to write down: the camera's (right) is the body's ; its (down) is the body's ; its (forward) is the body's . A point 2 m in front of the lens, 0.1 m right and 0.05 m up, ends up at in the body frame. In 3-D, is with a rotation block, and everything else in this lesson carries over unchanged. The next lesson is about that block.
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 is the set of real matrices with and : the rotations of -dimensional space. The special Euclidean group is the set of matrices with and : the rigid motions, or poses.
has three degrees of freedom (, , heading): the pose of a robot on a floor. has six (three of position, three of orientation): the pose of a drone, a gripper or a camera. A 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 free.
Neither group is commutative: and 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 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.
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.
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 — 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 , the pose of the source frame expressed in the target frame.
# 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 for the body in the space frame. Learn to translate notations; never mix them in one file.
Common misconceptions
Misconception
"To invert a transform, negate the translation." That works only when the rotation is the identity. The inverse of is ; the translation must be re-expressed in the other frame's axes. In the ward example, the world origin seen from the robot is , not .
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 , 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 , and check that adjacent labels match.
Misconception
"If the code runs, the chain is right." Every (or ) 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.
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 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
1. Frames and rigid transforms
Warm-upHumanIn homogeneous coordinates, which of these quantities should be written with last coordinate , so that the translation part of a transform applies to them? Select all that apply.
2. Frames and rigid transforms
CoreHumanA robot's body frame B has pose with heading and translation in the world frame W. A point has body coordinates : 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
CoreHumanYou know , the robot's pose in the world, and , the camera's pose on the robot. Which expression is , the camera's pose in the world?
4. Frames and rigid transforms
CoreHumanA robot's pose in the world is with heading and translation , as in the lesson's ward example. Compute . What is the component of its translation, that is, the coordinate of the world origin as seen from the robot?
5. Frames and rigid transforms
Warm-upHumanA homogeneous transform in 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
CoreComputationalWrite 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 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 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.
7. Frames and rigid transforms
StretchComputationalTwo 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 and . 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 . Use the rigid-transform inverse from the lesson rather than np.linalg.inv.
Further reading and resources
- Modern Robotics: Mechanics, Planning, and Control · Kevin M. Lynch and Frank C. Park · 2017Chapter 3 treats rigid-body motions with the same pose notation as this lesson; the preprint and videos are free.
- tf: The transform library · Tully Foote · 2013The design of ROS's transform library, by its author.
- REP 105: Coordinate Frames for Mobile PlatformsThe map, odom and base_link frames, and why odometry and localisation get separate frames.
- REP 103: Standard Units of Measure and Coordinate Conventionsx forward, y left, z up; and the z-forward "optical" frames of cameras.
- ROS 2 documentation: tf2 tutorials
- DrakeA robotics toolbox from MIT and Toyota Research Institute whose "monogram" notation writes the pose of B in A as X_AB, the same convention as here.