Coordinate Frames and Transforms

Frames and poses

A lidar on a mobile robot reports that a wall is 1.5 meters away, straight ahead. That is true for the lidar, but useless for the map until you know where the robot stands and which way it faces. Every measurement and every command in robotics is relative to something, and that something is a coordinate frame: an origin point plus a set of axes, attached to a thing. A robot arm in a cell has many of them at once:

• the world (or map) frame, fixed to the floor of the hall or the building;
• the base frame, fixed to the robot's foot or chassis;
• one frame per link, moving with that part of the arm (see Anatomy of a Robot);
• the tool frame at the gripper or welding torch;
• the camera frame, in which a vision system reports what it sees (see Robot Vision).

The pose of something is its position and its orientation, measured in one stated frame. On a flat floor a pose is three numbers, x, y and a heading angle. In space it is six: three for the position and three for the orientation, which is why a rigid body has six degrees of freedom (DOF). A pose without its frame is meaningless: "(0.5, 0.2, 90°)" is a different place in the world frame than in the base frame.

Most of this page is about one question: given a point or a pose in one frame, what is it in another? The tools are rotation matrices, transforms and quaternions. They build on the vectors and matrices of Vectors and Matrices.

Right-handed frames, meters and radians

Two frames can disagree in units and in the direction of their axes, so robot software fixes a convention. The ROS conventions (REP 103, the ROS Enhancement Proposal on units and coordinates) say: lengths in meters, angles in radians, and "All systems are right handed". For a robot's body frame, x points forward, y to the left and z up.

Three.js, the 3D library used in the simulations on the next pages, is right-handed as well, but its y-axis points up. The arm simulation on Kinematics is drawn from above in the x-y plane, so the difference does not show there; in a full 3D scene that mixes ROS data with Three.js you have to swap axes explicitly.

Rotation and translation in the plane

Start on the floor, in two dimensions. A point seen by the robot at (p_x, p_y) in its own frame lies somewhere else in the world: rotate it by the robot's heading \theta, then shift it by the robot's position (x_r, y_r). The rotation is the 2D rotation matrix from Matrices as transformations:

Worked example: the robot stands at (2.0, 1.0) in the world, turned 90° to the left, and its lidar sees a point 1.5 m straight ahead, (1.5, 0) in the robot frame.

1. cos 90° = 0 and sin 90° = 1.
2. Rotate: (0 × 1.5 − 1 × 0, 1 × 1.5 + 0 × 0) = (0, 1.5). "Ahead" of a robot facing world-y is along world-y.
3. Shift: (0 + 2.0, 1.5 + 1.0) = (2.0, 2.5).

import math

def to_world(robot_x, robot_y, heading_deg, px, py):
    """A point seen in the robot frame (x forward, y left), in the world frame."""
    t = math.radians(heading_deg)
    wx = robot_x + math.cos(t) * px - math.sin(t) * py
    wy = robot_y + math.sin(t) * px + math.cos(t) * py
    return round(wx, 4) + 0.0, round(wy, 4) + 0.0

# The robot stands at (2.0, 1.0) in the world. Its lidar sees a wall point
# 1.5 m straight ahead.
print(to_world(2.0, 1.0, 0, 1.5, 0.0))    # robot facing along world x
print(to_world(2.0, 1.0, 90, 1.5, 0.0))   # robot turned a quarter left
print(to_world(2.0, 1.0, 90, 1.5, 0.5))   # a point 0.5 m to its left

(3.5, 1.0)
(2.0, 2.5)
(1.5, 2.5)

Angles go into math.cos and math.sin in radians (see Degrees and radians); the helper converts from degrees only because degrees are easier to read. The + 0.0 after rounding turns a -0.0 into 0.0: a tiny negative rounding error would otherwise print with a minus sign (see rounding errors).

Rotation matrices in 3D

In space, an orientation is described by a rotation matrix: a 3 × 3 matrix whose columns are the axes of one frame, written in the coordinates of the other. Multiplying it by a vector expressed in the rotated frame gives the same vector in the reference frame. The three basic rotations turn about a single axis by an angle \theta:

R_z is the 2D rotation with an extra row and column that leave z alone. Every rotation matrix has two properties worth checking in code:

