Let \( \{s\} \) be a fixed space (base) frame and \( \{e\} \) the end-effector frame. The configuration of the end-effector is an element \( \mathbf{g}(t) \in SE(3) \) written as

\[ \mathbf{g}(t) = \begin{bmatrix} \mathbf{R}(t) & \mathbf{p}(t) \\ \mathbf{0}^T & 1 \end{bmatrix},\quad \mathbf{R}(t) \in SO(3),\ \mathbf{p}(t)\in\mathbb{R}^3. \]

The spatial twist of the end-effector, expressed in the space frame, is a 6-vector \( \mathbf{V}_s = \begin{bmatrix} \boldsymbol{\omega}_s \\ \mathbf{v}_s \end{bmatrix} \in \mathbb{R}^6 \) whose hat representation is the element of the Lie algebra \( \mathfrak{se}(3) \)

\[ \widehat{\mathbf{V}}_s = \begin{bmatrix} [\boldsymbol{\omega}_s]_\times & \mathbf{v}_s \\ \mathbf{0}^T & 0 \end{bmatrix}. \]

A fundamental result from Lie group theory (already introduced in the SE(3) chapter) is that the instantaneous velocity of a rigid motion satisfies

\[ \widehat{\mathbf{V}}_s = \dot{\mathbf{g}}(t)\,\mathbf{g}(t)^{-1}. \]

Similarly, we can express the same physical velocity in the end-effector (body) frame. The corresponding body twist \( \mathbf{V}_b = \begin{bmatrix} \boldsymbol{\omega}_b \\ \mathbf{v}_b \end{bmatrix} \) has hat representation

\[ \widehat{\mathbf{V}}_b = \mathbf{g}(t)^{-1}\dot{\mathbf{g}}(t). \]

These two twists describe the same instantaneous motion, but in different coordinates. They are related through the adjoint transformation, which we will make explicit after revisiting the adjoint representation of SE(3).

flowchart TD
  G["g(t) in SE(3)"] --> VS["V_s hat = dot(g) * g^{-1}"]
  G --> VB["V_b hat = g^{-1} * dot(g)"]
  VS --> REL["Same physical motion"]
  VB --> REL
  REL --> ADJ["Related by Adjoint: V_s = Ad_g V_b"]
        

For any configuration \( \mathbf{g} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3) \), the adjoint representation \( \text{Ad}_{\mathbf{g}} : \mathbb{R}^6 \rightarrow \mathbb{R}^6 \) is defined by

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

By construction, the adjoint satisfies the compatibility condition

\[ \mathbf{g}\,\widehat{\mathbf{V}}_b\,\mathbf{g}^{-1} = \widehat{\text{Ad}_{\mathbf{g}} \mathbf{V}_b}, \]

where the right-hand side uses the hat map applied to the 6-vector \( \text{Ad}_{\mathbf{g}} \mathbf{V}_b \).

Starting from the body twist definition \( \widehat{\mathbf{V}}_b = \mathbf{g}^{-1}\dot{\mathbf{g}} \), left-multiply by \( \mathbf{g} \) and right-multiply by \( \mathbf{g}^{-1} \):

\[ \mathbf{g}\,\widehat{\mathbf{V}}_b\,\mathbf{g}^{-1} = \mathbf{g}\,\mathbf{g}^{-1}\dot{\mathbf{g}}\,\mathbf{g}^{-1} = \dot{\mathbf{g}}\,\mathbf{g}^{-1} = \widehat{\mathbf{V}}_s. \]

Using the compatibility of the adjoint with the hat map, we deduce

\[ \widehat{\mathbf{V}}_s = \widehat{\text{Ad}_{\mathbf{g}}\mathbf{V}}_b \quad\Rightarrow\quad \mathbf{V}_s = \text{Ad}_{\mathbf{g}}\,\mathbf{V}_b. \]

Thus, the spatial and body twists for the same rigid motion are related simply by a linear change of coordinates determined by \( \mathbf{g} \). In particular,

\[ \mathbf{V}_b = \text{Ad}_{\mathbf{g}^{-1}} \mathbf{V}_s. \]

This relationship will later induce a corresponding relation between the spatial and body Jacobians of a manipulator: they differ only by the same adjoint transformation evaluated at the end-effector configuration.

