Consider an \( n \)-DOF serial manipulator. Let \( \mathcal{F}_s \) be a fixed space frame (often attached to the robot base) and \( \mathcal{F}_e \) the end-effector frame. The forward kinematics map is

\[ f : \mathbb{R}^n \to \mathrm{SE}(3), \quad \boldsymbol{\theta} = (\theta_1,\dots,\theta_n)^\top \longmapsto \mathbf{T}(\boldsymbol{\theta}), \]

where \( \mathbf{T}(\boldsymbol{\theta}) \) is the homogeneous transform of \( \mathcal{F}_e \) relative to \( \mathcal{F}_s \). The PoE formulation states that we can write

\[ \mathbf{T}(\boldsymbol{\theta}) = \exp([\mathbf{S}_1]^\wedge \theta_1) \exp([\mathbf{S}_2]^\wedge \theta_2) \cdots \exp([\mathbf{S}_n]^\wedge \theta_n)\,\mathbf{M}, \]

where each \( \mathbf{S}_i \in \mathbb{R}^6 \) is a screw axis (twist) of joint \( i \) expressed in the space frame at the home configuration, and \( \mathbf{M} \in \mathrm{SE}(3) \) is the end-effector configuration at \( \boldsymbol{\theta} = \mathbf{0} \).

Intuitively, each exponential term describes the rigid motion generated by one joint when that joint moves from its zero position, while the others are held fixed. The total motion is the composition (product) of these joint motions, followed by the fixed home configuration \( \mathbf{M} \).

flowchart TD
  Q["Joint variables \ntheta = (theta1,...,thetan)"]
  S["Screw axes S1,...,Sn \nin space frame"]
  M["Home configuration \nM in SE(3)"]
  PROD["Compute product exp(S1_hat * theta1) ... exp(Sn_hat * thetan)"]
  T["End-effector pose T(theta) in SE(3)"]

  Q --> PROD
  S --> PROD
  M --> PROD
  PROD --> T
        

Compared to the Denavit–Hartenberg (DH) convention, PoE is:

• Coordinate-free and directly tied to the Lie group structure of \( \mathrm{SE}(3) \).
• More systematic for complex or non-standard kinematic chains.
• Directly compatible with later chapters on differential kinematics and dynamics.

The Lie algebra \( \mathfrak{se}(3) \) is represented by matrices of the form

\[ [\mathbf{S}]^\wedge = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \mathbf{v} \\ \mathbf{0}^\top & 0 \end{bmatrix}, \quad \boldsymbol{\omega} \in \mathbb{R}^3, \; \mathbf{v} \in \mathbb{R}^3. \]

The corresponding 6-vector \( \mathbf{S} = \begin{bmatrix} \boldsymbol{\omega} \\ \mathbf{v} \end{bmatrix} \) is a twist, which encodes an instantaneous rigid-body motion:

• \( \boldsymbol{\omega} \) is the angular velocity (axis of rotation).
• \( \mathbf{v} \) is the linear velocity at the origin of the space frame.

The skew-symmetric matrix \( [\boldsymbol{\omega}]_\times \) satisfies \( [\boldsymbol{\omega}]_\times \mathbf{x} = \boldsymbol{\omega} \times \mathbf{x} \) for any \( \mathbf{x} \in \mathbb{R}^3 \).

Revolute joint screw. For a revolute joint with unit axis \( \boldsymbol{\omega} \) passing through a point \( \mathbf{q} \) in the space frame, the screw axis is

\[ \mathbf{S} = \begin{bmatrix} \boldsymbol{\omega} \\ -\boldsymbol{\omega} \times \mathbf{q} \end{bmatrix}. \]

Prismatic joint screw. For a prismatic joint with translation direction \( \mathbf{v} \) and no rotation, we have

\[ \boldsymbol{\omega} = \mathbf{0}, \quad \mathbf{S} = \begin{bmatrix} \mathbf{0} \\ \mathbf{v} \end{bmatrix}. \]

Matrix exponential of a twist. For a twist \( \mathbf{S} = (\boldsymbol{\omega}, \mathbf{v}) \) with \( \|\boldsymbol{\omega}\| = 1 \) and joint variable \( \theta \), the rigid-body motion generated by a motion of magnitude \( \theta \) is

\[ \exp([\mathbf{S}]^\wedge \theta) = \begin{bmatrix} \mathbf{R}(\theta) & \mathbf{p}(\theta) \\ \mathbf{0}^\top & 1 \end{bmatrix} \in \mathrm{SE}(3), \]

