Hello
I need some help to find the rotation angle of a matrix with respect to a general (world) coordinate.
Lets say i have the gravity vector gv=[gx,gy,gz], how can i obtain the euler angles of the rotation matrix R?
gv'=R*[0,0,g]
Previously, Bruno teached me how to obtain a set of euler angles, but the euler angles obtained "th2" are different than the originals "th1" and when they are ploted, they dont seem to refer to the same rotation. I would highly appreciate any comment.
close all
clear all;
clc
g=9.81;
gworld=[0;0;g/2];
psi1=3.1350;
theta1=0.0280;
phi1=3.1263;
th=[psi1,theta1,phi1]
rotx=[1 0 0; %define rotations in axes
0 cos(psi1), sin(psi1);
0 sin(psi1), cos(psi1);];
roty=[cos(theta1), 0, sin(theta1);
0, 1, 0;
sin(theta1), 0, cos(theta1)];
rotz=[cos(phi1), sin(phi1),0;
sin(phi1),cos(phi1),0;
0 , 0, 1;];
rotmat=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
vect1=rotmat*gworld
vectorin1=cross(gworld,vect1)/(norm(gworld)*norm(vect1));
nv1=min(max(norm(vectorin1),1),1);
theta2=asin(nv1);
if dot(gworld,vect1)<0
theta2=pitheta2;
end
rotmat2=rodrigues(vectorin1/nv1*theta2)
psi2=atan2(rotmat2(3,2)/cos(theta2),rotmat2(3,3)/cos(theta2)); %%Rot_x
phi2=atan2(rotmat2(2,1)/cos(theta2),rotmat2(1,1)/cos(theta2)); %%Rot_z
vect2=rotmat2*gworld
th=[psi2,theta2,phi2]
figure;
plot3(0,0,0)
hold on
grad2rad=pi/180;
point1o= [0;0;0];
point2o= [0;0;.3];
point3o= [0;.3;0];
point4o= [.3;0;0];
plot3([point1o(1),point2o(1)],[point1o(2),point2o(2)],[point1o(3),point2o(3)],'r','linewidth',2);% plot Z axis
plot3([point1o(1),point3o(1)],[point1o(2),point3o(2)],[point1o(3),point3o(3)],'r','linewidth',2);% plot Y axis
plot3([point1o(1),point4o(1)],[point1o(2),point4o(2)],[point1o(3),point4o(3)],'r','linewidth',2);% plot X axis
text(point2o(1),point2o(2),point2o(3),'Z_0');
text(point3o(1),point3o(2),point3o(3),'Y_0');
text(point4o(1),point4o(2),point4o(3),'X_0');
text(point1o(1),point1o(2),point1o(3),'O_0');
point1th1= (rotmat'*(point1o));
point2th1= (rotmat'*(point2o));
point3th1= (rotmat'*(point3o));
point4th1= (rotmat'*(point4o));
plot3([point1th1(1),point2th1(1)],[point1th1(2),point2th1(2)],[point1th1(3),point2th1(3)],'c','linewidth',2);% plot Z axis
plot3([point1th1(1),point3th1(1)],[point1th1(2),point3th1(2)],[point1th1(3),point3th1(3)],'c','linewidth',2);% plot Y axis
plot3([point1th1(1),point4th1(1)],[point1th1(2),point4th1(2)],[point1th1(3),point4th1(3)],'c','linewidth',2);% plot X axis
text(point2th1(1),point2th1(2),point2th1(3),'Z_{th1}');
text(point3th1(1),point3th1(2),point3th1(3),'Y_{th1}');
text(point4th1(1),point4th1(2),point4th1(3),'X_{th1}');
text(point1th1(1),point1th1(2),point1th1(3),'O_{th1}');
point1th2= (rotmat2'*(point1o));
point2th2= (rotmat2'*(point2o));
point3th2= (rotmat2'*(point3o));
point4th2= (rotmat2'*(point4o));
plot3([point1th2(1),point2th2(1)],[point1th2(2),point2th2(2)],[point1th2(3),point2th2(3)],'y','linewidth',2);% plot Z axis
plot3([point1th2(1),point3th2(1)],[point1th2(2),point3th2(2)],[point1th2(3),point3th2(3)],'y.','linewidth',2);% plot Y axis
plot3([point1th2(1),point4th2(1)],[point1th2(2),point4th2(2)],[point1th2(3),point4th2(3)],'y.','linewidth',2);% plot X axis
text(point2th2(1),point2th2(2),point2th2(3),'Z_{th2}');
text(point3th2(1),point3th2(2),point3th2(3),'Y_{th2}');
text(point4th2(1),point4th2(2),point4th2(3),'X_{th2}');
text(point1th2(1),point1th2(2),point1th2(3),'O_{th2}');
axis equal
