A matrix Lie group is a set of matrices closed under multiplication and inversion (a group), which is also a smooth manifold. The key examples in robotics are:

• \( \mathrm{SO}(3) \): rotation matrices in 3D.
• \( \mathrm{SE}(3) \): homogeneous transforms representing rotations and translations.

The Lie algebra of a matrix Lie group \( G \subset \mathbb{R}^{n \times n} \) is the tangent space at the identity, represented concretely as a set of matrices closed under commutators \( [A,B]=AB-BA \). We denote it by \( \mathfrak{g} \).

For our groups of interest:

\[ \mathfrak{so}(3) = \{ \Omega \in \mathbb{R}^{3 \times 3} \mid \Omega^\top = -\Omega \}, \quad \mathfrak{se}(3) = \left\{ \begin{bmatrix} \Omega & \mathbf{v} \\ \mathbf{0}^\top & 0 \end{bmatrix} \,\middle|\, \Omega \in \mathfrak{so}(3),\; \mathbf{v} \in \mathbb{R}^3 \right\}. \]

The matrix exponential of \( A \in \mathbb{R}^{n\times n} \) is defined by the power series

\[ \exp(A) \triangleq \sum_{k=0}^{\infty} \frac{A^k}{k!}. \]

A fundamental fact is that if \( A \in \mathfrak{g} \), then \( \exp(A) \in G \). Exponential coordinates exploit this by writing group elements as exponentials of Lie algebra elements, typically of the form \( \exp(\hat{\boldsymbol{\omega}}\,\theta) \) for rotations and \( \exp(\hat{\boldsymbol{\xi}}\,\theta) \) for rigid motions.

To connect vector coordinates with matrices, we introduce the hat and vee operators. For any vector \( \boldsymbol{\omega} = [\omega_1,\omega_2,\omega_3]^\top \in \mathbb{R}^3 \), define the skew-symmetric matrix

\[ \widehat{\boldsymbol{\omega}} \triangleq \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \in \mathfrak{so}(3). \]

The mapping \( \widehat{\cdot} : \mathbb{R}^3 \to \mathfrak{so}(3) \) is linear and satisfies \( \widehat{\boldsymbol{\omega}}\mathbf{x} = \boldsymbol{\omega} \times \mathbf{x} \) for all \( \mathbf{x} \in \mathbb{R}^3 \). The inverse mapping is the vee operator \( (\cdot)^\vee : \mathfrak{so}(3) \to \mathbb{R}^3 \).

For rigid motions, a twist is a 6D vector \( \boldsymbol{\xi} = \begin{bmatrix}\mathbf{v} \\ \boldsymbol{\omega}\end{bmatrix} \in \mathbb{R}^6 \), where \( \boldsymbol{\omega} \) is the angular velocity vector and \( \mathbf{v} \) is the linear part. Its hat form is

\[ \widehat{\boldsymbol{\xi}} \triangleq \begin{bmatrix} \widehat{\boldsymbol{\omega}} & \mathbf{v} \\ \mathbf{0}^\top & 0 \end{bmatrix} \in \mathfrak{se}(3), \quad (\widehat{\boldsymbol{\xi}})^\vee = \begin{bmatrix} \mathbf{v} \\[0.3em] \boldsymbol{\omega} \end{bmatrix}. \]

Exponential coordinates will be expressed either as \( (\boldsymbol{\omega},\theta) \) for rotations or \( (\boldsymbol{\xi},\theta) \) for rigid motions, with \( \theta \) typically having units of radians.

Let \( \boldsymbol{\omega} \in \mathbb{R}^3 \) with \( \|\boldsymbol{\omega}\| = 1 \) and angle \( \theta \in \mathbb{R} \). The element \( \widehat{\boldsymbol{\omega}} \in \mathfrak{so}(3) \) generates rotations about the axis \( \boldsymbol{\omega} \). The matrix exponential

\[ R(\theta) = \exp(\widehat{\boldsymbol{\omega}}\,\theta) \]

gives a rotation matrix in \( \mathrm{SO}(3) \) by angle \( \theta \) around axis \( \boldsymbol{\omega} \). Using the power series and the identities \( \widehat{\boldsymbol{\omega}}^3 = -\widehat{\boldsymbol{\omega}} \) and \( \widehat{\boldsymbol{\omega}}^2 = \boldsymbol{\omega}\boldsymbol{\omega}^\top - I_3 \), we can regroup terms to obtain the Rodrigues formula:

\[ \exp(\widehat{\boldsymbol{\omega}}\,\theta) = I_3 + \sin\theta\,\widehat{\boldsymbol{\omega}} + (1-\cos\theta)\,\widehat{\boldsymbol{\omega}}^2. \]

Proof sketch. Substitute \( A = \widehat{\boldsymbol{\omega}}\,\theta \) into the power series for \( \exp(A) \), separate even and odd powers, and use the identities \( \widehat{\boldsymbol{\omega}}^{2k+1} = (-1)^k \widehat{\boldsymbol{\omega}} \) and \( \widehat{\boldsymbol{\omega}}^{2k+2} = (-1)^k \widehat{\boldsymbol{\omega}}^2 \). The coefficients of \( \widehat{\boldsymbol{\omega}} \) and \( \widehat{\boldsymbol{\omega}}^2 \) sum to \( \sin\theta \) and \( 1-\cos\theta \), respectively.

This shows that every rotation can be parameterized by a unit axis \( \boldsymbol{\omega} \) and scalar angle \( \theta \), i.e., exponential coordinates \( (\boldsymbol{\omega},\theta) \).

The matrix logarithm inverts the exponential map (locally). For a given rotation \( R \in \mathrm{SO}(3) \), we seek \( \widehat{\boldsymbol{\omega}} \in \mathfrak{so}(3) \) and \( \theta \in [0,\pi] \) such that \( R = \exp(\widehat{\boldsymbol{\omega}}\,\theta) \).

Using the Rodrigues formula, one can show

\[ \operatorname{tr}(R) = 1 + 2\cos\theta \quad \Rightarrow \quad \theta = \cos^{-1}\!\left(\frac{\operatorname{tr}(R)-1}{2}\right). \]

For \( 0 < \theta < \pi \), the skew matrix is

\[ \widehat{\boldsymbol{\omega}} = \frac{\theta}{2\sin\theta}\,\bigl(R - R^\top\bigr), \]

and the unit axis is obtained via the vee operator \( \boldsymbol{\omega} = (\widehat{\boldsymbol{\omega}})^\vee \).

We define the matrix logarithm on \( \mathrm{SO}(3) \) as

\[ \log\!:\mathrm{SO}(3)\to\mathfrak{so}(3), \quad \log(R) \triangleq \begin{cases} 0, & \text{if } R = I_3, \\[0.3em] \dfrac{\theta}{2\sin\theta}\bigl(R - R^\top\bigr), & \text{otherwise}, \end{cases} \]

where \( \theta \) is computed from the trace, with care for numerical stability when \( \theta \approx 0 \) or \( \theta \approx \pi \).

flowchart TD
  RSO["Rotation matrix R in SO(3)"] --> THETA["theta = arccos((trace(R) - 1)/2)"]
  THETA --> OMEGAHAT["omega_hat = (theta / (2 sin(theta))) * (R - R^T)"]
  OMEGAHAT --> OMEGA["omega = vee(omega_hat)"]
  OMEGA --> COORD["Exponential coordinates (omega, theta)"]
        

A rigid-body motion (in a fixed reference frame) is represented by \( T \in \mathrm{SE}(3) \):

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

Given a twist \( \boldsymbol{\xi} = [\mathbf{v}^\top,\boldsymbol{\omega}^\top]^\top \) and scalar \( \theta \), the homogeneous transform generated by this twist is

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

where \( R(\theta) = \exp(\widehat{\boldsymbol{\omega}}\,\theta) \) is given by the Rodrigues formula. For \( \boldsymbol{\omega} \neq \mathbf{0} \), define the Jacobian of the exponential

\[ J(\theta) \triangleq I_3\,\theta + (1-\cos\theta)\,\widehat{\boldsymbol{\omega}} + (\theta - \sin\theta)\,\widehat{\boldsymbol{\omega}}^2. \]

Then the translational part is

\[ \mathbf{p}(\theta) = J(\theta)\,\mathbf{v}. \]