with

\[ \mathbf{R}(\theta) = \exp([\boldsymbol{\omega}]_\times \theta) = \mathbf{I} + \sin\theta\,[\boldsymbol{\omega}]_\times + (1-\cos\theta)\,[\boldsymbol{\omega}]_\times^2, \]

\[ \mathbf{p}(\theta) = (\mathbf{I} - \mathbf{R}(\theta))(\boldsymbol{\omega} \times \mathbf{v}) + \boldsymbol{\omega} \boldsymbol{\omega}^\top \mathbf{v} \,\theta. \]

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

\[ \exp([\mathbf{S}]^\wedge d) = \begin{bmatrix} \mathbf{I}_{3\times 3} & d \,\mathbf{v} \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

These formulas follow from the general series definition of the matrix exponential and the special algebraic properties of skew-symmetric matrices in \( \mathfrak{so}(3) \).

We now formalize the PoE formula. Consider an \( n \)-joint open chain with all joints at zero (home configuration) \( \boldsymbol{\theta} = \mathbf{0} \). Let \( \mathbf{M} \in \mathrm{SE}(3) \) be the transform from the space frame \( \mathcal{F}_s \) to the end-effector frame \( \mathcal{F}_e \) at home:

\[ \mathbf{T}(\mathbf{0}) = \mathbf{M}. \]

For each joint \( i \), let \( \mathbf{S}_i \in \mathbb{R}^6 \) be the twist of joint \( i \) expressed in the space frame at \( \boldsymbol{\theta} = \mathbf{0} \).

Joint \( i \) alone moving. Suppose only joint \( i \) moves while the others stay at zero:

\[ \boldsymbol{\theta} = (0,\dots,0,\theta_i,0,\dots,0)^\top. \]

Then the resulting transform of the end-effector is

\[ \mathbf{T}(\boldsymbol{\theta}) = \mathbf{G}_i(\theta_i) \,\mathbf{M}, \]

where \( \mathbf{G}_i(\theta_i) \in \mathrm{SE}(3) \) is the rigid motion generated by joint \( i \). By the theory of Lie groups and screw motions, any 1-DOF subgroup of \( \mathrm{SE}(3) \) passing through the identity has the form

\[ \mathbf{G}_i(\theta_i) = \exp([\mathbf{S}_i]^\wedge \theta_i), \]

for some twist \( \mathbf{S}_i \) (which we have defined as the screw axis of joint \( i \)).

Composition for multiple joints. Consider now a configuration \( \boldsymbol{\theta} = (\theta_1,\dots,\theta_n)^\top \). Informally, starting from home:

1. First actuate joint 1 by \( \theta_1 \), yielding \( \exp([\mathbf{S}_1]^\wedge \theta_1)\mathbf{M} \).
2. Then actuate joint 2 by \( \theta_2 \), yielding \( \exp([\mathbf{S}_1]^\wedge \theta_1)\exp([\mathbf{S}_2]^\wedge \theta_2)\mathbf{M} \).
3. Continue until joint \( n \).

By associativity of matrix multiplication, we obtain

\[ \mathbf{T}(\boldsymbol{\theta}) = \exp([\mathbf{S}_1]^\wedge \theta_1) \exp([\mathbf{S}_2]^\wedge \theta_2) \cdots \exp([\mathbf{S}_n]^\wedge \theta_n)\,\mathbf{M}. \]

Proof sketch (using a path in joint space). Define a scalar path \( \boldsymbol{\theta}(s) = s\,\boldsymbol{\theta} \), \( s \in [0,1] \), and the associated transform

\[ \mathbf{T}(s) = \exp([\mathbf{S}_1]^\wedge s\theta_1) \cdots \exp([\mathbf{S}_n]^\wedge s\theta_n)\,\mathbf{M}. \]

Then \( \mathbf{T}(0) = \mathbf{M} \), \( \mathbf{T}(1) = \mathbf{T}(\boldsymbol{\theta}) \), and \( \mathbf{T}(s) \) satisfies a left-invariant differential equation on \( \mathrm{SE}(3) \) driven by a twist that is a linear combination of the \( \mathbf{S}_i \) with coefficients \( \theta_i \). Since the forward kinematics of the physical manipulator satisfy the same ODE with the same initial condition (home configuration), uniqueness of solutions implies that the PoE expression coincides with the true forward kinematics for all \( \boldsymbol{\theta} \).

So far we have used space screws \( \mathbf{S}_i \), expressed in the space frame. Alternatively, we can use body screws \( \mathbf{B}_i \), expressed in the end-effector frame when the robot is at home.

The body PoE formula is

\[ \mathbf{T}(\boldsymbol{\theta}) = \mathbf{M} \exp([\mathbf{B}_1]^\wedge \theta_1) \exp([\mathbf{B}_2]^\wedge \theta_2) \cdots \exp([\mathbf{B}_n]^\wedge \theta_n). \]

The space and body screws are related through the adjoint representation of \( \mathrm{SE}(3) \). For \( \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^\top & 1 \end{bmatrix} \), the adjoint is

\[ \operatorname{Ad}_{\mathbf{T}} = \begin{bmatrix} \mathbf{R} & \mathbf{0} \\ [\mathbf{p}]_\times \mathbf{R} & \mathbf{R} \end{bmatrix}. \]

At home we have \( \mathbf{T}(\mathbf{0}) = \mathbf{M} \), and the relation between space and body screws is

\[ \mathbf{B}_i = \operatorname{Ad}_{\mathbf{M}^{-1}} \mathbf{S}_i, \quad \mathbf{S}_i = \operatorname{Ad}_{\mathbf{M}} \mathbf{B}_i. \]

Both formulations describe the same physical rigid-body motion; which one you choose is often dictated by convenience in later analyses (e.g., control design or Jacobian structure in later chapters).

Given:

• \( \mathbf{M} \in \mathrm{SE}(3) \): home configuration of the end-effector,
• \( \mathbf{S}_i \in \mathbb{R}^6 \) for \( i = 1,\dots,n \): screw axes in the space frame,
• \( \boldsymbol{\theta} \in \mathbb{R}^n \): vector of joint variables,

the forward kinematics can be computed as follows:

1. For each joint \( i \), construct \( [\mathbf{S}_i]^\wedge \in \mathfrak{se}(3) \).
2. Compute \( \mathbf{T}_i = \exp([\mathbf{S}_i]^\wedge \theta_i) \in \mathrm{SE}(3) \).
3. Compute the ordered product \( \mathbf{T} = \mathbf{T}_1 \mathbf{T}_2 \cdots \mathbf{T}_n \mathbf{M} \).

Numerically, the main cost is the computation of \( n \) matrix exponentials of 4×4 matrices, which can be done using closed-form formulas for twists to avoid generic exponential.

flowchart TD
  IN["Input: S_list (n screws), M, theta vector"]
  HAT["Build S_hat(i) in se(3)"]
  EXP["For each i: compute T_i = exp(S_hat(i) * theta_i)"]
  PROD["Multiply T = T1 * T2 * ... * Tn * M"]
  OUT["Output: T(theta)"]

  IN --> HAT
  HAT --> EXP
  EXP --> PROD
  PROD --> OUT
        

We present a from-scratch implementation using NumPy. In practice, the modern_robotics Python library (from Lynch & Park) provides tested implementations of these functions, but understanding the core code is critical.

import numpy as np

def skew3(omega):
    """Return [omega]_x for omega in R^3."""
    wx, wy, wz = omega
    return np.array([[0.0, -wz,  wy],
                     [wz,  0.0, -wx],
                     [-wy, wx,  0.0]])

def se3_hat(S):
    """From 6-vector S = [omega; v] to 4x4 matrix in se(3)."""
    omega = S[0:3]
    v = S[3:6]
    mat = np.zeros((4, 4))
    mat[0:3, 0:3] = skew3(omega)
    mat[0:3, 3] = v
    return mat

def matrix_exp3(omega, theta):
    """SO(3) exponential using Rodrigues formula."""
    # omega is assumed to be unit length when used for a revolute joint
    wx = skew3(omega)
    theta2 = theta * theta
    return (np.eye(3)
            + np.sin(theta) * wx
            + (1.0 - np.cos(theta)) * (wx @ wx))

def matrix_exp6(S, theta):
    """
    SE(3) exponential of twist S and joint displacement theta
    using closed-form formulas for revolute and prismatic joints.
    """
    omega = S[0:3]
    v = S[3:6]
    norm_w = np.linalg.norm(omega)

    if norm_w > 1e-8:
        # revolute case
        omega_unit = omega / norm_w
        theta_scaled = norm_w * theta
        R = matrix_exp3(omega_unit, theta_scaled)
        wx = skew3(omega_unit)
        G = (np.eye(3) * theta_scaled
             + (1.0 - np.cos(theta_scaled)) * wx
             + (theta_scaled - np.sin(theta_scaled)) * (wx @ wx))
        p = G @ (v / norm_w)
    else:
        # prismatic case
        R = np.eye(3)
        p = v * theta

    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = p
    return T

def fkine_space(M, S_list, theta_list):
    """
    Forward kinematics via PoE in the space frame.

    M: 4x4 home configuration (SE(3))
    S_list: array-like of shape (n, 6) with screw axes S_i
    theta_list: length-n iterable of joint variables
    """
    T = np.eye(4)
    for S, theta in zip(S_list, theta_list):
        T = T @ matrix_exp6(np.asarray(S, dtype=float), float(theta))
    return T @ M

if __name__ == "__main__":
    # Example: planar 2R manipulator in 3D with z-axis rotations
    # Joint 1 axis: z through origin, Joint 2 axis: z through point (L1, 0, 0)
    L1 = 1.0
    L2 = 1.0
    S1 = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])        # omega = (0,0,1), q = (0,0,0)
    S2 = np.array([0.0, 0.0, 1.0, 0.0, -L1, 0.0])        # omega = (0,0,1), q = (L1,0,0)
    S_list = np.vstack((S1, S2))

    # Home configuration: arm stretched along x-axis
    M = np.eye(4)
    M[0, 3] = L1 + L2   # end-effector at (L1+L2,0,0) at theta=(0,0)

    theta = [np.deg2rad(30.0), np.deg2rad(45.0)]
    T_theta = fkine_space(M, S_list, theta)
    print("T(theta) =")
    print(T_theta)
      