Consider an n-DOF serial manipulator modeled in the PoE form (already developed in the PoE chapter). Let \( \mathbf{S}_i \in \mathbb{R}^6 \) be the space screw axes of the joints, all expressed in the space frame at the zero configuration. Let \( \hat{\mathbf{S}}_i \in \mathfrak{se}(3) \) be their hat representations. The forward kinematics is

\[ \mathbf{T}(\mathbf{q}) = \exp(\hat{\mathbf{S}}_1 q_1)\, \exp(\hat{\mathbf{S}}_2 q_2)\, \cdots \exp(\hat{\mathbf{S}}_n q_n)\, \mathbf{M}, \]

where \( \mathbf{M} \in SE(3) \) is the home configuration at \( \mathbf{q} = \mathbf{0} \).

For joint velocities \( \dot{\mathbf{q}} = [\dot{q}_1,\dots,\dot{q}_n]^T \), the end-effector spatial twist satisfies

\[ \mathbf{V}_s = \mathbf{J}_s(\mathbf{q})\,\dot{\mathbf{q}}, \]

where \( \mathbf{J}_s(\mathbf{q}) \in \mathbb{R}^{6 \times n} \) is the spatial Jacobian. Each column corresponds to the contribution of one joint rate to the spatial twist.

Using the group product rule and the identity \( \frac{d}{dt}\exp(\hat{\mathbf{S}} q) = \hat{\mathbf{S}}\,\exp(\hat{\mathbf{S}} q)\,\dot{q} \) (which follows from the definition of the exponential map and the chain rule), one can show that the i-th column of the spatial Jacobian is

\[ \mathbf{J}_s(\mathbf{q}) = \begin{bmatrix} \mathbf{S}_1 & \text{Ad}_{\exp(\hat{\mathbf{S}}_1 q_1)}\,\mathbf{S}_2 & \cdots & \text{Ad}_{\exp(\hat{\mathbf{S}}_1 q_1)\cdots\exp(\hat{\mathbf{S}}_{n-1} q_{n-1})}\,\mathbf{S}_n \end{bmatrix}. \]

Intuitively, the first joint axis is fixed in space, so its column is just \( \mathbf{S}_1 \). The second joint axis is rotated by the first joint, hence its transformed axis appears as \( \text{Ad}_{\exp(\hat{\mathbf{S}}_1 q_1)}\,\mathbf{S}_2 \), and so on.

Instead of expressing twists in the space frame, one can express them in the end-effector frame. Let \( \mathbf{B}_i \in \mathbb{R}^6 \) be the body screw axes of the joints, measured at the zero configuration, and \( \hat{\mathbf{B}}_i \) their hat matrices. An alternative PoE representation is

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

The body twist of the end-effector satisfies

\[ \mathbf{V}_b = \mathbf{J}_b(\mathbf{q})\,\dot{\mathbf{q}}, \]

where \( \mathbf{J}_b(\mathbf{q}) \in \mathbb{R}^{6 \times n} \) is the body Jacobian. A direct derivation (similar in spirit to the spatial derivation but using the right-trivialization of velocities) leads to

\[ \mathbf{J}_b(\mathbf{q}) = \begin{bmatrix} \text{Ad}_{\exp(-\hat{\mathbf{B}}_2 q_2)\cdots\exp(-\hat{\mathbf{B}}_n q_n)}\,\mathbf{B}_1 & \cdots & \text{Ad}_{\exp(-\hat{\mathbf{B}}_n q_n)}\,\mathbf{B}_{n-1} & \mathbf{B}_n \end{bmatrix}. \]

The spatial and body Jacobians are not independent. Using the fact that the same motion has twists related by \( \mathbf{V}_s = \text{Ad}_{\mathbf{T}(\mathbf{q})}\mathbf{V}_b \) and substituting \( \mathbf{V}_s = \mathbf{J}_s(\mathbf{q})\dot{\mathbf{q}} \), \( \mathbf{V}_b = \mathbf{J}_b(\mathbf{q})\dot{\mathbf{q}} \), we obtain

\[ \mathbf{J}_s(\mathbf{q})\dot{\mathbf{q}} = \text{Ad}_{\mathbf{T}(\mathbf{q})}\, \mathbf{J}_b(\mathbf{q})\dot{\mathbf{q}}, \quad \forall\,\dot{\mathbf{q}}. \]

