ik2q.m

针对移动机械臂运动学

function d = ik2q (M,o)
%IK2Q(M, OPTION) Auxiliary: Inverse Kinematics

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

for i=1:size(M,1)
     d(i)=double(M(i,2));
     if (nargin==2) & (M(i,3)=='R') & (o=='rad' | o=='deg')
               d(i)=mod(d(i),2*pi)';
               if o=='rad'
                   if d(i)>pi,d(i)=d(i)-2*pi;end
               elseif o=='deg'
                       try,d(i)=rad2deg(d(i));catch,d(i)=d(i)*180/pi;end
                       if d(i)>180,d(i)=d(i)-360;end
               end
     end
end