For larger systems, you may replace the custom functions by calls to scipy.linalg.expm or the modern_robotics library, but the closed-form formulas are more efficient and numerically robust for twists.

In C++, a common choice is the Eigen library for linear algebra. Below is a minimal implementation of PoE-based forward kinematics.

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

using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Vector3d;
using Eigen::VectorXd;

Matrix3d skew3(const Vector3d& omega) {
    Matrix3d wx;
    wx << 0.0,      -omega(2),  omega(1),
           omega(2),  0.0,       -omega(0),
          -omega(1),  omega(0),  0.0;
    return wx;
}

Matrix3d matrixExp3(const Vector3d& omega, double theta) {
    Matrix3d wx = skew3(omega);
    double theta2 = theta * theta;
    return Matrix3d::Identity()
         + std::sin(theta) * wx
         + (1.0 - std::cos(theta)) * (wx * wx);
}

Matrix4d matrixExp6(const Eigen::Matrix<double, 6, 1>& S, double theta) {
    Vector3d omega = S.segment<3>(0);
    Vector3d v     = S.segment<3>(3);

    double norm_w = omega.norm();
    Matrix3d R;
    Vector3d p;

    if (norm_w > 1e-8) {
        Vector3d omega_unit = omega / norm_w;
        double theta_scaled = norm_w * theta;
        R = matrixExp3(omega_unit, theta_scaled);

        Matrix3d wx = skew3(omega_unit);
        Matrix3d G = Matrix3d::Identity() * theta_scaled
                   + (1.0 - std::cos(theta_scaled)) * wx
                   + (theta_scaled - std::sin(theta_scaled)) * (wx * wx);
        p = G * (v / norm_w);
    } else {
        R = Matrix3d::Identity();
        p = v * theta;
    }

    Matrix4d T = Matrix4d::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = p;
    return T;
}