Since this holds for all joint-rate vectors, it must be that

\[ \mathbf{J}_s(\mathbf{q}) = \text{Ad}_{\mathbf{T}(\mathbf{q})}\, \mathbf{J}_b(\mathbf{q}), \quad \mathbf{J}_b(\mathbf{q}) = \text{Ad}_{\mathbf{T}(\mathbf{q})^{-1}}\, \mathbf{J}_s(\mathbf{q}). \]

Thus, once either the spatial or body Jacobian is known, the other can be recovered by a single adjoint transformation, which is inexpensive to compute numerically.

Consider a planar 2R arm in the plane of the space frame with link lengths \( l_1, l_2 \) and joint angles \( q_1, q_2 \). We embed the planar motion in SE(3) by restricting motion to the x–y plane and rotation about the z-axis.

Choose screw axes in space coordinates:

• Joint 1: rotation about z through the origin: \( \mathbf{S}_1 = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}^T \).
• Joint 2: rotation about z through point \( (l_1, 0, 0) \): \( \mathbf{S}_2 = \begin{bmatrix} 0 & 0 & 1 & 0 & -l_1 & 0 \end{bmatrix}^T \).

The home configuration \( \mathbf{M} \) (when \( q_1 = q_2 = 0 \)) places the end-effector at \( (l_1 + l_2, 0, 0) \) with no rotation:

\[ \mathbf{M} = \begin{bmatrix} \mathbf{I}_3 & [l_1 + l_2, 0, 0]^T \\ \mathbf{0}^T & 1 \end{bmatrix}. \]

A straightforward computation of the spatial Jacobian yields

\[ \mathbf{J}_s(\mathbf{q}) = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 1 \\ 0 & -l_1 \sin q_1 \\ 0 & l_1 \cos q_1 \\ 0 & 0 \end{bmatrix}. \]

The body Jacobian can be computed either directly using body screws or via the adjoint relation \( \mathbf{J}_b = \text{Ad}_{\mathbf{T}(\mathbf{q})^{-1}} \mathbf{J}_s \). For this planar case one obtains

\[ \mathbf{J}_b(\mathbf{q}) = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 1 \\ -l_2 \sin(q_2) & -l_2 \sin(q_2) \\ l_2 \cos(q_2) & l_2 \cos(q_2) \\ 0 & 0 \end{bmatrix}, \]

where the body frame is attached to the end-effector. Although this example is low-dimensional, the same principles extend directly to general 6-DOF spatial manipulators.

Given screw axes \( \mathbf{S}_i \), body axes \( \mathbf{B}_i \), and joint variables \( \mathbf{q} \), a standard algorithm to compute \( \mathbf{J}_s(\mathbf{q}) \) proceeds by forward recursion of transforms; \( \mathbf{J}_b(\mathbf{q}) \) can then be obtained via the adjoint. A typical workflow is:

flowchart TD
  START["Input: S_i, M, q"] --> FK["Compute T(q) via PoE"]
  FK --> JS["Build J_s: columns using cumulative transforms"]
  JS --> JB["Compute J_b = Ad_{T(q)^{-1}} * J_s"]
  JB --> TWIST["Twists: V_s = J_s * qdot, V_b = J_b * qdot"]
        

This separation of tasks (first kinematics, then Jacobians, then twists) reflects a modular implementation pattern that will be reused in dynamics and control computations.

We implement helper routines for \( \mathfrak{se}(3) \) operations, PoE forward kinematics, and spatial/body Jacobians using NumPy. This is compatible with standard robotics libraries (e.g., modern_robotics), but implemented from scratch here for transparency.

import numpy as np

def skew(v):
    """Return [v]_x for v in R^3."""
    vx, vy, vz = v
    return np.array([[0.0, -vz,  vy],
                     [vz,  0.0, -vx],
                     [-vy, vx,  0.0]])

def se3_hat(S):
    """
    Map a twist S = [omega; v] in R^6 to se(3) hat matrix.
    S: shape (6,)
    """
    omega = S[0:3]
    v = S[3:6]
    mat = np.zeros((4, 4))
    mat[0:3, 0:3] = skew(omega)
    mat[0:3, 3] = v
    return mat

