2.1. Rigid Motion

Rigid Motion

A rigid body motion in three-dimensional space is a transformation that preserves distances and angles — a rotation followed by a translation. The collection of all such transformations forms the Special Euclidean Group $SE(3)$, whose elements are pairs $(R,p)$ with $R\in SO(3)$ (special orthonormal group) and $p\in {\mathbb{R}}^3$. Embedded into $4\times 4$ matrices via homogeneous coordinates, every rigid motion is
$g=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\in SE(3).$
Frames are tracked with subscripts: $g_{ab}\in SE(3)$ is the pose of frame $b$ expressed in frame $a$ — equivalently, the transformation that converts $b$-coordinates to $a$-coordinates. With this convention, composition stacks adjacent indices and the inverse swaps them:
$g_{ab}{g}_{bc}=g_{ac}=\left[\begin{array}{cc}{R}_{ab}{R}_{bc}& R_{ab}{p}_{bc}+p_{ab}\\ 0& 1\end{array}\right],g_{ab}^{-1}=g_{ba}=\left[\begin{array}{cc}{R}_{ab}^T& -R_{ab}^T{p}_{ab}\\ 0& 1\end{array}\right],g_{aa}=I_4.$

Homogeneous coordinates distinguish points from free vectors. A point $p\in {\mathbb{R}}^3$ lifts to $\bar{p}=(p,1{)}^T$; a free vector $v\in {\mathbb{R}}^3$ lifts to $\bar{v}=(v,0{)}^T$. The extra coordinate encodes whether translation acts:
$g\bar{p}=\left[\begin{array}{c}Rp+p_g\\ 1\end{array}\right],g\bar{v}=\left[\begin{array}{c}Rv\\ 0\end{array}\right]$
(where $p_g$ is the translation part of $g$). Points are translated and rotated; free vectors are only rotated.

$SE(3)$ is a six-dimensional Lie group (three rotation + three translation parameters). Its Lie algebra $\mathfrak{se}(3)$ consists of $4\times 4$ matrices
$\hat{\xi }=\left[\begin{array}{cc}\hat{\omega }& v\\ 0& 0\end{array}\right],\xi =\left[\begin{array}{c}v\\ \omega \end{array}\right]\in {\mathbb{R}}^6,$
called twists. The exponential map sends a unit twist ${\xi }_u$ ($\Vert \omega \Vert =1$) and scalar $\theta$ to the finite rigid motion
$e^{{\hat{\xi }}_u\theta }=\left[\begin{array}{cc}{e}^{\hat{\omega }\theta }& A(\omega ,\theta )v\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right],$
where $e^{\hat{\omega }\theta }$ is Rodrigues’ formula and
$A(\omega ,\theta )=I\theta +(1-\cos \theta )\hat{\omega }+(\theta -\sin \theta ){\hat{\omega }}^2.$
Pure translation ($\omega =0$) is the degenerate case $e^{{\hat{\xi }}_u\theta }=\left[\begin{array}{cc}I& v\theta \\ 0& 1\end{array}\right]$. The inverse log map solves $(\omega ,\theta )=\log (R)$ (see 1.5. Exponential Coordinates) then $v=A(\omega ,\theta {)}^{-1}p$.

The adjoint representation ${\mathrm{Ad}}_g:{\mathbb{R}}^6\to {\mathbb{R}}^6$ transforms twist coordinates between frames:
${\mathrm{Ad}}_g=\left[\begin{array}{cc}R& \hat{p}R\\ 0& R\end{array}\right],{\xi }^{\prime }={\mathrm{Ad}}_g\xi .$

These ingredients — the matrix form, the composition/inverse formulas, the twist exponential, and the adjoint — are the algebraic toolkit every downstream robotics topic (forward kinematics, Jacobians, dynamics, control) is built on.

Intuition

$SE(3)$ is the natural mathematical home for rigid body poses. Embedding the affine transformation $(R,p)$ into a $4\times 4$ matrix via homogeneous coordinates converts composition of rigid motions into matrix multiplication and inversion into a simple matrix inverse. This linearization trick is what makes the product-of-exponentials formula in forward kinematics possible.

The distinction between points ($\bar{p}=[p;1]$) and vectors ($\bar{v}=[v;0]$) in homogeneous coordinates is not just notational—it encodes the physics that translation moves points but not free directions. A force direction, for instance, should not change when the origin is shifted, and the $0$ in the fourth entry ensures this automatically.

Properties

Group Operations

(Composition Mechanism) The translation of $g_{ab}{g}_{bc}$ is $R_{ab}{p}_{bc}+p_{ab}$, not $p_{ab}+p_{bc}$.

Following the chain $a\leftarrow b\leftarrow c$: a point known in $c$-coordinates is first translated by $p_{bc}$ to reach $b$-coordinates, then rotated by $R_{ab}$ and translated by $p_{ab}$ to land in $a$-coordinates. The translation $p_{bc}$ lives in frame $b$, so it must be rotated through $R_{ab}$ before being added to $p_{ab}$ — they have to be in the same frame to add. The “rotation of the downstream translation” is what makes rigid-motion composition non-abelian and is the source of the adjacent-index cancellation rule: $g_{ab}{g}_{bc}=g_{ac}$ only because the inner $b$ indices match.

