User:BenFrantzDale/SE(3)

The Euclidean group in 3-dimensional space is known as SE(3)—the group of rigid-body motions in 3D. This has applications in kinematics, computer vision, robotics, etc. SE(3) is a Lie group, and has a corresponding Lie algebra representing the tangent space at the identity—the space of rigid-body velocities.

This group is generally represented as a homogeneous transformation matrix: a 4×4 matrix with the top-left 3×3 block a rotation matrix—an element of SO(3), the top-right 3×1 block a translation vector, and the bottom row being [0 0 0 1].

Exponential map

The exponential map on SE(3) is given by the matrix exponential and matrix logarithm. The logarithm turns the rotation part into a cross product matrix corresponding to the axis-angle representation of the rotation.

Exp

To compute

${\begin{bmatrix}R&t\\0&1\end{bmatrix}}=\exp {\begin{bmatrix}\omega \\v\end{bmatrix}}$:

let

$S={\begin{bmatrix}[\omega ]_{\times }&v\\0&0\end{bmatrix}}$

if $|\omega |=0$, there is no rotation and the result is $I+S$. Otherwise, the result is:

$\exp {\begin{bmatrix}\omega \\t\end{bmatrix}}=I+S+{\frac {1-\cos |\omega |}{|\omega |^{2}}}S^{2}+{\frac {|\omega |-\sin |\omega |}{|\omega |^{3}}}S^{3}$.

In practice the full equation breaks down numerically near $|\omega |=0$ and so terms should be replaced by a polynomial expansion.

Alternately, we can replace the S expressions with more-basic identities {{citation}}: Empty citation (help): From Rodrigues' rotation formula we have:

$R=I+\sin \theta [\mathbf {k} ]_{\times }+(1-\cos \theta )(\mathbf {k} \mathbf {k} ^{\top }-I)$ where $\mathbf {k} :=\omega /\|\omega \|=\omega /\theta$.

And then compute t:

$t={\frac {(I-R)\omega \times v+v\cdot \omega \omega }{\theta ^{2}}}$

which simplifies to

$t={\frac {(I-R)[\omega ]_{\times }+\omega \omega ^{\top }}{\theta ^{2}}}v$.

Note that

$I-R=I-I-\sin \theta [\mathbf {k} ]_{\times }-(1-\cos \theta )(\mathbf {k} \mathbf {k} ^{\top }-I)$

$I-R=-\sin \theta [\mathbf {k} ]_{\times }-(1-\cos \theta )(\mathbf {k} \mathbf {k} ^{\top }-I)$

and so

$(I-R)[\omega ]_{\times }={\frac {-\sin \theta }{\theta }}[\omega ]_{\times }^{2}+(1-\cos \theta )(\mathbf {k} \mathbf {k} ^{\top }[\omega ]_{\times }-[\omega ]_{\times })$

$(I-R)[\omega ]_{\times }={\frac {-\sin \theta }{\theta }}[\omega ]_{\times }^{2}+(1-\cos \theta )[\omega ]_{\times }$

Log

To compute ${\begin{bmatrix}\omega \\v\end{bmatrix}}=\log {\begin{bmatrix}R&t\\0&1\end{bmatrix}}$: First,

$\omega :=\log(R)$

according to the Rodrigues' rotation formula. Note that there are numerical issues around zero and around $\theta =\pi$. If $|\omega |=0$ (no rotation) then $v=t$ and we are done. Otherwise, it's complicated:

We first find the screw representation of the transformation. We have the $\omega$ part, so we need to find a point, u, on the screw axis. The action of the transformation, of course, will move points in a screw motion about that axis...

We'll transform a point, p, and take the perpendicular bisector to the segment between $p$ and $p''$ where $p''$ is p transformed projected back onto the plane through x that intersects p. Let:

p be any point

$p':=Rp+t$

$p'':=p'-((p'-p)\cdot {\hat {x}}){\hat {x}}=p'{\hat {x}}{\hat {x}}^{\top }(p'-p)=(I-{\hat {x}}{\hat {x}}^{\top })p'+{\hat {x}}{\hat {x}}^{\top }p$

Let

$b:={\frac {\|p''-p\|}{2}}$, the distance from p to the middle of the line to $p''$

$a:={\frac {\|p''-p\|}{2\tan {\frac {\theta }{2}}}}$, the height of the trangle formed by the center of rotation, p, and $p''$.

${\vec {a}}:={\hat {x}}\times (p''-p)$

${\bar {u}}:={\frac {p''+p}{2}}+{\frac {\|p''-p\|}{2\tan {\frac {\theta }{2}}}}\cdot {\frac {x\times (p''-p)}{\|{\hat {x}}\times (p''-p)\|}}$ where ${\overline {u}}$ is on the screw axis, not necessarily perpendicular to ${\hat {x}}$.

So in general,

$u={\bar {u}}+\alpha {\hat {x}}$