For a pure translation, \( \boldsymbol{\omega} = \mathbf{0} \) and \( \widehat{\boldsymbol{\xi}} = \begin{bmatrix} 0 & \mathbf{v} \\ \mathbf{0}^\top & 0 \end{bmatrix} \). Then the series for \( \exp(\widehat{\boldsymbol{\xi}}\,\theta) \) truncates and we obtain

\[ \exp(\widehat{\boldsymbol{\xi}}\,\theta) = \begin{bmatrix} I_3 & \mathbf{v}\,\theta \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad \boldsymbol{\omega} = \mathbf{0}. \]

Thus exponential coordinates \( (\boldsymbol{\xi},\theta) \) generalize axis-angle coordinates to rigid motions (screw motions).

flowchart TD
  TW["Twist xi = (omega, v), scalar theta"] --> RSTEP["Compute R = exp(omega_hat * theta)"]
  RSTEP --> JSTEP["Compute J(theta) = I*theta + (1 - cos(theta))*omega_hat + (theta - sin(theta))*omega_hat^2"]
  JSTEP --> PSTEP["p = J(theta) * v"]
  PSTEP --> TSTEP["Form T = [[R, p], [0, 1]] in SE(3)"]
        

Given \( T \in \mathrm{SE}(3) \), we want to recover exponential coordinates \( (\boldsymbol{\xi},\theta) \) such that \( T = \exp(\widehat{\boldsymbol{\xi}}\,\theta) \). Write

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

Step 1: compute \( \theta \) and \( \widehat{\boldsymbol{\omega}} \) from \( R \) as in Section 4. If \( \boldsymbol{\omega} = \mathbf{0} \) (i.e., \( R = I_3 \)), then the motion is pure translation and we can take

\[ \theta = \|\mathbf{p}\|, \quad \boldsymbol{\omega} = \mathbf{0}, \quad \mathbf{v} = \frac{\mathbf{p}}{\theta} \quad (\theta \neq 0). \]

Step 2: for \( \boldsymbol{\omega} \neq \mathbf{0} \), we use the inverse of \( J(\theta) \):

\[ J(\theta)^{-1} = I_3 - \frac{1}{2}\widehat{\boldsymbol{\omega}} + \left( \frac{1}{\theta^2} - \frac{1 + \cos\theta}{2\theta\sin\theta} \right) \widehat{\boldsymbol{\omega}}^2. \]

Then

\[ \mathbf{v} = J(\theta)^{-1}\,\mathbf{p}, \quad \boldsymbol{\xi} = \begin{bmatrix} \mathbf{v} \\[0.3em] \boldsymbol{\omega} \end{bmatrix}. \]

The matrix logarithm on \( \mathrm{SE}(3) \) is therefore

\[ \log(T) = \widehat{\boldsymbol{\xi}}\,\theta = \begin{bmatrix} \widehat{\boldsymbol{\omega}} & \mathbf{v} \\ \mathbf{0}^\top & 0 \end{bmatrix}\theta. \]

Numerically, careful branching is required to handle small \( \theta \), near-\( \pi \) rotations, and the pure-translation case. These formulas are the core of exponential coordinates used in modern robot kinematics libraries.

Below is a minimal implementation of \( \mathfrak{so}(3) \)/\( \mathfrak{se}(3) \) exponential and logarithm using NumPy. This code operates directly with 3D vectors and \( 3\times 3 \) or \( 4\times 4 \) matrices.

import numpy as np

def hat_omega(omega):
    """Skew-symmetric matrix from a 3-vector."""
    wx, wy, wz = omega
    return np.array([[0.0, -wz,  wy],
                     [wz,  0.0, -wx],
                     [-wy, wx,  0.0]])

def vee_omega(omega_hat):
    """Inverse of hat_omega."""
    return np.array([omega_hat[2, 1],
                     omega_hat[0, 2],
                     omega_hat[1, 0]])

def so3_exp(omega, theta):
    """Exponential map for SO(3) given unit axis omega and scalar theta."""
    omega = np.asarray(omega, dtype=float)
    if np.linalg.norm(omega) < 1e-9:
        return np.eye(3)
    omega = omega / np.linalg.norm(omega)
    omega_hat = hat_omega(omega)
    I = np.eye(3)
    return (I
            + np.sin(theta) * omega_hat
            + (1.0 - np.cos(theta)) * (omega_hat @ omega_hat))