(Inverse Mechanism) The inverse translation is $-R_{ab}^T{p}_{ab}$, not $-p_{ab}$.

Undoing $(R_{ab},p_{ab})$ means undoing the final state, which was reached by rotating first and then translating. To return home you translate back by $-p_{ab}$ — but $p_{ab}$ is expressed in frame $a$ while the “undo translation” must be applied before the rotation, in frame $b$. Rotating $-p_{ab}$ into frame $b$ via $R_{ab}^T=R_{ba}$ gives the correct $-R_{ab}^T{p}_{ab}=p_{ba}$. The result satisfies the index identity $g_{ab}^{-1}=g_{ba}$. A common bug in tabletop simulators is using $-p$ in place of $-R^{T}p$ and watching objects drift on rotation.

The Lie Algebra $\mathfrak{se}(3)$

(Twist Exponential Mechanism) $A(\omega ,\theta )$ integrates the time-varying rotation into the translation.

For a scalar ODE $\dot{x}=ax+b$, the solution is $x(t)=e^{at}x(0)+a^{-1}(e^{at}-1)b$. The $SE(3)$ exponential is the matrix generalization: the translation component accumulates the linear velocity $v$ rotated through every intermediate angle from $0$ to $\theta$, giving ${\int }_0^{\theta }{e}^{\hat{\omega }s}vds=A(\omega ,\theta )v$. The three terms in $A=I\theta +(1-\cos \theta )\hat{\omega }+(\theta -\sin \theta ){\hat{\omega }}^2$ come from integrating Rodrigues’ formula term-by-term. Pure rotation about an axis through the origin ($v=0$) gives zero translation; rotation about an off-origin axis ($v=-\omega \times q$) induces the familiar circular arc.

(Non-Unit Twist) For a non-unit twist, factor out $\Vert \omega \Vert$ and absorb it into $\theta$.

If $\Vert \omega \Vert \ne 1$, let $\tilde{\omega }=\omega /\Vert \omega \Vert$, $\tilde{\theta }=\theta \Vert \omega \Vert$, $\tilde{v}=v/\Vert \omega \Vert$, and apply the unit-twist formula with $(\tilde{\omega },\tilde{\theta },\tilde{v})$. This rescaling matters whenever twists come from velocity measurements rather than constructed unit screws.

(SE(3) Log Map Recipe) Extract $(\omega ,\theta ,v)$ from $g=(R,p)$ in two steps: SO(3) log on $R$ first, then $v=A(\omega ,\theta {)}^{-1}p$.

Step 1 — recover $(\omega ,\theta )$ from $R$ alone, using the SO(3) log map:
$\theta ={\cos }^{-1}\left(\frac{\mathrm{tr}(R)-1}{2}\right),\hat{\omega }=\frac{1}{2\sin \theta }(R-R^T).$
The trace formula uses $\mathrm{tr}(R)=1+2\cos \theta$, a direct corollary of Rodrigues’ formula. The axis comes from the antisymmetric part $R-R^T=2\sin \theta \hat{\omega }$. Two edge cases: $R=I$ gives $\theta =0$ with $\omega$ undefined (any axis works for a zero-angle rotation); $\mathrm{tr}(R)=-1$ gives $\theta =\pi$ where $\sin \theta =0$ kills the denominator — instead read off $\omega$ from any nonzero column of $R+I=2\omega {\omega }^T$ and normalize (sign is ambiguous since $(\omega ,\pi )\sim (-\omega ,\pi )$).

Step 2 — recover $v$ from $p$ using the inverse of the integration matrix:
$v=A(\omega ,\theta {)}^{-1}p,A(\omega ,\theta {)}^{-1}=\frac{1}{\theta }I-\frac{1}{2}\hat{\omega }+\left(\frac{1}{\theta }-\frac{1}{2}\cot \frac{\theta }{2}\right){\hat{\omega }}^2.$
The singularity at $\theta =0$ is handled by the pure-translation degenerate case: $R=I$ means $\omega =0$, and the rigid motion is a pure shift, so $v=p$ with $\theta =\Vert p\Vert$ and $\hat{\omega }$ pointing along $p$.

Note that $v$ cannot be recovered from $R$ alone. The rotational data $(\omega ,\theta )$ is encoded in $R$, but the same axis-angle pair is compatible with infinitely many translations $p\in {\mathbb{R}}^3$ — each pairing produces a different $v$. Step 2 explicitly mixes $R$ (through $A(\omega ,\theta {)}^{-1}$) with $p$ to undo the rotational coupling that the forward exponential introduced.

