Frames, rotations and transforms with numpy

Every robot program is full of positions and orientations, and every one of them is measured from somewhere. A lidar reports points relative to the lidar. The planner wants them relative to the map. The arm controller wants a grasp pose relative to its base. This lesson builds the tools that move numbers between those frames: rotation matrices and 4x4 homogeneous transforms, written in numpy so you can see every entry. It follows lesson 01, and lesson 03 uses everything here to compute where an arm’s tip is.

Frames and the right-hand rule

A coordinate frame is an origin plus three perpendicular unit axes, x, y and z. A robot has many: one fixed to the world, one on the mobile base, one on each link, one on each sensor, one on the tool. Positions only make sense with a frame attached. “The cup is at (0.5, 0.2, 0.1)” is meaningless until you say “in the base frame”.

Robotics frames are right-handed. Point the fingers of your right hand along x and curl them towards y. Your thumb points along z. Equivalently, z is the cross product of x and y. The same hand gives the sign of a rotation: point your thumb along the axis, and your fingers curl in the positive direction. A positive rotation about z therefore turns x towards y, which looks anticlockwise when z points at you.

A naming habit removes most frame bugs. Write T_a_b for the pose of frame b expressed in frame a, and p_a for a point expressed in frame a. Then T_a_b @ p_b gives p_a, and chains read cleanly: T_a_b @ T_b_c gives T_a_c because the inner letters match and cancel. If the inner letters do not match, the code is wrong.

Rotation matrices

A rotation matrix R_a_b is a 3x3 matrix whose columns are the x, y and z axes of frame b, written in the coordinates of frame a. That one sentence explains everything else about it. Multiplying R_a_b by a vector given in b coordinates takes a weighted sum of b’s axes, which is the same vector expressed in a.

Because the columns are perpendicular unit vectors, the matrix is orthonormal: its transpose times itself is the identity, and its inverse is its transpose. Its determinant is +1. A determinant of -1 would mean a reflection, which turns a right-handed frame into a left-handed one and is never a valid rotation. Those two checks are worth running on any rotation matrix that comes from a file, a sensor driver or a hand calculation.

The three basic rotations about the coordinate axes are enough to build any rotation.

import numpy as np

