A rigid body pose in 3D can be described by a pair \( (R, p) \), where \( R \in \mathrm{SO}(3) \) is a rotation matrix (orientation) and \( p \in \mathbb{R}^3 \) is a translation vector (position of the body frame origin expressed in some reference frame). The special Euclidean group in 3D is then

\[ \mathrm{SE}(3) = \left\{ T \in \mathbb{R}^{4\times 4} \,\middle|\, T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}, \; R \in \mathrm{SO}(3), \; p \in \mathbb{R}^3 \right\}. \]

The matrix \( T \) is called a homogeneous transformation. The last row is always \( [0\;0\;0\;1] \), and the upper-left block is a proper rotation.

Because \( \mathrm{SO}(3) \) has 3 degrees of freedom and \( p \in \mathbb{R}^3 \) has 3 components, the manifold \( \mathrm{SE}(3) \) has dimension \( 6 \). This is consistent with the intuitive notion that a free rigid body in 3D has 6 configuration degrees of freedom.

flowchart TD
  A["Rigid body pose (R, p)"] --> B["Homogeneous matrix T = [[R, p],[0, 1]]"]
  B --> C["Group operation: matrix multiplication"]
  C --> D["Inverse: T^{-1}"]
  D --> E["Action on points in homogeneous coordinates"]
        

Let frames \( A \) and \( B \) be two right-handed orthonormal coordinate frames attached to rigid bodies (or one of them could be a world frame). A 3D point has coordinates \( {}^A\mathbf{p} \) in frame \(A\) and \( {}^B\mathbf{p} \) in frame \(B\). By definition of a rigid-body transform,

\[ {}^A\mathbf{p} = R_{AB}\,{}^B\mathbf{p} + p_{AB}, \]

where \( R_{AB} \in \mathrm{SO}(3) \) is the rotation matrix from frame \(B\) to frame \(A\), and \( p_{AB} \in \mathbb{R}^3 \) is the position of the origin of frame \(B\) expressed in frame \(A\).

Introduce the homogeneous coordinates of the point as \( \tilde{\mathbf{p}} = \begin{bmatrix}\mathbf{p} \\ 1\end{bmatrix} \in \mathbb{R}^4 \). Then we can write

\[ \begin{bmatrix} {}^A\mathbf{p} \\ 1 \end{bmatrix} = {}^A T_B \begin{bmatrix} {}^B\mathbf{p} \\ 1 \end{bmatrix}, \quad {}^A T_B = \begin{bmatrix} R_{AB} & p_{AB} \\ 0 & 1 \end{bmatrix} \in \mathrm{SE}(3). \]

The map \( {}^A T_B : \mathbb{R}^4 \to \mathbb{R}^4 \), restricted to homogeneous coordinates (last component equal to 1), implements the coordinate change from frame \(B\) to frame \(A\).

Conceptually, \( {}^A T_B \) can be interpreted either as the pose of frame B expressed in frame A, or as the linear map that sends coordinates of geometric objects from frame \(B\) into frame \(A\), depending on context; both viewpoints are mathematically consistent.

A \(4\times 4\) matrix \( T \) is in \( \mathrm{SE}(3) \) if and only if:

• The last row is \( [0\;0\;0\;1] \).
• The upper-left \(3\times 3\) block \(R\) is a rotation matrix, i.e.

\[ R^\top R = I_3, \qquad \det R = 1. \]

Write \( T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \). It is often convenient to define an error functional that measures how far a numerical matrix is from being in \( \mathrm{SE}(3) \), for example:

\[ \phi(T) = \tfrac{1}{2}\big\|R^\top R - I_3\big\|_F^2 + \big(\det R - 1\big)^2 + \big\|T_{4,:} - [0\;0\;0\;1]\big\|_2^2. \]

Then \( T \in \mathrm{SE}(3) \) if and only if \( \phi(T) = 0 \). In numerical algorithms we typically tolerate a small nonzero value and explicitly project matrices back onto \( \mathrm{SE}(3) \) to control drift (see Section 6).

