%%ZYX欧拉角转旋转矩阵
%输入角度
R_z=input('Rotate around the z-axis in alpha =');
R_y=input('Rotate around the y-axis in beta =');
R_x=input('Rotate around the x-axis in gamma =');
%只要有一个输入为空,输出警告
if isempty(R_z)+isempty(R_y)+isempty(R_x)>0
fprintf('Error!');
else
R=rotz(R_z)*roty(R_y)*rotx(R_x);%ZYX
end
%%旋转矩阵转ZYX欧拉角
%输入旋转矩阵
R_11=input('R_11 =');
R_12=input('R_12 =');
R_13=input('R_13 =');
R_21=input('R_21 =');
R_22=input('R_22=');
R_23=input('R_23 =');
R_31=input('R_31 =');
R_32=input('R_32 =');
R_33=input('R_33 =');
%只要有一个输入为空,输出警告
if isempty(R_11)+isempty(R_12)+isempty(R_13)+isempty(R_21)+isempty(R_22)+isempty(R_23)+isempty(R_31)+isempty(R_32)+isempty(R_33)>0
fprintf('Error!');
else
R=[R_11,R_12,R_13;R_21,R_22,R_23;R_31,R_32,R_33];
%输出欧拉角
RPY=tr2rpy(R,'deg');%默认为弧度制,需要加‘deg’
%注意:tr2rpy中的pitch俯仰角范围[-pi/2,pi/2),所以90度近似取89.9
end
RAB=roty(20);
PB=[1 0 1]';
plot3(1,0,1,'o','color','red');
hold on;
%绘制三维箭头
%quiver3(X,Y,Z,U,V,W,scale,LineSpec)X,Y,Z起点坐标;U,V,W各轴分量;scale箭头缩放因子;LineSpec线性、标记和颜色
quiver3(0,0,0,PB(1),PB(2),PB(3),1);
hold on;
PA=RAB*PB;
plot3(PA(1),PA(2),PA(3),'*','color','black');
hold on;
quiver3(0,0,0,PA(1),PA(2),PA(3),1);
hold on;
%定义坐标范围
xlim([-1,1.5]);
ylim([-1,1.5]);
zlim([-1,1.5]);
trplot(RAB);
tranimate(RAB);
grid on
%%ZYX欧拉角+P转齐次变换矩阵T
%输入角度
R_z=input('Rotate around the z-axis in alpha =');
R_y=input('Rotate around the y-axis in beta =');
R_x=input('Rotate around the x-axis in gamma =');
%输入位置矢量
PBA_x = input ('X-axis distance = ');
PBA_y = input ('Y-axis distance = ');
PBA_z = input ('Z-axis distance = ');
%只要有一个输入为空,输出警告
if isempty(R_z)+isempty(R_y)+isempty(R_x)+isempty(PBA_x)+isempty(PBA_y)+isempty(PBA_z)>0
fprintf('Error!');
else
R=rotz(R_z)*roty(R_y)*rotx(R_x);%ZYX
T=SE3(R,[PBA_x PBA_y PBA_z]);%输出齐次变换矩阵
end
%%ZYX欧拉角+P转齐次变换矩阵T
%输入角度
R_z=input('Rotate around the z-axis in alpha =');
R_y=input('Rotate around the y-axis in beta =');
R_x=input('Rotate around the x-axis in gamma =');
%输入位置矢量
PBA_x = input ('X-axis distance = ');
PBA_y = input ('Y-axis distance = ');
PBA_z = input ('Z-axis distance = ');
%只要有一个输入为空,输出警告
if isempty(R_z)+isempty(R_y)+isempty(R_x)+isempty(PBA_x)+isempty(PBA_y)+isempty(PBA_z)>0
fprintf('Error!');
else
R=rotz(R_z)*roty(R_y)*rotx(R_x);%ZYX
T=SE3(R,[PBA_x PBA_y PBA_z]);%输出齐次变换矩阵
PA=T*[1;0;1];
end
TAB = [ 0.9254 0.0180 0.3785 1
0.1632 0.8826 -0.4410 2
-0.3420 0.4698 0.8138 3
0 0 0 1];
TCB = [ 0.9397 0 0.3420 3
0 1 0 0
-0.3420 0 0.9397 1
0 0 0 1];
TAC = TAB*TBC%求出TAC
tranimate(TBA,TAC)%显示动画