From Lessons 1–3 you know two basic ideas: (i) a rotation changes the orientation of one frame relative to another, and (ii) a translation shifts origins. In robotics, we constantly need to combine both, and do so repeatedly along chains of parts.

Suppose frame \( \{B\} \) is attached to a robot body and frame \( \{A\} \) is a world frame. A point has coordinates \( \mathbf{x}_B \in \mathbb{R}^3 \) in \( \{B\} \). A rotation matrix \( \mathbf{R}_{AB} \) and translation vector \( \mathbf{p}_{AB} \) relate coordinates by

\[ \mathbf{x}_A = \mathbf{R}_{AB}\,\mathbf{x}_B + \mathbf{p}_{AB}. \]

This is an affine map: rotate then shift. It is not “just a matrix multiply” because of the \( +\mathbf{p}_{AB} \) term. Homogeneous coordinates package this affine map into a single linear map in 4D.

We augment points with a 1: \( \tilde{\mathbf{x}} = \begin{bmatrix}\mathbf{x}\\1\end{bmatrix} \in \mathbb{R}^4 \). Then define the homogeneous transform

\[ \mathbf{T}_{AB} = \begin{bmatrix} \mathbf{R}_{AB} & \mathbf{p}_{AB}\\ \mathbf{0}^\top & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4}. \]

Now the same relation becomes a single matrix product:

\[ \tilde{\mathbf{x}}_A = \mathbf{T}_{AB}\,\tilde{\mathbf{x}}_B. \]

Intuition: the fourth coordinate is a “switch” that lets translation be encoded as a matrix. With the bottom row fixed as \( [0\;0\;0\;1] \), matrix multiplication reproduces “rotate then add a shift.”

The block structure is meaningful:

• \( \mathbf{R}_{AB} \) (top-left 3×3) encodes the orientation of frame \( \{B\} \) expressed in frame \( \{A\} \).
• \( \mathbf{p}_{AB} \) (top-right 3×1) is the origin of \( \{B\} \) expressed in \( \{A\} \).
• Bottom row \( [0\;0\;0\;1] \) makes the transform affine (not arbitrary 4×4).

If you apply \( \mathbf{T}_{AB} \) to a point, you get its coordinates in \( \{A\} \). So the subscript order matters: “from B to A.”

flowchart LR
  B["Frame B"] -->|T_AB| A["Frame A"]
  PB["Point x_B"] -->|rotate by R_AB| PR["R_AB x_B"]
  PR -->|shift by p_AB| PA["Point x_A"]
  

Mermaid note: rotation/translation shown conceptually with ASCII, no LaTeX inside diagram.

The true power of homogeneous transforms is that we can chain many frame changes with ordinary matrix multiplication. Suppose we have three frames \( \{A\},\{B\},\{C\} \).

A point in \( \{C\} \) gets to \( \{A\} \) via

\[ \tilde{\mathbf{x}}_A = \mathbf{T}_{AB}\,\tilde{\mathbf{x}}_B = \mathbf{T}_{AB}\,\mathbf{T}_{BC}\,\tilde{\mathbf{x}}_C. \]

Therefore the composed transform is

\[ \mathbf{T}_{AC} = \mathbf{T}_{AB}\,\mathbf{T}_{BC}. \]

Expanding blocks shows the familiar “rotate-then-shift” rule:

