Matlab练习——rpy2tr函数与自己实现的ZYX欧拉角的结果不同的问题

 1 %afa:绕着z轴的旋转角——yaw；beta：绕着y轴的旋转角——pitch；gama：绕着x轴的旋转角——roll（都是角度制）
 2 %机器人工具箱里面提供的函数是rpy2tr()(是弧度制）——且对应的角度是：roll—pitch—yaw
 3 
 4 function R=EularToR(afa, beta,gama)
 5 
 6 Rz=[cosd(afa), -sind(afa), 0; sind(afa), cosd(afa), 0; 0, 0, 1]; %afa是绕z轴旋转，偏航角——yaw
 7 Ry=[cosd(beta),0, sind(beta);0, 1, 0; -sind(beta), 0, cosd(beta)];%beta是绕y轴旋转，俯仰角——pitch
 8 Rx=[1, 0, 0; 0, cosd(gama), -sind(gama); 0, sind(gama), cosd(gama)];%gama是绕x轴旋转，翻滚角——roll
 9 
10 R=Rz*Ry*Rx;

function T = rpy2tr(roll, varargin)

    R = rpy2r(roll, varargin{:});
    T = r2t(R);

function R = rpy2r(roll, varargin) 
%varargin提供了一种函数可变参数列表机制，
%允许调用者调用该函数时根据需要来改变输入参数的个数

%设置默认参数
    opt.zyx = false;
    opt.deg = false;
    [opt,args] = tb_optparse(opt, varargin);

    % unpack the arguments
    if numcols(roll) == 3
        pitch = roll(:,2);%pitch(y)应该是输入参数的第二个值
        yaw = roll(:,3);%yaw(z)应该是输入参数的第三个值
        roll = roll(:,1);%roll(x)应该是输入参数的第一个值
    elseif nargin >= 3%还不太理解这一段的意思
        pitch = args{1};
        yaw = args{2};
    else
        error(‘RTB:rpy2r:badarg‘, ‘bad arguments‘)
    end

    % optionally convert from degrees考虑如果输入的参数是角度，且已经将"deg"作为选项输入函数，需将角度转化为弧度
    if opt.deg
        d2r = pi/180.0;
        roll = roll * d2r;
        pitch = pitch * d2r;
        yaw = yaw * d2r;
    end

    if ~opt.zyx
        % XYZ order如果是先绕着X轴，再Y轴，最后Z轴旋转的话，是如下的矩阵相乘顺序
        if numrows(roll) == 1
            R = rotx(roll) * roty(pitch) * rotz(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotx(roll(i)) * roty(pitch(i)) * rotz(yaw(i));
            end
        end
    else
        % old ZYX order (as per Paul book)如果是先绕着Z轴，再Y轴，再X轴旋转的话，又将是不同的相乘顺序
        if numrows(roll) == 1
            R = rotz(roll) * roty(pitch) * rotx(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotz(roll(i)) * roty(pitch(i)) * rotx(yaw(i));
            end
        end
    end