Universal Robots pose format

The pose format that is used by Universal Robots consists of a position $XYZ$ in millimeters and an orientation in angle-axis format $V=(\begin{array}{ccc}RX & RY & RZ\end{array})^T$. The rotation angle $\theta$ in radians is the length of the rotation axis $U$.

$$\begin{split}V = \left(\begin{array}{c}RX \\ RY \\ RZ\end{array}\right) = \left(\begin{array}{c}\theta u_x \\ \theta u_y \\ \theta u_z\end{array}\right)\end{split}$$

$V$ is called a rotation vector.

Conversion from angle-axis format to quaternion

The conversion from a rotation vector $V$ to a quaternion $q=(\begin{array}{cccc}x & y & z & w\end{array})^T$ can be done as follows.

We first recover the angle $\theta$ in radians from the rotation vector $V$ by

$$\theta = \sqrt{RX^2 + RY^2 + RZ^2}\text{.}$$

If $\theta = 0$, then the quaternion is $q=(\begin{array}{cccc}0 & 0 & 0 & 1\end{array})^T$, otherwise it is

$$\begin{split}x = RX \frac{\sin(\theta/2)}{\theta}\text{,} \\ y = RY \frac{\sin(\theta/2)}{\theta}\text{,} \\ z = RZ \frac{\sin(\theta/2)}{\theta}\text{,} \\ w = \cos(\theta/2)\text{.}\end{split}$$

Conversion from quaternion to angle-axis format

The conversion from a quaternion $q=(\begin{array}{cccc}x & y & z & w\end{array})^T$ with $||q||=1$ to a rotation vector in angle-axis form can be done as follows.

We first recover the angle $\theta$ in radians from the quaternion by

$$\theta = 2\cdot\text{acos}(w)\text{.}$$

If $\theta = 0$, then the rotation vector is $V=(\begin{array}{ccc}0 & 0 & 0\end{array})^T$, otherwise it is

$$\begin{split}RX = \theta \frac{x}{\sqrt{1-w^2}}\text{,} \\ RY = \theta \frac{y}{\sqrt{1-w^2}}\text{,} \\ RZ = \theta \frac{z}{\sqrt{1-w^2}}\text{.}\end{split}$$