|
| 1 | +# SE(3) Transform, Twists and Wrenches |
| 2 | + |
| 3 | +## Overview & Motivation |
| 4 | + |
| 5 | +Every pose-level algorithm — manipulator kinematics and Jacobians, pose inverse kinematics, Cartesian interpolation, task-space control, drone navigation, visual odometry — manipulates the same few objects: a rigid transform, a velocity-like 6-vector (a *twist*), a force-like 6-vector (a *wrench*), and the maps between a constant twist and the finite motion it produces. Defining them once, with one ordering convention, prevents the silent sign and ordering mismatches that appear when independently written modules exchange 6-vectors. |
| 6 | + |
| 7 | +A transform is a rotation plus a translation: twelve scalars, composed with a few $3\times3$ products and inverted in closed form, with no matrix solve. A twist is re-expressed in another frame by the *adjoint*: rotate both parts, then add the lever-arm effect of the frame offset to the linear part. Wrenches follow the dual rule so that power is the same in every frame. The exponential turns "rotate about a line while sliding along it" into a finite motion; the logarithm recovers that motion, which is exactly what pose-error feedback needs. |
| 8 | + |
| 9 | +## Mathematical Theory |
| 10 | + |
| 11 | +### Rigid Transform |
| 12 | + |
| 13 | +A transform $T = (R, p)$ with $R \in SO(3)$ ($R^T R = I$, $\det R = +1$) and $p \in \mathbb{R}^3$ maps coordinates in frame B to frame A, $x_A = R\,x_B + p$. Its homogeneous form is |
| 14 | + |
| 15 | +$$T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix}.$$ |
| 16 | + |
| 17 | +$$T_1 T_2 = (R_1 R_2,\; R_1 p_2 + p_1), \qquad T^{-1} = (R^T,\; -R^T p), \qquad T\cdot x = R x + p.$$ |
| 18 | + |
| 19 | +The composition applies $T_2$ first. The inverse follows from $R^{-1} = R^T$. A free direction is only rotated, $x \mapsto R x$; the translation is ignored. |
| 20 | + |
| 21 | +### Ordering Convention (normative) |
| 22 | + |
| 23 | +All 6-vectors are ordered **linear part first**: twists $V = (v;\, \omega)$ and wrenches $W = (f;\, n)$. This matches a geometric Jacobian with linear rows first and the tangent ordering of Sophus and manif. Lynch & Park and Murray et al. use $(\omega;\, v)$; the two conventions differ by a block permutation, so any formula from those texts converts by swapping the blocks (for the adjoint below, the rotation blocks stay on the diagonal and the $[p]_\times R$ block moves to the lower left). |
| 24 | + |
| 25 | +### Adjoint, Twists and Wrenches |
| 26 | + |
| 27 | +Let $[a]_\times$ be the skew matrix with $[a]_\times b = a \times b$. A twist expressed in frame B is re-expressed in frame A by |
| 28 | + |
| 29 | +$$V_A = \mathrm{Ad}_T V_B, \qquad \mathrm{Ad}_T = \begin{pmatrix} R & [p]_\times R \\ 0 & R \end{pmatrix}, \qquad \text{i.e. } v' = R v + p \times R\omega,\;\; \omega' = R\omega.$$ |
| 30 | + |
| 31 | +The wrench map is the power-consistent dual, $\mathrm{Ad}_T^{-T}$: |
| 32 | + |
| 33 | +$$f' = R f, \qquad n' = R n + p \times R f.$$ |
| 34 | + |
| 35 | +Power is frame invariant because the cross terms cancel by the antisymmetry of the scalar triple product: |
| 36 | + |
| 37 | +$$W' \cdot V' = f \cdot v + n \cdot \omega + \underbrace{Rf \cdot (p \times R\omega) + (p \times Rf) \cdot R\omega}_{=\,0} = W \cdot V.$$ |
| 38 | + |
| 39 | +The adjoint is a group homomorphism, $\mathrm{Ad}_{AB} = \mathrm{Ad}_A\,\mathrm{Ad}_B$, and $\mathrm{Ad}_{T^{-1}} = \mathrm{Ad}_T^{-1}$. |
| 40 | + |
| 41 | +### Exponential |
| 42 | + |
| 43 | +A twist $\xi = (v;\, \omega)$ held for $\theta$ produces the finite motion $e^{[\xi]\theta}$ with $[\xi] = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix}$. It has the block form $\begin{pmatrix} e^{K\theta} & G v \\ 0 & 1 \end{pmatrix}$ with $K = [\omega]_\times$ and $G = \int_0^\theta e^{Ks}\,ds$: Rodrigues' formula for the rotation, integrated term by term for the translation. Without normalising $\omega$, and with $\varphi = \theta\,\|\omega\|$, |
| 44 | + |
| 45 | +$$R = I + \theta\,a_1\,K + \theta^2 a_2\,K^2, \qquad p = \theta v + \theta^2 a_2\,K v + \theta^3 a_3\,K^2 v,$$ |
| 46 | + |
| 47 | +$$a_1 = \frac{\sin\varphi}{\varphi}, \qquad a_2 = \frac{1-\cos\varphi}{\varphi^2} = \frac{2\sin^2(\varphi/2)}{\varphi^2}, \qquad a_3 = \frac{\varphi - \sin\varphi}{\varphi^3}.$$ |
| 48 | + |
| 49 | +Below $\varphi = 0.1$ the coefficients switch to short Taylor series, |
| 50 | + |
| 51 | +$$a_1 \approx 1 - \tfrac{\varphi^2}{6} + \tfrac{\varphi^4}{120}, \qquad a_2 \approx \tfrac12 - \tfrac{\varphi^2}{24} + \tfrac{\varphi^4}{720}, \qquad a_3 \approx \tfrac16 - \tfrac{\varphi^2}{120} + \tfrac{\varphi^4}{5040},$$ |
| 52 | + |
| 53 | +so there is no singular branch at $\omega = 0$ (pure translation falls out as $R = I$, $p = \theta v$) and no $0/0$ or cancellation for small $\varphi$. Because $\theta$ scales the whole twist, a non-unit $\omega$ needs no normalisation. |
| 54 | + |
| 55 | +Re-expressing a twist in another frame generates the same motion conjugated by that frame: |
| 56 | + |
| 57 | +$$\mathrm{Exp}(\mathrm{Ad}_A\,\xi,\, t) = A\,\mathrm{Exp}(\xi,\, t)\,A^{-1}.$$ |
| 58 | + |
| 59 | +### Logarithm of a Rotation |
| 60 | + |
| 61 | +Let $s = \tfrac12\,\mathrm{vee}(R - R^T) = \sin\varphi\,\hat\omega$ and $\cos\varphi = \tfrac12(\operatorname{tr}R - 1)$. The angle is |
| 62 | + |
| 63 | +$$\varphi = \mathrm{atan2}\big(\|s\|,\; \cos\varphi\big),$$ |
| 64 | + |
| 65 | +which is accurate near $0$ and $\pi$, unlike $\arccos$. |
| 66 | + |
| 67 | +- **$\varphi \le \pi/2$:** the rotation vector is $s\,\varphi/\sin\varphi$ (zero when $R = I$). |
| 68 | +- **$\varphi > \pi/2$:** $s/\sin\varphi$ would amplify noise as $\sin\varphi \to 0$, so the axis comes from the symmetric part, $\tfrac12(R + R^T) = \cos\varphi\, I + (1-\cos\varphi)\,\hat\omega\hat\omega^T$: |
| 69 | + |
| 70 | +$$\hat\omega_i \hat\omega_j = \frac{\tfrac12(R_{ij} + R_{ji}) - \delta_{ij}\cos\varphi}{1 - \cos\varphi}.$$ |
| 71 | + |
| 72 | +The pivot component $i$ is the largest diagonal entry, so $\hat\omega_i^2 \ge \tfrac13$ and the divisions are benign; $\hat\omega_i = \sqrt{\hat\omega_i^2}$, the other components follow from the off-diagonal products, and the sign is taken from $s_i$. At exactly $\pi$ either sign is valid. The result satisfies $\|\omega\| \le \pi$: rotations are re-expressed the short way. |
| 73 | + |
| 74 | +### Logarithm of a Transform |
| 75 | + |
| 76 | +With $\omega = \mathrm{Log}(R)$ and $\varphi = \|\omega\|$, the inverse of the integral $G$ is |
| 77 | + |
| 78 | +$$v = \Big(I - \tfrac12[\omega]_\times + c(\varphi)\,[\omega]_\times^2\Big)\,p, \qquad c(\varphi) = \frac{1 - \tfrac{\varphi}{2}\cot\tfrac{\varphi}{2}}{\varphi^2} \approx \tfrac1{12} + \tfrac{\varphi^2}{720} + \tfrac{\varphi^4}{30240} \;\;(\varphi < 0.1).$$ |
| 79 | + |
| 80 | +The result is $(v;\, \omega)$ with $\theta = 1$, so $\mathrm{Exp}(\mathrm{Log}(T), 1) = T$. The coefficient stays finite on $[0, \pi]$, reaching $1/\pi^2$ at $\varphi = \pi$. |
| 81 | + |
| 82 | +### Pose Error |
| 83 | + |
| 84 | +With target $(R_t, p_t)$ and current $(R_c, p_c)$, the base-frame error is |
| 85 | + |
| 86 | +$$e = \big(p_t - p_c;\;\; \mathrm{Log}(R_t R_c^T)\big),$$ |
| 87 | + |
| 88 | +the translation difference stacked on the rotation vector that carries the current orientation onto the target. It has the linear-first ordering of a geometric Jacobian, and $\|e_\omega\| \le \pi$ always points the short way round. |
| 89 | + |
| 90 | +## Complexity Analysis |
| 91 | + |
| 92 | +| Operation | Time | Space | Notes | |
| 93 | +|-----------|------|-------|-------| |
| 94 | +| Compose | O(1) | O(1) | one $3\times3$ product, one matrix–vector product | |
| 95 | +| Inverse | O(1) | O(1) | transpose and one matrix–vector product; no solve | |
| 96 | +| Apply / rotate | O(1) | O(1) | one matrix–vector product (plus 3 additions for apply) | |
| 97 | +| Twist / wrench transform | O(1) | O(1) | two matrix–vector products and one cross product, no $6\times6$ formed | |
| 98 | +| Adjoint | O(1) | O(1) | $36$ scalars, one extra $3\times3$ product; built only on request | |
| 99 | +| Exp | O(1) | O(1) | two sines of $\varphi$ and $\varphi/2$, none below $\varphi = 0.1$ | |
| 100 | +| Log | O(1) | O(1) | one atan2; a half-angle sine/cosine pair above $\varphi = 0.1$; one square root for large angles | |
| 101 | + |
| 102 | +A transform holds $12$ scalars; the $6\times6$ adjoint appears only when requested. All operations are stack-only: no heap, no recursion, constant work per call. |
| 103 | + |
| 104 | +## Step-by-Step Walkthrough |
| 105 | + |
| 106 | +**Apply.** $R = R_z(\pi/2)$, $p = (1,0,0)$: $R\,(1,0,0) = (0,1,0)$, plus $p$ gives $(1,1,0)$. |
| 107 | + |
| 108 | +**Exp of a screw.** Rotation about $\hat z$ through the point $(1,0,0)$: $\omega = \hat z$, $v = -\hat z \times (1,0,0) = (0,-1,0)$, $\theta = \pi/2$. |
| 109 | + |
| 110 | +| Step | Value | |
| 111 | +|------|-------| |
| 112 | +| $\varphi = \theta\|\omega\|$ | $\pi/2 \ge 0.1$, direct forms | |
| 113 | +| Coefficients | $a_1 = 2/\pi$, $a_2 = 4/\pi^2$, $a_3 = (\pi/2 - 1)/(\pi/2)^3$ | |
| 114 | +| Vectors | $Kv = \hat z \times v = (1,0,0)$, $K^2 v = (0,1,0)$ | |
| 115 | +| $R$ | $\theta a_1 = 1$, $\theta^2 a_2 = 1$: $R = I + K + K^2 = R_z(\pi/2)$ | |
| 116 | +| $p$ | $\tfrac{\pi}{2}(0,-1,0) + (1,0,0) + (\tfrac{\pi}{2} - 1)(0,1,0) = (1,-1,0)$ | |
| 117 | + |
| 118 | +Check: the origin sits at $(-1,0,0)$ relative to the axis; a quarter turn takes it to $(0,-1,0)$, and adding the axis point gives $(1,-1,0)$. |
| 119 | + |
| 120 | +**Log of that transform.** $s = (0,0,1)$, $\cos\varphi = 0$, so $\varphi = \pi/2$ and $\omega = (0,0,\pi/2)$. Then $\omega \times p = (\tfrac\pi2, \tfrac\pi2, 0)$, $\omega \times (\omega \times p) = (-\tfrac{\pi^2}{4}, \tfrac{\pi^2}{4}, 0)$, and $c(\pi/2)\,\pi^2/4 = 1 - \pi/4$, so |
| 121 | + |
| 122 | +$$v = (1 - \tfrac\pi4 - (1 - \tfrac\pi4),\; -1 - \tfrac\pi4 + (1 - \tfrac\pi4),\; 0) = (0, -\tfrac\pi2, 0).$$ |
| 123 | + |
| 124 | +The result $\big((0,-\tfrac\pi2,0);\,(0,0,\tfrac\pi2)\big)$ is $\theta\xi$, as expected. |
| 125 | + |
| 126 | +**Pose error, short way round.** Target rotated $+190^\circ$ about $\hat z$ relative to the current pose: $\cos\varphi = \cos 190^\circ = -0.9848$, $\|s\| = \sin 170^\circ = 0.1736$, so $\varphi = 170^\circ = 2.967$ rad $> \pi/2$. The symmetric part gives $\hat\omega_z^2 = (1 + 0.9848)/1.9848 = 1$, and $s_z = \sin 190^\circ < 0$ selects $-\hat z$. The rotation error is $-170^\circ\cdot\hat z$, not $+190^\circ$. |
| 127 | + |
| 128 | +## Pitfalls & Edge Cases |
| 129 | + |
| 130 | +- **Ordering mismatches.** Formulas from texts using $(\omega;\, v)$ must have their blocks swapped; applying the twist adjoint to a wrench breaks power invariance, since wrenches need the dual map. |
| 131 | +- **Drift off SO(3).** Long chains of products in single precision accumulate error in $R$. Re-orthonormalise a transform that is integrated over time, not after every single composition. |
| 132 | +- **Non-orthonormal input to Log.** The trace and antisymmetric-part formulas assume $R \in SO(3)$; a rotation far from orthonormal gives a meaningless angle. |
| 133 | +- **Small angles.** $\sin\varphi/\varphi$, $(1-\cos\varphi)/\varphi^2$ and $(\varphi-\sin\varphi)/\varphi^3$ are $0/0$ or lose all digits near zero; the series below $\varphi = 0.1$ remove both problems with no special case at $\omega = 0$. |
| 134 | +- **Angles near $\pi$.** The antisymmetric part vanishes, so $s/\sin\varphi$ is unusable; the symmetric-part axis is used beyond $\pi/2$. At exactly $\pi$ the sign is arbitrary. The rotation vector is discontinuous across $\pi$ because $\pm\pi\hat\omega$ describe the same rotation. |
| 135 | +- **Wrong logarithm.** Twice the vector part of a unit quaternion is $2\sin(\varphi/2)\,\hat\omega$, only a small-angle approximation of $\varphi\,\hat\omega$. |
| 136 | +- **No general inverse.** Never invert the $4\times4$ matrix; the closed form is exact and cheaper. |
| 137 | +- **Measured accuracy.** On $3000$ random twists, including $\varphi \approx 10^{-4}$ and $\varphi \in [3, 3.1405]$, Exp agrees with a double-precision $4\times4$ matrix exponential to $\approx 6\times10^{-7}$, and the Exp–Log round trip is accurate to $\approx 6\times10^{-7}$. |
| 138 | + |
| 139 | +## Variants & Generalizations |
| 140 | + |
| 141 | +| Variant | Key difference | |
| 142 | +|---------|----------------| |
| 143 | +| **Angular-first ordering** $(\omega;\, v)$ | Same algebra with blocks permuted; used by Lynch & Park and Murray et al. | |
| 144 | +| **Body vs. spatial twist** | Body twists are $T^{-1}\dot T$, spatial twists $\dot T\,T^{-1}$; related by the adjoint | |
| 145 | +| **Quaternion rotation part** | Four scalars and cheap renormalisation; the rotation vector is the vector part scaled by $\varphi/\sin(\varphi/2)$ | |
| 146 | +| **Dual quaternions** | Rotation and translation in one algebra; screw interpolation | |
| 147 | +| **Similarity group** | Adds a uniform scale for monocular visual odometry | |
| 148 | + |
| 149 | +## Applications |
| 150 | + |
| 151 | +- **Manipulator kinematics** — forward kinematics as a product of exponentials; Jacobian columns are adjoint-transformed joint twists. |
| 152 | +- **Pose inverse kinematics and task-space control** — the pose error drives a Jacobian-based update with consistent ordering. |
| 153 | +- **Cartesian interpolation** — Exp of a scaled Log of the relative transform gives a constant-twist screw path. |
| 154 | +- **Force and torque sensing** — moving a measured wrench between sensor, tool and base frames with power preserved. |
| 155 | +- **Navigation and odometry** — integrating body twists into poses on drones and in visual odometry. |
| 156 | + |
| 157 | +## Connections to Other Algorithms |
| 158 | + |
| 159 | +- **Quaternion** — an alternative rotation representation; renormalisation there is the cheap fix for drift, and its vector part is not the logarithm. |
| 160 | +- **Angle Utilities** — the shortest signed angle difference is the one-dimensional analogue of the short-way rotation error: $+190^\circ$ becomes $-170^\circ$. |
| 161 | +- **Givens Rotation** — a plane rotation embedded in a coordinate plane is the single-axis special case of the rotation block. |
| 162 | +- **Matrix Exponential** — the general matrix exponential of the $4\times4$ twist matrix is the closed form's reference; it is used only for verification, not at run time. |
| 163 | + |
| 164 | +## References & Further Reading |
| 165 | + |
| 166 | +- K. M. Lynch and F. C. Park, *Modern Robotics: Mechanics, Planning, and Control*, Cambridge University Press, 2017 — Chapter 3. |
| 167 | +- R. M. Murray, Z. Li, and S. S. Sastry, *A Mathematical Introduction to Robotic Manipulation*, CRC Press, 1994 — Chapter 2. |
| 168 | +- J. Solà, J. Deray, and D. Atchuthan, "A micro Lie theory for state estimation in robotics," arXiv:1812.01537, 2018. |
0 commit comments