Recall from Lessons 1–2 that a coordinate frame is an origin plus three orthonormal axes. If a point has coordinates \( \mathbf{x}^A \in \mathbb{R}^3 \) in frame A, and \( \mathbf{x}^B \in \mathbb{R}^3 \) in frame B, then the change of coordinates between the frames is always a combination of:

• Rotation: changes orientation of axes.
• Translation: changes origin location.

The generic rigid relation is:

\[ \mathbf{x}^B = \mathbf{R}_{BA}\,\mathbf{x}^A + \mathbf{p}_{BA}, \quad \mathbf{R}_{BA}\in \mathbb{R}^{3\times 3},\ \mathbf{p}_{BA}\in \mathbb{R}^3. \]

Here \( \mathbf{R}_{BA} \) encodes the rotation from A to B, and \( \mathbf{p}_{BA} \) is the translation vector from the origin of B to the origin of A, expressed in B.

flowchart TD
  A["Point coords in frame A: x^A"] --> B["Rotate axes or point"]
  B --> C["Apply rotation matrix R_BA"]
  C --> D["Translate origin by p_BA"]
  D --> E["Point coords in frame B: x^B"]
  

A rotation in 3D is a distance- and angle-preserving linear transformation. Mathematically, rotation matrices form the special orthogonal group:

\[ SO(3)=\left\{\mathbf{R}\in\mathbb{R}^{3\times 3}\ \middle|\ \mathbf{R}^\top\mathbf{R}=\mathbf{I},\ \det(\mathbf{R})=1\right\}. \]

2.1 Orthonormality and Length Preservation

Let \( \mathbf{y}=\mathbf{R}\mathbf{x} \) with \( \mathbf{R}\in SO(3) \). Then

\[ \|\mathbf{y}\|^2 = \mathbf{y}^\top\mathbf{y} = (\mathbf{R}\mathbf{x})^\top(\mathbf{R}\mathbf{x}) = \mathbf{x}^\top\mathbf{R}^\top\mathbf{R}\mathbf{x} = \mathbf{x}^\top\mathbf{I}\mathbf{x} = \|\mathbf{x}\|^2. \]

Therefore rotations preserve Euclidean length and thus distances between points.

2.2 Angle Preservation

Using the dot product identity \( \mathbf{a}^\top\mathbf{b}=\|\mathbf{a}\|\,\|\mathbf{b}\|\cos\theta \), for any two vectors \( \mathbf{u},\mathbf{v} \):

\[ (\mathbf{R}\mathbf{u})^\top(\mathbf{R}\mathbf{v}) = \mathbf{u}^\top\mathbf{R}^\top\mathbf{R}\mathbf{v} = \mathbf{u}^\top\mathbf{v}. \]

Since lengths are also preserved, the cosine of the angle between \( \mathbf{u} \) and \( \mathbf{v} \) is unchanged. Hence angles are preserved.

2.3 Right-Handedness and Determinant

The condition \( \det(\mathbf{R})=1 \) excludes reflections. If a matrix is orthonormal with determinant \( -1 \), it flips handedness and is not a physical rotation in robotics.

2.4 Example: Axis Rotations

Rotations about the coordinate axes by angle \( \theta \) (right-hand rule) are:

\[ \mathbf{R}_x(\theta)= \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{bmatrix},\quad \mathbf{R}_y(\theta)= \begin{bmatrix} \cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{bmatrix}, \]

\[ \mathbf{R}_z(\theta)= \begin{bmatrix} \cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}. \]

2.5 Non-Commutativity (Important!)

In 3D, rotations generally do not commute: \( \mathbf{R}_1\mathbf{R}_2 \neq \mathbf{R}_2\mathbf{R}_1 \). A concrete proof uses a counterexample.

Proof (by counterexample): Let \( \mathbf{R}_1=\mathbf{R}_x(\frac{\pi}{2}) \) and \( \mathbf{R}_2=\mathbf{R}_y(\frac{\pi}{2}) \). Apply to the vector \( \mathbf{e}_z=[0,0,1]^\top \):

\[ \mathbf{R}_2\mathbf{e}_z = \mathbf{e}_x,\quad \mathbf{R}_1(\mathbf{R}_2\mathbf{e}_z)=\mathbf{R}_1\mathbf{e}_x=\mathbf{e}_x. \]

