Chapter 2: Rigid-Body Motion and Lie Groups
Lesson 5: Geometric Interpretation of Lie Groups in Robotics
This lesson provides a geometric viewpoint on Lie groups as configuration spaces for rigid bodies. We interpret \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \) as smooth manifolds with group operations, relate their tangent spaces to twists and instantaneous motion, and connect the exponential map to screw motions used in robot kinematics. The emphasis is on geometric insight that will underpin later kinematic and dynamic modeling.
1. Lie Groups as Configuration Manifolds
A Lie group is a set that is simultaneously a smooth manifold and a group, where the group operations are smooth maps. Formally, a Lie group \( G \) is a smooth manifold with:
- A group multiplication map \( m : G \times G \to G \), \( m(g,h)=gh \).
- An inversion map \( \iota : G \to G \), \( \iota(g) = g^{-1} \).
Both maps are smooth (infinitely differentiable).
\[ m \text{ and } \iota \text{ are } C^\infty \text{ maps}, \quad m(g,h)=gh,\; \iota(g)=g^{-1}. \]
For robotics, the most important examples are the rotation group \( \mathrm{SO}(3) \) and the rigid-body motion group \( \mathrm{SE}(3) \). Recall from the previous lessons:
\[ \mathrm{SO}(3) = \big\{ R \in \mathbb{R}^{3\times 3} \mid R^\top R = I,\; \det(R)=1 \big\}, \]
\[ \mathrm{SE}(3) = \left\{ g = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \,\middle|\, R \in \mathrm{SO}(3),\; p \in \mathbb{R}^3 \right\}. \]
Geometrically, \( \mathrm{SO}(3) \) is the manifold of all orientations of a rigid body about a fixed point, while \( \mathrm{SE}(3) \) corresponds to all poses (orientation and position) of a rigid body in 3D space. Each element of \( \mathrm{SE}(3) \) can be seen as a rigid motion acting on points in space.
From a configuration-space viewpoint, the configuration manifold of a free rigid body in space is \( Q = \mathrm{SE}(3) \). For manipulators, each joint configuration \( q \in \mathbb{R}^n \) maps to a configuration in \( \mathrm{SE}(3) \) through the forward kinematics map, which will be studied in later chapters.
flowchart TD
Q["Joint space (R^n)"] --> F["Forward kinematics map"]
F --> G["Configuration group SE(3)"]
G --> A["Rigid motion acting on points"]
A --> P["Physical pose of robot in workspace"]
G --> T["Tangent space at identity: se(3)"]
T --> V["Twists (instantaneous motions)"]
2. Group Actions and Rigid-Body Motion
Lie groups act on other spaces via smooth maps that respect the group structure. A left action of a Lie group \( G \) on a manifold \( M \) is a smooth map
\[ \Phi : G \times M \to M,\quad \Phi(g,x)=g \cdot x \]
such that
\[ e \cdot x = x,\quad g_1 \cdot (g_2 \cdot x) = (g_1 g_2) \cdot x \quad \forall g_1,g_2 \in G,\; x \in M. \]
For \( \mathrm{SE}(3) \), there is a natural action on Euclidean 3D space \( \mathbb{R}^3 \). Represent a point \( x \in \mathbb{R}^3 \) in homogeneous coordinates:
\[ \tilde{x} = \begin{bmatrix} x \\ 1 \end{bmatrix} \in \mathbb{R}^4. \]
For \( g \in \mathrm{SE}(3) \) written as
\[ g = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}, \]
the action on \( x \) is
\[ g \cdot x = \pi\big( g \tilde{x} \big) = \pi\left( \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ 1 \end{bmatrix} \right) = \pi \begin{bmatrix} Rx + p \\ 1 \end{bmatrix} = Rx + p, \]
where \( \pi : \mathbb{R}^4 \setminus \{x_4 = 0\} \to \mathbb{R}^3 \) projects away the last coordinate. Geometrically, \( g \) first rotates the point and then translates it.
The orbit of a point \( x_0 \in \mathbb{R}^3 \) under this action is \( \mathcal{O}(x_0) = \{ g \cdot x_0 \mid g \in \mathrm{SE}(3) \} \), i.e. all possible positions that the point can occupy when the rigid body moves through all poses. For a free body, this orbit is the whole space \( \mathbb{R}^3 \).
3. Tangent Spaces, Lie Algebras, and Twists
The tangent space at a point \( g \in G \), denoted \( T_g G \), consists of velocities of smooth curves on \( G \) passing through \( g \). Let \( \gamma : (-\epsilon,\epsilon) \to G \) be a smooth curve with \( \gamma(0) = g \); then the tangent vector is
\[ \dot{\gamma}(0) \in T_g G. \]
The Lie algebra of \( G \) is the tangent space at the identity element \( e \):
\[ \mathfrak{g} = T_e G. \]
Equipped with the Lie bracket (commutator), \( \mathfrak{g} \) becomes a vector space with additional algebraic structure.
For matrix Lie groups (subgroups of \( \mathrm{GL}(n,\mathbb{R}) \)), a curve \( \gamma(t) \) is matrix-valued. Differentiating the condition \( \gamma(t) \in G \) at \( t=0 \) yields algebraic constraints on \( \dot{\gamma}(0) \).
Example: \( \mathfrak{so}(3) \).
For \( \mathrm{SO}(3) \), we have \( R(t)^\top R(t) = I \). Differentiating:
\[ \frac{d}{dt}\bigg|_{t=0} \big( R(t)^\top R(t) \big) = \dot{R}(0)^\top R(0) + R(0)^\top \dot{R}(0) = 0. \]
At \( t=0 \), set \( R(0) = I \), so \( \dot{R}(0)^\top + \dot{R}(0) = 0 \). Hence any tangent vector \( \dot{R}(0) \) is a skew-symmetric matrix:
\[ \mathfrak{so}(3) = \left\{ \Omega \in \mathbb{R}^{3\times 3} \mid \Omega^\top = -\Omega \right\}. \]
There is an isomorphism between \( \mathbb{R}^3 \) and \( \mathfrak{so}(3) \) given by the hat map:
\[ \omega = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix} \in \mathbb{R}^3 \;\mapsto\; [\omega]_\times = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \in \mathfrak{so}(3). \]
Example: \( \mathfrak{se}(3) \).
For \( \mathrm{SE}(3) \), an element \( g(t) \) is
\[ g(t) = \begin{bmatrix} R(t) & p(t) \\ 0 & 1 \end{bmatrix}. \]
Differentiating at \( t=0 \) with \( g(0) = I_4 \) gives the general form of an element of the Lie algebra \( \mathfrak{se}(3) \):
\[ \hat{\xi} = \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix},\quad \omega, v \in \mathbb{R}^3. \]
We call \( \xi = (\omega, v) \in \mathbb{R}^6 \) a twist. It represents an instantaneous rigid-body motion combining angular and linear velocity.
Let \( x(t) \) be the trajectory of a point attached to the moving rigid body, with current pose \( g(t) \) and body-fixed coordinates \( x_B \):
\[ x(t) = R(t) x_B + p(t). \]
If \( \dot{g}(0) = \hat{\xi} g(0) = \hat{\xi} \), then the instantaneous spatial velocity of the point at \( t=0 \) is
\[ \dot{x}(0) = \omega \times x(0) + v, \]
which is precisely the linear velocity field generated by the twist \( \xi \). This gives a geometric meaning: twists are vector fields on space induced by infinitesimal rigid motions.
4. Exponential Map and Screw Motions
The exponential map associates an element of the Lie algebra with a finite group element. For matrix Lie groups, it coincides with the usual matrix exponential. For \( \mathfrak{se}(3) \):
\[ \exp : \mathfrak{se}(3) \to \mathrm{SE}(3),\quad \exp(\hat{\xi}) = \sum_{k=0}^{\infty} \frac{1}{k!} \hat{\xi}^k. \]
Consider a one-parameter subgroup \( g(t) = \exp(t \hat{\xi}) \). The curve \( g(t) \) is a geodesic (in the sense of the group structure) and represents the finite rigid motion obtained by integrating the instantaneous twist \( \xi \) over time.
Case 1: Pure rotation \( v = 0 \).
If \( \xi = (\omega, 0) \) with \( \omega \neq 0 \), then \( \hat{\xi} \) reduces to
\[ \hat{\xi} = \begin{bmatrix} [\omega]_\times & 0 \\ 0 & 0 \end{bmatrix}. \]
Using properties of block matrices,
\[ \exp(t \hat{\xi}) = \begin{bmatrix} \exp\big(t[\omega]_\times\big) & 0 \\ 0 & 1 \end{bmatrix}, \]
so the exponential recovers the \( \mathrm{SO}(3) \) exponential for rotations.
Case 2: General twist \( \xi = (\omega, v) \).
If \( \omega \neq 0 \), define \( \theta = \|\omega\| \) and \( \hat{\omega} = \omega / \theta \). Then
\[ \exp(\hat{\xi} \theta) = \begin{bmatrix} R(\theta) & p(\theta) \\ 0 & 1 \end{bmatrix}, \]
where
\[ R(\theta) = \exp\big([\hat{\omega}]_\times \theta\big) \]
is given by Rodrigues' formula (already seen in the previous lesson), and the translation part satisfies the closed-form expression
\[ p(\theta) = \big(I - R(\theta)\big)\,[\hat{\omega}]_\times v + \hat{\omega}\hat{\omega}^\top v\,\theta. \]
This motion is a screw motion about a screw axis (a line in space) with appropriately defined pitch and direction. If the body is initially at identity, its configuration after moving "angle" \( \theta \) along the screw is exactly \( \exp(\hat{\xi}\theta) \).
For \( \omega = 0 \), the twist is a pure translation, and the exponential reduces to
\[ \exp(\hat{\xi}\theta) = \exp \begin{bmatrix} 0 & v\theta \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} I & v\theta \\ 0 & 1 \end{bmatrix}, \]
i.e. a straight-line translation with displacement \( v\theta \).
flowchart TD
X["Twist xi = (omega, v)"] --> S["Form hat matrix xi_hat"]
S --> E["Compute exp(xi_hat * theta)"]
E --> G2["Rigid motion g(theta) in SE(3)"]
G2 --> M["Screw motion of rigid body in workspace"]
5. Adjoint Representation and Change of Frames
Twists depend on the reference frame. The adjoint representation describes how twist coordinates change when we change frames. For a matrix Lie group, the adjoint action on the Lie algebra is defined by
\[ \mathrm{Ad}_g : \mathfrak{g} \to \mathfrak{g},\quad \mathrm{Ad}_g(X) = g X g^{-1}. \]
For \( \mathrm{SE}(3) \), write \( g = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \) and \( \hat{\xi} = \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix} \). Then
\[ \mathrm{Ad}_g(\hat{\xi}) = g \hat{\xi} g^{-1} = \begin{bmatrix} R[\omega]_\times R^\top & Rv + [p]_\times R\omega \\ 0 & 0 \end{bmatrix}. \]
Using the identity \( R[\omega]_\times R^\top = [R\omega]_\times \), we can identify the transformed twist \( \xi' = (\omega', v') \) as
\[ \omega' = R\omega,\quad v' = Rv + p \times R\omega. \]
In block matrix form, the adjoint representation is
\[ \mathrm{Ad}_g = \begin{bmatrix} R & 0 \\ [p]_\times R & R \end{bmatrix}, \quad \xi' = \mathrm{Ad}_g \,\xi. \]
Geometrically, this means that when we describe the same physical rigid-body motion in a different coordinate frame, the twist coordinates transform linearly via \( \mathrm{Ad}_g \). This is the geometric backbone for relating "spatial" and "body" twists in robot kinematics.
6. Implementation Lab: Lie Groups and Twists in Code
In this section we implement basic Lie group operations for \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \), focusing on the hat map, exponential map, and adjoint. We provide snippets in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These implementations can be used as building blocks for forward kinematics and dynamics in later chapters.
6.1 Python (NumPy / SciPy / Robotics Libraries)
In Python, numpy and scipy.linalg are the
standard tools. Robotics-oriented libraries such as
modern_robotics (Lynch & Park) or
spatialmath-python build on the same ideas.
import numpy as np
from numpy.linalg import norm
from scipy.linalg import expm
def hat_so3(omega):
"""Map R^3 to so(3)."""
wx, wy, wz = omega
return np.array([[0.0, -wz, wy],
[wz, 0.0, -wx],
[-wy, wx, 0.0]])
def hat_se3(xi):
"""Map R^6 (omega, v) to se(3)."""
omega = xi[:3]
v = xi[3:]
Xi_hat = np.zeros((4, 4))
Xi_hat[:3, :3] = hat_so3(omega)
Xi_hat[:3, 3] = v
return Xi_hat
def exp_se3(xi, theta=1.0):
"""
Exponential map for se(3) using closed form for screw motions.
xi = (omega, v), theta is motion parameter.
"""
omega = xi[:3]
v = xi[3:]
th = theta
if norm(omega) < 1e-9:
# Pure translation
g = np.eye(4)
g[:3, 3] = v * th
return g
# Normalize
w = omega / norm(omega)
w_hat = hat_so3(w)
th = th * norm(omega)
R = expm(w_hat * th)
I = np.eye(3)
A = I * th + (1.0 - np.cos(th)) * w_hat + (th - np.sin(th)) * (w_hat @ w_hat)
p = A @ v
g = np.eye(4)
g[:3, :3] = R
g[:3, 3] = p
return g
def adjoint_SE3(g):
"""
Compute Ad_g for SE(3).
g is 4x4 homogeneous transform.
"""
R = g[:3, :3]
p = g[:3, 3]
p_hat = hat_so3(p)
Ad = np.zeros((6, 6))
Ad[:3, :3] = R
Ad[3:, :3] = p_hat @ R
Ad[3:, 3:] = R
return Ad
if __name__ == "__main__":
omega = np.array([0.0, 0.0, 1.0])
v = np.array([0.1, 0.2, 0.0])
xi = np.hstack((omega, v))
g = exp_se3(xi, theta=0.5)
print("g(theta)=\\n", g)
Adg = adjoint_SE3(g)
print("Ad_g=\\n", Adg)
6.2 C++ with Eigen
In C++, the Eigen library is widely used for linear algebra
in robotics. More specialized Lie group libraries include
Sophus or kindr. Below is a minimal
\( \mathrm{SE}(3) \) exponential using Eigen only.
#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
Matrix3d hat_so3(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;
}
Matrix4d hat_se3(const VectorXd &xi) {
Vector3d w = xi.segment<3>(0);
Vector3d v = xi.segment<3>(3);
Matrix4d Xi = Matrix4d::Zero();
Xi.block<3,3>(0,0) = hat_so3(w);
Xi.block<3,1>(0,3) = v;
return Xi;
}
Matrix4d exp_se3(const VectorXd &xi, double theta = 1.0) {
Vector3d w = xi.segment<3>(0);
Vector3d v = xi.segment<3>(3);
double wnorm = w.norm();
Matrix4d g = Matrix4d::Identity();
if (wnorm < 1e-9) {
// Pure translation
g.block<3,1>(0,3) = v * theta;
return g;
}
Vector3d what = w / wnorm;
Matrix3d W = hat_so3(what);
double th = theta * wnorm;
Matrix3d I = Matrix3d::Identity();
Matrix3d R = I + std::sin(th) * W + (1.0 - std::cos(th)) * (W * W);
Matrix3d A = I * th + (1.0 - std::cos(th)) * W + (th - std::sin(th)) * (W * W);
Vector3d p = A * v;
g.block<3,3>(0,0) = R;
g.block<3,1>(0,3) = p;
return g;
}
Matrix<double,6,6> adjoint_SE3(const Matrix4d &g) {
Matrix3d R = g.block<3,3>(0,0);
Vector3d p = g.block<3,1>(0,3);
Matrix3d Phat = hat_so3(p);
Matrix<double,6,6> Ad = Matrix<double,6,6>::Zero();
Ad.block<3,3>(0,0) = R;
Ad.block<3,3>(3,0) = Phat * R;
Ad.block<3,3>(3,3) = R;
return Ad;
}
int main() {
VectorXd xi(6);
xi << 0.0, 0.0, 1.0, 0.1, 0.2, 0.0;
Matrix4d g = exp_se3(xi, 0.5);
std::cout << "g(theta):\\n" << g << std::endl;
Matrix<double,6,6> Adg = adjoint_SE3(g);
std::cout << "Ad_g:\\n" << Adg << std::endl;
return 0;
}
6.3 Java (Using Simple Matrices)
In Java, one can use libraries such as ojAlgo or
Apache Commons Math. For clarity, we show a minimal
implementation based on 2D arrays. This is suitable for educational
purposes or for integration in simple simulation tools.
public class SE3LieGroup {
public static double[][] hatSO3(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[][] hatSE3(double[] xi) {
double[] w = new double[] { xi[0], xi[1], xi[2] };
double[] v = new double[] { xi[3], xi[4], xi[5] };
double[][] Xi = new double[4][4];
double[][] W = hatSO3(w);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
Xi[i][j] = W[i][j];
}
Xi[i][3] = v[i];
}
return Xi;
}
public static double[][] matMul3(double[][] A, double[][] B) {
double[][] C = new double[3][3];
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
C[i][j] = 0.0;
for (int k = 0; k < 3; ++k) {
C[i][j] += A[i][k] * B[k][j];
}
}
}
return C;
}
public static double[][] expSO3Rodrigues(double[] w, double theta) {
double norm = Math.sqrt(w[0]*w[0] + w[1]*w[1] + w[2]*w[2]);
if (norm < 1e-9) {
double[][] I = { {1,0,0}, {0,1,0}, {0,0,1} };
return I;
}
double[] wh = { w[0]/norm, w[1]/norm, w[2]/norm };
double[][] W = hatSO3(wh);
double th = theta * norm;
double[][] I = { {1,0,0}, {0,1,0}, {0,0,1} };
double s = Math.sin(th);
double c = Math.cos(th);
// R = I + s*W + (1-c)*W^2
double[][] W2 = matMul3(W, W);
double[][] R = new double[3][3];
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
R[i][j] = I[i][j] + s * W[i][j] + (1.0 - c) * W2[i][j];
}
}
return R;
}
public static double[][] expSE3(double[] xi, double theta) {
double[] w = { xi[0], xi[1], xi[2] };
double[] v = { xi[3], xi[4], xi[5] };
double wnorm = Math.sqrt(w[0]*w[0] + w[1]*w[1] + w[2]*w[2]);
double[][] g = new double[4][4];
for (int i = 0; i < 4; ++i) g[i][i] = 1.0;
if (wnorm < 1e-9) {
g[0][3] = v[0] * theta;
g[1][3] = v[1] * theta;
g[2][3] = v[2] * theta;
return g;
}
double[] wh = { w[0]/wnorm, w[1]/wnorm, w[2]/wnorm };
double[][] W = hatSO3(wh);
double th = theta * wnorm;
double[][] R = expSO3Rodrigues(w, theta);
// Compute A = I*th + (1-cos th)*W + (th - sin th)*W^2
double[][] I = { {1,0,0}, {0,1,0}, {0,0,1} };
double[][] W2 = matMul3(W, W);
double s = Math.sin(th);
double c = Math.cos(th);
double[][] A = new double[3][3];
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
A[i][j] = I[i][j] * th + (1.0 - c) * W[i][j] + (th - s) * W2[i][j];
}
}
double[] p = new double[3];
for (int i = 0; i < 3; ++i) {
p[i] = A[i][0]*v[0] + A[i][1]*v[1] + A[i][2]*v[2];
}
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
g[i][j] = R[i][j];
}
g[i][3] = p[i];
}
return g;
}
}
6.4 MATLAB / Simulink
MATLAB with the Robotics System Toolbox provides high-level primitives. Here we implement basic operations by hand; they can be placed into MATLAB Function blocks inside Simulink to build rigid body kinematics subsystems.
function Xi_hat = hat_se3(xi)
% xi = [omega; v] in R^6
omega = xi(1:3);
v = xi(4:6);
Xi_hat = zeros(4,4);
Xi_hat(1:3,1:3) = hat_so3(omega);
Xi_hat(1:3,4) = v;
end
function W = hat_so3(w)
W = [ 0 -w(3) w(2);
w(3) 0 -w(1);
-w(2) w(1) 0 ];
end
function g = exp_se3(xi, theta)
omega = xi(1:3);
v = xi(4:6);
wnorm = norm(omega);
g = eye(4);
if wnorm < 1e-9
% Pure translation
g(1:3,4) = v * theta;
return;
end
w_hat = omega / wnorm;
W = hat_so3(w_hat);
th = theta * wnorm;
I3 = eye(3);
R = I3 + sin(th)*W + (1-cos(th))*(W*W);
A = I3*th + (1-cos(th))*W + (th-sin(th))*(W*W);
p = A * v;
g(1:3,1:3) = R;
g(1:3,4) = p;
end
% Example usage:
xi = [0; 0; 1; 0.1; 0.2; 0];
g = exp_se3(xi, 0.5);
disp(g);
In Simulink, one can wrap exp_se3 in a MATLAB Function
block, feed twist parameters and joint variables as inputs, and route
the resulting homogeneous transforms to visualization or downstream
blocks that compute end-effector positions.
6.5 Wolfram Mathematica
Wolfram Mathematica is convenient for symbolic manipulation of Lie group expressions and for verifying algebraic identities (e.g., properties of the adjoint). Below is a symbolic definition of the hat map, exponential, and adjoint for \( \mathrm{SE}(3) \).
(* Hat maps *)
hatSO3[{wx_, wy_, wz_}] := { {0, -wz, wy},
{wz, 0, -wx},
{-wy, wx, 0} };
hatSE3[{wx_, wy_, wz_, vx_, vy_, vz_}] :=
Module[{W, v},
W = hatSO3[{wx, wy, wz}];
v = {vx, vy, vz};
{ {W[[1,1]], W[[1,2]], W[[1,3]], v[[1]]},
{W[[2,1]], W[[2,2]], W[[2,3]], v[[2]]},
{W[[3,1]], W[[3,2]], W[[3,3]], v[[3]]},
{0, 0, 0, 0} }
];
(* Matrix exponential for SE(3) *)
expSE3[xi_List, theta_:1] :=
Module[{Xi, g},
Xi = hatSE3[xi];
g = MatrixExp[theta Xi];
g
];
(* Adjoint of SE(3) *)
adjSE3[g_] :=
Module[{R, p, hatp},
R = g[[1;;3, 1;;3]];
p = g[[1;;3, 4]];
hatp = hatSO3[p];
ArrayFlatten[{
{R, ConstantArray[0, {3,3}]},
{hatp.R, R}
}]
];
(* Example numerical twist *)
xi = {0, 0, 1, 1/10, 2/10, 0};
g = expSE3[xi, 1/2];
Adg = adjSE3[g];
{g // MatrixForm, Adg // MatrixForm}
Symbolic tools can be leveraged to check identities such as \( \mathrm{Ad}_{g_1 g_2} = \mathrm{Ad}_{g_1}\mathrm{Ad}_{g_2} \) or to derive linearized approximations of exponential and logarithm maps for use in kinematic calibration.
7. Problems and Solutions
Problem 1 (Velocity Field of a Twist): Let \( \xi = (\omega, v) \in \mathbb{R}^6 \) be a twist and consider \( g(t) = \exp(t\hat{\xi}) \in \mathrm{SE}(3) \), with \( g(0) = I \). For a point \( x_B \in \mathbb{R}^3 \) fixed in the body, the spatial position is \( x(t) = R(t)x_B + p(t) \). Show that the spatial velocity at \( t=0 \) satisfies \( \dot{x}(0) = \omega \times x(0) + v \).
Solution:
By definition, \( g(t) = \begin{bmatrix} R(t) & p(t) \\ 0 & 1 \end{bmatrix} \), with \( g(0) = I \). Hence \( R(0) = I \), \( p(0) = 0 \). Differentiating \( x(t) = R(t)x_B + p(t) \):
\[ \dot{x}(t) = \dot{R}(t) x_B + \dot{p}(t). \]
From \( \dot{g}(t) = \hat{\xi} g(t) \) and evaluating at \( t=0 \):
\[ \dot{g}(0) = \hat{\xi} = \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} \dot{R}(0) & \dot{p}(0) \\ 0 & 0 \end{bmatrix}. \]
Thus \( \dot{R}(0) = [\omega]_\times \), \( \dot{p}(0) = v \). Using \( x(0) = R(0) x_B + p(0) = x_B \):
\[ \dot{x}(0) = \dot{R}(0) x_B + \dot{p}(0) = [\omega]_\times x_B + v = \omega \times x(0) + v. \]
This proves that a twist generates a rigid-body velocity field of the form \( \omega \times x + v \).
Problem 2 (Lie Algebra of SO(3)): Show that the set of skew-symmetric matrices \( \mathfrak{so}(3) \) is closed under the matrix commutator \( [A,B] = AB - BA \), hence forms a Lie algebra.
Solution:
Let \( A,B \in \mathfrak{so}(3) \), so \( A^\top = -A \), \( B^\top = -B \). Consider the commutator \( [A,B] = AB - BA \). Then
\[ [A,B]^\top = (AB - BA)^\top = B^\top A^\top - A^\top B^\top = (-B)(-A) - (-A)(-B) = BA - AB = -[A,B]. \]
Thus \( [A,B] \) is skew-symmetric, hence lies in \( \mathfrak{so}(3) \). Bilinearity and antisymmetry of the commutator, plus the Jacobi identity (verified by matrix algebra), complete the Lie algebra structure.
Problem 3 (Adjoint of SE(3) in Vector Form): Starting from the matrix expression \( \mathrm{Ad}_g(\hat{\xi}) = g \hat{\xi} g^{-1} \) for \( g = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \in \mathrm{SE}(3) \) and \( \hat{\xi} = \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix} \), derive the vector form \( \xi' = (\omega', v') \) with \( \omega' = R\omega \), \( v' = Rv + p \times R\omega \).
Solution:
Compute explicitly:
\[ g \hat{\xi} g^{-1} = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix} \begin{bmatrix} R^\top & -R^\top p \\ 0 & 1 \end{bmatrix}. \]
First multiply the left two matrices:
\[ \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} R[\omega]_\times & Rv \\ 0 & 0 \end{bmatrix}. \]
Then multiply on the right:
\[ \begin{bmatrix} R[\omega]_\times & Rv \\ 0 & 0 \end{bmatrix} \begin{bmatrix} R^\top & -R^\top p \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R[\omega]_\times R^\top & -R[\omega]_\times R^\top p + Rv \\ 0 & 0 \end{bmatrix}. \]
Using \( R[\omega]_\times R^\top = [R\omega]_\times \) and the identity \( [a]_\times b = a \times b \), we obtain
\[ -R[\omega]_\times R^\top p = -[R\omega]_\times p = p \times R\omega. \]
Hence the resulting \( \hat{\xi}' \) is
\[ \hat{\xi}' = \begin{bmatrix} [R\omega]_\times & Rv + p \times R\omega \\ 0 & 0 \end{bmatrix}, \]
which corresponds to \( \xi' = (\omega', v') \) with \( \omega' = R\omega \), \( v' = Rv + p \times R\omega \), as claimed.
Problem 4 (Exponential of a Pure Translation): Let \( \xi = (0, v) \) be a twist with zero angular part. Show that \( \exp(\hat{\xi}\theta) = \begin{bmatrix} I & v\theta \\ 0 & 1 \end{bmatrix} \) for all real \( \theta \).
Solution:
For \( \omega = 0 \), \( \hat{\xi} = \begin{bmatrix} 0 & v \\ 0 & 0 \end{bmatrix} \). Observe that \( \hat{\xi}^2 = 0 \) (since the upper-right block times itself yields zero in the 4x4 structure). Thus the exponential series truncates:
\[ \exp(\hat{\xi}\theta) = I + \hat{\xi}\theta + \frac{1}{2!}(\hat{\xi}\theta)^2 + \cdots = I + \hat{\xi}\theta, \]
which explicitly gives
\[ \exp(\hat{\xi}\theta) = \begin{bmatrix} I & v\theta \\ 0 & 1 \end{bmatrix}. \]
This corresponds to a uniform translation with displacement \( v\theta \).
8. Summary
In this lesson we developed a geometric interpretation of Lie groups and Lie algebras as they appear in rigid-body robotics. We saw \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \) as smooth manifolds with group operations that act naturally on physical space. The tangent space at the identity defines the Lie algebras \( \mathfrak{so}(3) \) and \( \mathfrak{se}(3) \), whose elements (twists) encode instantaneous rigid-body velocities.
The exponential map from \( \mathfrak{se}(3) \) to \( \mathrm{SE}(3) \) captures finite screw motions, while the adjoint representation describes how twists transform under change of coordinates. Computationally, these concepts translate into matrix operations (hat, exponential, adjoint) that can be implemented in Python, C++, Java, MATLAB/Simulink, and Mathematica. They serve as a foundation for the product-of-exponentials formulation of forward kinematics and for geometric dynamics formulations in later chapters.
9. References
- Brockett, R. W. (1972). "System theory on group manifolds and coset spaces." SIAM Journal on Control, 10(2), 265–284.
- Chirikjian, G. S. (1995). "Hyper-redundant manipulator dynamics: A continuum approximation." Advanced Robotics, 9(3), 217–243. (For its Lie group framework.)
- Park, J. and Brockett, R. W. (1994). "Kinematic dexterity of robotic mechanisms." International Journal of Robotics Research, 13(1), 1–15.
- Selig, J. M. (1996). "Lie groups and Lie algebras in robotics." Computational kinematics, Springer, 101–118.
- Bullo, F. and Murray, R. M. (1999). "Proportional derivative (PD) control on the Euclidean group." European Journal of Control, 4(1), 1–16.
- Lynch, K. M. and Park, F. C. (2017). "Modern Robotics: Mechanics, Planning, and Control." Cambridge University Press. (Monograph with Lie group based robotics.)
- Marsden, J. E. and Ratiu, T. S. (1994). "Introduction to Mechanics and Symmetry." Springer. (Geometric mechanics and Lie group methods.)
- Murray, R. M., Li, Z., and Sastry, S. S. (1994). "A Mathematical Introduction to Robotic Manipulation." CRC Press. (Seminal reference on Lie groups in robotics.)