Write code to convert a rotation matrix to YPR angles.

A function in which you can pass a rotation matrix R and return a vector ypr would be convenient. However, if you prefer you can write code that you can copy and paste into future programs as needed. All angles should be in radians.

Here is pseudocode for converting R to YPR angles. Remember that arrays in C++ are zero indexed.

https://robotics.stackexchange.com/questions/10833/pitch-angle-is-either-90-or-90

if abs(R(1,1))

// check for singularity

rpy(1) = 0.0; // roll is zero

rpy(2) = atan2(-R(3,1), R(1,1)); // pitch

rpy(3) = atan2(-R(2,3), R(2,2)); // yaw is difference yaw-roll

else

rpy(1) = atan2(R(2,1), R(1,1));

sp = sin(rpy(1));

cp = cos(rpy(1));

rpy(2) = atan2(-R(3,1), cp * R(1,1) + sp * R(2,1));

rpy(3) = atan2(sp * R(1,3) - cp * R(2,3), cp*R(2,2) - sp*R(1,2));

Please find the required code in c++ strictly based on the pseudocode given. Any subsequent modifications in the code, if required, must be carried in the code too.

//============================================

#include #include #include using namespace std;

int main() { int i, j; float rot[3][3],roll,pitch,yaw,sp,cp; cout>rot[i][j]; } }

if(abs(rot[0][0])

return 1; }

//==================================

Calculate the current orientation error in the angle/axis formulation

Refer to (2.43) of the textbook for equations to map from R to angle/axis kq. Return both angle and axis. Be sure to test for special cases when there is no solution. Again, a function would be useful, but you can simply write reusable code if you want. The angle should be in radians and the vector should be a unit vector.

CHAPTER 2, RIGID MOTIONS 58 Figure 2.12: Rotation about an arbitrary axis. Note that the final two equations follow from the fact that k is a unit vector. Substituting Equations (2.41) and (2.42) into Equation (2.40), we obtain after some lengthy calculation (Problem 2-17) (2.43) In fact, auy rotation matrix R E SO(3) can be represented by a single rotation about a suitable axis in space by a suitable angle, (2.44) where k is a unit vector defining the axis of rotation, and is the angle of rotation about k. The pair (k, 0) is called the axis/angle representa- tion of R. Given an arbitrary rotation matrix R with components rij, the equivalent angle 0 and equivalent axis k are given by the expressions cos-1 ( r11 + T22+ r33-1 and r32 T23 2aiuoT13 T31 sin 13-r r21 T12 (2.45) CHAPTER 2, RIGID MOTIONS 58 Figure 2.12: Rotation about an arbitrary axis. Note that the final two equations follow from the fact that k is a unit vector. Substituting Equations (2.41) and (2.42) into Equation (2.40), we obtain after some lengthy calculation (Problem 2-17) (2.43) In fact, auy rotation matrix R E SO(3) can be represented by a single rotation about a suitable axis in space by a suitable angle, (2.44) where k is a unit vector defining the axis of rotation, and is the angle of rotation about k. The pair (k, 0) is called the axis/angle representa- tion of R. Given an arbitrary rotation matrix R with components rij, the equivalent angle 0 and equivalent axis k are given by the expressions cos-1 ( r11 + T22+ r33-1 and r32 T23 2aiuoT13 T31 sin 13-r r21 T12 (2.45)