For an \(n\)-DOF serial manipulator with generalized coordinates \( \mathbf{q} = [q_1,\dots,q_n]^\top \in \mathbb{R}^n \), forward kinematics is the map

\[ \mathcal{F} : \mathbb{R}^n \rightarrow SE(3), \qquad \mathbf{T}(\mathbf{q}) = \mathcal{F}(\mathbf{q}), \]

where \( \mathbf{T}(\mathbf{q}) \in SE(3) \) encodes the end-effector pose as a homogeneous transform

\[ \mathbf{T}(\mathbf{q}) = \begin{bmatrix} \mathbf{R}(\mathbf{q}) & \mathbf{p}(\mathbf{q}) \\ \mathbf{0}^\top & 1 \end{bmatrix}, \]

with rotation \( \mathbf{R}(\mathbf{q}) \in SO(3) \) and position \( \mathbf{p}(\mathbf{q}) \in \mathbb{R}^3 \). In previous lessons we expressed \( \mathcal{F} \) symbolically using the Product of Exponentials (PoE) and Denavit–Hartenberg (DH) conventions. In this lesson we focus on numerical evaluation of \( \mathbf{T}(\mathbf{q}) \) for many joint vectors.

In the PoE formulation, assuming screw axes \( \widehat{\boldsymbol{\xi}}_i \in \mathfrak{se}(3) \) expressed in the space frame and a home configuration \( \mathbf{M} \in SE(3) \),

\[ \mathbf{T}(\mathbf{q}) = \exp(\widehat{\boldsymbol{\xi}}_1 q_1)\, \exp(\widehat{\boldsymbol{\xi}}_2 q_2)\,\cdots\, \exp(\widehat{\boldsymbol{\xi}}_n q_n)\,\mathbf{M}. \]

In the (standard) DH formulation with link transforms \( \mathbf{A}_i(q_i) \in SE(3) \):

\[ \mathbf{T}(\mathbf{q}) = \mathbf{A}_1(q_1)\,\mathbf{A}_2(q_2)\,\cdots\,\mathbf{A}_n(q_n). \]

A numerical FK toolbox thus needs:

• A data structure for storing screw axes or DH parameters.
• Reliable routines for \( \exp(\widehat{\boldsymbol{\xi}}\, q) \) or \( \mathbf{A}_i(q_i) \).
• Efficient and stable matrix multiplication in \(SE(3)\).
• Unit tests to ensure \( \mathbf{T}(\mathbf{q}) \in SE(3) \) up to numerical tolerance.

Let a twist in 6D coordinates be \( \boldsymbol{\xi} = (\boldsymbol{\omega}, \mathbf{v}) \in \mathbb{R}^6 \), with angular part \( \boldsymbol{\omega} \in \mathbb{R}^3 \) and linear part \( \mathbf{v} \in \mathbb{R}^3 \). Its hat representation in \( \mathfrak{se}(3) \) is

\[ \widehat{\boldsymbol{\xi}} = \begin{bmatrix} \widehat{\boldsymbol{\omega}} & \mathbf{v} \\ \mathbf{0}^\top & 0 \end{bmatrix}, \quad \widehat{\boldsymbol{\omega}} = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}. \]

For a revolute joint with joint variable \( q \) and \( \|\boldsymbol{\omega}\| = 1 \), the matrix exponential \( \exp(\widehat{\boldsymbol{\xi}} q) \) admits a closed form:

\[ \exp(\widehat{\boldsymbol{\xi}} q) = \begin{bmatrix} \mathbf{R}(q) & \mathbf{p}(q) \\ \mathbf{0}^\top & 1 \end{bmatrix}, \]

\[ \mathbf{R}(q) = \mathbf{I}_3 + \sin q\,\widehat{\boldsymbol{\omega}} + (1-\cos q)\,\widehat{\boldsymbol{\omega}}^2, \]

\[ \mathbf{p}(q) = \left( \mathbf{I}_3\,q + (1-\cos q)\,\widehat{\boldsymbol{\omega}} + (q-\sin q)\,\widehat{\boldsymbol{\omega}}^2 \right)\mathbf{v}. \]

For a prismatic joint with \( \boldsymbol{\omega} = \mathbf{0} \),