Let \( T_1, T_2 \in \mathrm{SE}(3) \) with

\[ T_1 = \begin{bmatrix} R_1 & p_1 \\ 0 & 1 \end{bmatrix}, \qquad T_2 = \begin{bmatrix} R_2 & p_2 \\ 0 & 1 \end{bmatrix}. \]

Their composition in the group is matrix multiplication:

\[ T_1 T_2 = \begin{bmatrix} R_1 & p_1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_2 & p_2 \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R_1 R_2 & R_1 p_2 + p_1 \\ 0 & 1 \end{bmatrix} \in \mathrm{SE}(3). \]

Proof sketch: Block-matrix multiplication yields the formula above. Because the product of two rotations is a rotation and affine translations add, the result is again a rigid-body motion.

The identity element is \( I_4 = \begin{bmatrix} I_3 & 0 \\ 0 & 1 \end{bmatrix} \). The inverse of \( T \) is

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

Proof: Using orthogonality \( R^\top R = I_3 \), we compute

\[ T T^{-1} = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^\top & -R^\top p \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R R^\top & -R R^\top p + p \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} I_3 & 0 \\ 0 & 1 \end{bmatrix} = I_4. \]

A similar computation shows \( T^{-1}T = I_4 \), so this is indeed the group inverse in \( \mathrm{SE}(3) \).

Suppose we have three frames \(A\), \(B\), and \(C\). Let \( {}^A T_B \) represent the pose of frame \(B\) in frame \(A\), and similarly \( {}^B T_C \) the pose of frame \(C\) in frame \(B\). Then the pose of frame \(C\) in frame \(A\) is obtained by composition:

\[ {}^A T_C = {}^A T_B \, {}^B T_C. \]

In block form, if \( {}^A T_B = \begin{bmatrix} R_{AB} & p_{AB} \\ 0 & 1 \end{bmatrix} \) and \( {}^B T_C = \begin{bmatrix} R_{BC} & p_{BC} \\ 0 & 1 \end{bmatrix} \), then

\[ {}^A T_C = \begin{bmatrix} R_{AB} R_{BC} & R_{AB} p_{BC} + p_{AB} \\ 0 & 1 \end{bmatrix}. \]

Relative transforms (e.g. from frame \(B\) back to frame \(A\)) are just inverses:

\[ {}^B T_A = \left({}^A T_B\right)^{-1} = \begin{bmatrix} R_{AB}^\top & -R_{AB}^\top p_{AB} \\ 0 & 1 \end{bmatrix}. \]

Hence, for any point with coordinates \( {}^C\mathbf{p} \) in frame \(C\), its coordinates in frame \(A\) are given by \( {}^A\mathbf{p} = {}^A T_C \, {}^C \tilde{\mathbf{p}} \), where \( {}^C \tilde{\mathbf{p}} \) is the homogeneous coordinate vector.

flowchart LR
  A["Frame A"] --> B["Frame B"]
  B --> C["Frame C"]
  A --> AC["T_A_C = T_A_B * T_B_C"]
  B --> BA["T_B_A = (T_A_B)^{-1}"]
        

In finite-precision arithmetic, repeated composition of homogeneous transforms causes the rotation parts to drift away from strict orthogonality. That is, numerically, \( R^\top R \neq I_3 \) and \( \det R \neq 1 \) exactly, even though the true motion is rigid.

A standard remedy is to project the numerically obtained \(3\times3\) matrix \(M\) onto \( \mathrm{SO}(3) \) using the singular value decomposition (SVD). Let \( M = U \Sigma V^\top \) be the SVD with \( U, V \in \mathrm{SO}(3) \). Then define

\[ R_{\text{proj}} = U \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(UV^\top) \end{bmatrix} V^\top \in \mathrm{SO}(3). \]

