# 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$$.

$\begin{split}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)\end{split}$

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:

$\begin{split}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})}\end{split}$

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

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

and then calculating the quaternion with

$\begin{split}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{.}\end{split}$

## 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:

$\begin{split}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)\end{split}$

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.

$\begin{split}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}\end{split}$

## 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

$\begin{split}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)\end{split}$