def rot_z(theta: float) -> np.ndarray:
    """Rotation about the z axis by theta radians (right-hand rule)."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0.0],
                     [s,  c, 0.0],
                     [0.0, 0.0, 1.0]])

def rot_x(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1.0, 0.0, 0.0],
                     [0.0, c, -s],
                     [0.0, s,  c]])

def rot_y(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[ c, 0.0, s],
                     [0.0, 1.0, 0.0],
                     [-s, 0.0, c]])

R = rot_z(np.deg2rad(30))
np.set_printoptions(precision=3, suppress=True)
print("R_z(30 deg) =\n", R)
print("columns are the rotated x, y, z axes; x axis now points at", R[:, 0])
print("R.T @ R is the identity:", np.allclose(R.T @ R, np.eye(3)))
print("det(R) =", round(float(np.linalg.det(R)), 6))

# A point 1 m along the rotated frame's x axis, expressed in the original frame
p_b = np.array([1.0, 0.0, 0.0])
print("p in frame a:", R @ p_b)

R_z(30 deg) =
 [[ 0.866 -0.5    0.   ]
 [ 0.5    0.866  0.   ]
 [ 0.     0.     1.   ]]
columns are the rotated x, y, z axes; x axis now points at [0.866 0.5   0.   ]
R.T @ R is the identity: True
det(R) = 1.0
p in frame a: [0.866 0.5   0.   ]

The first column, (0.866, 0.5, 0), is the new x axis: cos 30° along the old x and sin 30° along the old y. Note the sign pattern in rot_y. The minus sign sits in the bottom-left corner, not the top-right as in the other two. That follows from the right-hand rule (a positive rotation about y turns z towards x) and it is a classic source of bugs when the matrix is typed from memory.

Composing rotations

To rotate twice, multiply the matrices. The catch is that matrix multiplication does not commute, and neither do rotations in 3D. Take a book, turn it 90° about the vertical axis and then 90° about its own long edge. Start again and do the two turns in the other order. The book ends up in a different orientation.

import numpy as np

def rot_x(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])

def rot_z(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])

def show(m: np.ndarray) -> np.ndarray:
    return np.round(m, 3) + 0.0   # adding 0.0 turns -0.0 into 0.0 for printing

q = np.pi / 2
zx = rot_z(q) @ rot_x(q)   # yaw 90 deg, then roll 90 deg about the new x axis
xz = rot_x(q) @ rot_z(q)   # roll 90 deg, then yaw 90 deg about the new z axis
print("Rz @ Rx =\n", show(zx))
print("Rx @ Rz =\n", show(xz))
print("same rotation?", np.allclose(zx, xz))
print("body x axis after Rz @ Rx:", show(zx[:, 0]))
print("body x axis after Rx @ Rz:", show(xz[:, 0]))

Rz @ Rx =
 [[0. 0. 1.]
 [1. 0. 0.]
 [0. 1. 0.]]
Rx @ Rz =
 [[ 0. -1.  0.]
 [ 0.  0. -1.]
 [ 1.  0.  0.]]
same rotation? False
body x axis after Rz @ Rx: [0. 1. 0.]
body x axis after Rx @ Rz: [0. 0. 1.]

In the first case the body’s x axis ends up pointing along world y, in the second along world z. There are two equivalent ways to read a product such as rot_z(a) @ rot_x(b). Read left to right, each rotation is about the axes of the frame produced by the previous rotation (the moving, or body, axes). Read right to left, each rotation is about the fixed starting axes. Both readings give the same matrix. Pick one reading and stick to it in a code base, and write it in a comment.

This is also why a list of three angles is ambiguous until the axis order and the fixed or moving convention are stated. There are 24 such conventions in use, a point REP 103 makes when it discourages Euler angles (see below).

Homogeneous transforms

A rotation says how frame b is turned. A full pose also needs where its origin is. A homogeneous transformation packs both into one 4x4 matrix:

T_a_b = | R_a_b   t_a_b |
        | 0 0 0     1   |

Here t_a_b is the position of b’s origin in a. To transform a point, append a 1 to make it a 4-vector, multiply, and drop the 1. The bottom row makes translation a matrix product, so whole chains of frames collapse into one matrix multiplication. Chaining is how forward kinematics works in lesson 03, and how ROS’s tf2 library answers “where is this frame relative to that one”.

The inverse has a closed form. If T_a_b has rotation R and translation t, then T_b_a has rotation R.T and translation -R.T @ t. There is no need for a general matrix inverse, which is slower and loses the exact structure through rounding.

The example below is a mobile base standing in an odom-style world frame, facing north, with a forward-looking sensor mounted on it. The sensor reports an object, and we want that object in the world frame. Then we go back the other way.

import numpy as np

def rot_z(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])

def make_transform(R: np.ndarray, t: np.ndarray) -> np.ndarray:
    """4x4 homogeneous transform from a 3x3 rotation and a 3-vector translation."""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def invert_transform(T: np.ndarray) -> np.ndarray:
    """Inverse of a rigid transform without a general matrix inverse."""
    R, t = T[:3, :3], T[:3, 3]
    return make_transform(R.T, -R.T @ t)

def apply(T: np.ndarray, p: np.ndarray) -> np.ndarray:
    return (T @ np.append(p, 1.0))[:3]

np.set_printoptions(precision=3, suppress=True)

# Pose of the robot base in the world (odom) frame: 2 m east, 1 m north, facing north
T_world_base = make_transform(rot_z(np.pi / 2), np.array([2.0, 1.0, 0.0]))
# Pose of a forward-looking sensor on the base: 0.2 m forward, 0.5 m up, no rotation
T_base_sensor = make_transform(np.eye(3), np.array([0.2, 0.0, 0.5]))
# The sensor sees an object 1.5 m straight ahead and 0.3 m to its left
p_sensor = np.array([1.5, 0.3, 0.0])

T_world_sensor = T_world_base @ T_base_sensor
p_world = apply(T_world_sensor, p_sensor)
print("object in world frame:", p_world)

# Go back: where is that world point in the base frame?
T_base_world = invert_transform(T_world_base)
p_base = apply(T_base_world, p_world)
print("object in base frame: ", p_base)
print("closed-form inverse matches np.linalg.inv:",
      np.allclose(T_base_world, np.linalg.inv(T_world_base)))
print("T @ inv(T) is identity:", np.allclose(T_world_base @ T_base_world, np.eye(4)))

object in world frame: [1.7 2.7 0.5]
object in base frame:  [1.7 0.3 0.5]
closed-form inverse matches np.linalg.inv: True
T @ inv(T) is identity: True

Check the answer by hand. The base is at (2, 1) facing north, so “forward” is +y in the world and “left” is -x. The sensor sits 0.2 m ahead of the base, at world (2, 1.2). The object is 1.5 m further ahead and 0.3 m to the left: world (2 - 0.3, 1.2 + 1.5) = (1.7, 2.7), at the sensor’s height of 0.5 m. In the base frame the same object is 0.2 + 1.5 = 1.7 m ahead and 0.3 m left, which matches the second line.

REP 103: the conventions ROS uses

Conventions only help if everyone uses the same ones. In the ROS ecosystem they are written down in REP 103, “Standard Units of Measure and Coordinate Conventions”, by Tully Foote and Mike Purvis. This site follows it. The main points:

• Units are SI: metres, kilograms, seconds, and radians for angles.
• All frames are right-handed.
• A robot body has x forward, y left and z up.
• A short-range world frame uses ENU: x east, y north, z up. The REP recommends putting the origin somewhere nearby, such as the start position, to avoid precision problems with large float32 values.
• A camera optical frame, named with an _optical suffix, uses z forward, x right and y down. This is the convention of image processing, and it differs from the camera’s body frame. Mixing the two up rotates every point cloud by 90 degrees.
• Outdoor systems that need north-east-down get a separate frame with a _ned suffix.
• Rotation representations, in order of preference: quaternions, then rotation matrices, then fixed-axis roll, pitch and yaw about x, y and z. Euler angles are discouraged because of the many competing conventions.

A companion document, REP 105, names the standard frames of a mobile base: map, odom and base_link. Lesson 07 uses them, and the spatial math hub collects the rest.

A note on quaternions

Rotation matrices use nine numbers for three degrees of freedom. That is wasteful to store and send, and after many multiplications rounding errors make the matrix drift away from orthonormal. Roll, pitch and yaw use three numbers but have ambiguous conventions and a singularity (gimbal lock) where two of the angles describe the same motion.

A unit quaternion uses four numbers with one constraint (length 1). It has no gimbal lock, it is cheap to renormalise, and it interpolates smoothly between orientations. That is why REP 103 puts it first and why ROS messages carry orientations as quaternions. The price is that the numbers are not intuitive, and that libraries disagree on component order. SciPy and ROS messages use (x, y, z, w), with the scalar part last. MuJoCo and spatialmath-python use (w, x, y, z), with the scalar part first.

import numpy as np
from scipy.spatial.transform import Rotation

r = Rotation.from_euler("z", 90, degrees=True)   # yaw of 90 degrees about z
np.set_printoptions(precision=4, suppress=True)
print("quaternion, SciPy default order (x, y, z, w):", r.as_quat())
print("quaternion, scalar first (w, x, y, z):       ", r.as_quat(scalar_first=True))
print("same rotation as a matrix:\n", np.round(r.as_matrix(), 4) + 0.0)

quaternion, SciPy default order (x, y, z, w): [0.     0.     0.7071 0.7071]
quaternion, scalar first (w, x, y, z):        [0.7071 0.     0.     0.7071]
same rotation as a matrix:
 [[ 0. -1.  0.]
 [ 1.  0.  0.]
 [ 0.  0.  1.]]

The two printed quaternions hold the same numbers in a different order. Read with the wrong convention, the second would describe a rotation about x instead of z. The rotation representations cheat sheet lists the conversions and which library uses which order.

Exercise

1. Write rot_y from the right-hand rule without looking at the code above, then check it: rot_y(np.pi / 2) @ [0, 0, 1] should give (1, 0, 0), because a positive turn about y takes z to x.
2. A camera is mounted on the base 0.3 m forward and 0.4 m up, pitched down by 20° (a positive rotation about the base y axis). It sees a point 2 m straight ahead along its own x axis. Where is the point in the base frame, and how far in front of the base does it hit the floor plane z = 0? Build the transform with make_transform and rot_y.
3. Starting from a random rotation built from three rot_* calls, multiply it by itself 10,000 times in float32 and measure how far R.T @ R drifts from the identity. Then repeat with SciPy Rotation objects. Explain the difference.
4. Rewrite the sensor example so that the sensor is a camera optical frame (z forward, x right, y down). Which rotation do you need between the sensor body frame and the optical frame?

Summary

• A frame is an origin and three right-handed axes. Name transforms T_a_b (b in a) so that chains such as T_a_b @ T_b_c can be checked by eye.
• A rotation matrix’s columns are the rotated frame’s axes. Valid rotations satisfy R.T @ R = I and det(R) = +1, and the inverse is the transpose.
• Rotations do not commute. Say whether a sequence is about fixed or moving axes.
• A 4x4 homogeneous transform carries rotation and translation together. Its inverse is R.T with translation -R.T @ t.
• REP 103 fixes SI units, right-handed frames, x forward y left z up for bodies, ENU for local world frames and z forward for camera optical frames. Quaternion order differs between libraries.

Further reading

• Lynch and Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017: chapter 3, “Rigid-Body Motions”, covers rotation matrices, homogeneous transforms and exponential coordinates, with the notation used by much of the field.
• Corke, Robotics, Vision and Control: Fundamental Algorithms in Python, 3rd edition, Springer, 2023: pairs each idea with runnable code from the author’s open-source Python toolboxes, including spatialmath-python for poses and rotations.
• REP 103, Standard Units of Measure and Coordinate Conventions: the ROS conventions summarised above, short enough to read in full.
• SciPy Rotation documentation: conversions between matrices, quaternions, rotation vectors and Euler angles, and the scalar_first option.