scm.m

针对移动机械臂运动学

function d = scm (robot,op1,op2)
%SCM (MANIPULATOR,OPTION1,OPTION2) - Sistema Coordenado Movel - (Mobile Coordinate System)
%Run SCMDEMO or load a script (scm_manipulator) and choose one of the options below:
%1->scm(manipulator,0) ==> Forward Kinematics;
%2->scm(manipulator,1) ==> Jacobian (J0);
%3->scm(manipulator,2) ==> Jacobian (Jn);
%4->scm(manipulator,-1) ==> Inverted Transformation (Forward Kinematics ^ -1);
%
%                          Destination             Transl Rot(RPY Angles) 
%5->scm(manipulator,Homogeneous Transformation or [X Y Z A B C]) ==> Inverse Kinematics;
%
%                          Destination                            Origin
%6->scm(manipulator,Homogeneous Transformation or [X Y Z A B C],[q1 ... qn]) ==> Inverse Kinematics;

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

global VARIABLES
       VARIABLES=robot.chain.variables;
       jp=robot.jointParameters;ftr=robot.finalTransformation;n=length(op1);
if nargin==2 & op1>=-1 & op1<3 & n==1
elseif nargin==2 & (size(op1)==[4,4] | size(op1)==[1,6])
             origin=zeros(1,size(jp,1));target=double(op1);op1=3;
elseif nargin==3 & size(jp,1)==length(op2) & (size(op1)==[4,4] | size(op1)==[1,6])
             origin=op2;target=double(op1);op1=3;
else
             op1='error';
end
if op1==3
      jp=subs(jp,robot.chain.links,robot.chain.linksValues);
      ftr=double(subs(ftr,robot.chain.links,robot.chain.linksValues));
end
disp(robot.description)
if robot.verifyData~=0 | op1=='error'
              VerifyData(jp,ftr,op1);
else
              DataOk(jp,ftr,op1)
end
switch op1
      case {-1,0}
            d=ForwardKinematics(jp,op1);
      case {1,2}
            d=Jacobian(jp,op1);
      case 3
            if n==6,target=Frame(target,3);end
            d=InverseKinematics(jp,origin,target);
end
%**************************************************************************
function DataOk (jp,ftr,op1)

global VW_CELL VARIABLES QS RP
       n=size(jp,1);VW_CELL={};RP=sym([]);
       disp('Wait ...');
if op1>0       
      for i=1:n
           V=jp(i,1:3);W=jp(i,4:6);
           sumV=sum(V);sumW=sum(W);
           Vi=[0;0;0];Wi=[0;0;0];
           if sumW~=0
                  Wi=W.'/VARIABLES(i);
                  Wi=Wi(3:-1:1);
                  RP(i)='R';
           end
           if sumV~=0 & sumW==0
                  Vi=V.'/VARIABLES(i);
                  RP(i)='P';
           end
           VWi=[Vi,Wi];
           if op1==3
                 VWi=double(VWi);
           end
           VW_CELL{i}=VWi;
      end
end
FinalFrame(ftr,op1);
if op1==3,QS=VARIABLES;
      disp('Inverse kinematics ...');
end
%****************************************************************************        
function FinalFrame (ftr,op1)

global FINAL_TRANSFORMATION
if size(ftr)==[4,4],FINAL_TRANSFORMATION=ftr;
            if op1==2 | op1==-1
                  r1=Get(ftr,'homogeneousRotation').';
                  p1=[eye(3),-Get(ftr,'positionVector');0 0 0 1];
                  FINAL_TRANSFORMATION=r1*p1;
            end
else
            n=size(ftr,1);finalTransformation=eye(4);
            if op1==-1 | op1==2,a=n;b=-1;c=1;else,a=1;b=1;c=n;end
            if op1<3,ftr=sym(ftr);end
            for i=a:b:c
                 finalParameters=[ftr(i,:)];
                 finalFrame=Frame(b*finalParameters,op1);
                 finalTransformation=finalTransformation*finalFrame;
            end
            FINAL_TRANSFORMATION=finalTransformation;
end
%**************************************************************************
function d = Frame (P,op1)