\[ \mathbf{R}_1\mathbf{e}_z = \mathbf{e}_y,\quad \mathbf{R}_2(\mathbf{R}_1\mathbf{e}_z)=\mathbf{R}_2\mathbf{e}_y=\mathbf{e}_y. \]

Since \( \mathbf{e}_x \neq \mathbf{e}_y \), the compositions differ, proving non-commutativity.

A translation shifts all points by a fixed vector \( \mathbf{p}\in\mathbb{R}^3 \):

\[ T_{\mathbf{p}}:\ \mathbf{x}\mapsto \mathbf{x}+\mathbf{p}. \]

3.1 Properties

• Additivity: \( T_{\mathbf{p}_1}\circ T_{\mathbf{p}_2} = T_{\mathbf{p}_1+\mathbf{p}_2} \).
• Commutativity: \( T_{\mathbf{p}_1}\circ T_{\mathbf{p}_2} = T_{\mathbf{p}_2}\circ T_{\mathbf{p}_1} \).
• Inverse: \( T_{\mathbf{p}}^{-1}=T_{-\mathbf{p}} \).

Proof of commutativity:

\[ T_{\mathbf{p}_1}(T_{\mathbf{p}_2}(\mathbf{x})) = (\mathbf{x}+\mathbf{p}_2)+\mathbf{p}_1 = \mathbf{x}+(\mathbf{p}_1+\mathbf{p}_2) = T_{\mathbf{p}_2}(T_{\mathbf{p}_1}(\mathbf{x})). \]

3.2 Why Translations Are Not Linear

A linear map must satisfy \( f(\mathbf{0})=\mathbf{0} \). But for a translation:

\[ T_{\mathbf{p}}(\mathbf{0})=\mathbf{p}\neq \mathbf{0}\ \text{unless }\mathbf{p}=\mathbf{0}. \]

Thus translations are affine transformations, not linear ones. This is why combining rotations and translations cleanly motivates homogeneous coordinates later (Lesson 4).

A rigid-body motion keeps internal distances fixed. The most general rigid motion between two frames is:

\[ \mathbf{x}^B = \mathbf{R}_{BA}\mathbf{x}^A + \mathbf{p}_{BA}. \]

We interpret this in two equivalent ways:

1. Passive view (change of basis): the physical point stays fixed, the coordinates change because the frame B is rotated/translated w.r.t. A.
2. Active view (move the point): the frame stays fixed, the point is rotated/translated in space.

4.1 Composition Rule

Suppose frame A to B uses \( (\mathbf{R}_{BA},\mathbf{p}_{BA}) \), and B to C uses \( (\mathbf{R}_{CB},\mathbf{p}_{CB}) \). Then A to C is:

\[ \mathbf{R}_{CA}=\mathbf{R}_{CB}\mathbf{R}_{BA},\quad \mathbf{p}_{CA}=\mathbf{R}_{CB}\mathbf{p}_{BA}+\mathbf{p}_{CB}. \]

Proof:

\[ \mathbf{x}^C = \mathbf{R}_{CB}\mathbf{x}^B + \mathbf{p}_{CB} = \mathbf{R}_{CB}(\mathbf{R}_{BA}\mathbf{x}^A + \mathbf{p}_{BA}) + \mathbf{p}_{CB} = (\mathbf{R}_{CB}\mathbf{R}_{BA})\mathbf{x}^A + (\mathbf{R}_{CB}\mathbf{p}_{BA}+\mathbf{p}_{CB}). \]

4.2 Inverse Motion

From \( \mathbf{x}^B = \mathbf{R}_{BA}\mathbf{x}^A + \mathbf{p}_{BA} \), solve for \( \mathbf{x}^A \):

\[ \mathbf{x}^A = \mathbf{R}_{BA}^\top(\mathbf{x}^B - \mathbf{p}_{BA}), \quad \text{since }\mathbf{R}_{BA}^{-1}=\mathbf{R}_{BA}^\top. \]

Hence the inverse transformation is \( (\mathbf{R}_{AB},\mathbf{p}_{AB}) \) where \( \mathbf{R}_{AB}=\mathbf{R}_{BA}^\top \) and