def so3_log(R):
    """Logarithm map for SO(3) returning (omega, theta)."""
    R = np.asarray(R, dtype=float)
    assert R.shape == (3, 3)
    cos_theta = (np.trace(R) - 1.0) / 2.0
    cos_theta = np.clip(cos_theta, -1.0, 1.0)
    theta = np.arccos(cos_theta)
    if theta < 1e-9:
        return np.zeros(3), 0.0
    omega_hat = (theta / (2.0 * np.sin(theta))) * (R - R.T)
    omega = vee_omega(omega_hat)
    return omega / np.linalg.norm(omega), theta

def hat_xi(xi):
    """Hat operator for a 6-vector xi = [v; omega]."""
    xi = np.asarray(xi, dtype=float).reshape(6)
    v = xi[:3]
    omega = xi[3:]
    omega_hat = hat_omega(omega)
    Xi_hat = np.zeros((4, 4))
    Xi_hat[:3, :3] = omega_hat
    Xi_hat[:3, 3] = v
    return Xi_hat

def se3_exp(xi, theta):
    """Exponential map on SE(3): returns a 4x4 homogeneous transform."""
    xi = np.asarray(xi, dtype=float).reshape(6)
    v = xi[:3]
    omega = xi[3:]
    if np.linalg.norm(omega) < 1e-9:
        # pure translation
        T = np.eye(4)
        T[:3, 3] = v * theta
        return T
    omega = omega / np.linalg.norm(omega)
    omega_hat = hat_omega(omega)
    R = so3_exp(omega, theta)
    omega_hat2 = omega_hat @ omega_hat
    I = np.eye(3)
    J = (I * theta
         + (1.0 - np.cos(theta)) * omega_hat
         + (theta - np.sin(theta)) * omega_hat2)
    p = J @ v
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = p
    return T

def se3_log(T):
    """Logarithm on SE(3): returns (xi, theta)."""
    T = np.asarray(T, dtype=float)
    R = T[:3, :3]
    p = T[:3, 3]
    omega, theta = so3_log(R)
    if theta < 1e-9:
        # pure translation
        theta = np.linalg.norm(p)
        if theta < 1e-9:
            return np.zeros(6), 0.0
        v = p / theta
        xi = np.hstack((v, np.zeros(3)))
        return xi, theta
    omega_hat = hat_omega(omega)
    omega_hat2 = omega_hat @ omega_hat
    I = np.eye(3)
    J_inv = (I
             - 0.5 * omega_hat
             + (1.0 / (theta ** 2)
                - (1.0 + np.cos(theta)) / (2.0 * theta * np.sin(theta)))
               * omega_hat2)
    v = J_inv @ p
    xi = np.hstack((v, omega))
    return xi, theta
      

This implementation is sufficient for experimentation and for validating other language implementations numerically.

Here we use the Eigen library to implement \( \mathfrak{so}(3) \)/\( \mathfrak{se}(3) \) exponentials and logarithms. The code assumes #include <Eigen/Dense> and that Eigen::Matrix3d and Eigen::Matrix4d are available.

#include <Eigen/Dense>

using namespace Eigen;

Matrix3d hatOmega(const Vector3d &w) {
    Matrix3d W;
    W << 0.0,   -w(2),  w(1),
          w(2),  0.0,   -w(0),
         -w(1),  w(0),   0.0;
    return W;
}

Vector3d veeOmega(const Matrix3d &W) {
    return Vector3d(W(2, 1), W(0, 2), W(1, 0));
}

Matrix3d so3Exp(const Vector3d &omega, double theta) {
    double n = omega.norm();
    if (n < 1e-9) {
        return Matrix3d::Identity();
    }
    Vector3d w = omega / n;
    Matrix3d W = hatOmega(w);
    Matrix3d W2 = W * W;
    return Matrix3d::Identity()
           + std::sin(theta) * W
           + (1.0 - std::cos(theta)) * W2;
}

void so3Log(const Matrix3d &R, Vector3d &omega, double &theta) {
    double cos_theta = (R.trace() - 1.0) / 2.0;
    cos_theta = std::max(-1.0, std::min(1.0, cos_theta));
    theta = std::acos(cos_theta);
    if (theta < 1e-9) {
        omega = Vector3d::Zero();
        theta = 0.0;
        return;
    }
    Matrix3d W = (theta / (2.0 * std::sin(theta))) * (R - R.transpose());
    omega = veeOmega(W);
    omega.normalize();
}

