h2j.m

针对移动机械臂运动学

function J = h2j(H,v,o)
%H2J(H, Manipulator, Option) Homogeneous Transformation to Jacobian Matrix
%    j0 = h2j(H, manipulator)
%    jn = h2j(H, manipulator, 'n')

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

if nargin==1,error('Input argument ''v'' is undefined ==> h2j(homogeneous transform,''symbolic variables'',optional).');
elseif isa(v,'struct'),v=v.chain.variables;
end
n=length(v);
dH=zeros(length(H));
for j=1:n
     dH=dH+diff(H,v(j))*char([100 double(char(v(j)))]);
end
vw=dH*[H(1:3,1:3).' [0;0;0];0 0 0 1];
for i=1:6
     for j=1:n
          if i<4,vwi=i;vwj=4;
          elseif i==4,vwi=3;vwj=2;
          elseif i==5,vwi=1;vwj=3;
          elseif i==6,vwi=2;vwj=1;
          end
          J(i,j)=diff(vw(vwi,vwj),char([100 double(char(v(j)))]));
     end
end
if nargin==2 | o~='n'
else
    J=[H(1:3,1:3).' zeros(3);zeros(3) H(1:3,1:3).']*J;
end