XYZ = [eye(3) [P(1);P(2);P(3)];0 0 0 1];
C = [1 0 0 0;0 cos(P(6)) -sin(P(6)) 0;0 sin(P(6)) cos(P(6)) 0;0 0 0 1];
B = [cos(P(5)) 0 sin(P(5)) 0;0 1 0 0;-sin(P(5)) 0 cos(P(5)) 0;0 0 0 1];
A = [cos(P(4)) -sin(P(4)) 0 0;sin(P(4)) cos(P(4)) 0 0;0 0 1 0;0 0 0 1];

switch op1
      case {0,1,3}
            d=XYZ*A*B*C;
      case {-1,2}
            d=C*B*A*XYZ;
end
%**************************************************************************
function d = ForwardKinematics (jp,op1)

global FINAL_TRANSFORMATION
       n=size(jp,1);a=1;b=1;c=n;homogeneousTransformation=eye(4);
if op1==-1,a=n;b=-1;c=1;homogeneousTransformation=FINAL_TRANSFORMATION;end
for i=a:b:c
     joint_i=[jp(i,:)];
     homogeneousTransformation=homogeneousTransformation*Frame(b*joint_i,op1);
end
switch op1
      case -1
           disp('Inverted Transformation (Forward Kinematics ^ -1)');
           d=homogeneousTransformation;
      case {0,3}
            if op1==0,disp('Forward kinematics');end
               d=homogeneousTransformation*FINAL_TRANSFORMATION;
end
%**************************************************************************         
function d = Jacobian (jp,op1)

global FINAL_TRANSFORMATION VW_CELL
       n=size(jp,1);a=1;b=1;c=n;m=0;homogeneousTransformation=eye(4);J=[];
       homogeneousRotation=zeros(4,4,0);frame_i=zeros(4,4,0);columnsJ=zeros(3,2,0);
if op1==2,a=n;b=-1;c=1;m=n+1;frame_i=FINAL_TRANSFORMATION;end
for i=a:b:c
     joint_i=[jp(i,:)];
     frame_i=cat(3,frame_i,Frame(b*joint_i,op1));
     homogeneousTransformation=homogeneousTransformation*frame_i(:,:,b*i+m);
     columnJ_i=Get(homogeneousTransformation,'rotationMatrix')*VW_CELL{i};
     if op1==1 | op1==3
           columnsJ=cat(3,columnsJ,columnJ_i);
           homogeneousRotation=cat(3,homogeneousRotation,Get(homogeneousTransformation,'homogeneousRotation'));
     else
           jw_i=columnJ_i(:,2);
           jv_i=columnJ_i(:,1);
           jv_i=cross(jw_i,-Get(homogeneousTransformation,'positionVector'))+jv_i;
           if i==n & op1<3
               disp('Constructing Jn ...');
           end                                 
           J=[[jv_i;jw_i] J];
     end
end
if op1==1 | op1==3,frame_i=cat(3,frame_i,FINAL_TRANSFORMATION);
      for i=1:n
           homogeneousTransformation=homogeneousRotation(:,:,i);
           for k=(i+1):(n+1)
                homogeneousTransformation=homogeneousTransformation*frame_i(:,:,k);
           end
           jw_i=columnsJ(:,2,i);
           jv_i=columnsJ(:,1,i);
           jv_i=cross(jw_i,Get(homogeneousTransformation,'positionVector'))+jv_i;
           if i==1 & op1<3
               disp('Constructing J0 ...');
           end
           J=[J [jv_i;jw_i]];
      end
end
d=J;
%**************************************************************************
function d = Get (H,o)