Replacing the upper-left block of the homogeneous transform with \( R_{\text{proj}} \) yields a matrix closer to \( \mathrm{SE}(3) \) and stabilizes long chains of transforms.

A simple scalar measure of orthogonality error is

\[ \varepsilon_R = \big\| R^\top R - I_3 \big\|_F, \]

where \( \|\cdot\|_F \) is the Frobenius norm. In practical implementations we monitor \( \varepsilon_R \) and re-orthogonalize \( R \) when the error exceeds a small threshold.

In Python, homogeneous transforms are conveniently represented as 4x4 NumPy arrays. Robotics-oriented libraries such as spatialmath-python and the modern_robotics toolbox (based on Modern Robotics by Lynch & Park) provide ready-made SE3 classes, but it is instructive to implement the basics from scratch.

import numpy as np

def make_transform(R: np.ndarray, p: np.ndarray) -> np.ndarray:
    """
    Construct a homogeneous transform T from rotation R and translation p.

    Parameters
    ----------
    R : (3, 3) ndarray
        Rotation matrix in SO(3).
    p : (3,) ndarray
        Translation vector.

    Returns
    -------
    T : (4, 4) ndarray
        Homogeneous transformation matrix in SE(3).
    """
    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = p.reshape(3)
    return T

def is_SE3(T: np.ndarray, tol: float = 1e-9) -> bool:
    """
    Check whether T is numerically close to an element of SE(3).
    """
    if T.shape != (4, 4):
        return False

    R = T[0:3, 0:3]
    last_row = T[3, :]

    cond_last = np.allclose(
        last_row, np.array([0.0, 0.0, 0.0, 1.0]), atol=tol
    )
    cond_orth = np.allclose(R.T @ R, np.eye(3), atol=tol)
    cond_det = abs(np.linalg.det(R) - 1.0) < tol

    return bool(cond_last and cond_orth and cond_det)

def inverse_transform(T: np.ndarray) -> np.ndarray:
    """
    Compute the inverse of a homogeneous transform T.
    """
    assert is_SE3(T), "T must be in SE(3)"
    R = T[0:3, 0:3]
    p = T[0:3, 3]
    T_inv = np.eye(4)
    T_inv[0:3, 0:3] = R.T
    T_inv[0:3, 3] = -R.T @ p
    return T_inv

def transform_point(T: np.ndarray, x: np.ndarray) -> np.ndarray:
    """
    Apply T to a point x expressed in the source frame.

    Parameters
    ----------
    T : (4, 4) ndarray
        Homogeneous transform from frame B to frame A.
    x : (3,) ndarray
        Coordinates of the point in frame B.

    Returns
    -------
    y : (3,) ndarray
        Coordinates of the point in frame A.
    """
    x_h = np.hstack((x.reshape(3), 1.0))
    y_h = T @ x_h
    return y_h[0:3]

# Example: chain A->B and B->C to get A->C
R_AB = np.eye(3)
p_AB = np.array([0.5, 0.0, 0.0])

theta = np.deg2rad(90.0)
R_BC = np.array([
    [np.cos(theta), -np.sin(theta), 0.0],
    [np.sin(theta),  np.cos(theta), 0.0],
    [0.0,            0.0,           1.0],
])
p_BC = np.array([0.0, 0.2, 0.0])

T_AB = make_transform(R_AB, p_AB)
T_BC = make_transform(R_BC, p_BC)
T_AC = T_AB @ T_BC

x_C = np.array([0.1, 0.0, 0.0])  # point in frame C
x_A = transform_point(T_AC, x_C)
print("Point in frame A:", x_A)
      

Using robotics libraries, the same operations become even more compact. For example, with spatialmath-python:

from spatialmath import SE3

T_AB = SE3(R_AB, p_AB)      # construct SE3 from rotation and translation
T_BC = SE3(R_BC, p_BC)
T_AC = T_AB * T_BC          # group multiplication in SE(3)