Matrix4d se3Exp(const Vector3d &v, const Vector3d &omega, double theta) {
    Matrix4d T = Matrix4d::Identity();
    double n = omega.norm();
    if (n < 1e-9) {
        // pure translation
        T.block<3, 1>(0, 3) = v * theta;
        return T;
    }
    Vector3d w = omega / n;
    Matrix3d W = hatOmega(w);
    Matrix3d W2 = W * W;
    Matrix3d R = so3Exp(w, theta);
    Matrix3d I = Matrix3d::Identity();
    Matrix3d J = I * theta
                 + (1.0 - std::cos(theta)) * W
                 + (theta - std::sin(theta)) * W2;
    Vector3d p = J * v;

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

void se3Log(const Matrix4d &T, Vector3d &v, Vector3d &omega, double &theta) {
    Matrix3d R = T.block<3, 3>(0, 0);
    Vector3d p = T.block<3, 1>(0, 3);

    so3Log(R, omega, theta);
    if (theta < 1e-9) {
        // pure translation
        theta = p.norm();
        if (theta < 1e-9) {
            v.setZero();
            omega.setZero();
            theta = 0.0;
            return;
        }
        v = p / theta;
        omega.setZero();
        return;
    }

    Matrix3d W = hatOmega(omega);
    Matrix3d W2 = W * W;
    Matrix3d I = Matrix3d::Identity();
    Matrix3d Jinv = I
                    - 0.5 * W
                    + (1.0 / (theta * theta)
                       - (1.0 + std::cos(theta)) / (2.0 * theta * std::sin(theta)))
                      * W2;
    v = Jinv * p;
}
      

This implementation is a natural C++ counterpart of the Python functions and can be integrated into larger kinematics libraries.

In Java we implement essential operations using double[][] matrices and helper functions. This is a low-level reference implementation; in practice one would wrap these in a matrix class or use a numerical library.

public class LieGroups {

    public static double[][] hatOmega(double[] w) {
        double wx = w[0], wy = w[1], wz = w[2];
        return new double[][]{
            {0.0,   -wz,   wy},
            {wz,    0.0,  -wx},
            {-wy,   wx,   0.0}
        };
    }

    public static double[] veeOmega(double[][] W) {
        return new double[]{
            W[2][1],
            W[0][2],
            W[1][0]
        };
    }

    public static double[][] so3Exp(double[] omega, double theta) {
        double n = norm(omega);
        if (n < 1e-9) {
            return identity3();
        }
        double[] w = scale(omega, 1.0 / n);
        double[][] W = hatOmega(w);
        double[][] W2 = matMul3(W, W);
        double[][] I = identity3();

        double[][] R = add3(
            add3(I, scale3(W, Math.sin(theta))),
            scale3(W2, 1.0 - Math.cos(theta))
        );
        return R;
    }

    public static So3LogResult so3Log(double[][] R) {
        double trace = R[0][0] + R[1][1] + R[2][2];
        double cosTheta = (trace - 1.0) / 2.0;
        cosTheta = Math.max(-1.0, Math.min(1.0, cosTheta));
        double theta = Math.acos(cosTheta);
        if (theta < 1e-9) {
            return new So3LogResult(new double[]{0.0, 0.0, 0.0}, 0.0);
        }
        double[][] diff = sub3(R, transpose3(R));
        double factor = theta / (2.0 * Math.sin(theta));
        double[][] W = scale3(diff, factor);
        double[] omega = veeOmega(W);
        double n = norm(omega);
        omega = scale(omega, 1.0 / n);
        return new So3LogResult(omega, theta);
    }

    public static Se3LogResult se3Log(double[][] T) {
        double[][] R = new double[3][3];
        double[] p = new double[3];
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                R[i][j] = T[i][j];
            }
            p[i] = T[i][3];
        }
        So3LogResult logR = so3Log(R);
        double[] omega = logR.omega;
        double theta = logR.theta;

        double[] v;
        if (theta < 1e-9) {
            theta = norm(p);
            if (theta < 1e-9) {
                v = new double[]{0.0, 0.0, 0.0};
                omega = new double[]{0.0, 0.0, 0.0};
                return new Se3LogResult(v, omega, 0.0);
            }
            v = scale(p, 1.0 / theta);
            omega = new double[]{0.0, 0.0, 0.0};
            return new Se3LogResult(v, omega, theta);
        }

        double[][] W = hatOmega(omega);
        double[][] W2 = matMul3(W, W);
        double[][] I = identity3();

        double c = Math.cos(theta);
        double s = Math.sin(theta);
        double[][] Jinv = add3(
            add3(I, scale3(W, -0.5)),
            scale3(
                W2,
                (1.0 / (theta * theta)) - (1.0 + c) / (2.0 * theta * s)
            )
        );

        v = matVec3(Jinv, p);
        return new Se3LogResult(v, omega, theta);
    }

    // Helper classes and vector/matrix utility functions omitted for brevity:
    // norm, scale, identity3, matMul3, matVec3, add3, sub3, transpose3, scale3, etc.
    // They should implement standard 3x3 operations.
}
      