def se3_exp(S, theta, tol=1e-9):
    """
    Exponential map exp( [S]^ ^ theta ) for twist S, scalar theta.
    Uses the Rodrigues formula for SO(3) and standard SE(3) formula.
    """
    S_hat = se3_hat(S)
    omega = S[0:3]
    v = S[3:6]
    omega_norm = np.linalg.norm(omega)
    if omega_norm < tol:
        # Pure translation case
        T = np.eye(4)
        T[0:3, 3] = v * theta
        return T
    # Rotation part
    w = omega / omega_norm
    w_hat = skew(w)
    theta_w = omega_norm * theta
    R = (np.eye(3)
         + np.sin(theta_w) * w_hat
         + (1.0 - np.cos(theta_w)) * (w_hat @ w_hat))
    # Translation part
    G = (np.eye(3) * theta_w
         + (1.0 - np.cos(theta_w)) * w_hat
         + (theta_w - np.sin(theta_w)) * (w_hat @ w_hat))
    p = G @ (v / omega_norm)
    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = p
    return T

def adjoint(T):
    """
    Ad_T for T in SE(3) given as 4x4 matrix.
    """
    R = T[0:3, 0:3]
    p = T[0:3, 3]
    Ad = np.zeros((6, 6))
    Ad[0:3, 0:3] = R
    Ad[3:6, 3:6] = R
    Ad[3:6, 0:3] = skew(p) @ R
    return Ad

def fk_poe_space(S_list, M, q):
    """
    Forward kinematics using space PoE.
    S_list: list of twists S_i in space frame, each shape (6,)
    M: 4x4 home configuration
    q: array of joint angles, shape (n,)
    """
    T = np.eye(4)
    for S, theta in zip(S_list, q):
        T = T @ se3_exp(S, theta)
    T = T @ M
    return T

def jacobian_space(S_list, q):
    """
    Spatial Jacobian J_s(q) for list of space screws S_i and configuration q.
    Returns 6 x n matrix.
    """
    n = len(S_list)
    J = np.zeros((6, n))
    T = np.eye(4)
    for i in range(n):
        if i == 0:
            J[:, 0] = S_list[0].reshape(6,)
        else:
            Ad_T = adjoint(T)
            J[:, i] = Ad_T @ S_list[i]
        # update T for next column
        T = T @ se3_exp(S_list[i], q[i])
    return J

def jacobian_body(S_list, M, q):
    """
    Body Jacobian J_b(q) computed via J_b = Ad_{T(q)^{-1}} * J_s(q).
    """
    T = fk_poe_space(S_list, M, q)
    J_s = jacobian_space(S_list, q)
    Ad_inv = adjoint(np.linalg.inv(T))
    return Ad_inv @ J_s

# Example usage for planar 2R with l1, l2
if __name__ == "__main__":
    l1, l2 = 1.0, 0.8
    S1 = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
    S2 = np.array([0.0, 0.0, 1.0, 0.0, -l1, 0.0])
    S_list = [S1, S2]
    M = np.eye(4)
    M[0, 3] = l1 + l2
    q = np.array([0.5, -0.4])
    Tq = fk_poe_space(S_list, M, q)
    Js = jacobian_space(S_list, q)
    Jb = jacobian_body(S_list, M, q)
    print("T(q) =\n", Tq)
    print("J_s(q) =\n", Js)
    print("J_b(q) =\n", Jb)
      

This code is directly usable as a basis for more advanced tasks such as differential IK and manipulability analysis in later chapters.

The following C++ code (using the Eigen library) implements the core operations. Note that angle brackets are escaped as HTML entities <, > for embedding into HTML.

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

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

Matrix3d skew(const Vector3d& v) {
    Matrix3d S;
    S <<     0.0, -v(2),  v(1),
           v(2),    0.0, -v(0),
          -v(1),  v(0),   0.0;
    return S;
}

Matrix4d se3Hat(const VectorXd& S) {
    Matrix4d M = Matrix4d::Zero();
    Vector3d omega = S.segment<3>(0);
    Vector3d v = S.segment<3>(3);
    M.block<3,3>(0,0) = skew(omega);
    M.block<3,1>(0,3) = v;
    return M;
}