• Its columns are unit vectors at right angles to each other (orthonormal), so R^\mathsf{T} R = I, the identity matrix. That makes the inverse cheap: undoing a rotation is multiplying by its transpose.
• Its determinant is +1. A determinant of −1 would mean a mirror image: a left-handed frame slipped in somewhere.

Rotations are combined by multiplying their matrices, and order matters. With column vectors, the rotation applied first stands on the right: R_x R_z v means "first R_z, then R_x". Apply both quarter turns to the x-axis in the two orders and the results differ:

import math

def Rx(deg):
    c, s = math.cos(math.radians(deg)), math.sin(math.radians(deg))
    return [[1, 0, 0], [0, c, -s], [0, s, c]]

def Rz(deg):
    c, s = math.cos(math.radians(deg)), math.sin(math.radians(deg))
    return [[c, -s, 0], [s, c, 0], [0, 0, 1]]

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 apply(R, v):
    return tuple(round(sum(R[i][k] * v[k] for k in range(3)), 4) + 0.0 for i in range(3))

def transpose(R):
    return [list(row) for row in zip(*R)]

def det3(R):
    (a, b, c), (d, e, f), (g, h, i) = R
    return a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)

x_axis = (1, 0, 0)
print("Rz(90) first, then Rx(90):", apply(matmul(Rx(90), Rz(90)), x_axis))
print("Rx(90) first, then Rz(90):", apply(matmul(Rz(90), Rx(90)), x_axis))

R = matmul(Rz(30), Rx(45))
print("R transposed times R:")
for row in matmul(transpose(R), R):
    print("  ", [round(x, 4) + 0.0 for x in row])
print("determinant:", round(det3(R), 4))

Rz(90) first, then Rx(90): (0.0, 0.0, 1.0)
Rx(90) first, then Rz(90): (0.0, 1.0, 0.0)
R transposed times R:
   [1.0, 0.0, 0.0]
   [0.0, 1.0, 0.0]
   [0.0, 0.0, 1.0]
determinant: 1.0

Homogeneous transforms

A rotation alone keeps the origin in place. To go from one frame to another you also need the shift between their origins, the translation t. A homogeneous transform packs both into one square matrix by adding a row of zeros ending in 1 — (0, 0, 1) in the plane — and writing points with an extra 1 at the end:

In the plane that is a 3 × 3 matrix (the right-hand side above); in space a 4 × 4 matrix with a 3 × 3 rotation and a 3-entry translation. The trick of the extra 1 is what makes it work: rotation and shift become a single matrix product, and a chain of frames becomes a chain of products.

Worked example (the figure): the tool frame sits at (0.5, 0.2) in the base frame, turned 90°. The point P = (0.1, 0) in the tool frame, for example the tip of a gripper finger, is in the base frame:

If the robot base itself stands at (2.0, 1.0) in the world, turned 90°, multiply the two transforms and you get the tool in the world in one matrix. The inverse goes the other way: rotate back with the transpose and move back, T^{-1} = \begin{pmatrix} R^\mathsf{T} & -R^\mathsf{T} t \\ 0 & 1 \end{pmatrix}.

import math

def transform(x, y, heading_deg):
    """The 3 x 3 homogeneous transform of a frame at (x, y), turned by heading_deg."""
    c, s = math.cos(math.radians(heading_deg)), math.sin(math.radians(heading_deg))
    return [[c, -s, x],
            [s,  c, y],
            [0,  0, 1]]

def matmul(A, B):
    return [[sum(A[i][k] * B[k][j] for k in range(len(B))) for j in range(len(B[0]))]
            for i in range(len(A))]

def apply(T, point):
    column = [[point[0]], [point[1]], [1]]          # (x, y) becomes (x, y, 1)
    x, y, _ = (row[0] for row in matmul(T, column))
    return round(x, 4) + 0.0, round(y, 4) + 0.0

def inverse(T):
    """Undo a rigid transform: rotate back with R transposed, then move back."""
    (c, ms, x), (s, _, y), _ = T
    return [[c,  s, -(c * x + s * y)],
            [ms, c, -(ms * x + c * y)],
            [0,  0, 1]]

base_T_tool = transform(0.5, 0.2, 90)     # the tool, as seen from the robot base
world_T_base = transform(2.0, 1.0, 90)    # the base, as seen from the world