switch o
      case 'rotationMatrix'
            d=H(1:3,1:3);
      case 'homogeneousRotation'
            d=[H(1:3,1:3),[0 0 0]';0 0 0 1];
      case 'positionVector'
            d=H(1:3,4);
end
%**************************************************************************
function d = InverseKinematics (jp,ori,dest)

%This function was written based in M-file ikine(single xform case),by Peter I. Corke

global QS RP
       limite=1000;stol=1e-6;nm=1;count=0;q=ori;at=0;
while nm>stol
        vj=subs(jp,QS,q);
        pti=ForwardKinematics(vj,3);
        j0=Jacobian(vj,3);
        e=DifferentialMotion(pti,dest);
        dq=pinv(j0)*e;
        q=q+dq';
	    nm=norm(dq);
		count=count+1;
		if count>limite,disp('nm <= 1e-6 :');disp(nm);
				error('Solution wouldn''t converge')
	    end
        if nm<stol,vj=subs(jp,QS,q);ptf=ForwardKinematics(vj,3);
             if norm(dest-ptf)>1e-6,at=1;end
        end
end
if at
   disp('The solution did not converge totally to the target. See the result:');
   disp('Press any key ... ');
   pause;
   disp('Desired');disp(dest);
   disp('Calculated');disp(ptf);
end
d=vpa([QS.',q',RP.'],16);
%**************************************************************************
function d = DifferentialMotion (t1,t2)

%This function was written based in M-file tr2diff, by Peter I. Corke

dXYZ=[t2(1:3,4)-t1(1:3,4)];
dABC=[0.5*(cross(t1(1:3,1),t2(1:3,1))+cross(t1(1:3,2),t2(1:3,2))+cross(t1(1:3,3),t2(1:3,3)))];
d=[dXYZ;dABC];
%**************************************************************************
function VerifyData (jp,ftr,op1)

global VW_CELL FINAL_TRANSFORMATION VARIABLES QS RP
if isa(jp,'sym');else,error('class(robot.jointParameters)~=sym ==>SCM joint parameters must be class "sym"');end
if op1==-1 | op1==0 | op1==1 | op1==2 | op1==3,disp('Verifying data ...');
else,error('Error in the data entry. Type "help scm" or read the tutorial!');
end
    n=size(jp,1);crp=0;VW_CELL={};mk=sym([]);RP=sym([]);
for i=1:n
     V=jp(i,1:3);W=jp(i,4:6);sumV=sum(abs(V));sumW=sum(abs(W));Vi=[0;0;0];Wi=[0;0;0];
     if sumW~=0,cr=0;
            for k=1:3,if W(k)~=0,cr=cr+1;mk(1,i)=W(k);end;end
            if cr>1,crp=1;disp('Error ==> Revolute joint:');disp(i);disp(jp(i,:));end
            if cr==1
                 Wi=W.'/findsym(sumW);Wi=Wi(3:-1:1);mk(2,i)=sum(Wi);RP(i)='R';
            end
     end
     if sumV~=0 & sumW==0,cp=0;
            for k=1:3,if V(k)~=0,cp=cp+1;mk(1,i)=V(k);end;end
            if cp>1,crp=1;disp('Error ==> Prismatic joint:');disp(i);disp(jp(i,:));end
            if  cp==1
                  Vi=V.'/findsym(sumV);mk(2,i)=sum(Vi);RP(i)='P';
            end
     end
     if op1>0
           VWi=[Vi,Wi];if op1==3,VWi=double(VWi);end
           VW_CELL{i}=VWi;
     end
end
if crp~=0,disp('Press any key ...');pause;error('           Too many joint variables!!');else,
      FinalFrame(ftr,op1);
      QS=mk(1,:).*mk(2,:);
      if VARIABLES-QS==zeros(1,length(QS))
      else
            disp('variables in the robot.jointParameters are not equal to robot.chain.variables');
            disp('robot.jointParameters');disp(QS);
            disp('robot.chain.variables');disp(VARIABLES);
            error('verify robot.jointParameters and/or robot.chain.variables');
      end
      if op1<3,disp('OK!');end
      if op1==3,vj=subs(jp,QS,zeros(1,length(QS)));
            if isa(vj,'sym')
                   disp('                        * * *');disp('Suggestion:');
                   disp('robot.jointParameters=subs(robot.jointParameters,robot.chain.links,robot.chain.linksValues)');
                   error('Replace robot.chain.links with robot.chain.linksValues!!(SCM joint parameters)');
            elseif isa(FINAL_TRANSFORMATION,'sym')
                   disp('                        * * *');disp('Suggestion:');
                   disp('robot.finalTransformation=double(subs(robot.finalTransformation,robot.chain.links,robot.chain.linksValues))');
                   error('Replace robot.chain.links with robot.chain.linksValues!!(finalTransformation)');
            end
                   disp('OK!');disp('Inverse kinematics ...');
      end
end
%****************************************************************************