// Exponential for twist S and scalar theta
Matrix4d se3Exp(const VectorXd& S, double theta, double tol=1e-9) {
    Matrix4d result = Matrix4d::Identity();
    Vector3d omega = S.segment<3>(0);
    Vector3d v = S.segment<3>(3);
    double wnorm = omega.norm();
    if (wnorm < tol) {
        // Pure translation
        result.block<3,1>(0,3) = v * theta;
        return result;
    }
    Vector3d w = omega / wnorm;
    Matrix3d w_hat = skew(w);
    double th = wnorm * theta;
    Matrix3d R = Matrix3d::Identity()
                 + std::sin(th) * w_hat
                 + (1.0 - std::cos(th)) * (w_hat * w_hat);
    Matrix3d G = Matrix3d::Identity() * th
                 + (1.0 - std::cos(th)) * w_hat
                 + (th - std::sin(th)) * (w_hat * w_hat);
    Vector3d p = G * (v / wnorm);
    result.block<3,3>(0,0) = R;
    result.block<3,1>(0,3) = p;
    return result;
}

Eigen::Matrix<double,6,6> adjoint(const Matrix4d& T) {
    Eigen::Matrix<double,6,6> Ad = Eigen::Matrix<double,6,6>::Zero();
    Matrix3d R = T.block<3,3>(0,0);
    Vector3d p = T.block<3,1>(0,3);
    Ad.block<3,3>(0,0) = R;
    Ad.block<3,3>(3,3) = R;
    Ad.block<3,3>(3,0) = skew(p) * R;
    return Ad;
}

Matrix4d fkPoeSpace(const std::vector<VectorXd>& Slist,
                    const Matrix4d& M,
                    const VectorXd& q) {
    Matrix4d T = Matrix4d::Identity();
    for (int i = 0; i < (int)Slist.size(); ++i) {
        T = T * se3Exp(Slist[i], q(i));
    }
    T = T * M;
    return T;
}

MatrixXd jacobianSpace(const std::vector<VectorXd>& Slist,
                       const VectorXd& q) {
    int n = (int)Slist.size();
    MatrixXd J(6, n);
    J.setZero();
    Matrix4d T = Matrix4d::Identity();
    for (int i = 0; i < n; ++i) {
        if (i == 0) {
            J.col(0) = Slist[0];
        } else {
            Eigen::Matrix<double,6,6> AdT = adjoint(T);
            J.col(i) = AdT * Slist[i];
        }
        T = T * se3Exp(Slist[i], q(i));
    }
    return J;
}

MatrixXd jacobianBody(const std::vector<VectorXd>& Slist,
                      const Matrix4d& M,
                      const VectorXd& q) {
    Matrix4d T = fkPoeSpace(Slist, M, q);
    MatrixXd Js = jacobianSpace(Slist, q);
    Eigen::Matrix<double,6,6> AdInv = adjoint(T.inverse());
    return AdInv * Js;
}

int main() {
    double l1 = 1.0, l2 = 0.8;
    VectorXd S1(6), S2(6);
    S1 << 0, 0, 1, 0, 0, 0;
    S2 << 0, 0, 1, 0, -l1, 0;
    std::vector<VectorXd> Slist = {S1, S2};

    Matrix4d M = Matrix4d::Identity();
    M(0,3) = l1 + l2;

    VectorXd q(2);
    q << 0.5, -0.4;

    Matrix4d T = fkPoeSpace(Slist, M, q);
    MatrixXd Js = jacobianSpace(Slist, q);
    MatrixXd Jb = jacobianBody(Slist, M, q);

    std::cout << "T(q):\n" << T << std::endl;
    std::cout << "J_s(q):\n" << Js << std::endl;
    std::cout << "J_b(q):\n" << Jb << std::endl;
    return 0;
}
      

This implementation mirrors the Python version and is suitable for real-time applications when compiled with optimizations.

In Java, one can use EJML for matrix computations. Below is a minimal skeleton illustrating SE(3) operations and Jacobian computation. For brevity, we show core parts; in a full project, encapsulate these in classes (e.g., Twist, SE3).

import org.ejml.simple.SimpleMatrix;

public class JacobianSE3 {

