I have a body in space, precisely a robots foot, defined by xyz and a roll pitch and yaw. The pitch is along the degree of freedom defined by the pitch joint. I want to rotate the foot using a rotation matrix, and then recover the roll pitch and yaw such that the pitch parameter still represents a rotation as afforded by the pitch joint. Similarly for roll and yaw. There are plenty of tricks for recovering yaw pitch and roll from a rotation matrix, but that doesn't neccesarily guarantee that the parameters corrospond with the axes of rotation on the robot. Does that make sense? Any ideas? Thank you.
[Math] rotate a roll pitch and yaw using a rotation matrix
geometry
Related Solutions
So, it looks like I've finally found the solution and it's based on rotation matrix from axis and angle (wikipedia).
We have 3 deltas $[\Delta x,\Delta y,\Delta z]$ and three angles $\psi,\sigma,\theta$ for track, roll and pitch respectively. The idea is to carefully rotate 3 times.
- For the first rotation we set $u_1=[0,0,1]$ and $\psi$ and then calculate the matrix $R_1$. We can directly apply it to our vector of shifts: $v_1 = R_1*u$.
- For the second rotation we set $u_2=[v_{1y},-v_{1x},0]$ and $\theta$, calculate the rotation matrix $R_2$ and apply it: $v_2 = R_2*u_2$. The $u_2$ is orthogonal to $v_1$.
- For the third rotation we set $u_3 = v_2$ and $\sigma$: $v_3 = R_3*u_3$.
I've included Scilab code in the question, though it's not very neat and commented.
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.
Best Answer
Just some thoughts (I am not entirely sure what you are asking for precisely):
1) If you look at the rotation matrix here, you see that you can basically read off the pitch from the lower left entry and knowing the pitch you can read of yaw and roll from the first column and last line respectively.
2) If you have your object in some position and you want to move it wrt to given yaw pitch and roll, then you can just compute the matrix.
3) If you want to move your object from some position to another you can perform the rotation for every degree of freedom sepreately and combine them to find out yaw pitch and roll.
4) If you want to undo a rotation you can compute the inverse of the matrix which will again be a rotation matrix.