Matrix4d fkine_space(const Matrix4d& M,
                     const std::vector<Eigen::Matrix<double, 6, 1>>& S_list,
                     const VectorXd& theta) {
    Matrix4d T = Matrix4d::Identity();
    std::size_t n = S_list.size();
    for (std::size_t i = 0; i < n; ++i) {
        T = T * matrixExp6(S_list[i], theta(static_cast<int>(i)));
    }
    return T * M;
}

int main() {
    // Example: 2R planar arm
    double L1 = 1.0;
    double L2 = 1.0;

    Eigen::Matrix<double, 6, 1> S1;
    Eigen::Matrix<double, 6, 1> S2;
    S1 << 0.0, 0.0, 1.0, 0.0,  0.0, 0.0;
    S2 << 0.0, 0.0, 1.0, 0.0, -L1, 0.0;

    std::vector<Eigen::Matrix<double, 6, 1>> S_list = {S1, S2};

    Matrix4d M = Matrix4d::Identity();
    M(0,3) = L1 + L2;

    VectorXd theta(2);
    theta(0) = 30.0 * M_PI / 180.0;
    theta(1) = 45.0 * M_PI / 180.0;

    Matrix4d T = fkine_space(M, S_list, theta);
    std::cout << "T(theta) =\n" << T << std::endl;
    return 0;
}
      