x_C_h = SE3(0, 0, 0) * 0    # dummy; in practice use vector operations
x_C = np.array([0.1, 0.0, 0.0])
x_A = (T_AC * x_C).A[0:3]   # apply transform, then convert to ndarray
      

In C++, the de facto standard for matrix computations in robotics is Eigen. Homogeneous transforms can be represented using Eigen::Matrix4d, and many robotics frameworks (e.g. ROS tf2, Orocos KDL, Pinocchio) interoperate with Eigen matrices.

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

Eigen::Matrix4d makeTransform(const Eigen::Matrix3d& R,
                              const Eigen::Vector3d& p) {
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3,3>(0, 0) = R;
    T.block<3,1>(0, 3) = p;
    return T;
}

Eigen::Matrix4d inverseTransform(const Eigen::Matrix4d& T) {
    Eigen::Matrix3d R = T.block<3,3>(0, 0);
    Eigen::Vector3d p = T.block<3,1>(0, 3);
    Eigen::Matrix4d Tinv = Eigen::Matrix4d::Identity();
    Tinv.block<3,3>(0, 0) = R.transpose();
    Tinv.block<3,1>(0, 3) = -R.transpose() * p;
    return Tinv;
}

Eigen::Vector3d transformPoint(const Eigen::Matrix4d& T,
                               const Eigen::Vector3d& x) {
    Eigen::Vector4d x_h;
    x_h << x(0), x(1), x(2), 1.0;
    Eigen::Vector4d y_h = T * x_h;
    return y_h.head<3>();
}

int main() {
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 4.0,
                                          Eigen::Vector3d::UnitZ()).toRotationMatrix();
    Eigen::Vector3d p(0.5, 0.0, 0.0);

    Eigen::Matrix4d T = makeTransform(R, p);

    Eigen::Vector3d x(0.1, 0.0, 0.0);
    Eigen::Vector3d y = transformPoint(T, x);

    std::cout << "y = " << y.transpose() << std::endl;

    return 0;
}
      

In ROS-based systems, homogeneous transforms are often represented using geometry_msgs::TransformStamped, but internally they are frequently converted to Eigen matrices for algebraic operations (e.g. via tf2_eigen).

For Java-based robotics (e.g. ROSJava, custom industrial controllers), matrix operations are commonly implemented using libraries such as EJML or Apache Commons Math. Below is a simple implementation using EJML's SimpleMatrix.

import org.ejml.simple.SimpleMatrix;

public class SE3 {

    public static SimpleMatrix makeTransform(SimpleMatrix R, SimpleMatrix p) {
        // R: 3x3, p: 3x1
        SimpleMatrix T = SimpleMatrix.identity(4);
        T.insertIntoThis(0, 0, R);
        T.insertIntoThis(0, 3, p);
        return T;
    }

    public static SimpleMatrix inverseTransform(SimpleMatrix T) {
        SimpleMatrix R = T.extractMatrix(0, 3, 0, 3);
        SimpleMatrix p = T.extractMatrix(0, 3, 3, 4);
        SimpleMatrix R_t = R.transpose();

        SimpleMatrix Tinv = SimpleMatrix.identity(4);
        Tinv.insertIntoThis(0, 0, R_t);
        Tinv.insertIntoThis(0, 3, R_t.mult(p).scale(-1.0));
        return Tinv;
    }

    public static SimpleMatrix transformPoint(SimpleMatrix T, SimpleMatrix x) {
        // x: 3x1
        SimpleMatrix x_h = new SimpleMatrix(4, 1, true,
                new double[]{x.get(0), x.get(1), x.get(2), 1.0});
        SimpleMatrix y_h = T.mult(x_h);
        return y_h.extractMatrix(0, 3, 0, 1);
    }

    public static void main(String[] args) {
        // Example: identity rotation, translation along x
        SimpleMatrix R = SimpleMatrix.identity(3);
        SimpleMatrix p = new SimpleMatrix(3, 1, true, new double[]{0.5, 0.0, 0.0});

        SimpleMatrix T = makeTransform(R, p);
        SimpleMatrix x = new SimpleMatrix(3, 1, true, new double[]{0.1, 0.0, 0.0});
        SimpleMatrix y = transformPoint(T, x);

        System.out.println("y = ");
        y.print();
    }
}
      

