dhconv.m

针对移动机械臂运动学

function d = dhconv (dh,o)
%DHCONV(DH Parameters) or DHCONV(DH Parameters,OPTIONS) D&H Conversions
%       1 - Modified to Standard or Standard to Modified:
%           DH = dhconv(DH) or dhconv(DH, 'exc')
%       2 - Modified to Standard:
%           SDH = dhconv(DH, 'm2s')
%       3 - Standard to Modified:
%           MDH = dhconv(DH, 's2m')
%       4 - Modified to Homogeneous Transformation:
%           H = dhconv(DH, 'm2h')
%       5 - Standard to Homogeneous Transformation:
%           H = dhconv(DH, 's2h')

%July ~ December 2005, by Reinaldo M. do Nascimento

if size(dh,2)~=4,error('It is not possible to convert. Data must be ''n x 4''');end
d=dh;n=size(d,1);k=0;theta=0;alpha=0;s=d;
if n>1,s=sum(d);end
try,isnumeric(eval(s(2)));catch,theta=1;end
try,isnumeric(eval(s(4)));catch,alpha=1;end
if theta==0 & alpha==0
        try,isnumeric(eval(s(1)));catch,theta=1;end
        try,isnumeric(eval(s(3)));catch,alpha=1;end
end
if nargin==2 & o=='exc'
    disp('The obtained result is not a usual conversion! That means:')
    if (theta==1 & alpha==0)
        disp('- The utilized parameters are not of the "standard" type, but of the "modified" type.')
        disp('Modified ==> [a,alpha,d,theta]');
    elseif (theta==0 & alpha==1)
            disp('- The utilized parameters are not of the "modified" type, but of the "standard" type.')
            disp('Standard ==> [d,theta,a,alpha]');
    end
elseif (nargin==1 & (theta==1 & alpha==0)) | ((nargin==2 & (o=='s2m' | o=='s2h')) & ((theta==1 & alpha==0) | (theta==alpha)))
        if (nargin==2 & o=='s2h')
                 H=eye(4);k=1;
                 for i=1:n
%                                d          theta          a          alpha
                      H=H*htz(dh(i,1))*hrz(dh(i,2))*htx(dh(i,3))*hrx(dh(i,4));
                 end
                 disp('Homogeneous Transformation with standard DH parameters');
                 d=H;
        else
            disp('standard to modified ==> [a,alpha,d,theta]');
            if sum(d(n,3:4))~=0
                   d=[d;zeros(1,4)];n=n+1;
            end
            d(2:n,3:4)=d(1:n-1,3:4);d(1,3:4)=0;
            if sum(d(1,:))==0
                   d=d(2:n,:);
            end
        end
elseif (nargin==1 & (theta==0 & alpha==1)) | ((nargin==2 & (o=='m2s' | o=='m2h')) & ((theta==0 & alpha==1) | (theta==alpha)))
        if (nargin==2 & o=='m2h')
                 H=eye(4);k=1;
                 for i=1:n
%                                a          alpha          d          theta                    
                      H=H*htx(dh(i,1))*hrx(dh(i,2))*htz(dh(i,3))*hrz(dh(i,4));
                 end
                 disp('Homogeneous Transformation with modified DH parameters');
                 d=H;
        else
            disp('modified to standard ==> [d,theta,a,alpha]');
            if sum(d(1,1:2))~=0
                   d=[zeros(1,4);d];n=n+1;
            end
            d(1:n-1,1:2)=d(2:n,1:2);d(n,1:2)=0;
            if sum(d(n,:))==0
                   d=d(1:n-1,:);
            end
        end
elseif (theta==1 & alpha==1) | (theta==0 & alpha==0)
        error ('Specify the conversion type. Choose ''s2m'' (standard to modified) or ''m2s'' (modified to standard)');
else
    k=1;
    disp('It was not possible to accomplish this operation. The data are incompatible');
    if (theta==1 & alpha==0)
        disp('standard ==> [d,theta,a,alpha]');
    elseif (theta==0 & alpha==1)
            disp('modified ==> [a,alpha,d,theta]');
    end
end
if k==0
    d=[d(:,3:4),d(:,1:2)];
end