scm2dh.m

针对移动机械臂运动学

function DH = scm2dh(manipulator)
%SCM2DH(MANIPULATOR) SCM parameters to modified D&H parameters.
%                    See also: DHCONV

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

if isa(manipulator,'struct')
       jp=[manipulator.jointParameters;manipulator.finalTransformation(1,:)];
       disp(manipulator.description);
else
    if size(manipulator,2)==6,jp=manipulator;else,error('It is not possible to convert. Data must be ''n x 6''');end
end
R=eye(3);Rx=[1 0 0;0 0 -1;0 1 0];Rz=[0 -1 0;1 0 0;0 0 1];
k=0;n=size(jp,1);DH=sym(zeros(n,4));
for i=1:n
     k=k+1;
     jpi(1,:)=[jp(i,1:3),jp(i,6:-1:4)];
     if i>1 & i<n
         jpi=reorder(R,jpi);
     end
     if i~=n
         if jpi(6)~=0
                  fs=findsign(jpi(6));
                  if fs==1,alpha=0;ex=0;else,alpha=pi;ex=2;end
                  if jpi(2)==0
                           if  i>1 & DH(k-1,1)==0 & DH(k-1,4)==0
                                DH(k-1,:)=DH(k-1,:)+[jpi(1) 0 0 0];
                                DH(k,:)=[0 alpha jpi(3)*fs jpi(6)*fs];
                           else
                               DH(k,:)=[jpi(1) alpha jpi(3)*fs jpi(6)*fs];
                           end
                           R=R*Rx^ex;
                  else
                          if i==1
                              DH(1,:)=[jpi(1) 0 0 pi/2];
                              DH(2,:)=[jpi(2) alpha jpi(3)*fs jpi(6)*fs];
                              k=k+1;
                          else
                              if jpi(1)==0 | DH(k-1,4)==0
                                       DH(k-1,:)=DH(k-1,:)+[jpi(1) 0 0 pi/2];
                                       DH(k,:)=[jpi(2) alpha jpi(3)*fs jpi(6)*fs];
                              else
                                  DH(k,:)=[jpi(1) 0 0 pi/2];
                                  k=k+1;
                                  DH(k,:)=[jpi(2) alpha jpi(3)*fs jpi(6)*fs];
                              end
                          end
                             R=R*Rz*Rx^ex;
                  end
         elseif jpi(5)~=0
                      fs=findsign(jpi(5));
                      if i==1
                          DH(1,:)=[0 0 jpi(3) 0];
                          DH(2,:)=[jpi(1) -pi/2*fs jpi(2)*fs jpi(5)*fs];
                          k=k+1;
                      else
                          DH(k-1,:)=DH(k-1,:)+[0 0 jpi(3) 0];
                          DH(k,:)=[jpi(1) -pi/2*fs jpi(2)*fs jpi(5)*fs];
                      end
                      R=R*Rx^-fs;
         elseif jpi(4)~=0
                      fs=findsign(jpi(4));
                      if i==1
                          DH(1,:)=[0 0 jpi(3) pi/2];
                          DH(2,:)=[jpi(2) pi/2*fs jpi(1)*fs jpi(4)*fs];
                          k=k+1;
                      else
                          DH(k-1,:)=DH(k-1,:)+[0 0 jpi(3) pi/2];
                          DH(k,:)=[jpi(2) pi/2*fs jpi(1)*fs jpi(4)*fs];
                      end
                      R=R*Rz*Rx^fs;
         elseif jpi(3)~=0
                      if findsign(jpi(3))==1;
                                         DH(k,:)=[0 0 jpi(3) 0];
                      else
                          DH(k,:)=[0 pi -jpi(3) 0];
                          R=R*Rx^2;
                      end
         elseif jpi(2)~=0
                      fs=findsign(jpi(2));
                      DH(k,:)=[0 -pi/2*fs jpi(2)*fs 0];
                      R=R*Rx^-fs;
         elseif jpi(1)~=0
                      fs=findsign(jpi(1));
                      if i==1
                          DH(1,:)=[0 0 0 pi/2];
                          DH(2,:)=[0 pi/2*fs jpi(1)*fs 0];
                          k=k+1;
                      else
                          DH(k-1,:)=DH(k-1,:)+[0 0 0 pi/2];
                          DH(k,:)=[0 pi/2*fs jpi(1)*fs 0];
                      end
                      R=R*Rz*Rx^fs;
         else
             k=k-1;
         end
     else
         if k==1,k=k+1;jpi=sym(jpi);end
         H=hrz(jpi(6),0)*hry(jpi(5),0)*hrx(jpi(4));
         Rh=H(1:3,1:3);
         if sum(abs(jpi(1:3)))~=0
                         jpi=reorder(R,jpi);
                         if jpi(2)==0
                                  Rd=R'*Rh;
                                  zxz=R2_zxz(Rd);
                                  if DH(k-1,4)==0 | jpi(1)==0
                                              DH(k-1,:)=DH(k-1,:)+[jpi(1) 0 jpi(3) zxz(1)];
                                              DH(k,:)=[0 zxz(2) 0 zxz(3)];
                                  else
                                      DH(k,:)=[jpi(1) 0 jpi(3) zxz(1)];
                                      k=k+1;
                                      DH(k,:)=[0 zxz(2) 0 zxz(3)];
                                  end
                         else
                             R=R*Rx^-1;Rd=R'*Rh;
                             zxz=R2_zxz(Rd);
                             if DH(k-1,4)==0
                                         DH(k-1,:)=DH(k-1,:)+[jpi(1) 0 jpi(3) 0];
                                         DH(k,:)=[ 0 -pi/2 jpi(2) zxz(1)];
                                         k=k+1;
                                         DH(k,:)=[0 zxz(2) 0 zxz(3)];
                             else
                                 DH(k-1,:)=DH(k-1,:)+[0 0 jpi(3) 0];
                                 DH(k,:)=[jpi(1) -pi/2 jpi(2) zxz(1)];
                                 k=k+1;
                                 DH(k,:)=[0 zxz(2) 0 zxz(3)];
                             end
                         end
         else
             Rd=R'*Rh;
             zxz=R2_zxz(Rd);
             DH(k-1,:)=DH(k-1,:)+[0 0 0 zxz(1)];
             DH(k,:)=[0 zxz(2) 0 zxz(3)];
         end
     end
end
if sum(abs(DH(1,:)))==0
                    DH=DH(2:k,:);
                    k=k-1;
end
if sum(abs(DH(k,:)))==0
                    DH=DH(1:k-1,:);
end
disp('modified D&H parameters ==> [a,alpha,d,theta]');
%==========================================================================
function d = findsign(JVi)

s=JVi;
if s~=0
    ch=double(char(s));
    if ch(1)==45,s=char(ch(2:length(ch)));end
    d=JVi/s;
else
    d=0;
end
%==========================================================================
function d = reorder (R,JP)

for k=1:3
    xyz(1:3,k)=R(1:3,k).*JP(1:3).';
    abc(1:3,k)=R(1:3,k).*JP(4:6).';
end
d=[sum(xyz),sum(abc)];
%==========================================================================
function d = R2_zxz (R)

za=atan(R(1,3),-R(2,3));
sza=sin(za);
cza=cos(za);
xb=atan(sza*R(1,3)-cza*R(2,3),R(3,3));
zc=atan(-cza*R(1,2)-sza*R(2,2),cza*R(1,1)+sza*R(2,1));
 if xb==0
      za=za+zc;
      zc=0;
 end
d=[za xb zc];
%==========================================================================