    public static SimpleMatrix skew(SimpleMatrix v) {
        double vx = v.get(0), vy = v.get(1), vz = v.get(2);
        double[][] data = {
            {0.0, -vz,  vy},
            {vz,  0.0, -vx},
            {-vy, vx,   0.0}
        };
        return new SimpleMatrix(data);
    }

    public static SimpleMatrix se3Hat(SimpleMatrix S) {
        SimpleMatrix M = SimpleMatrix.identity(4);
        M.zero();
        SimpleMatrix omega = S.rows(0, 3);
        SimpleMatrix v = S.rows(3, 6);
        M.insertIntoThis(0, 0, skew(omega));
        M.insertIntoThis(0, 3, v);
        return M;
    }

    public static SimpleMatrix se3Exp(SimpleMatrix S, double theta) {
        double tol = 1e-9;
        SimpleMatrix omega = S.rows(0, 3);
        SimpleMatrix v = S.rows(3, 6);
        double wnorm = omega.normF();
        SimpleMatrix T = SimpleMatrix.identity(4);
        if (wnorm < tol) {
            // pure translation
            SimpleMatrix p = v.scale(theta);
            T.insertIntoThis(0, 3, p);
            return T;
        }
        SimpleMatrix w = omega.scale(1.0 / wnorm);
        SimpleMatrix w_hat = skew(w);
        double th = wnorm * theta;

        SimpleMatrix I3 = SimpleMatrix.identity(3);
        SimpleMatrix R = I3.plus(w_hat.scale(Math.sin(th)))
                           .plus(w_hat.mult(w_hat).scale(1.0 - Math.cos(th)));

        SimpleMatrix G = I3.scale(th)
                .plus(w_hat.scale(1.0 - Math.cos(th)))
                .plus(w_hat.mult(w_hat).scale(th - Math.sin(th)));

        SimpleMatrix p = G.mult(v.scale(1.0 / wnorm));
        T.insertIntoThis(0, 0, R);
        T.insertIntoThis(0, 3, p);
        return T;
    }

    public static SimpleMatrix adjoint(SimpleMatrix T) {
        SimpleMatrix R = T.extractMatrix(0, 3, 0, 3);
        SimpleMatrix p = T.extractMatrix(0, 3, 3, 4);
        SimpleMatrix Ad = new SimpleMatrix(6, 6);
        Ad.insertIntoThis(0, 0, R);
        Ad.insertIntoThis(3, 3, R);
        Ad.insertIntoThis(3, 0, skew(p).mult(R));
        return Ad;
    }

    public static SimpleMatrix fkPoeSpace(SimpleMatrix[] Slist,
                                          SimpleMatrix M,
                                          double[] q) {
        SimpleMatrix T = SimpleMatrix.identity(4);
        for (int i = 0; i < Slist.length; ++i) {
            T = T.mult(se3Exp(Slist[i], q[i]));
        }
        return T.mult(M);
    }

    public static SimpleMatrix jacobianSpace(SimpleMatrix[] Slist, double[] q) {
        int n = Slist.length;
        SimpleMatrix J = new SimpleMatrix(6, n);
        SimpleMatrix T = SimpleMatrix.identity(4);
        for (int i = 0; i < n; ++i) {
            if (i == 0) {
                J.insertIntoThis(0, 0, Slist[0]);
            } else {
                SimpleMatrix AdT = adjoint(T);
                SimpleMatrix col = AdT.mult(Slist[i]);
                J.insertIntoThis(0, i, col);
            }
            T = T.mult(se3Exp(Slist[i], q[i]));
        }
        return J;
    }

    public static SimpleMatrix jacobianBody(SimpleMatrix[] Slist,
                                            SimpleMatrix M,
                                            double[] q) {
        SimpleMatrix T = fkPoeSpace(Slist, M, q);
        SimpleMatrix Js = jacobianSpace(Slist, q);
        SimpleMatrix AdInv = adjoint(T.invert());
        return AdInv.mult(Js);
    }

