# 3D Coordinate Rotation Using Roll-Pitch-Yaw

Given an orthogonal unit vector triad (i,j,k) described in the 3-dimensional coordinate space (X,Y,Z), as shown below.

https://i.imgur.com/dyqKSKf.jpg

I need to rotate the XYZ coordinate system such that X is collinear with i, Y is collinear with j, and Z is collinear with k. I am constrained to performing 3 rotations: First about the X axis, then about the ORIGINAL Y axis, and finally about the ORIGINAL Z axis, as shown below.

https://i.imgur.com/b4lmemP.jpg

How can I determine the rotation angles a, b, and c? I realize that I can use pitch-roll-yaw rotation matrices to rotate one vector, but there is not a unique solution for the rotation of one vector. How can I combine the equations to find the unique solution that will rotate X, Y, and Z to be collinear with i,j,k?

• The constraints are unfortunate, because you could directly construct the rotation matrix needed using the $i$, $j$, and $k$ vectors (normalized to unit length). It is the decomposition to your $x$-$y^\prime$-$z^{\prime\prime}$ Tait-Bryan angles ($R = Z(\gamma) Y(\beta) X(\alpha)$) that is the hard part. I would personally construct the matrix first, then extract the sines and cosines of the angles from the matrix. – Nominal Animal May 25 '18 at 20:53
• Yes, I can easily find the intrinsic Euler angles (z-x'-z'') but am unsure of how to convert those to the angles I need. – R. Gleiser May 25 '18 at 21:46
• No, I was referring to your extrinsic X-Y-Z angles specified by the rotations you described. (I am hoping I picked the correct one!) The procedure is the same as for Euler angles. If you have the algebraic form of the rotation matrix (i.e., containing the sines and cosines of the three rotation axes), it is a matter of feeding it through e.g. Maple to find the inverse solution, I guess. – Nominal Animal May 25 '18 at 21:49
• So, to clarify, if I find my intrinsic (z-x'-z'') Euler angles, evaluate the rotation matrix, equate it to the general form rotation matrix for extrinsic (X-Y-Z) angles given by your linked tables, then the solution will yield the correct the extrinsic angles? – R. Gleiser May 25 '18 at 22:03
• I mean that if you know the order of the rotations, you can construct the rotation matrix in analytical form (containing $\sin$ and $\cos$ of the angles). By inspecting those analytical forms, you can find methods of extracting the angles from any matrix with numerical values. The values are calculated based on the three normalized original vectors $i$, $j$, $k$. See the Rotation formalisms in three dimensions Wikipedia article; it describes how this is done. – Nominal Animal May 26 '18 at 0:14

## 2 Answers

Let's say your three orthogonal unit vectors are $$\hat{i} = \left [ \begin{matrix} i_x \\ i_y \\ i_z \end{matrix} \right ], \quad \hat{j} = \left [ \begin{matrix} j_x \\ j_y \\ j_z \end{matrix} \right ], \quad \hat{k} = \left [ \begin{matrix} k_x \\ k_y \\ k_z \end{matrix} \right ]$$ The rotation matrix $\mathbf{R}$ that rotates $x$ axis to $\hat{i}$, $y$ axis to $\hat{j}$, and $z$ axis to $\hat{k}$ is $$\mathbf{R} = \left [ \begin{matrix} i_x & j_x & k_x \\ i_y & j_y & k_y \\ i_z & j_z & k_z \end{matrix} \right ]$$ Because $\mathbf{R}$ is orthonormal, its inverse is its transpose; and the matrix $\mathbf{R}^{-1}$ that rotates $\hat{i}$ to $x$ axis, $\hat{j}$ to $y$ axis, and $\hat{k}$ to $z$ axis, is $$\mathbf{R}^{-1} = \mathbf{R}^T = \left [ \begin{matrix} i_x & i_y & i_z \\ j_x & j_y & j_z \\ k_x & k_y & k_z \end{matrix} \right ]$$

Euler and Tait-Bryan rotations are combinations of the three axis rotations, \begin{aligned} \mathbf{R}_x &= \left [ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{matrix} \right ] \\ \mathbf{R}_y &= \left [ \begin{matrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{matrix} \right ] \\ \mathbf{R}_z &= \left [ \begin{matrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right ] \end{aligned} where $\phi$, $\theta$, and $\psi$ specify the intrinsic rotations around the $x$, $y$, and $z$ axis, respectively. The combined rotation matrix is a product of three matrices (two or three of the above; first one can be repeated) multiplied together, the first intrinsic rotation rightmost, last leftmost.

The hard part, in my humble opinion, is to determine exactly which the correct order of rotations is. I do believe that in OP's case it is $\mathbf{R}_z \mathbf{R}_y \mathbf{R}_x$, but I am not sure; since this is apparently for an articulated arm of some sort, I would definitely numerically verify this first.

Except that I would not, because Euler/Tait-Bryan angles are evil due to their inherent ambiguity; I would use unit quaternions (versors) to describe the orientation instead. They're easier, logical, and well defined.

The problem at hand, is that at "run time", we have the nine numerical components of $\mathbf{R}$, but need to find out the angles $\phi$, $\theta$, and $\psi$.

If the correct rotation formalism indeed is $\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x$ then $$\mathbf{R} = \left [ \begin{matrix} \cos\theta \cos\psi & \sin\phi \sin\theta \cos\psi - \cos\phi \sin\psi & \cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi \\ \cos\theta \sin\psi & \sin\phi \sin\theta \sin\psi + \cos\phi \cos\psi & \cos\phi \sin\theta \sin\psi - \sin\phi \cos\psi \\ -\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta \end{matrix} \right ]$$ and comparing to $\mathbf{R}$, we pick the five entries with the simpler terms (and all Euler and Tait-Bryan angles have five such entries; their position and exact terms vary), finding that \left\lbrace \begin{aligned} i_x &= \cos\theta \cos\psi \\ i_y &= \cos\theta \sin\psi \\ i_z &= -\sin\theta \\ j_z &= \sin\phi \cos\theta \\ k_z &= \cos\phi \cos\theta \end{aligned} \right . If we choose $-90° \le \theta \le +90°$, then $\cos\theta \ge 0$, and we can solve the angles, getting: \left\lbrace \begin{aligned} \cos\phi &= \frac{ k_z }{\sqrt{1 - i_z^2}} \\ \sin\phi &= \frac{ j_z }{\sqrt{1 - i_z^2}} \\ \cos\theta &= \sqrt{1 - i_z^2} \\ \sin\theta &= -i_z \\ \cos\psi &= \frac{ i_x }{\sqrt{1 - i_z^2}} \\ \sin\psi &= \frac{ i_y }{\sqrt{1 - i_z^2}} \end{aligned} \right . In other words, \left\lbrace \begin{aligned} \phi &= \arctan\left(\frac{j_z}{k_z}\right) \\ \theta &= \arctan\left(\frac{-i_z}{\sqrt{1-i_z^2}}\right) = \arcsin\left(-i_z\right) \\ \psi &= \arctan\left(\frac{i_y}{i_x}\right) \end{aligned} \right . On a microcontroller without hardware floating-point operation support, one can use the numeric $\sin$ and $\cos$ values as an unit vector (noting that they might not be exactly unit length, due to rounding errors when calculating the original matrix $\mathbf{R}$), and apply e.g. CORDIC to obtain the angles at desired precision.

• Yes, that is the correct order of rotations. I came up with the same rotation matrix and broke it into a few equations but thankfully your breakdown is much simpler. I did a couple tests by creating a few rotation matrices with random angles and feeding them back into the equations and it seemed to work. I will implement it into my code and test it in the machine soon with real data. Thanks for all of your help! – R. Gleiser Jun 1 '18 at 22:22

Here's an intuitive approach with minimal formulas (and no matrices). It helps that the rotation angles in your example are all relatively small, that is, we do not need to consider rotations by more than $\frac\pi2$ radians about any of the axes.

The first rotation (about the $x$ axis) does not move the $x$ axis. The second rotation (about the $y$ axis) moves the $x$ axis, but only within the $x,z$ plane.

The rotation about the $z$ axis is therefore solely responsible for moving the image of the $x$ axis from the $x,z$ plane until it is parallel with $\hat \imath.$ We can determine the angle of rotation about the $z$ axis by reversing that rotation to bring $\hat\imath$ back into the $x,z$ plane. The required angle has magnitude $$\lvert \Delta c\rvert = \left\lvert\arctan\frac{\hat\imath_y}{\hat\imath_x}\right\rvert.$$

Now that we have the image of $\hat\imath$ in the $x,z$ plane, we see that it forms a right triangle with hypotenuse $1$ and legs $\hat\imath_z$ and $\sqrt{\hat\imath_x^2 + \hat\imath_y^2}.$ On examining the angles of this triangle, it should be clear that the rotation about the $y$ axis that returns the image of $\hat\imath$ to the $x$ axis has magnitude $$\lvert \Delta b\rvert = \left\lvert\arcsin\hat\imath_z\right\rvert.$$

The rotation about the $x$ axis is more difficult to work out because it has to account for the effects of the other two rotations. The inverxe $z$ rotation does not change the heights of the images of the vectors above the $x,y$ plane, that is, the images of $\hat\jmath$ and $\hat k$ still have $z$-components $\hat\jmath_z$ and $\hat k_z,$ respectively. The inverse $y$ rotation usually does change the $z$ coordinates of the images of the vectors, but it affects the images of $\hat\jmath$ and $\hat k$ in the same proportions, that is, the inverse $y$ rotation produces image vectors whose $z$ coordinates are $t\hat\jmath_z$ and $t\hat k_z,$ respectively, for some real number $t.$ So after reversing the $z$ rotation and the $y$ rotation, we end up with images of $\hat\jmath$ and $\hat k$ in the $y,z$ plane that have $z$-coordinates $t\hat\jmath_z$ and $t\hat k_z$; and since these are two perpendicular unit vectors, the right triangles they each form on the $y$ axis are congruent, so the image of $\hat\jmath$ in particular has $z$ coordinate $t\hat\jmath_z$ and $y$ coordinate $t\hat k_z$. Examining the angles of this triangle, clearly the angle required to rotate the image of $\hat\jmath$ back to the $y$ axis has magnitude $$\lvert \Delta z\rvert = \left\lvert\arctan\frac{t\hat\jmath_z}{t\hat k_z}\right\rvert = \left\lvert\arctan\frac{\hat\jmath_z}{\hat k_z}\right\rvert.$$

Additional inspection should reveal how the signs of the angles depend on the signs of the coordinates of $\hat\imath,$ $\hat\jmath,$ and $\hat k,$ confirming the results that were obtained via matrix algebra in another answer.

Things get a little more complicated if the rotations around the axes can be large, because you might need angles that are not in the ranges of $\arcsin$ and $\arctan.$ You would then need additional rules to obtain the correct angles. But it seems likely this concern does not apply to the rotations you need to work with.