and

$({\bar {u}}+\alpha {\hat {x}})\cdot {\hat {x}}=0\Rightarrow \alpha {\hat {x}}\cdot {\hat {x}}=-{\bar {u}}\cdot {\hat {x}}=\Rightarrow \alpha =-{\frac {{\bar {u}}\cdot {\hat {x}}}{{\hat {x}}\cdot {\hat {x}}}}=-{\hat {u}}\cdot {\hat {x}}$

so if we enforce that u is perpendicular to ${\hat {x}}$, we have

$u={\bar {u}}-({\bar {u}}\cdot {\hat {x}}){\hat {x}}=(I-{\hat {x}}{\hat {x}}^{\top }){\bar {u}}$.

Since p is arbitrary, we can let it be the zero vector. Then we have

$u=\left((I-{\hat {x}}{\hat {x}}^{\top })+{\frac {[{\hat {x}}]_{\times }}{\tan {\frac {\theta }{2}}}}\right)t/2$.

Now, the v component of the 6-vector we are looking for is not u, it is:

$v=u\times \omega +{\hat {x}}{\hat {x}}^{\top }t={\hat {x}}{\hat {x}}^{\top }t-\omega \times u$ (why?)

We can substitute to expand this out:

$\omega \times u=\theta [{\hat {x}}]_{\times }u$

since $\omega$ is the vector in the direction of the unit vector ${\hat {x}}$ with magnitude $\theta$. So we can plug in the u from above. Also, note that:

${\hat {x}}{\hat {x}}^{\top }=I+[{\hat {x}}]^{2}$

and:

$[{\hat {x}}]_{\times }{\hat {x}}{\hat {x}}^{\top }=0$

so we have

$\omega \times u=\theta [{\hat {x}}]_{\times }u$

$\omega \times u=\theta [{\hat {x}}]_{\times }\left(I-{\hat {x}}{\hat {x}}^{\top }+{\frac {[{\hat {x}}]_{\times }}{\tan {\frac {\theta }{2}}}}\right){\frac {t}{2}}$

$\omega \times u=\theta \left([{\hat {x}}]_{\times }+{\frac {[{\hat {x}}]_{\times }^{2}}{\tan {\frac {\theta }{2}}}}\right){\frac {t}{2}}$

So including the full expression for v, we get:

$v=\left(-{\frac {\theta }{2}}\left([{\hat {x}}]_{\times }+{\frac {[{\hat {x}}]_{\times }^{2}}{\tan {\frac {\theta }{2}}}}\right)t+{\hat {x}}{\hat {x}}^{\top }t\right)$

$v=\left(-{\frac {\theta }{2}}\left([{\hat {x}}]_{\times }+{\frac {[{\hat {x}}]_{\times }^{2}}{\tan {\frac {\theta }{2}}}}\right)+{\hat {x}}{\hat {x}}^{\top }\right)t$

$v=\left(-{\frac {\theta }{2}}[{\hat {x}}]_{\times }+{\frac {-\theta }{2\tan {\frac {\theta }{2}}}}+{\hat {x}}{\hat {x}}^{\top }\right)t$

$v=\left(I+{\frac {-\theta }{2}}[{\hat {x}}]_{\times }+\left(1-{\frac {\theta }{2\tan {\frac {\theta }{2}}}}\right)[{\hat {x}}]_{\times }^{2}\right)t$

$v=\left(I-{\frac {[\omega ]_{\times }}{2}}+{\frac {1}{\theta ^{2}}}\left(1-{\frac {\theta }{2\tan {\frac {\theta }{2}}}}\right)[\omega ]_{\times }^{2}\right)t$.

Finally, if you want to rearrange that into a form shown in other papers, you can apply the half angle formula for tangent:

$\tan {\frac {\theta }{2}}={\frac {\sin \theta }{1+\cos \theta }}$

so

${\frac {1}{\theta ^{2}}}\left(1-{\frac {\theta (1+\cos \theta )}{2\sin \theta }}\right)$

$={\frac {1}{\theta ^{2}}}{\frac {2\sin \theta -\theta (1+\cos \theta )}{2\sin \theta }}$

$={\frac {2\sin \theta -\theta (1+\cos \theta )}{2\theta ^{2}\sin \theta }}$

which is found in various references such as http://www.kramirez.net/Robotica/Material/Libros/A%20mathematical%20Introduction%20to%20Robotic%20manipulation.pdf p. 414.

Note: In practice the full equation breaks down numerically near $|\omega |=0$ and so terms should be replaced by a polynomial expansion.

See also

• Screw theory
• Rigid transformation

External links

• Introduction to Intelligent Robotics by Herman Bruyninckx and Joris De Schutter
• Lie Groups and Lie Algebras in Robotics by J.M. Selig
• http://www.cds.caltech.edu/~murray/mlswiki/index.php/Rigid_Body_Motion
• PD Control by Murray