User:BenFrantzDale/SE(3)

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.

To compute

::

let

:

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

:$\backslash exp\backslash begin\{bmatrix\}\backslash omega\; \backslash \backslash t\; \backslash end\{bmatrix\}=\; I\; +\; S\; +\; \backslash frac\{1-\backslash cos|\backslash omega|\}$

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

Alternately, we can replace the S expressions with more-basic identities{{citation}}:

From Rodrigues' rotation formula we have:

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

And then compute t:

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

which simplifies to

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

Note that

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

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

and so

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

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

To compute

:

First,

:$\backslash omega:=\backslash log(R)$

according to the Rodrigues' rotation formula. Note that there are numerical issues around zero and around $\backslash theta=\backslash pi$.

If $|\backslash 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 $\backslash 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\text{'}:=\; Rp+t$

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

Let

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

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

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

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

So in general,

:$u=\backslash bar\; u\; +\; \backslash alpha\; \backslash hat\; x$

and

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

so if we enforce that u is perpendicular to $\backslash hat\; x$, we have

:$u=\backslash bar\; u\; -\; (\backslash bar\; u\; \backslash cdot\; \backslash hat\; x)\backslash hat\; x\; =\; (I-\backslash hat\; x\backslash hat\; x^\backslash top)\; \backslash bar\; u$.

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

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

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

:$v=u\backslash times\; \backslash omega\; +\; \backslash hat\; x\; \backslash hat\; x^\backslash top\; t\; =\; \backslash hat\; x\; \backslash hat\; x\; ^\backslash top\; t\; -\; \backslash omega\; \backslash times\; u$ (why?)

We can substitute to expand this out:

:$\backslash omega\backslash times\; u\; =\; \backslash theta\; [\backslash hat\; x]\_\backslash times\; u$

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

:$\backslash hat\; x\; \backslash hat\; x^\backslash top\; =\; I\; +[\backslash hat\; x]^2$

and:

:$[\backslash hat\; x]\_\backslash times\; \backslash hat\; x\; \backslash hat\; x^\backslash top\; =\; 0$

so we have

:$\backslash omega\; \backslash times\; u\; =\; \backslash theta\; [\backslash hat\; x]\_\backslash times\; u$

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

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

So including the full expression for v, we get:

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

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

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

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

:$v=\backslash left(I-\backslash frac\{[\backslash omega]\_\backslash times\}\{2\}\; +\; \backslash frac\{1\}\{\backslash theta^2\}\; \backslash left(1-\backslash frac\{\backslash theta\}\{2\; \backslash tan\backslash frac\{\backslash theta\}\{2\}\}\backslash right)\; [\backslash omega]\_\backslash times^2\backslash 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:

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

so

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

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

:::$=\; \backslash frac\{2\backslash sin\backslash theta-\backslash theta(1+\backslash cos\backslash theta)\}\{2\backslash theta^2\backslash sin\backslash 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 $|\backslash omega|=0$ and so terms should be replaced by a polynomial expansion.

• Screw theory
• Rigid transformation

• [http://tom-itx.dyndns.org:81/~webpage/pdf/robot-kinematics.pdf Introduction to Intelligent Robotics] by Herman Bruyninckx and Joris De Schutter
• [http://www.prometheus-inc.com/asi/algebra2003/papers/selig.pdf Lie Groups and Lie Algebras in Robotics] by J.M. Selig
• http://www.cds.caltech.edu/~murray/mlswiki/index.php/Rigid_Body_Motion
• [http://caltechcdstr.library.caltech.edu/19/00/95-010.pdf PD Control by Murray]