For clarity, auxiliary methods like matMul3 and identity3 have been omitted; they follow standard 3D linear algebra patterns.

MATLAB provides expm and logm for generic matrices, but for numerical robustness it is preferable to use the closed-form formulas for \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \). Below is a script implementing the core maps; the functions can be used in a MATLAB Function block inside Simulink to build pose-integration subsystems.

function demo_se3_exp_log()
    omega = [0; 0; 1];
    theta = pi/4;
    v = [0.1; 0.0; 0.0];
    xi = [v; omega];

    T = se3_exp(xi, theta);
    [xi_rec, theta_rec] = se3_log(T);

    disp('T ='); disp(T);
    disp('Recovered xi and theta:');
    disp(xi_rec.'); disp(theta_rec);
end

function W = hat_omega(omega)
    wx = omega(1); wy = omega(2); wz = omega(3);
    W = [  0,   -wz,   wy;
          wz,    0,   -wx;
         -wy,   wx,    0 ];
end

function [omega, theta] = so3_log(R)
    cos_theta = (trace(R) - 1) / 2;
    cos_theta = max(-1, min(1, cos_theta));
    theta = acos(cos_theta);
    if theta < 1e-9
        omega = [0; 0; 0];
        theta = 0;
        return;
    end
    W = (theta / (2 * sin(theta))) * (R - R');
    omega = [W(3, 2); W(1, 3); W(2, 1)];
    omega = omega / norm(omega);
end

function R = so3_exp(omega, theta)
    n = norm(omega);
    if n < 1e-9
        R = eye(3);
        return;
    end
    omega = omega / n;
    W = hat_omega(omega);
    W2 = W * W;
    R = eye(3) + sin(theta) * W + (1 - cos(theta)) * W2;
end

function T = se3_exp(xi, theta)
    v = xi(1:3);
    omega = xi(4:6);
    T = eye(4);
    if norm(omega) < 1e-9
        % pure translation
        T(1:3, 4) = v * theta;
        return;
    end
    omega = omega / norm(omega);
    W = hat_omega(omega);
    W2 = W * W;
    R = so3_exp(omega, theta);
    I3 = eye(3);
    J = I3 * theta + (1 - cos(theta)) * W + (theta - sin(theta)) * W2;
    p = J * v;
    T(1:3, 1:3) = R;
    T(1:3, 4) = p;
end

function [xi, theta] = se3_log(T)
    R = T(1:3, 1:3);
    p = T(1:3, 4);

    [omega, theta] = so3_log(R);
    if theta < 1e-9
        theta = norm(p);
        if theta < 1e-9
            xi = zeros(6, 1);
            theta = 0;
            return;
        end
        v = p / theta;
        xi = [v; 0; 0; 0];
        return;
    end
    W = hat_omega(omega);
    W2 = W * W;
    I3 = eye(3);
    Jinv = I3 ...
           - 0.5 * W ...
           + (1 / (theta^2) - (1 + cos(theta)) / (2 * theta * sin(theta))) * W2;
    v = Jinv * p;
    xi = [v; omega];
end
      

In Simulink, a MATLAB Function block can wrap se3_exp to integrate twist commands into homogeneous transforms along a trajectory, without leaving the group manifold.

Wolfram Mathematica includes MatrixExp and MatrixLog, which can be specialized to \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \) by enforcing skew-symmetry and homogeneous structure.

Clear[hatOmega, veeOmega, so3Exp, so3Log, hatXi, se3Exp, se3Log];

hatOmega[{wx_, wy_, wz_}] := {
  {0,   -wz,  wy},
  {wz,   0,  -wx},
  {-wy,  wx,  0}
};

veeOmega[W_] := {W[[3, 2]], W[[1, 3]], W[[2, 1]]};

so3Exp[omega_List, theta_] := Module[
  {n, w, W, W2, id},
  n = Norm[omega];
  If[n < 10^-9, Return[IdentityMatrix[3]]];
  w = omega/n;
  W = hatOmega[w];
  W2 = W . W;
  id = IdentityMatrix[3];
  id + Sin[theta] W + (1 - Cos[theta]) W2
];

so3Log[R_?MatrixQ] := Module[
  {tr, cosTheta, theta, W, omega},
  tr = Tr[R];
  cosTheta = (tr - 1)/2;
  cosTheta = Max[-1, Min[1, cosTheta]];
  theta = ArcCos[cosTheta];
  If[theta < 10^-9,
    Return[{ {0, 0, 0}, 0 }];
  ];
  W = (theta/(2 Sin[theta])) (R - Transpose[R]);
  omega = veeOmega[W];
  omega = omega/Norm[omega];
  {omega, theta}
];

hatXi[xi_List] := Module[
  {v, omega, W},
  v = xi[[1 ;; 3]];
  omega = xi[[4 ;; 6]];
  W = hatOmega[omega];
  ArrayFlatten[{ {W, Transpose[{v}]}, { {0, 0, 0, 0} } }]
];

se3Exp[xi_List, theta_] := Module[
  {v, omega, n, W, W2, R, id, J, p},
  v = xi[[1 ;; 3]];
  omega = xi[[4 ;; 6]];
  n = Norm[omega];
  If[n < 10^-9,
    Return[ArrayFlatten[{ {IdentityMatrix[3], Transpose[{theta v}]},
                         { {0, 0, 0, 1} } }]]
  ];
  omega = omega/n;
  W = hatOmega[omega];
  W2 = W . W;
  R = so3Exp[omega, theta];
  id = IdentityMatrix[3];
  J = id theta + (1 - Cos[theta]) W + (theta - Sin[theta]) W2;
  p = J . v;
  ArrayFlatten[{ {R, Transpose[{p}]}, { {0, 0, 0, 1} } }]
];

se3Log[T_?MatrixQ] := Module[
  {R, p, omega, theta, W, W2, id, Jinv, v},
  R = T[[1 ;; 3, 1 ;; 3]];
  p = T[[1 ;; 3, 4]];
  {omega, theta} = so3Log[R];
  If[theta < 10^-9,
    theta = Norm[p];
    If[theta < 10^-9, Return[{ConstantArray[0, 6], 0}]];
    v = p/theta;
    Return[{Join[v, {0, 0, 0}], theta}];
  ];
  W = hatOmega[omega];
  W2 = W . W;
  id = IdentityMatrix[3];
  Jinv = id
         - 1/2 W
         + (1/theta^2 - (1 + Cos[theta])/(2 theta Sin[theta])) W2;
  v = Jinv . p;
  {Join[v, omega], theta}
];
      

These functions can be combined with symbolic variables in Mathematica to perform analytical differentiation of exponential coordinates, which is useful for deriving Jacobians and sensitivity expressions later in the course.

In this lesson we constructed exponential coordinates for rotations and rigid-body motions using the matrix exponential and logarithm on \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \). The hat and vee operators provide a bridge between vector representations (\( \boldsymbol{\omega} \), \( \boldsymbol{\xi} \)) and the Lie algebras \( \mathfrak{so}(3) \), \( \mathfrak{se}(3) \). Rodrigues' formula gives a closed form for \( \exp(\widehat{\boldsymbol{\omega}}\,\theta) \), while the Jacobian \( J(\theta) \) characterizes the translational part of \( \exp(\widehat{\boldsymbol{\xi}}\,\theta) \) in \( \mathrm{SE}(3) \).

We also derived practical formulas for the logarithm maps, enabling the recovery of axis-angle and twist parameters from rotation and transformation matrices. Finally, we implemented these maps in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, preparing the ground for their use in forward kinematics and Jacobian computations in later chapters.