For large systems, this core code can be wrapped into classes and integrated into ROS-based frameworks (e.g., KDL, Pinocchio, or RBDL), which often store kinematic trees and use PoE internally or in equivalent screw-theoretic forms.

In Java, a convenient linear algebra library is EJML. The following example shows a compact implementation of PoE-based forward kinematics.

import org.ejml.simple.SimpleMatrix;
import java.util.List;
import java.util.ArrayList;

public class PoEForwardKinematics {

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

    public static SimpleMatrix matrixExp3(SimpleMatrix omega, double theta) {
        SimpleMatrix wx = skew3(omega);
        SimpleMatrix wx2 = wx.mult(wx);
        return SimpleMatrix.identity(3)
                .plus(wx.scale(Math.sin(theta)))
                .plus(wx2.scale(1.0 - Math.cos(theta)));
    }

    public static SimpleMatrix matrixExp6(SimpleMatrix S, double theta) {
        SimpleMatrix omega = S.rows(0, 3);
        SimpleMatrix v = S.rows(3, 6);

        double normW = omega.normF();
        SimpleMatrix R, p;

        if (normW > 1e-8) {
            SimpleMatrix omegaUnit = omega.divide(normW);
            double thetaScaled = normW * theta;
            R = matrixExp3(omegaUnit, thetaScaled);

            SimpleMatrix wx = skew3(omegaUnit);
            SimpleMatrix wx2 = wx.mult(wx);

            SimpleMatrix G = SimpleMatrix.identity(3).scale(thetaScaled)
                    .plus(wx.scale(1.0 - Math.cos(thetaScaled)))
                    .plus(wx2.scale(thetaScaled - Math.sin(thetaScaled)));

            p = G.mult(v.divide(normW));
        } else {
            R = SimpleMatrix.identity(3);
            p = v.scale(theta);
        }

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

    public static SimpleMatrix fkineSpace(SimpleMatrix M,
                                          List<SimpleMatrix> Slist,
                                          double[] theta) {
        SimpleMatrix T = SimpleMatrix.identity(4);
        for (int i = 0; i < Slist.size(); ++i) {
            T = T.mult(matrixExp6(Slist.get(i), theta[i]));
        }
        return T.mult(M);
    }

    public static void main(String[] args) {
        double L1 = 1.0;
        double L2 = 1.0;

        SimpleMatrix S1 = new SimpleMatrix(6, 1, true,
                new double[]{0.0, 0.0, 1.0, 0.0,  0.0, 0.0});
        SimpleMatrix S2 = new SimpleMatrix(6, 1, true,
                new double[]{0.0, 0.0, 1.0, 0.0, -L1, 0.0});

        ArrayList<SimpleMatrix> Slist = new ArrayList<>();
        Slist.add(S1);
        Slist.add(S2);

        SimpleMatrix M = SimpleMatrix.identity(4);
        M.set(0, 3, L1 + L2);

        double[] theta = {
            Math.toRadians(30.0),
            Math.toRadians(45.0)
        };

        SimpleMatrix T = fkineSpace(M, Slist, theta);
        T.print();
    }
}
      

For Java-based robotics stacks, this core routine can be encapsulated in higher-level kinematic classes and used in simulation or control modules.

MATLAB is widely used in robotics courses. Below is a script-level implementation of PoE-based forward kinematics. It can be placed into a function and called from a MATLAB Function block in Simulink.

function T = fkine_space(M, S_list, theta)
% FKINE_SPACE  Forward kinematics via PoE (space frame).
%   M      : 4x4 home configuration
%   S_list : 6xn screw axes, each column is S_i
%   theta  : nx1 vector of joint variables

T = eye(4);
n = size(S_list, 2);
for i = 1:n
    S = S_list(:, i);
    T = T * matrixExp6(S, theta(i));
end
T = T * M;
end

function wx = skew3(omega)
wx = [   0,       -omega(3),  omega(2);
      omega(3),      0,      -omega(1);
     -omega(2),   omega(1),     0      ];
end

function R = matrixExp3(omega, theta)
wx = skew3(omega);
R = eye(3) + sin(theta) * wx + (1 - cos(theta)) * (wx * wx);
end

function T = matrixExp6(S, theta)
omega = S(1:3);
v     = S(4:6);
normW = norm(omega);

if normW > 1e-8
    omega_unit = omega / normW;
    thetaScaled = normW * theta;
    R = matrixExp3(omega_unit, thetaScaled);
    wx = skew3(omega_unit);
    G = eye(3) * thetaScaled + (1 - cos(thetaScaled)) * wx ...
        + (thetaScaled - sin(thetaScaled)) * (wx * wx);
    p = G * (v / normW);
else
    R = eye(3);
    p = v * theta;
end

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

% Example usage (script section):
L1 = 1.0;
L2 = 1.0;
S1 = [0;0;1; 0; 0;  0];
S2 = [0;0;1; 0;-L1; 0];
S_list = [S1, S2];
M = eye(4);
M(1,4) = L1 + L2;

theta = deg2rad([30; 45]);
T_theta = fkine_space(M, S_list, theta)
      

In Simulink, this code can be wrapped into a MATLAB Function block that takes M, S_list, and theta as inputs and outputs T. Combined with joint trajectory generators, this provides a kinematics subsystem suitable for later control-oriented simulations.

Mathematica provides symbolic and numeric matrix operations and a built-in MatrixExp function. We can exploit the closed-form formulas or rely on MatrixExp directly for teaching and symbolic derivations.

skew3[omega_] := {
  {0,        -omega[[3]],  omega[[2]]},
  {omega[[3]], 0,         -omega[[1]]},
  {-omega[[2]], omega[[1]], 0}
};

se3Hat[S_] := Module[{omega, v},
  omega = S[[1 ;; 3]];
  v = S[[4 ;; 6]];
  ArrayFlatten[{
    {skew3[omega],  Transpose[{v}]},
    {{0, 0, 0, 0}}
  }]
];

matrixExp6[S_, theta_] := Module[{omega, v, normW, R, p, wx, G, thetaScaled},
  omega = S[[1 ;; 3]];
  v = S[[4 ;; 6]];
  normW = Norm[omega];

  If[normW > 10^-8,
    (* revolute case *)
    thetaScaled = normW * theta;
    R = MatrixExp[skew3[omega/normW] * thetaScaled];
    wx = skew3[omega/normW];
    G = IdentityMatrix[3] * thetaScaled
        + (1 - Cos[thetaScaled]) * wx
        + (thetaScaled - Sin[thetaScaled]) * (wx.wx);
    p = G . (v/normW),
    (* prismatic case *)
    R = IdentityMatrix[3];
    p = v * theta
  ];

  ArrayFlatten[{
    {R, Transpose[{p}]},
    {{0, 0, 0, 1}}
  }]
];

fkineSpace[M_, Slist_, thetalist_] := Module[{T},
  T = IdentityMatrix[4];
  Do[
    T = T . matrixExp6[Slist[[i]], thetalist[[i]]],
    {i, 1, Length[Slist]}
  ];
  T . M
];

(* Example: 2R planar *)
L1 = 1.0;
L2 = 1.0;
S1 = {0, 0, 1, 0, 0, 0};
S2 = {0, 0, 1, 0, -L1, 0};
Slist = {S1, S2};

M = IdentityMatrix[4];
M[[1, 4]] = L1 + L2;

theta = {30 Degree, 45 Degree};
Ttheta = fkineSpace[M, Slist, theta]
      

Symbolic manipulations (e.g., leaving \( L_1, L_2, \theta_1, \theta_2 \) as symbols) can reveal structure in \( \mathbf{T}(\boldsymbol{\theta}) \) and are useful for theoretical analysis exercises.

In this lesson we used the Lie group structure of \( \mathrm{SE}(3) \) to build a coordinate-free formulation of forward kinematics for serial manipulators: the Product of Exponentials (PoE) representation. We:

• Reviewed twists and their representation as 6-vectors and 4×4 matrices in \( \mathfrak{se}(3) \).
• Derived closed-form expressions for \( \exp([\mathbf{S}]^\wedge \theta) \) for revolute and prismatic joints.
• Proved that the forward kinematics of any open-chain manipulator can be written as \( \mathbf{T}(\boldsymbol{\theta}) = \prod_{i=1}^n \exp([\mathbf{S}_i]^\wedge \theta_i)\mathbf{M} \).
• Discussed space and body formulations via the adjoint representation.
• Implemented PoE-based forward kinematics in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

The PoE formulation is the foundation for differential kinematics (Jacobians and twists of moving frames) and will be heavily reused in subsequent chapters on inverse kinematics and dynamics.