Franka Emika Pose Format¶

Franka Emika robots use a transformation matrix $T$ to define a pose. A transformation matrix combines a rotation matrix $R$ and a translation vector $t=(\begin{array}{cccc}x & y & z\end{array})^T$ .

$T = \left(\begin{array}{cccc} r_{00} & r_{01} & r_{02} & x\\ r_{10} & r_{11} & r_{12} & y\\ r_{20} & r_{21} & r_{22} & z\\ 0 & 0 & 0 & 1 \end{array}\right)$

The pose given by Franka Emika’s “Measure Pose” App consists of a translation $x, y, z$ in millimeters and a rotation $x, y, z$ in degrees. The rotation convention is $z$ - $y'$ - $x''$ (i.e. $x$ - $y$ - $z$ ) and is computed by $r_z(z) r_y(y) r_x(x).$

Conversion from transformation matrix to quaternion¶

The conversion from a rotation matrix (with $det(R)=1$ ) to a quaternion $q=(\begin{array}{cccc}q_x & q_y & q_z & q_w \end{array})$ can be done as follows:

$q_x &= \text{sign}(r_{21}-r_{12}) \frac{1}{2}\sqrt{\text{max}(0, 1 + r_{00} - r_{11} - r_{22})} \\ q_y &= \text{sign}(r_{02}-r_{20}) \frac{1}{2}\sqrt{\text{max}(0, 1 - r_{00} + r_{11} - r_{22})} \\ q_z &= \text{sign}(r_{10}-r_{01}) \frac{1}{2}\sqrt{\text{max}(0, 1 - r_{00} - r_{11} + r_{22})} \\ q_w &= \frac{1}{2}\sqrt{\text{max}(0, 1 + r_{00} + r_{11} + r_{22})}$

The $\text{sign}$ operator returns -1 if the argument is negative. Otherwise, 1 is returned. It is used to recover the sign for the square root. The $\text{max}$ function ensures that the argument of the square root function is not negative, which can happen in practice due to round-off errors.

Conversion from Rotation-XYZ to quaternion¶

The conversion from the $x, y, z$ angles in degrees to a quaternion $q=(\begin{array}{cccc}q_x & q_y & q_z & q_w\end{array})$ can be done by first converting all angles to radians

$X_r = x \frac{\pi}{180} \text{,} \\ Y_r = y \frac{\pi}{180} \text{,} \\ Z_r = z \frac{\pi}{180} \text{,} \\$

and then calculating the quaternion with

$q_x = \cos{(Z_r/2)}\cos{(Y_r/2)}\sin{(X_r/2)} - \sin{(Z_r/2)}\sin{(Y_r/2)}\cos{(X_r/2)} \text{,} \\ q_y = \cos{(Z_r/2)}\sin{(Y_r/2)}\cos{(X_r/2)} + \sin{(Z_r/2)}\cos{(Y_r/2)}\sin{(X_r/2)} \text{,} \\ q_z = \sin{(Z_r/2)}\cos{(Y_r/2)}\cos{(X_r/2)} - \cos{(Z_r/2)}\sin{(Y_r/2)}\sin{(X_r/2)} \text{,} \\ q_w = \cos{(Z_r/2)}\cos{(Y_r/2)}\cos{(X_r/2)} + \sin{(Z_r/2)}\sin{(Y_r/2)}\sin{(X_r/2)} \text{.}$

Conversion from quaternion and translation to transformation¶

The conversion from a quaternion $q=(\begin{array}{cccc}q_x & q_y & q_z & q_w\end{array})$ and a translation vector $t=(\begin{array}{cccc}x & y & z\end{array})^T$ to a transformation matrix $T$ can be done as follows:

$T = \left(\begin{array}{cccc} 1 - 2s(q_y^2+q_z^2) & 2s(q_x q_y-q_z q_w) & 2s(q_x q_z+q_y q_w) & x\\ 2s(q_x q_y+q_z q_w) & 1 - 2s(q_x^2+q_z^2) & 2s(q_y q_z-q_x q_w) & y\\ 2s(q_x q_z-q_y q_w) & 2s(q_y q_z+q_x q_w) & 1 - 2s(q_x^2+q_y^2) & z\\ 0 & 0 & 0 & 1 \end{array}\right)$

where $s=||q||^{-2}=\frac{1}{q_x^2+q_y^2+q_z^2+q_w^2}$ and $s=1$ if $q$ is a unit quaternion.

Conversion from quaternion to Rotation-XYZ¶

The conversion from a quaternion $q=(\begin{array}{cccc}q_x & q_y & q_z & q_w\end{array})$ with $||q||=1$ to the $x, y, z$ angles in degrees can be done as follows.

$x &= \text{atan}_2{(2(q_w q_z + q_x q_y), 1 - 2(q_y^2 + q_z^2))} \frac{180}{\pi} \\ y &= \text{asin}{(2(q_w q_y - q_z q_x))} \frac{180}{\pi} \\ z &= \text{atan}_2{(2(q_w q_x + q_y q_z), 1 - 2(q_x^2 + q_y^2))} \frac{180}{\pi}$

Pose representation in RaceCom messages and state machines¶

In RaceCom messages and in state machines a pose is usually defined as one-dimensional array of 16 float values, representing the transformation matrix in column-major order. The indices of the matrix entries below correspond to the array indices

$T = \left(\begin{array}{cccc} a_0 & a_4 & a_8 & a_{12}\\ a_1 & a_5 & a_9 & a_{13}\\ a_2 & a_6 & a_{10} & a_{14}\\ a_3 & a_7 & a_{11} & a_{15} \end{array}\right)$