Coordinate Frames and Homogeneous Transforms

A robot arm has many moving parts — base, links, end-effector, tool, and the object being grasped. Each part has its own natural reference frame. To say "the gripper is 0.3 m above the table" you need to express a point in one frame relative to another. In robotics we attach a right-handed Cartesian frame $\{x, y, z\}$ to every rigid body. The world frame $\{0\}$ (or base frame) is fixed; link frames $\{1\}, \{2\}, \ldots$ move with their links. A pose is the full description of where a frame sits: both its orientation (how it is rotated) and its position (where its origin is). Getting this bookkeeping right is 80% of kinematics — the rest is just multiplying matrices.

The orientation of frame $\{B\}$ relative to frame $\{A\}$ is captured by a rotation matrix $R \in \mathbb{R}^{3 \times 3}$, whose columns are the unit vectors of $\{B\}$'s axes expressed in $\{A\}$. Two non-negotiable properties:

$R^\top R = I \qquad \det(R) = +1$

The set of all such matrices is the Special Orthogonal group $SO(3)$. A planar rotation by angle $\theta$ about the $z$-axis:

$R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$

Because $R^\top = R^{-1}$, inverting an orientation is free — just transpose. Composite rotations multiply: $R_{AC} = R_{AB} R_{BC}$. Read subscripts right to left, just like function composition.

Orientation alone is not enough; we also need the position $p \in \mathbb{R}^3$ of a frame's origin. Stacking them into one $4 \times 4$ matrix gives the homogeneous transform:

$T = \begin{bmatrix} R & p \\ 0^\top & 1 \end{bmatrix}$

To transform a point $q_B$ in frame $\{B\}$ into frame $\{A\}$:

$\begin{bmatrix} q_A \\ 1 \end{bmatrix} = T_A^B \begin{bmatrix} q_B \\ 1 \end{bmatrix}$

The appended "1" makes translation linear — a clean trick that collapses the affine map into one matrix multiply. The set of valid $T$ matrices forms the Special Euclidean group $SE(3)$. The efficient inverse is:

$T^{-1} = \begin{bmatrix} R^\top & -R^\top p \\ 0^\top & 1 \end{bmatrix}$

If you know the pose of frame $\{B\}$ in $\{A\}$, and the pose of frame $\{C\}$ in $\{B\}$, then $\{C\}$ in $\{A\}$ is:

$T_A^C = T_A^B \; T_B^C$

Read right to left: first express things in $\{B\}$, then in $\{A\}$. Order matters — matrix multiplication is not commutative. For a 3-link arm with transforms $T_0^1, T_1^2, T_2^3$, the end-effector pose in the world is:

$T_0^3 = T_0^1 \; T_1^2 \; T_2^3$

This chain is exactly forward kinematics. The associativity of matrix multiplication means you can evaluate left-to-right or right-to-left; the numerics are identical (within floating-point precision). A common bug: accidentally using $T_0^1 T_0^2$ instead of $T_0^1 T_1^2$ — check your superscript/subscript conventions.

Convention: frame $\{i\}$ is rigidly attached to link $i$ and moves with it. When joint $i$ rotates by $q_i$, the transform $T_{i-1}^{i}$ changes — the rotation part absorbs $q_i$ while the translation encodes the fixed link geometry.

Choosing frame origins and axes haphazardly creates ugly algebra. The Denavit-Hartenberg convention (next lesson) gives a systematic 4-parameter recipe that places frames consistently along the kinematic chain.

Here is the everyday pattern for a robotics engineer:

import numpy as np

def rot_z(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0],
                     [s,  c, 0],
                     [0,  0, 1]])

def make_T(R, p):
    """4x4 homogeneous transform from 3x3 R and 3-vector p."""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3,  3] = p
    return T

def T_inv(T):
    R, p = T[:3, :3], T[:3, 3]
    Ti = np.eye(4)
    Ti[:3, :3] = R.T
    Ti[:3,  3] = -R.T @ p
    return Ti

# Link 1: rotate 45 deg about z, translate 1 m along x
T01 = make_T(rot_z(np.pi/4), [1.0, 0, 0])
# Link 2: rotate 30 deg about z, translate 0.5 m along x
T12 = make_T(rot_z(np.pi/6), [0.5, 0, 0])

T02 = T01 @ T12   # end-effector in world frame
print(T02[:3, 3]) # position of EE

Always use np.eye(4) + slice assignment — it is readable and immune to sign errors in the bottom row.

Each joint introduces one transform; they multiply left to right:

Two checks catch most bugs before they spread down the kinematic chain:

def check_T(T, label="T"):
    R = T[:3, :3]
    assert np.allclose(R @ R.T, np.eye(3), atol=1e-9), \
        f"{label}: rotation not orthogonal"
    assert np.isclose(np.linalg.det(R), 1.0, atol=1e-9), \
        f"{label}: det(R) != 1"
    assert np.allclose(T[3], [0, 0, 0, 1]), \
        f"{label}: bad bottom row"

Numerical drift accumulates when you compose many transforms. If you multiply 50+ matrices, re-orthogonalize via SVD:

U, _, Vt = np.linalg.svd(R)
R_clean = U @ Vt

This projects the drift-corrupted matrix back onto $SO(3)$. In production code, run check_T after every FK solve — it costs microseconds and saves hours of debugging.