    public static void main(String[] args) {
        double l1 = 1.0, l2 = 0.8;
        SimpleMatrix S1 = new SimpleMatrix(6, 1, true,
                new double[]{0, 0, 1, 0, 0, 0});
        SimpleMatrix S2 = new SimpleMatrix(6, 1, true,
                new double[]{0, 0, 1, 0, -l1, 0});
        SimpleMatrix[] Slist = new SimpleMatrix[]{S1, S2};

        SimpleMatrix M = SimpleMatrix.identity(4);
        M.set(0, 3, l1 + l2);

        double[] q = new double[]{0.5, -0.4};
        SimpleMatrix T = fkPoeSpace(Slist, M, q);
        SimpleMatrix Js = jacobianSpace(Slist, q);
        SimpleMatrix Jb = jacobianBody(Slist, M, q);

        System.out.println("T(q) = \n" + T);
        System.out.println("J_s(q) = \n" + Js);
        System.out.println("J_b(q) = \n" + Jb);
    }
}
      

This Java implementation emphasizes clarity and direct correspondence with the underlying mathematics, making it suitable for educational toolkits or robot simulators implemented in Java.

MATLAB, combined with the Robotics System Toolbox, offers native support for SE(3) and Jacobians. Below we show a self-contained implementation without relying on built-in Jacobian functions, plus notes on Simulink integration.

function lesson7_3_demo()
    % Planar 2R example: spatial and body Jacobians
    l1 = 1.0; l2 = 0.8;
    S1 = [0;0;1; 0;0;0];
    S2 = [0;0;1; 0;-l1;0];
    Slist = [S1, S2];

    M = eye(4);
    M(1,4) = l1 + l2;

    q = [0.5; -0.4];
    T = fk_poe_space(Slist, M, q);
    Js = jacobian_space(Slist, q);
    Jb = jacobian_body(Slist, M, q);

    disp('T(q) ='); disp(T);
    disp('J_s(q) ='); disp(Js);
    disp('J_b(q) ='); disp(Jb);
end

function S_hat = se3_hat(S)
    omega = S(1:3);
    v = S(4:6);
    S_hat = zeros(4,4);
    S_hat(1:3,1:3) = skew(omega);
    S_hat(1:3,4) = v;
end

function Sx = skew(v)
    vx = v(1); vy = v(2); vz = v(3);
    Sx = [ 0,  -vz,  vy;
          vz,   0,  -vx;
         -vy,  vx,   0];
end

function T = se3_exp(S, theta)
    omega = S(1:3);
    v = S(4:6);
    wnorm = norm(omega);
    if wnorm < 1e-9
        T = eye(4);
        T(1:3,4) = v * theta;
        return;
    end
    w = omega / wnorm;
    w_hat = skew(w);
    th = wnorm * theta;
    R = eye(3) + sin(th)*w_hat + (1-cos(th))*(w_hat*w_hat);
    G = eye(3)*th + (1-cos(th))*w_hat + (th - sin(th))*(w_hat*w_hat);
    p = G * (v/wnorm);
    T = eye(4);
    T(1:3,1:3) = R;
    T(1:3,4) = p;
end

function Ad = adjoint(T)
    R = T(1:3,1:3);
    p = T(1:3,4);
    Ad = zeros(6,6);
    Ad(1:3,1:3) = R;
    Ad(4:6,4:6) = R;
    Ad(4:6,1:3) = skew(p)*R;
end

function T = fk_poe_space(Slist, M, q)
    T = eye(4);
    n = size(Slist,2);
    for i = 1:n
        T = T * se3_exp(Slist(:,i), q(i));
    end
    T = T * M;
end

function Js = jacobian_space(Slist, q)
    n = size(Slist,2);
    Js = zeros(6,n);
    T = eye(4);
    for i = 1:n
        if i == 1
            Js(:,1) = Slist(:,1);
        else
            AdT = adjoint(T);
            Js(:,i) = AdT * Slist(:,i);
        end
        T = T * se3_exp(Slist(:,i), q(i));
    end
end

function Jb = jacobian_body(Slist, M, q)
    T = fk_poe_space(Slist, M, q);
    Js = jacobian_space(Slist, q);
    AdInv = adjoint(inv(T));
    Jb = AdInv * Js;
end
      

Simulink integration. One practical approach is to wrap jacobian_space and jacobian_body inside a MATLAB Function block. The block inputs are the joint angles and geometry parameters, and the outputs are the Jacobian matrices. This enables real-time computation of twists \( \mathbf{V}_s = \mathbf{J}_s \dot{\mathbf{q}} \) in simulation diagrams without explicitly coding SE(3) blocks.

