recover basic rotations

Dear reader.

Is it possible to recover the basic rotations from a general rotation matrix? i do know the order of rotating. It is for a mechanical model in simmechanics, where i use the body sensor to retrieve the matrix. I used the following test script to see if it is possible, but the result is not always ok. Is there another way to get the basic rotations that is always succes?

% recover generalized coordinates from a general 3d rotation matrix
clear all  
q0=rand*2*pi-pi;
q1=rand*2*pi-pi;
q2=rand*2*pi-pi;

s0 =sin(q0);    c0=cos(q0);     
s1 =sin(q1);    c1=cos(q1);
s2 =sin(q2);    c2=cos(q2);

R0 = [c0 -s0 0;s0 c0 0;0 0 1];
R1 = [1 0 0;0 c1 -s1;0 s1 c1];
R2 = [c2 0 s2;0 1 0;-s2 0 c2];

Rm1=R0*R1*R2;

% now try to recover q0,q1 and q2 from R

q1_rec = asin(Rm1(3,2));
q0_rec = atan2(-Rm1(1,2),Rm1(2,2));
q2_rec = atan2(-Rm1(3,1),Rm1(3,3));

% comparison of rotations with recovered rotations
format long
[q0 q1 q2;q0_rec q1_rec q2_rec] 
format short

s0 =sin(q0_rec);    c0=cos(q0_rec);     
s1 =sin(q1_rec);    c1=cos(q1_rec);
s2 =sin(q2_rec);    c2=cos(q2_rec);

R0 = [c0 -s0 0;s0 c0 0;0 0 1];
R1 = [1 0 0;0 c1 -s1;0 s1 c1];
R2 = [c2 0 s2;0 1 0;-s2 0 c2];

Rm1_rec=R0*R1*R2;

Rm1-Rm1_rec

Answer 1

[James Tursa]

You should take a look at SpinCalc in the FEX by John Fuller:

http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors

Answer 2

[Jan]

The shown code and the results look fine. Due to rounding errors you cannot reproduce the inputs exactly. Especially if q1 is near to +-Pi/2 rounding errors are amplified. To learn more about this problem look for Google: gimbal lock.

[Wesley Ooms]

Well, Rm1 and Rm1_rec are indeed similar, but

[q0 q1 q2]are not always similar to [q0_rec q1_rec q2_rec]

(you should try a couple of times, sometimes it's similar, sometimes not) It is these angles (q0_rec q1_rec q2_rec) that i'm interested in