\[ \mathbf{p}_{AB}=-\mathbf{R}_{BA}^\top\mathbf{p}_{BA}. \]

We implement (i) axis rotations, (ii) rigid motion application \( \mathbf{x}'=\mathbf{R}\mathbf{x}+\mathbf{p} \), and (iii) composition in four languages.

5.1 Python (NumPy)

import numpy as np

def Rz(theta):
    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 rigid_apply(R, p, x):
    # x' = R x + p
    return R @ x + p

def rigid_compose(R_cb, p_cb, R_ba, p_ba):
    # R_ca = R_cb R_ba ; p_ca = R_cb p_ba + p_cb
    R_ca = R_cb @ R_ba
    p_ca = R_cb @ p_ba + p_cb
    return R_ca, p_ca

theta = np.pi/4
R = Rz(theta)
p = np.array([0.5, -0.2, 0.1])
xA = np.array([1.0, 0.0, 0.0])

xB = rigid_apply(R, p, xA)
print("xB =", xB)
      

5.2 C++ (Eigen)

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

Eigen::Matrix3d Rz(double theta) {
    double c = std::cos(theta), s = std::sin(theta);
    Eigen::Matrix3d R;
    R << c, -s, 0,
         s,  c, 0,
         0,  0, 1;
    return R;
}

Eigen::Vector3d rigidApply(const Eigen::Matrix3d& R,
                           const Eigen::Vector3d& p,
                           const Eigen::Vector3d& x) {
    return R * x + p;
}

void rigidCompose(const Eigen::Matrix3d& R_cb, const Eigen::Vector3d& p_cb,
                  const Eigen::Matrix3d& R_ba, const Eigen::Vector3d& p_ba,
                  Eigen::Matrix3d& R_ca, Eigen::Vector3d& p_ca) {
    R_ca = R_cb * R_ba;
    p_ca = R_cb * p_ba + p_cb;
}

int main() {
    double theta = M_PI / 4.0;
    Eigen::Matrix3d R = Rz(theta);
    Eigen::Vector3d p(0.5, -0.2, 0.1);
    Eigen::Vector3d xA(1.0, 0.0, 0.0);

    Eigen::Vector3d xB = rigidApply(R, p, xA);
    std::cout << "xB = " << xB.transpose() << std::endl;
    return 0;
}
      

5.3 Java (EJML)

import org.ejml.simple.SimpleMatrix;

public class RigidMotion {
    public static SimpleMatrix Rz(double theta) {
        double c = Math.cos(theta), s = Math.sin(theta);
        double[][] data = {
            { c, -s, 0 },
            { s,  c, 0 },
            { 0,  0, 1 }
        };
        return new SimpleMatrix(data);
    }

    public static SimpleMatrix rigidApply(SimpleMatrix R, SimpleMatrix p, SimpleMatrix x) {
        return R.mult(x).plus(p);
    }

    public static SimpleMatrix[] rigidCompose(SimpleMatrix R_cb, SimpleMatrix p_cb,
                                              SimpleMatrix R_ba, SimpleMatrix p_ba) {
        SimpleMatrix R_ca = R_cb.mult(R_ba);
        SimpleMatrix p_ca = R_cb.mult(p_ba).plus(p_cb);
        return new SimpleMatrix[]{R_ca, p_ca};
    }

    public static void main(String[] args) {
        double theta = Math.PI/4.0;
        SimpleMatrix R = Rz(theta);
        SimpleMatrix p = new SimpleMatrix(3,1,true, new double[]{0.5, -0.2, 0.1});
        SimpleMatrix xA = new SimpleMatrix(3,1,true, new double[]{1,0,0});

        SimpleMatrix xB = rigidApply(R, p, xA);
        System.out.println("xB = " + xB);
    }
}
      

5.4 MATLAB / Simulink (built-ins)

function R = Rz(theta)
    c = cos(theta); s = sin(theta);
    R = [ c -s  0;
          s  c  0;
          0  0  1 ];
end

function xB = rigid_apply(R, p, xA)
    xB = R*xA + p;
end

function [R_ca, p_ca] = rigid_compose(R_cb, p_cb, R_ba, p_ba)
    R_ca = R_cb * R_ba;
    p_ca = R_cb * p_ba + p_cb;
end

% Demo
theta = pi/4;
R = Rz(theta);
p = [0.5; -0.2; 0.1];
xA = [1;0;0];
xB = rigid_apply(R, p, xA)
      

Simulink note: you can implement \( \mathbf{x}'=\mathbf{R}\mathbf{x}+\mathbf{p} \) using a “Matrix Multiply” block followed by a “Sum” block. Later in the course, this becomes part of full robot pose pipelines.

Problem 1 (Verify a Rotation): Let \( \mathbf{R}=\begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} \). Show that \( \mathbf{R}\in SO(3) \).

Solution:

Compute \( \mathbf{R}^\top\mathbf{R} \). Since columns are \( [0,1,0]^\top,\ [-1,0,0]^\top,\ [0,0,1]^\top \), they are orthonormal, thus \( \mathbf{R}^\top\mathbf{R}=\mathbf{I} \). The determinant is \( \det(\mathbf{R})=1 \) (it is a +90° rotation around z). Hence \( \mathbf{R}\in SO(3) \).

Problem 2 (Angle Preservation): Prove that for any rotation \( \mathbf{R}\in SO(3) \) and vectors \( \mathbf{u},\mathbf{v} \), the angle between them is unchanged after rotation.

Solution:

Let \( \theta \) be the angle between \( \mathbf{u},\mathbf{v} \). Then \( \cos\theta = \frac{\mathbf{u}^\top\mathbf{v}}{\|\mathbf{u}\|\|\mathbf{v}\|} \). Using orthonormality:

\[ \cos\theta' = \frac{(\mathbf{R}\mathbf{u})^\top(\mathbf{R}\mathbf{v})} {\|\mathbf{R}\mathbf{u}\|\ \|\mathbf{R}\mathbf{v}\|} = \frac{\mathbf{u}^\top\mathbf{R}^\top\mathbf{R}\mathbf{v}} {\|\mathbf{u}\|\ \|\mathbf{v}\|} = \cos\theta. \]

Therefore \( \theta'=\theta \).

Problem 3 (Composition): Frame A to B is a rotation \( \mathbf{R}_{BA} \) and translation \( \mathbf{p}_{BA} \). Frame B to C is \( \mathbf{R}_{CB},\mathbf{p}_{CB} \). Derive the A to C transformation.

Solution:

Starting from \( \mathbf{x}^B=\mathbf{R}_{BA}\mathbf{x}^A+\mathbf{p}_{BA} \) and \( \mathbf{x}^C=\mathbf{R}_{CB}\mathbf{x}^B+\mathbf{p}_{CB} \), substitute to get:

\[ \mathbf{x}^C = \mathbf{R}_{CB}\mathbf{R}_{BA}\mathbf{x}^A + \mathbf{R}_{CB}\mathbf{p}_{BA}+\mathbf{p}_{CB}. \]

Hence \( \mathbf{R}_{CA}=\mathbf{R}_{CB}\mathbf{R}_{BA} \) and \( \mathbf{p}_{CA}=\mathbf{R}_{CB}\mathbf{p}_{BA}+\mathbf{p}_{CB} \).

Problem 4 (Translation Not Linear): Show that \( T_{\mathbf{p}}(\alpha\mathbf{x}) \neq \alpha T_{\mathbf{p}}(\mathbf{x}) \) in general, for scalar \( \alpha \neq 1 \).

Solution:

We have \( T_{\mathbf{p}}(\alpha\mathbf{x})=\alpha\mathbf{x}+\mathbf{p} \), while \( \alpha T_{\mathbf{p}}(\mathbf{x})=\alpha(\mathbf{x}+\mathbf{p}) =\alpha\mathbf{x}+\alpha\mathbf{p} \). If \( \alpha\neq 1 \) and \( \mathbf{p}\neq\mathbf{0} \), these are unequal. Therefore translations are not linear.

We modeled rotations as elements of \( SO(3) \), proved their distance and angle preservation, and highlighted their non-commutativity. We treated translations as affine shifts, proved their commutative composition, and showed why they are not linear. Combining both yields the rigid-motion rule \( \mathbf{x}^B=\mathbf{R}_{BA}\mathbf{x}^A+\mathbf{p}_{BA} \), with clear composition and inverse formulas. Lesson 4 will package this into homogeneous transformations for compact representation.