Mathematica is well suited to symbolic derivations and numerical evaluation of Jacobians. The following code sets up generic twists and uses matrix exponentials to compute \( \mathbf{J}_s \) and \( \mathbf{J}_b \).

ClearAll["Global`*"];

skew[v_List] := {
  {0, -v[[3]], v[[2]]},
  {v[[3]], 0, -v[[1]]},
  {-v[[2]], v[[1]], 0}
};

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

adjoint[T_] := Module[{R, p},
  R = T[[1 ;; 3, 1 ;; 3]];
  p = T[[1 ;; 3, 4]];
  ArrayFlatten[{
    {R, ConstantArray[0, {3, 3}]},
    {skew[p].R, R}
  }]
];

se3Exp[S_List, theta_] := Module[{omega, v, wnorm, w, wHat, th, R, G, p},
  omega = S[[1 ;; 3]];
  v = S[[4 ;; 6]];
  wnorm = Sqrt[omega.omega];
  If[wnorm < 10^(-9),
    (* pure translation *)
    Return[{ {1, 0, 0, v[[1]] theta},
            {0, 1, 0, v[[2]] theta},
            {0, 0, 1, v[[3]] theta},
            {0, 0, 0, 1} }];
  ];
  w = omega/wnorm;
  wHat = skew[w];
  th = wnorm theta;
  R = IdentityMatrix[3]
      + Sin[th] wHat
      + (1 - Cos[th]) (wHat . wHat);
  G = IdentityMatrix[3] th
      + (1 - Cos[th]) wHat
      + (th - Sin[th]) (wHat . wHat);
  p = G . (v/wnorm);
  ArrayFlatten[{
    {R, Transpose[{p}]},
    {{0, 0, 0, 1}}
  }]
];

fkPoeSpace[Slist_List, M_, q_List] := Module[{T = IdentityMatrix[4]},
  Do[
    T = T . se3Exp[Slist[[i]], q[[i]]],
    {i, 1, Length[Slist]}
  ];
  T . M
];

jacobianSpace[Slist_List, q_List] := Module[{n, J, T, col},
  n = Length[Slist];
  J = ConstantArray[0, {6, n}];
  T = IdentityMatrix[4];
  Do[
    If[i == 1,
      col = Slist[[1]];
      ,
      col = adjoint[T].Slist[[i]];
    ];
    J[[All, i]] = col;
    T = T . se3Exp[Slist[[i]], q[[i]]];
    ,
    {i, 1, n}
  ];
  J
];

jacobianBody[Slist_List, M_, q_List] := Module[{T, Js},
  T = fkPoeSpace[Slist, M, q];
  Js = jacobianSpace[Slist, q];
  adjoint[Inverse[T]].Js
];

(* Planar 2R example *)
l1 = 1.0; l2 = 0.8;
S1 = {0, 0, 1, 0, 0, 0};
S2 = {0, 0, 1, 0, -l1, 0};
Slist = {S1, S2};
M = { {1, 0, 0, l1 + l2},
     {0, 1, 0, 0},
     {0, 0, 1, 0},
     {0, 0, 0, 1} };
q = {0.5, -0.4};

Tq = fkPoeSpace[Slist, M, q];
Jsq = jacobianSpace[Slist, q];
Jbq = jacobianBody[Slist, M, q];

Tq // MatrixForm
Jsq // MatrixForm
Jbq // MatrixForm
      

Symbolic parameters (e.g., generic link lengths and joint variables) can be used instead of numeric values to obtain closed-form Jacobians for analytical work.

In this lesson we defined the end-effector twist as the Lie-algebra representation of rigid-body velocity and showed how it arises as \( \dot{\mathbf{g}}\mathbf{g}^{-1} \) or \( \mathbf{g}^{-1}\dot{\mathbf{g}} \). We derived the spatial Jacobian from the PoE forward kinematics by differentiating the chain of exponentials, and obtained the body Jacobian by right trivialization and the adjoint relation. The adjoint map \( \text{Ad}_{\mathbf{T}(\mathbf{q})} \) provides a clean linear mapping between spatial and body twists and their Jacobians. Finally, we developed concrete algorithms and multi-language implementations for computing spatial and body Jacobians and associated twists, laying the foundation for differential IK, manipulability analysis, and dynamic modeling in later chapters.