FANUC XYZ-WPR Format

Das Posenformat, welches von FANUC Robotern benutzt wird, besteht aus einer Position $XYZ$ in Millimetern und einer Orientierung $WPR$, welche durch drei Winkel in Grad gegeben ist. $W$ rotiert um die $x$-Achse, $P$ rotiert um die $y$-Achse und $R$ rotiert um die $z$-Achse. Die Rotationsreihenfolge ist $x$-$y$-$z$ und wird berechnet durch $r_z(R) r_y(P) r_x(W)$.

Umrechnung von FANUC-WPR in Quaternionen

Zur Umrechnung von $WPR$ Winkeln in Grad in eine Quaternion $q=(\begin{array}{cccc}x & y & z & w\end{array})$ werden zunächst die Winkel ins Bogenmaß umgerechnet

$$\begin{split}W_r = W \frac{\pi}{180} \text{,} \\ P_r = P \frac{\pi}{180} \text{,} \\ R_r = R \frac{\pi}{180} \text{,} \\\end{split}$$

und damit wird die Quaternion berechnet als

$$\begin{split}x = \cos{(R_r/2)}\cos{(P_r/2)}\sin{(W_r/2)} - \sin{(R_r/2)}\sin{(P_r/2)}\cos{(W_r/2)} \text{,} \\ y = \cos{(R_r/2)}\sin{(P_r/2)}\cos{(W_r/2)} + \sin{(R_r/2)}\cos{(P_r/2)}\sin{(W_r/2)} \text{,} \\ z = \sin{(R_r/2)}\cos{(P_r/2)}\cos{(W_r/2)} - \cos{(R_r/2)}\sin{(P_r/2)}\sin{(W_r/2)} \text{,} \\ w = \cos{(R_r/2)}\cos{(P_r/2)}\cos{(W_r/2)} + \sin{(R_r/2)}\sin{(P_r/2)}\sin{(W_r/2)} \text{.}\end{split}$$

Umrechnung von Quaternionen in FANUC-WPR

Die Umrechnung von einer Quaternion $q=(\begin{array}{cccc}x & y & z & w\end{array})$ mit $||q||=1$ in $WPR$ Winkel in Grad kann wie folgt durchgeführt werden.

$$\begin{split}R &= \text{atan}_2{(2(wz + xy), 1 - 2(y^2 + z^2))} \frac{180}{\pi} \\ P &= \text{asin}{(2(wy - zx))} \frac{180}{\pi} \\ W &= \text{atan}_2{(2(wx + yz), 1 - 2(x^2 + y^2))} \frac{180}{\pi}\end{split}$$