tip = (0.1, 0.0)                          # a point 0.1 m along the tool's x-axis
print("in the base frame: ", apply(base_T_tool, tip))

world_T_tool = matmul(world_T_base, base_T_tool)
in_world = apply(world_T_tool, tip)
print("in the world frame:", in_world)
print("back to the tool:  ", apply(inverse(world_T_tool), in_world))

in the base frame:  (0.5, 0.3)
in the world frame: (1.7, 1.5)
back to the tool:   (0.1, 0.0)

Reading transform names

The variable names in the example follow a habit that prevents most transform bugs: world_T_base is the base as seen from the world, and it turns base coordinates into world coordinates. In print it is written {}^{w}T_{b}. When you chain transforms, the inner names must match and cancel:

If the names do not line up, for instance world_T_base times tool_T_base, one of the two needs to be inverted. Libraries and authors differ in where they put the letters, so check the documentation of the one you use; the principle stays the same.

Roll, pitch and yaw

Nine numbers in a matrix are hard to read. People prefer three angles, and that is what Euler angles are: an orientation written as three rotations about coordinate axes, one after another. The most common set in robotics, aviation and ships is roll, pitch and yaw: roll about the forward axis (x), pitch about the sideways axis (y) and yaw about the vertical axis (z). In the ZYX convention the matrix is

with yaw \psi, pitch \vartheta and roll \varphi. There are twelve possible axis orders (six that use three different axes, like ZYX, and six that repeat the first, like ZXZ), and each order gives different angles for the same orientation. Three numbers called "roll, pitch, yaw" mean nothing until you know the order and whether the rotations are about the fixed axes or about the moving ones. Always state the convention, in code and in documentation.

import math

def Rx(a): c, s = math.cos(a), math.sin(a); return [[1, 0, 0], [0, c, -s], [0, s, c]]
def Ry(a): c, s = math.cos(a), math.sin(a); return [[c, 0, s], [0, 1, 0], [-s, 0, c]]
def Rz(a): c, s = math.cos(a), math.sin(a); return [[c, -s, 0], [s, c, 0], [0, 0, 1]]

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 from_ypr(yaw, pitch, roll):
    """ZYX convention: R = Rz(yaw) Ry(pitch) Rx(roll), angles in degrees."""
    y, p, r = (math.radians(a) for a in (yaw, pitch, roll))
    return matmul(matmul(Rz(y), Ry(p)), Rx(r))

def to_ypr(R):
    """Back from the matrix to yaw, pitch and roll in degrees (pitch not +-90)."""
    yaw = math.atan2(R[1][0], R[0][0])
    pitch = math.asin(-R[2][0])
    roll = math.atan2(R[2][1], R[2][2])
    return tuple(round(math.degrees(a), 2) + 0.0 for a in (yaw, pitch, roll))

def same(A, B):
    return all(abs(A[i][j] - B[i][j]) < 1e-12 for i in range(3) for j in range(3))

print("round trip:", to_ypr(from_ypr(30, 20, 10)))

# Pitch 90 degrees: only yaw minus roll still matters
print("pitch 90:", same(from_ypr(30, 90, 10), from_ypr(50, 90, 30)))
print("pitch 89:", same(from_ypr(30, 89, 10), from_ypr(50, 89, 30)))

round trip: (30.0, 20.0, 10.0)
pitch 90: True
pitch 89: False

The round trip works, and the last two lines show the problem of the next section.

Gimbal lock

In the ZYX convention, a pitch of exactly ±90° points the robot's forward axis straight down or straight up. The roll axis now lines up with the yaw axis, so yaw and roll turn about the same line and only their combination matters — their difference at +90°, their sum at −90°: (yaw 30°, pitch 90°, roll 10°) and (50°, 90°, 30°) produce exactly the same matrix, as the example prints. One degree of freedom has disappeared. That is gimbal lock, named after the nested rings (gimbals) of a mechanical gyroscope, where it happens physically.

Near the lock the trouble starts already: converting a matrix back to angles becomes numerically unstable (the formulas take the arctangent of two numbers that are both close to zero), and a small change in orientation can make yaw and roll jump by large amounts. For a ground robot whose pitch stays small this rarely matters. For a drone, a robot wrist or a camera that can point straight up, it does, which is why software stores orientations differently.