A robotics-specific layer can be built on top of this, encapsulating SE(3) operations (composition, inversion, interpolation) and interfacing with sensor drivers or middleware written in Java.

MATLAB, especially with the Robotics System Toolbox and Simscape Multibody, provides native support for homogeneous transforms. A homogeneous transform can be built manually or using toolbox functions such as trvec2tform, rotm2tform, and axang2tform.

% Rotation about z-axis by 45 degrees
theta = pi/4;
R = [cos(theta) -sin(theta) 0;
     sin(theta)  cos(theta) 0;
     0           0          1];

% Translation vector
p = [0.5; 0.2; 0.0];

% Build homogeneous transform manually
T = [R, p;
     0 0 0 1];

% Use Robotics System Toolbox utilities
T_rot = axang2tform([0 0 1 theta]);
T_tr  = trvec2tform([0.5 0.2 0.0]);
T_toolbox = T_tr * T_rot;

% Transform a point
x_B = [0.1; 0.0; 0.0; 1.0];   % homogeneous coordinates
x_A = T * x_B;                % coordinates in frame A
      

In Simulink, Simscape Multibody provides a Rigid Transform block that encapsulates rotation and translation. Internally, this block uses homogeneous transforms to relate the connected frames. A typical Simulink workflow for kinematic chains is:

1. Define base and follower frames via Rigid Transform blocks.
2. Parameterize the transform (e.g. rotation angle as a Simulink signal, translation as constants or signals).
3. Connect sensors/actuators to the appropriate frames. The resulting multibody model automatically manages the chain of \( \mathrm{SE}(3) \) transforms.

In Wolfram Mathematica, homogeneous transforms are represented as ordinary lists of lists (matrices). It is straightforward to define utility functions for constructing and manipulating elements of \( \mathrm{SE}(3) \).

(* Construct SE(3) transform from R (3x3) and p (3x1) *)
SE3[R_, p_] := ArrayFlatten[{
    {R, p},
    { {0, 0, 0}, {1} }
}];

(* Inverse of SE(3) transform *)
SE3Inverse[T_] := Module[{R, p},
  R = T[[1 ;; 3, 1 ;; 3]];
  p = T[[1 ;; 3, 4]];
  SE3[Transpose[R], -Transpose[R].p]
];

(* Apply transform to point x (3x1) *)
SE3TransformPoint[T_, x_] := Module[{xH, yH},
  xH = Join[x, {1}];
  yH = T.xH;
  yH[[1 ;; 3]]
];

(* Example *)
theta = Pi/4;
Rz = {
  {Cos[theta], -Sin[theta], 0},
  {Sin[theta],  Cos[theta], 0},
  {0,           0,          1}
};
p = {0.5, 0.0, 0.0};

T = SE3[Rz, p];
x = {0.1, 0.0, 0.0};
y = SE3TransformPoint[T, x]
      

Because Mathematica treats matrices symbolically as well as numerically, it is particularly convenient for proving algebraic identities in \( \mathrm{SE}(3) \) and for deriving closed-form expressions for kinematic chains.

In this lesson we introduced homogeneous transformation matrices as a compact representation of 3D rigid-body configurations. We defined the special Euclidean group \( \mathrm{SE}(3) \) as the set of transforms of the form \( T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \) with \( R \in \mathrm{SO}(3) \), \( p \in \mathbb{R}^3 \), proved the composition and inverse formulas, and showed how to chain and invert coordinate transforms between frames. We also discussed numerical issues and projection onto \( \mathrm{SE}(3) \), and we provided multi-language implementations (Python, C++, Java, MATLAB/Simulink, Wolfram Mathematica) that will be reused throughout the kinematics and dynamics parts of this course.