Given an orthogonal unit vector triad (i,j,k) described in the 3-dimensional coordinate space (X,Y,Z), as shown below.
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.
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?
Best Answer
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.