Foundations · Lesson 2 of 12
Spatial maths for robots: rotations and transforms
Rotation matrices, roll-pitch-yaw and gimbal lock, axis-angle, quaternions and SLERP, homogeneous transforms, and how a neural network should output a rotation.
What you will learn
- Build 3D rotation matrices, check that they are valid and compose them in the right order
- Convert between roll-pitch-yaw angles, axis-angle, quaternions and rotation matrices, and explain gimbal lock
- Interpolate orientations with SLERP and rotate vectors with unit quaternions
- Chain and invert homogeneous transforms to move points between world, base and camera frames
- Choose a continuous rotation representation, such as the 6D form, for a neural network output
Before you start
- Robots, AI and the SO-101 arm (lesson 1)
- Vectors, matrix multiplication and the dot and cross products
After this lesson you can describe the orientation and position of any part of a robot, convert between the common rotation representations, and move points between frames such as world, robot base and camera. You will also know which rotation representation to use when a neural network has to predict an orientation, and why.
Frames, points and poses
A coordinate frame is an origin plus three perpendicular unit axes, x, y and z, arranged right-handed. A robot carries many frames: one fixed to the world, one at the base of the arm, one on each link, one on the gripper and one on each camera. Kinematics, perception and learning all keep asking the same question: given a point or direction in one frame, what are its coordinates in another?
The answer has two parts. A rotation says how the axes of one frame are turned relative to another. A translation says where its origin is. Together they form a pose. This lesson builds the rotation part first, then adds translation with homogeneous transforms.
Notation: is the orientation of frame B expressed in frame A. It maps a vector’s coordinates in B to its coordinates in A, . Read the subscripts right to left: from B to A.
Rotation matrices
Two dimensions
Turning a 2D frame by an angle gives
The columns are the rotated x and y axes written in the original frame. That reading works in 3D too, and it is the quickest way to build or check a rotation matrix by hand.
Three dimensions
The three elementary rotations turn about one axis each:
Every 3D rotation matrix has two properties, and together they define the group :
- Orthonormal: the columns are unit vectors at right angles to each other, so and the inverse is the transpose, .
- Determinant +1: the axes stay right-handed. An orthonormal matrix with determinant is a reflection.
A 3×3 matrix has nine numbers but only three degrees of freedom. The six orthonormality constraints use up the rest. Numerical code drifts away from these constraints after many multiplications, so a long-running estimator re-orthonormalises its rotation from time to time.
Composing rotations
Rotations combine by matrix multiplication, and the order matters. If frame B is rotated relative to A, and C relative to B, then
The inner subscripts match and cancel. Matrix multiplication is not commutative: . Try it with a book. Turn it 90 degrees about a vertical axis and then about a horizontal one, and compare with the opposite order.
There are two ways to read a product such as . Read right to left, each rotation is about the fixed axes of the original frame: first x, then y, then z. Read left to right, each rotation is about the axes of the frame produced by the previous step (moving axes): first z, then the new y, then the newest x. Both readings give the same matrix.
Euler angles and roll, pitch, yaw
Three angles are the most compact way to write an orientation, and they are easy for people to read. An Euler angle convention picks three axes and an order. There are twelve valid sequences, and libraries disagree on defaults, so always state the convention.
Robotics software commonly uses roll-pitch-yaw (RPY) about fixed axes, which is the convention in URDF robot models: roll about x, then pitch about y, then yaw about z, all about the fixed parent axes. The matrix is
SciPy calls this sequence "xyz" (lower case means fixed, or extrinsic, axes) in Rotation.from_euler. The test file checks the lesson code against it.
Gimbal lock
At pitch the roll and yaw axes line up, and one degree of freedom disappears. Multiplying out at :
Only the difference appears. Roll 10°, yaw 30° and roll 40°, yaw 60° give exactly the same matrix (the code checks this). Two consequences follow. Converting a matrix back to RPY has no unique answer at that pitch. And near it, a small change in orientation can need a large change in roll and yaw, which is a problem for any controller or learning algorithm that works directly on the angles.
Gimbal lock is a property of the three-angle description, not of the physical rotation. The fix is to use a representation without that singularity.
Axis-angle and Rodrigues’ formula
Any rotation is a turn by an angle about some unit axis (Euler’s rotation theorem). Rodrigues’ formula builds the matrix:
is the skew-symmetric matrix with . Check: with the formula gives .
Axis-angle has a clear geometric meaning, and the vector (a rotation vector) is a compact three-number form that SciPy calls rotvec. It is the natural way to describe a small orientation error. The axis is undefined at and flips at , so it is still not a perfect global representation.
Unit quaternions
A quaternion has four numbers, , with a scalar part and a vector part . A rotation by about unit axis is the unit quaternion
The code in this lesson puts the scalar first. SciPy’s Rotation.as_quat() puts it last by default, and ROS messages store it last too. Mixing the two orders is one of the most common bugs in robot software.
Product and rotating a vector
Quaternions compose with the Hamilton product. For and :
The product matches matrix multiplication: the matrix of is . To rotate a vector , write it as a quaternion with zero scalar part and sandwich it:
Example: 90° about z is , and it turns into .
Double cover
The angle appears halved, so and give the same rotation: is a turn of about the same axis. Every rotation has two quaternions. Code that converts a matrix to a quaternion usually picks the one with . Code that compares or averages quaternions must allow for both signs.
Quaternions have no gimbal lock, need only four numbers with one constraint (), and are cheap to renormalise. Robot software often stores orientation as a quaternion (a ROS Pose message does) and converts to a matrix when it needs to transform many points.
SLERP
A robot that moves its gripper from one orientation to another needs the orientations in between. Interpolating roll, pitch and yaw separately gives a curved, uneven path. Spherical linear interpolation (SLERP) moves along the shortest arc at constant angular speed:
with from 0 to 1. The SIGGRAPH 1985 paper that proposed SLERP for animating rotations (listed under further reading) gives this form and the equivalent . If , negate first, or the interpolation takes the long way round. When the two are almost equal, is close to zero, and the code falls back to normalised linear interpolation (nlerp): average the four components and renormalise.
Both methods follow the same great-circle path; only the timing differs. For small rotations nlerp is close enough and cheaper. For long rotations that drive a robot directly, use SLERP.
Homogeneous transforms
A pose combines a rotation and a translation . Putting them in one 4×4 matrix lets you compose poses by multiplication:
is the origin of B written in A. A point gets a 1 appended, so the translation applies; a direction gets a 0, so only the rotation applies. These matrices form the group .
Chains of frames
Transforms chain like rotations, with matching inner subscripts:
Worked example. The robot base sits at m in the world, turned 90° about z. A camera is mounted 0.10 m in front of the base origin and 0.40 m up, looking forward along the base x axis. Cameras use z forward, x right and y down, so the camera axes, written in the base frame, are , , . Those are the columns of , which equals RPY :
An object 0.60 m straight ahead of the camera is at . Then m. In the world, the camera looks along +y, because the base x axis points along world y.
Inverse of a transform
To go the other way, invert. Because , the inverse has a closed form, which is cheaper and more accurate than a general 4×4 inverse:
The minus sign alone is not enough. The translation has to be re-expressed in the new frame, which is what the does. This step comes up constantly in camera geometry, for example when calibration gives the pose of a marker in the camera frame and you need the camera pose in the marker frame.
Rotations as network outputs
Learned policies and pose estimators often have to output an orientation. The obvious choices are three Euler angles or a four-number quaternion. Both cause trouble.
Consider yaw alone. Headings of and are 0.2° apart, but their target values are 359.8 apart. A network is a continuous function of its input, so to fit those targets it has to change its output steeply over a tiny range of inputs. Near that seam its predictions are poor, and training is slow. Quaternions have a similar seam. Picking makes the map from rotation to quaternion jump for 180° rotations, where and the sign of the vector part flips.
A CVPR 2019 paper on the continuity of rotation representations (listed under further reading) formalised this. It defines a representation as continuous when the map from rotation to representation is continuous, and shows that every representation of 3D rotations in four or fewer real numbers is discontinuous somewhere. That includes Euler angles, axis-angle and quaternions. It then gives a continuous 6D representation: the first two columns of the rotation matrix. In its autoencoder experiments the discontinuous representations had mean errors up to 6 to 14 times higher than the continuous ones.
The 6D map back to a rotation is a Gram-Schmidt step. The network outputs two 3-vectors and , and
Any six numbers (except degenerate cases where and are parallel) give a valid rotation, so the network output never has to be repaired. The loss can then be computed on itself, for example as the squared difference to the target matrix. When the range of rotations is small, for example a gripper that never turns past ±90°, Euler angles work, because the seam is never reached.
The code
spatial.py collects everything in this lesson as small NumPy functions: rotx, roty, rotz, rpy_to_R and R_to_rpy, axis_angle_to_R, R_to_quat and quat_to_R, quat_mul, quat_rotate, slerp, make_T, transform_inverse, R_to_6d and sixd_to_R. The quaternion conversion picks the largest of first, so it stays accurate near 180°.
def transform_inverse(T: Array) -> Array:
"""Closed-form inverse: [R p; 0 1]^-1 = [R^T, -R^T p; 0 1]."""
R, p = T[:3, :3], T[:3, 3]
return make_T(R.T, -R.T @ p)
def sixd_to_R(v: Array) -> Array:
a1, a2 = np.asarray(v[:3], dtype=float), np.asarray(v[3:], dtype=float)
b1 = a1 / np.linalg.norm(a1)
u2 = a2 - np.dot(b1, a2) * b1
b2 = u2 / np.linalg.norm(u2)
return np.column_stack((b1, b2, np.cross(b1, b2)))
test_spatial.py is a set of property tests on 200 seeded random rotations. It checks orthonormality and determinant, RPY against SciPy’s extrinsic "xyz", Rodrigues against SciPy’s rotation vectors, quaternion round trips up to sign, the product matching matrix multiplication, SLERP endpoints, unit norm and constant angular speed, the short-path rule, transform inverses, the camera example above, gimbal lock, and that the 6D vector stays continuous where yaw jumps by 360°.
spatial.py
Download test_spatial.py
python spatial.py
python -m pytest -q # 29 passed
Locate an object seen by the camera
Use the frames from the worked example, with as given.
- The camera reports an object at m. Where is it in the world frame?
- Where is the robot base origin, world point , in the camera frame? Explain the sign of each coordinate.
Show solution
- . The rows of are , and , so . Adding gives m.
- Use the inverse: . , and has rows , and , so m. The base is 0.40 m along camera +y, which points down, and 0.10 m along camera −z, behind the image plane. That matches the mounting: 0.40 m up and 0.10 m forward of the base.
In code: transform_point(T_WC, [0.05, -0.02, 0.5]) and transform_point(transform_inverse(T_WC), [0.5, 0.2, 0.0]).
Compose two quaternions
Let be 90° about z and be 90° about x.
- Write both quaternions.
- Compute and use it to rotate .
- Compute , rotate the same vector, and explain why the result differs.
Show solution
- and .
- . This applies first, then , about fixed axes. turns into , and leaves that unchanged, so the result is .
- . Now acts first: becomes , and leaves a vector on the x axis unchanged, so the result is . Rotations do not commute, and the quaternion product keeps the same order as the matrix product versus .
Summary
- A rotation matrix is orthonormal with determinant +1; its columns are the rotated axes, its inverse is its transpose, and rotations compose by multiplication in a fixed order.
- Roll-pitch-yaw angles (URDF uses fixed-axis ) are compact and readable but hit gimbal lock at pitch ±90°, where only roll minus yaw is defined.
- Axis-angle and Rodrigues’ formula describe a single turn about an axis. Unit quaternions store a rotation in four numbers without gimbal lock, with and equal.
- SLERP interpolates orientations along the shortest arc at constant speed; take the short path by flipping the sign when .
- Homogeneous transforms chain with matching subscripts, , and invert in closed form, .
- Every rotation representation with four or fewer numbers is discontinuous somewhere. A network that must output any rotation should predict the 6D form (two matrix columns) and map it back with Gram-Schmidt.
Check your understanding
Pick an answer for each question, then check. Answers stay hidden until you submit.
Further reading
- Book
- Book
- Paper
- Paper
Found a mistake or something unclear?Report a correction.
Follow this course
Get an email when new lessons and projects are added to this course.