机器人学导论-第二章MATLAB练习答案_机器人学导论matlab课后答案-CSDN博客

本文链接：https://blog.csdn.net/weixin_48457982/article/details/131788019

该文展示了在MATLAB环境中，如何进行ZYX欧拉角到旋转矩阵的转换，以及旋转矩阵到ZYX欧拉角的反转换。同时，文中还涉及了齐次变换矩阵的构建和使用，包括位置向量的结合，以及通过齐次变换矩阵进行三维空间的物体变换和动画展示。

摘要由CSDN通过智能技术生成

在这里插入图片描述

%%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)%显示动画

在这里插入图片描述