Quaternions

A quaternion is a set of four numbers q = (w, x, y, z) that, with a length of 1, describes a rotation. A rotation by angle \theta about a unit axis u = (u_x, u_y, u_z) is

Worked example: a yaw of 90° is a rotation about z = (0, 0, 1). Half the angle is 45°, cos 45° = sin 45° ≈ 0.7071, so q = (0.7071, 0, 0, 0.7071). To rotate a vector v, write it as the quaternion (0, v_x, v_y, v_z) and multiply on both sides, with the conjugate q^{*} = (w, -x, -y, -z):

import math

def from_axis_angle(axis, degrees):
    """A unit quaternion (w, x, y, z) for a rotation about a unit axis."""
    half = math.radians(degrees) / 2
    return (math.cos(half), *(a * math.sin(half) for a in axis))

def qmul(a, b):
    w1, x1, y1, z1 = a
    w2, x2, y2, z2 = b
    return (w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
            w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
            w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
            w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2)

def rotate(q, v):
    """v' = q v q*, with v written as the quaternion (0, x, y, z)."""
    conjugate = (q[0], -q[1], -q[2], -q[3])
    return qmul(qmul(q, (0.0, *v)), conjugate)[1:]

def clean(values):
    return tuple(round(v, 4) + 0.0 for v in values)

yaw90 = from_axis_angle((0, 0, 1), 90)
print("q =", clean(yaw90))
print("x-axis turns to", clean(rotate(yaw90, (1, 0, 0))))

minus = tuple(-c for c in yaw90)
print("-q gives", clean(rotate(minus, (1, 0, 0))))

twice = qmul(yaw90, yaw90)                     # 90 degrees, then 90 more
print("twice:", clean(twice), "->", clean(rotate(twice, (1, 0, 0))))

q = (0.7071, 0.0, 0.0, 0.7071)
x-axis turns to (0.0, 1.0, 0.0)
-q gives (0.0, 1.0, 0.0)
twice: (0.0, 0.0, 0.0, 1.0) -> (-1.0, 0.0, 0.0)

Why robot software stores quaternions rather than angles:

• They have no gimbal lock: every orientation has a smooth neighborhood.
• Two rotations combine with one quaternion product (16 multiplications, against 27 for two 3 × 3 matrices), and a quaternion drifting away from length 1 is fixed by dividing by its length; a drifting matrix is harder to repair.
• They interpolate smoothly between two orientations (slerp, spherical linear interpolation), which is what a camera or a wrist should do when it turns from one pose to the next.

Two details surprise everyone once. First, q and −q are the same rotation, as the example shows, so comparing quaternions with == can fail for identical orientations. Second, the order of the four numbers is a convention, not part of the math: many math texts write (w, x, y, z), while ROS 2 messages store (x, y, z, w), and Three.js's Quaternion(x, y, z, w) takes the same order as ROS. Read the API before you copy numbers across.

import math

# A ROS 2 message lists the orientation as (x, y, z, w)
x, y, z, w = (0.0, 0.0, 0.3827, 0.9239)

# Heading (yaw) of a robot on a flat floor, from the quaternion
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
print(f"yaw {math.degrees(yaw):.1f} degrees")

# The same numbers read in the wrong order, as if they were (w, x, y, z)
w, x, y, z = (0.0, 0.0, 0.3827, 0.9239)
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
print(f"yaw {math.degrees(yaw):.1f} degrees (wrong)")

yaw 45.0 degrees
yaw 180.0 degrees (wrong)

Frames in ROS

ROS 2 (see ROS 2) keeps all of a robot's frames in one tree, maintained by its transform library tf2: every frame has one parent, and each edge holds the transform from parent to child, stamped with a time. A program can then ask "where was the camera relative to the map 50 milliseconds ago?" and tf2 chains the transforms along the tree, exactly as the example above chained two matrices.

For mobile robots, REP 105 fixes the names of the top of that tree:

The chain is map → odom → base_link. Wheel odometry publishes odom → base_link (see Mobile Robots and Odometry), and localization publishes the correction map → odom (see Localization and Mapping). Below base_link hang the frames of sensors and links, usually described in the robot's model file (see Simulation and Digital Twins).