Frame of the extracted twist. In isolation, the recipe is frame-ambiguous: at the identity, spatial and body twists coincide (${\mathrm{Ad}}_I=I$), so the same $\xi$ exponentiates to $g$ whether interpreted as a constant spatial or constant body screw. The frame is fixed by what you do with the result. With the current pose $g_{sc}$ and target $g_{sd}$ both expressed in the spatial frame $s$, the index-matched composition gives $g_{sc}^{-1}{g}_{sd}=g_{cs}{g}_{sd}=g_{cd}$ — the relative motion expressed in the current body frame. So $\log (g_{cd})$ produces a body twist of $g_{sc}$. Reversing the order gives $g_{sd}{g}_{sc}^{-1}=g_{sd}{g}_{cs}$ — which does not collapse into a single index pair and instead represents the spatial version of the same screw. The two are related by the adjoint: ${\xi }_s={\mathrm{Ad}}_{g_{sc}}{\xi }_b$.

(Log Map in IK) $\log (g_{sc}^{-1}{g}_{sd})=\log (g_{cd})\in \mathfrak{se}(3)$ is the body twist that drives the current pose to the target in unit time.

Given current pose $g_{sc}$ and target pose $g_{sd}$, the pose error $g_{cd}=g_{sc}^{-1}{g}_{sd}$ lives in $SE(3)$, and its log is the 6-vector $(v,\omega )$ fed into the Jacobian update $\dot{q}=J^{-1}\xi$ in Jacobian IK. Near $\theta =0$ the cotangent term in $A^{-1}$ becomes ill-conditioned numerically; production code uses a Taylor series expansion of $A^{-1}$ around $\theta =0$ instead of the closed form.

Adjoint Representation

(Frame-Change Mechanism) The off-diagonal block $\hat{p}R$ encodes the lever-arm coupling between rotation and translation under a frame change.

When a twist is reexpressed in a frame shifted by $p$ and rotated by $R$, the angular part $\omega$ rotates cleanly into $R\omega$ — angular velocity is independent of origin. The linear part $v$ picks up an extra term $\hat{p}(R\omega )=p\times (R\omega )$ because shifting the origin by $p$ changes the linear velocity of the moving point by exactly the cross-product with the angular velocity. The block $\hat{p}R$ is “rotate $\omega$ first, then take the cross product with $p$” — exactly the rigid-body velocity transport law (see adjoint). This is why pure rotation in one frame can look like rotation-plus-translation in another.

(Algebraic Properties) $\mathrm{Ad}$ is a group homomorphism: ${\mathrm{Ad}}_{g_{ab}{g}_{bc}}={\mathrm{Ad}}_{g_{ab}}{\mathrm{Ad}}_{g_{bc}}$, ${\mathrm{Ad}}_{g_{ab}^{-1}}={\mathrm{Ad}}_{g_{ba}}=({\mathrm{Ad}}_{g_{ab}}{)}^{-1}$, $\det {\mathrm{Ad}}_g=1$.

Composition lifts: changing twist frames along a chain $a\leftarrow b\leftarrow c$ is the same as changing once by the composed transform $g_{ac}$. The inverse equals ${\mathrm{Ad}}_{g_{ba}}=\left[\begin{array}{cc}{R}_{ab}^T& -R_{ab}^T{\hat{p}}_{ab}\\ 0& R_{ab}^T\end{array}\right]$ — useful when converting body twists (${\xi }_b$, in the body’s own frame) to spatial twists (${\xi }_s$, in the world frame) and back via ${\xi }_s={\mathrm{Ad}}_{g_{sb}}{\xi }_b$. The unit determinant means twist coordinates change frames without volume distortion in ${\mathbb{R}}^6$, which matters for the symplectic structure of rigid-body dynamics.

Examples

Composing Two Rigid Motions

Let $g_{ab}$: rotate $90°$ about $z$, translate by $(1,0,0)$. Let $g_{bc}$: translate by $(0,1,0)$ (no rotation).

Composition $g_{ac}=g_{ab}{g}_{bc}$ $g_{ab}=\left[\begin{array}{cc}{R}_z(\pi /2)& e_1\\ 0& 1\end{array}\right]$, $g_{bc}=\left[\begin{array}{cc}I& e_2\\ 0& 1\end{array}\right]$.

$g_{ac}=g_{ab}{g}_{bc}=\left[\begin{array}{cc}{R}_z(\pi /2)\cdot I& R_z(\pi /2)e_2+e_1\\ 0& 1\end{array}\right]$.

Compute the rotated translation: $R_z(\pi /2)e_2=R_z(\pi /2)(0,1,0{)}^T=(-1,0,0{)}^T$. So $p_{ac}=(-1,0,0{)}^T+(1,0,0{)}^T=(0,0,0{)}^T$.

Result: $g_{ac}=\left[\begin{array}{cc}{R}_z(\pi /2)& 0\\ 0& 1\end{array}\right]$ — a pure rotation. The translation of $g_{bc}$ along $b$‘s $y$-axis got rotated by $g_{ab}$ into $a$‘s $-x$ direction, canceling $g_{ab}$‘s translation along $a$‘s $+x$. The same vector lives in different frames before and after the rotation, which is exactly what the $R_{ab}{p}_{bc}$ term tracks.