\[ \exp(\widehat{\boldsymbol{\xi}} q) = \begin{bmatrix} \mathbf{I}_3 & \mathbf{v}\,q \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

In practice, we implement these formulas directly instead of calling a general matrix exponential routine, achieving both efficiency and better numerical structure preservation (orthogonality of \( \mathbf{R}(q) \)).

A generic numerical FK routine for PoE-based models can be written as:

1. Read joint vector \( \mathbf{q} \).
2. For each joint \( i \), compute \( \mathbf{T}_i(q_i) = \exp(\widehat{\boldsymbol{\xi}}_i q_i) \).
3. Multiply transforms in the correct order.
4. Optionally multiply by the home configuration \( \mathbf{M} \).
5. Return \( \mathbf{T}(\mathbf{q}) \) and, if required, intermediate frames.

flowchart TD
  Q["Joint vector q"] --> MODEL["Robot model (screws or DH params)"]
  MODEL --> LOOP["For i = 1..n"]
  LOOP --> Ti["Compute transform T_i(q_i)"]
  Ti --> PROD["Accumulate product T = T * T_i"]
  PROD --> EE["End-effector pose T(q) in SE(3)"]
  EE --> CHECK["Optional checks: det(R) approx 1, R^T R approx I"]
        

For DH, the internal loop replaces the twist exponential with \( \mathbf{A}_i(q_i) \) computed from \( (a_i, \alpha_i, d_i, \theta_i) \). For example, the standard DH transform is

\[ \mathbf{A}_i(q_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}, \]

where typically \( \theta_i = q_i + \theta_i^{\text{offset}} \) or \( d_i = q_i + d_i^{\text{offset}} \) depending on joint type.

Python is a natural choice for a prototype FK toolbox thanks to numpy for linear algebra and the availability of robotics libraries like Peter Corke's roboticstoolbox-python. Here we implement a minimal PoE-based FK core without depending on external robotics packages, to make the underlying mathematics explicit.

4.1 Core Lie Algebra Utilities

import numpy as np

def skew3(omega):
    """Return 3x3 skew-symmetric matrix hat(omega)."""
    wx, wy, wz = omega
    return np.array([
        [0.0, -wz,  wy],
        [wz,  0.0, -wx],
        [-wy, wx,  0.0]
    ])

def exp_twist(xi, q, eps=1e-9):
    """
    Exponential map for a twist xi = (omega, v) in R^6.
    Returns a 4x4 SE(3) matrix exp(hat(xi) q).
    """
    omega = np.asarray(xi[:3]).reshape(3,)
    v = np.asarray(xi[3:]).reshape(3,)
    theta = q
    if np.linalg.norm(omega) < eps:  # prismatic
        R = np.eye(3)
        p = v * theta
    else:
        omega = omega / np.linalg.norm(omega)
        w_hat = skew3(omega)
        R = (
            np.eye(3)
            + np.sin(theta) * w_hat
            + (1.0 - np.cos(theta)) * (w_hat @ w_hat)
        )
        V = (
            np.eye(3) * theta
            + (1.0 - np.cos(theta)) * w_hat
            + (theta - np.sin(theta)) * (w_hat @ w_hat)
        )
        p = V @ v
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = p
    return T
      

4.2 FK for a Serial Chain (PoE)

class RobotPoe:
    """
    Simple serial manipulator model using the PoE formulation.

    Attributes
    ----------
    S_list : (6, n) array
        Columns are screw axes in the space frame.
    M : (4, 4) array
        Home configuration of the end-effector.
    """

    def __init__(self, S_list, M):
        S_list = np.asarray(S_list, dtype=float)
        assert S_list.shape[0] == 6
        self.S_list = S_list
        self.n = S_list.shape[1]
        self.M = np.asarray(M, dtype=float)

    def fk(self, q):
        """
        Compute T(q) = exp(S1 q1) ... exp(Sn qn) M.

        Parameters
        ----------
        q : array_like, shape (n,)
            Joint angles (revolute) or displacements (prismatic).

        Returns
        -------
        T : (4, 4) ndarray
            Homogeneous transform in SE(3).
        """
        q = np.asarray(q, dtype=float).reshape(self.n,)
        T = np.eye(4)
        for i in range(self.n):
            xi = self.S_list[:, i]
            T = T @ exp_twist(xi, q[i])
        return T @ self.M

    def check_se3(self, T, tol=1e-6):
        """
        Check whether T is numerically in SE(3).
        Returns (ok, eps_R, eps_det).
        """
        R = T[:3, :3]
        RtR = R.T @ R
        eps_R = np.linalg.norm(RtR - np.eye(3), ord="fro")
        eps_det = abs(np.linalg.det(R) - 1.0)
        ok = (eps_R < tol) and (eps_det < tol)
        return ok, eps_R, eps_det
      

This design separates geometric information (\( \mathbf{S}_i \), \( \mathbf{M} \)) from joint variables. It is straightforward to wrap this in higher-level tooling (e.g., loading parameters from a YAML file, adding visualization, etc.).

To support multiple languages, we often separate the model description (e.g., PoE or DH parameters) from language-specific bindings. A typical layout:

flowchart TD
  P["Parameter file (JSON/YAML)"] --> LOAD["Model loader"]
  LOAD --> ROBOT["RobotModel (language-independent concept)"]
  ROBOT --> FKFN["fk(q) core routine"]
  FKFN --> PY["Python wrapper"]
  FKFN --> CPP["C++ wrapper"]
  FKFN --> JAVA["Java wrapper"]
  PY --> APPS["Simulation / plotting"]
  CPP --> APPS
  JAVA --> APPS
        

Mathematically, all these bindings implement the same map \( \mathcal{F} \); differences are in memory layout, numeric precision, and performance. The rest of the lesson illustrates one implementation in each language.

In C++, the Eigen library is the de facto standard for matrix computations, while robot libraries such as KDL (Kinematics and Dynamics Library) provide higher-level kinematic primitives. Below is a compact PoE-based FK implementation using Eigen, following the same mathematics as in the Python toolbox.

#include <Eigen/Dense>
#include <vector>

using SE3 = Eigen::Matrix4d;
using Vec3 = Eigen::Vector3d;
using Twist = Eigen::Matrix<double, 6, 1>;

Eigen::Matrix3d skew3(const Vec3& w) {
    Eigen::Matrix3d W;
    W << 0.0,   -w.z(),  w.y(),
          w.z(),  0.0,   -w.x(),
         -w.y(),  w.x(),  0.0;
    return W;
}

SE3 expTwist(const Twist& xi, double q, double eps = 1e-9) {
    Vec3 w = xi.head<3>();
    Vec3 v = xi.tail<3>();

    SE3 T = SE3::Identity();
    double theta = q;
    if (w.norm() < eps) {
        // prismatic
        T.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
        T.block<3,1>(0,3) = v * theta;
    } else {
        w.normalize();
        Eigen::Matrix3d W = skew3(w);
        Eigen::Matrix3d W2 = W * W;
        Eigen::Matrix3d R =
            Eigen::Matrix3d::Identity()
            + std::sin(theta) * W
            + (1.0 - std::cos(theta)) * W2;
        Eigen::Matrix3d V =
            Eigen::Matrix3d::Identity() * theta
            + (1.0 - std::cos(theta)) * W
            + (theta - std::sin(theta)) * W2;
        T.block<3,3>(0,0) = R;
        T.block<3,1>(0,3) = V * v;
    }
    return T;
}

struct RobotPoeCpp {
    std::vector<Twist> S_list;
    SE3 M;

    SE3 fk(const std::vector<double>& q) const {
        SE3 T = SE3::Identity();
        for (std::size_t i = 0; i < S_list.size(); ++i) {
            T = T * expTwist(S_list[i], q[i]);
        }
        return T * M;
    }
};
      

This skeleton mirrors the Python design. In a full robotics library, one would further integrate with KDL segment-based structures and add unit tests for \( \det(\mathbf{R}) \) and orthogonality.

For Java, a widely used numerical library is EJML (Efficient Java Matrix Library). We outline a minimal PoE-based FK class. The mathematical operations are exactly those used in Python and C++.

import org.ejml.simple.SimpleMatrix;

public class RobotPoeJava {
    // S_list: 6 x n matrix (each column is a twist)
    private final SimpleMatrix S_list;
    private final SimpleMatrix M;
    private final int n;

    public RobotPoeJava(SimpleMatrix S_list, SimpleMatrix M) {
        if (S_list.numRows() != 6) {
            throw new IllegalArgumentException("S_list must be 6 x n");
        }
        this.S_list = S_list;
        this.M = M;
        this.n = S_list.numCols();
    }

    private SimpleMatrix skew3(SimpleMatrix w) {
        double wx = w.get(0);
        double wy = w.get(1);
        double wz = w.get(2);
        double[][] data = {
            {0.0,   -wz,   wy},
            {wz,    0.0,  -wx},
            {-wy,   wx,    0.0}
        };
        return new SimpleMatrix(data);
    }

    private SimpleMatrix expTwist(SimpleMatrix xi, double q, double eps) {
        SimpleMatrix w = xi.extractMatrix(0, 3, 0, 1);
        SimpleMatrix v = xi.extractMatrix(3, 6, 0, 1);
        double theta = q;
        double normW = Math.sqrt(
            w.get(0) * w.get(0) +
            w.get(1) * w.get(1) +
            w.get(2) * w.get(2)
        );

        SimpleMatrix T = SimpleMatrix.identity(4);
        if (normW < eps) {
            // prismatic
            T.insertIntoThis(0, 0, SimpleMatrix.identity(3));
            T.insertIntoThis(0, 3, v.scale(theta));
        } else {
            SimpleMatrix w_unit = w.divide(normW);
            SimpleMatrix W = skew3(w_unit);
            SimpleMatrix W2 = W.mult(W);
            SimpleMatrix I3 = SimpleMatrix.identity(3);

            SimpleMatrix R = I3
                .plus(W.scale(Math.sin(theta)))
                .plus(W2.scale(1.0 - Math.cos(theta)));

            SimpleMatrix V = I3.scale(theta)
                .plus(W.scale(1.0 - Math.cos(theta)))
                .plus(W2.scale(theta - Math.sin(theta)));

            T.insertIntoThis(0, 0, R);
            T.insertIntoThis(0, 3, V.mult(v));
        }
        return T;
    }

    public SimpleMatrix fk(double[] q) {
        if (q.length != n) {
            throw new IllegalArgumentException("q must have length n");
        }
        SimpleMatrix T = SimpleMatrix.identity(4);
        double eps = 1e-9;
        for (int i = 0; i < n; ++i) {
            SimpleMatrix xi = S_list.extractMatrix(0, 6, i, i + 1);
            T = T.mult(expTwist(xi, q[i], eps));
        }
        return T.mult(M);
    }
}
      

This provides a reusable FK core in Java. The same pattern can be wrapped inside Android or desktop applications for visualization and simulation.

MATLAB has a rich ecosystem for robotics, notably Robotics System Toolbox with rigidBodyTree objects. We again implement a minimal PoE-based FK function from scratch and mention Simulink integration.

8.1 MATLAB Function for PoE FK

function T = fk_poe(S_list, M, q)
%FK_POE Forward kinematics using the product of exponentials.
%   S_list: 6 x n matrix of screw axes (space frame)
%   M     : 4 x 4 home configuration
%   q     : n x 1 vector of joint variables

n = size(S_list, 2);
T = eye(4);
for i = 1:n
    xi = S_list(:, i);
    T = T * exp_twist(xi, q(i));
end
T = T * M;
end

function T = exp_twist(xi, q)
omega = xi(1:3);
v = xi(4:6);
theta = q;

if norm(omega) < 1e-9
    R = eye(3);
    p = v * theta;
else
    omega = omega / norm(omega);
    wx = omega(1); wy = omega(2); wz = omega(3);
    W = [   0, -wz,  wy;
          wz,   0, -wx;
         -wy,  wx,   0 ];
    W2 = W * W;
    R = eye(3) + sin(theta) * W + (1 - cos(theta)) * W2;
    V = eye(3) * theta + (1 - cos(theta)) * W + (theta - sin(theta)) * W2;
    p = V * v;
end

T = eye(4);
T(1:3,1:3) = R;
T(1:3,4) = p;
end
      

8.2 Simulink Use

To use this in Simulink, one typically wraps fk_poe in a MATLAB Function block:

1. Create a MATLAB Function block and paste the core of fk_poe.
2. Expose \( \mathbf{q} \) as input and components of \( \mathbf{T} \) as outputs.
3. Set S_list and M as block parameters or constants.
4. Connect to downstream blocks (visualization, logging, or further kinematic processing).

This produces a real-time-capable numeric FK block suitable for simulation of joint trajectories (without yet involving dynamics or control).

Wolfram Mathematica supports both symbolic and numeric matrix exponentials, making it convenient for verifying FK formulas. We implement the PoE map symbolically and then evaluate numerically.

(* Hat operator for so(3) *)
Skew3[{wx_, wy_, wz_}] := {
  {0, -wz, wy},
  {wz, 0, -wx},
  {-wy, wx, 0}
};

(* Hat operator for se(3) given a 6-vector twist *)
TwistHat[{wx_, wy_, wz_, vx_, vy_, vz_}] := Module[
  {W, v},
  W = Skew3[{wx, wy, wz}];
  v = { {vx}, {vy}, {vz} };
  ArrayFlatten[{
    {W, v},
    {{0, 0, 0, 0}}
  }]
];

(* Exponential of a twist *)
ExpTwist[xi_List, q_] := MatrixExp[TwistHat[xi] q];

(* Forward kinematics for a list of twists SList and home configuration M *)
FKPoE[SList_List, M_, q_List] := Module[
  {n = Length[SList], T},
  T = IdentityMatrix[4];
  Do[
    T = T . ExpTwist[SList[[i]], q[[i]]],
    {i, 1, n}
  ];
  T . M
];

(* Example: numeric evaluation for some q *)
SList = {
  {0, 0, 1, 0, 0, 0},
  {0, 0, 1, 0, -0.5, 0},
  {0, 0, 1, 0, -1.0, 0}
};
M = { {1, 0, 0, 1.5},
     {0, 1, 0, 0},
     {0, 0, 1, 0},
     {0, 0, 0, 1} };

qvec = {Pi/6, -Pi/4, Pi/3};
Tq = FKPoE[SList, M, qvec] // N
      

Symbolic expressions for \( \mathbf{T}(\mathbf{q}) \) can be simplified and exported to other languages (e.g., as C or Java code) while retaining the exact analytic structure derived earlier in the course.

Because FK is a composition of many nonlinear operations, floating-point errors can accumulate. We discuss some simple diagnostics.

10.1 SE(3) Consistency Tests

Given a numerically computed transform \( \mathbf{T}(\mathbf{q}) \), define

\[ \epsilon_R(\mathbf{q}) = \left\|\mathbf{R}(\mathbf{q})^\top\mathbf{R}(\mathbf{q}) - \mathbf{I}_3\right\|_F, \quad \epsilon_{\det}(\mathbf{q}) = \bigl|\det(\mathbf{R}(\mathbf{q})) - 1\bigr|. \]

In exact arithmetic, both would be zero; in finite precision they should remain small (e.g., \( \epsilon_R \approx 10^{-12} \) for double precision in moderate chain lengths). Large deviations indicate bugs or extreme ill-conditioning.

10.2 Pose Error Metrics

Suppose we have an analytic reference transform \( \mathbf{T}_{\text{ref}}(\mathbf{q}) \) and a numerical implementation \( \widetilde{\mathbf{T}}(\mathbf{q}) \). A common translational error metric is

\[ e_{\text{pos}}(\mathbf{q}) = \left\|\mathbf{p}_{\text{ref}}(\mathbf{q}) - \widetilde{\mathbf{p}}(\mathbf{q})\right\|_2. \]

For rotation, define the relative rotation \( \mathbf{R}_{\text{rel}}(\mathbf{q}) = \mathbf{R}_{\text{ref}}(\mathbf{q})\,\widetilde{\mathbf{R}}(\mathbf{q})^\top \) and the geodesic angle

\[ e_{\text{rot}}(\mathbf{q}) = \arccos\left( \frac{\operatorname{trace}(\mathbf{R}_{\text{rel}}(\mathbf{q})) - 1}{2} \right). \]

Empirically sampling many joint vectors \( \mathbf{q} \) and measuring \( e_{\text{pos}} \) and \( e_{\text{rot}} \) is a robust way of validating the toolbox implementation against a known-good formulation (e.g., symbolic Mathematica expressions).

In this lab-oriented lesson we translated the abstract PoE and DH formulations of forward kinematics into concrete, numerical algorithms and reusable software components. We implemented the twist exponential and FK map in Python, C++, Java, MATLAB, Simulink, and Wolfram Mathematica, and discussed diagnostics for checking that numeric transforms remain in \( SE(3) \). These FK toolboxes form a foundation for subsequent topics such as inverse kinematics, differential kinematics, and eventually dynamics and control, where accurate evaluation of \( \mathbf{T}(\mathbf{q}) \) is essential.