\[ \mathbf{T}_{AB}\mathbf{T}_{BC} = \begin{bmatrix} \mathbf{R}_{AB}\mathbf{R}_{BC} & \mathbf{R}_{AB}\mathbf{p}_{BC}+\mathbf{p}_{AB}\\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

So composition is exactly what intuition says: orientations multiply, and positions add after being rotated into the same frame.

If \( \mathbf{T}_{AB} \) maps coordinates from B to A, then \( \mathbf{T}_{BA} \) must undo it. We want \( \mathbf{T}_{AB}\mathbf{T}_{BA} = \mathbf{I}_4 \).

Using only properties you already know: rotation matrices are orthonormal, so \( \mathbf{R}_{AB}^{-1}=\mathbf{R}_{AB}^\top \). The translation must cancel after reversing orientation. Hence

\[ \mathbf{T}_{AB}^{-1}=\mathbf{T}_{BA}= \begin{bmatrix} \mathbf{R}_{AB}^\top & -\mathbf{R}_{AB}^\top\mathbf{p}_{AB}\\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

Quick verification (block proof). Multiply the candidate inverse:

\[ \begin{aligned} \mathbf{T}_{AB}\mathbf{T}_{BA} &= \begin{bmatrix} \mathbf{R} & \mathbf{p}\\ \mathbf{0}^\top & 1 \end{bmatrix} \begin{bmatrix} \mathbf{R}^\top & -\mathbf{R}^\top \mathbf{p}\\ \mathbf{0}^\top & 1 \end{bmatrix} \\[4pt] &= \begin{bmatrix} \mathbf{R}\mathbf{R}^\top + \mathbf{p}\mathbf{0}^\top & -\mathbf{R}\mathbf{R}^\top\mathbf{p}+\mathbf{p}\\ \mathbf{0}^\top\mathbf{R}^\top + 1\cdot\mathbf{0}^\top & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{I}_3 & \mathbf{0}\\ \mathbf{0}^\top & 1 \end{bmatrix}. \end{aligned} \]

We used only \( \mathbf{R}\mathbf{R}^\top=\mathbf{I}_3 \). So the inverse formula is trustworthy without a deeper derivation.

Homogeneous coordinates also distinguish points from pure directions. A direction vector (like a velocity direction or axis) should rotate but not translate.

We encode:

• Point: \( \tilde{\mathbf{x}} = \begin{bmatrix}\mathbf{x}\\1\end{bmatrix} \)
• Direction: \( \tilde{\mathbf{v}} = \begin{bmatrix}\mathbf{v}\\0\end{bmatrix} \)

Then

\[ \mathbf{T}_{AB}\tilde{\mathbf{v}}_B = \begin{bmatrix} \mathbf{R}_{AB}\mathbf{v}_B\\0 \end{bmatrix}, \]

so translation disappears automatically when the last coordinate is 0. This is heavily used later (e.g., axes and normals).

These examples are intentionally small. The goal is to make the algebra feel concrete.

6.1 Python (NumPy + SciPy)

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

# Example: rotate 90 deg about z, then translate by (1,2,0) in frame A
R_AB = Rot.from_euler('z', 90, degrees=True).as_matrix()
p_AB = np.array([1.0, 2.0, 0.0])

T_AB = np.eye(4)
T_AB[0:3, 0:3] = R_AB
T_AB[0:3, 3] = p_AB

# Point in B
x_B = np.array([1.0, 0.0, 0.0])
x_B_h = np.hstack([x_B, 1.0])

# Map to A
x_A_h = T_AB @ x_B_h
x_A = x_A_h[0:3]
print("x_A =", x_A)

# Inverse using block formula
T_BA = np.eye(4)
T_BA[0:3, 0:3] = R_AB.T
T_BA[0:3, 3] = -R_AB.T @ p_AB
print("T_AB @ T_BA =\n", T_AB @ T_BA)
      

6.2 C++ (Eigen)

#include <Eigen/Dense>
#include <iostream>

int main() {
  Eigen::Matrix3d R_AB;
  R_AB = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ());

  Eigen::Vector3d p_AB(1.0, 2.0, 0.0);

  Eigen::Matrix4d T_AB = Eigen::Matrix4d::Identity();
  T_AB.block<3,3>(0,0) = R_AB;
  T_AB.block<3,1>(0,3) = p_AB;

  Eigen::Vector4d x_B_h(1.0, 0.0, 0.0, 1.0);
  Eigen::Vector4d x_A_h = T_AB * x_B_h;

  std::cout << "x_A = " << x_A_h.head<3>().transpose() << std::endl;

  Eigen::Matrix4d T_BA = Eigen::Matrix4d::Identity();
  T_BA.block<3,3>(0,0) = R_AB.transpose();
  T_BA.block<3,1>(0,3) = -R_AB.transpose() * p_AB;

  std::cout << "T_AB*T_BA =\n" << (T_AB*T_BA) << std::endl;
  return 0;
}
      

6.3 Java (EJML)

import org.ejml.simple.SimpleMatrix;

public class HomoTransformDemo {
  public static void main(String[] args) {
    // Rotation about z by 90 degrees
    double c = 0.0, s = 1.0; // cos(90)=0, sin(90)=1
    SimpleMatrix R = new SimpleMatrix(new double[][]{
      {c, -s, 0},
      {s,  c, 0},
      {0,  0, 1}
    });
    SimpleMatrix p = new SimpleMatrix(3,1,true, new double[]{1,2,0});

    SimpleMatrix T = SimpleMatrix.identity(4);
    T.insertIntoThis(0,0,R);
    T.insertIntoThis(0,3,p);

    SimpleMatrix xBh = new SimpleMatrix(4,1,true, new double[]{1,0,0,1});
    SimpleMatrix xAh = T.mult(xBh);

    System.out.println("x_A = " + xAh.extractMatrix(0,3,0,1));

    // Inverse
    SimpleMatrix RT = R.transpose();
    SimpleMatrix Tinv = SimpleMatrix.identity(4);
    Tinv.insertIntoThis(0,0,RT);
    Tinv.insertIntoThis(0,3, RT.scale(-1).mult(p));

    System.out.println("T*Tinv =\n" + T.mult(Tinv));
  }
}
      

6.4 MATLAB / Simulink (Robotics System Toolbox)

% Rotation about z by 90 deg and translation [1 2 0]
R_AB = axang2rotm([0 0 1 pi/2]);
p_AB = [1;2;0];

T_AB = eye(4);
T_AB(1:3,1:3) = R_AB;
T_AB(1:3,4) = p_AB;

x_B = [1;0;0;1];
x_A = T_AB * x_B;

disp("x_A = ");
disp(x_A(1:3));

% Inverse
T_BA = eye(4);
T_BA(1:3,1:3) = R_AB';
T_BA(1:3,4)   = -R_AB' * p_AB;
disp("T_AB*T_BA = ");
disp(T_AB*T_BA);
      

Simulink note: In Simulink, you typically represent \( \mathbf{T} \) as a 4×4 signal and use Matrix Multiply blocks to chain transforms.

flowchart TD
  S["Start with known R and p between frames"] --> B["Build T = [[R p],[0 1]]"]
  B --> A1["Apply: x_A_h = T_AB x_B_h"]
  B --> C1["Chain: T_AC = T_AB T_BC"]
  B --> I1["Invert: T_BA = [[R^T, -R^T p],[0 1]]"]
  A1 --> E["Interpret result in target frame"]
  C1 --> E
  I1 --> E
        

This “recipe” is what you’ll keep using whenever you see multiple frames in a robot.

Problem 1 (Build and Apply): Let \( \mathbf{R}_{AB}=\mathbf{I}_3 \) and \( \mathbf{p}_{AB}=\begin{bmatrix}2\\-1\\3\end{bmatrix} \). Construct \( \mathbf{T}_{AB} \) and compute \( \mathbf{x}_A \) for a point \( \mathbf{x}_B=\begin{bmatrix}1\\1\\1\end{bmatrix} \).

Solution:

\[ \mathbf{T}_{AB}= \begin{bmatrix} \mathbf{I}_3 & \mathbf{p}_{AB}\\ \mathbf{0}^\top & 1 \end{bmatrix} = \begin{bmatrix} 1&0&0&2\\ 0&1&0&-1\\ 0&0&1&3\\ 0&0&0&1 \end{bmatrix}. \]

\[ \tilde{\mathbf{x}}_A= \mathbf{T}_{AB}\tilde{\mathbf{x}}_B = \begin{bmatrix} 1&0&0&2\\ 0&1&0&-1\\ 0&0&1&3\\ 0&0&0&1 \end{bmatrix} \begin{bmatrix}1\\1\\1\\1\end{bmatrix} = \begin{bmatrix}3\\0\\4\\1\end{bmatrix} \Rightarrow \mathbf{x}_A=\begin{bmatrix}3\\0\\4\end{bmatrix}. \]

Problem 2 (Composition): Suppose \( \mathbf{T}_{AB}=\begin{bmatrix}\mathbf{R}_{AB}&\mathbf{p}_{AB}\\\mathbf{0}^\top&1\end{bmatrix} \) and \( \mathbf{T}_{BC}=\begin{bmatrix}\mathbf{R}_{BC}&\mathbf{p}_{BC}\\\mathbf{0}^\top&1\end{bmatrix} \). Show that the translation part of \( \mathbf{T}_{AC}=\mathbf{T}_{AB}\mathbf{T}_{BC} \) is \( \mathbf{p}_{AC}=\mathbf{R}_{AB}\mathbf{p}_{BC}+\mathbf{p}_{AB} \).

Solution:

\[ \mathbf{T}_{AB}\mathbf{T}_{BC} = \begin{bmatrix} \mathbf{R}_{AB}\mathbf{R}_{BC} & \mathbf{R}_{AB}\mathbf{p}_{BC}+\mathbf{p}_{AB}\\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

The top-right block is the new translation, hence \( \mathbf{p}_{AC}=\mathbf{R}_{AB}\mathbf{p}_{BC}+\mathbf{p}_{AB} \).

Problem 3 (Inverse Check): For a valid homogeneous transform \( \mathbf{T}_{AB} \), prove that \( \det(\mathbf{T}_{AB})=1 \).

Solution:

Use block determinant properties. Since the bottom row is \( [\mathbf{0}^\top \; 1] \), we have \( \det(\mathbf{T}_{AB}) = 1\cdot\det(\mathbf{R}_{AB}) \). But \( \mathbf{R}_{AB}\in SO(3) \) from Lesson 3, so \( \det(\mathbf{R}_{AB})=1 \). Therefore \( \det(\mathbf{T}_{AB})=1 \).

Problem 4 (Directions vs Points): Let \( \tilde{\mathbf{v}}_B=\begin{bmatrix}\mathbf{v}_B\\0\end{bmatrix} \). Show that applying \( \mathbf{T}_{AB} \) leaves translation irrelevant.

Solution:

\[ \mathbf{T}_{AB}\tilde{\mathbf{v}}_B = \begin{bmatrix} \mathbf{R}_{AB} & \mathbf{p}_{AB}\\ \mathbf{0}^\top & 1 \end{bmatrix} \begin{bmatrix} \mathbf{v}_B\\0 \end{bmatrix} = \begin{bmatrix} \mathbf{R}_{AB}\mathbf{v}_B\\0 \end{bmatrix}. \]

Because the last coordinate is 0, the translation block multiplies 0 and vanishes.

Homogeneous transforms encode rotation and translation in one 4×4 matrix: \( \mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{p}\\\mathbf{0}^\top&1\end{bmatrix} \). They let us: (1) transform points with a single multiply, (2) chain multiple frames via matrix products, and (3) invert frame relations using the simple block formula. The “extra 1” coordinate makes translations behave linearly in this augmented space.