function dq = InvKinDiff3dof( q,dr,L )
% Computes the differential inverse kinematics using the Jacobian
  l0 = L(1);  l1 = L(2);  l2 = L(3);
  ang1 = q(1);  ang2 = q(2);  ang3 = q(3);
 %-- Jacobian matrix ------------------------------------------------------
%   J(1,1) = 0;
%   J(1,2) = l1*cos(ang2)+l2*cos(ang2+ang3);
%   J(1,3) =              l2*cos(ang2+ang3);
%   
%   J(2,1) = -l1*cos(ang1)*cos(ang2)-l2*cos(ang1)*cos(ang2+ang3);
%   J(2,2) =  l1*sin(ang1)*sin(ang2)+l2*sin(ang1)*sin(ang2+ang3);
%   J(2,3) =                         l2*sin(ang1)*sin(ang2+ang3);
%   
%   J(3,1) = -l1*sin(ang1)*cos(ang2)-l2*sin(ang1)*cos(ang2+ang3);
%   J(3,2) = -l1*cos(ang1)*sin(ang2)-l2*cos(ang1)*sin(ang2+ang3);
%   J(3,3) =                        -l2*cos(ang1)*sin(ang2+ang3);
%   
%   inv_JJ = inv(J)
%   J*inv_JJ
 %-- Inverse Jacobian: algebric equations ---------------------------------
  inv_J(1,1) = 0;
  inv_J(1,2) =  l1*l2*cos(ang1)*sin(ang3);
  inv_J(1,3) =  l1*l2*sin(ang1)*sin(ang3);
  
  inv_J(2,1) = -l2*sin(ang2+ang3)*( l1*cos(ang2)+l2*cos(ang2+ang3) );
  inv_J(2,2) =  l2*sin(ang1)*cos(ang2+ang3)*( l1*cos(ang2)+l2*cos(ang2+ang3) );
  inv_J(2,3) = -l2*cos(ang1)*cos(ang2+ang3)*( l1*cos(ang2)+l2*cos(ang2+ang3) );
  
  inv_J(3,1) = l1^2*sin(ang2)*cos(ang2)+l2^2*sin(ang2+ang3)*cos(ang2+ang3)+...
               l1*l2*sin(2*ang2+ang3);
  inv_J(3,2) = -sin(ang1)*( l1*cos(ang2)+l2*cos(ang2+ang3) )^2;         
  inv_J(3,3) =  cos(ang1)*( l1*cos(ang2)+l2*cos(ang2+ang3) )^2;        
 %-- Determinant J --------------------------------------------------------
  det_J = -l1*l2*( l1*cos(ang2)*sin(ang3)+l2*sin(ang3)*cos(ang2+ang3) );
 %-- Inverse matrix -------------------------------------------------------
  inv_J = inv_J/det_J;
 %-- differential inverse kinematics --------------------------------------
  dq = inv_J*dr;
  
end

