function dq = InvKinDiff3dof_COG( q,dr,L,R,M )

global CM0x CM1y 

% Computes the differential inverse kinematics using the Jacobian ---------
  l0 = L(1);  l1 = L(2);  l2 = L(3);
  r0 = R(1);  r1 = R(2);  r2 = R(3);
  m0 = M(1);  m1 = M(2);  m2 = M(3);
  ang1 = q(1);  ang2 = q(2);  ang3 = q(3);
  
  s1 = sin(ang1); s2 = sin(ang2); s3 = sin(ang3);
  c1 = cos(ang1); c2 = cos(ang2); c3 = cos(ang3);
  s23 = sin(ang2+ang3); c23 = cos(ang2+ang3);
 %-- Jacobian matrix ------------------------------------------------------
%   J(1,1) = 0;
%   J(1,2) = m1*r1*c2+m2*(l1*c2+r2*c23);
%   J(1,3) = m2*r2*c23;
%   
%   J(2,1) = -m1*(r1*c1*c2+CM1y*s1)-m2*(l1*c1*c2+r2*c1*c23);
%   J(2,2) =  m1*r1*s1*s2+m2*(l1*s1*s2+r2*s1*s23);
%   J(2,3) =  m2*r2*s1*s23;
%   
%   J(3,1) = -m1*(r1*s1*c2-CM1y*c1)-m2*(l1*s1*c2+r2*s1*c23);
%   J(3,2) = -m1*r1*c1*s2-m2*(l1*c1*s2+r2*c1*s23);
%   J(3,3) = -m2*r2*c1*s23;
%   
%   inv_JJ = inv(J)
%   J*inv_JJ
%   pause
 %-- Inverse Jacobian: algebric equations ---------------------------------
  inv_J(1,1) = 0;
  inv_J(1,2) =  m2*r2*c1*s3*(m1*r1+m2*l1);
  inv_J(1,3) =  m2*r2*s1*s3*(m1*r1+m2*l1);
  
  inv_J(2,1) = -m2*r2*s23*( (m1*r1+m2*l1)*c2+m2*r2*c23 );
  inv_J(2,2) =  m2*r2*c23*( (m1*r1+m2*l1)*s1*c2-m1*CM1y*c1+m2*r2*s1*c23 );
  inv_J(2,3) = -m2*r2*c23*( (m1*r1+m2*l1)*c1*c2+m1*CM1y*s1+m2*r2*c1*c23 );
  
  inv_J(3,1) = (m1^2*r1^2+m1*m2*l1*r1+m1*m2*l1*r1+m2^2*l1^2)*s2*c2+...
               (m1*m2*r1*r2+m2^2*l1*r2)*sin(2*ang2+ang3)+m2^2*r2^2*s23*c23;
  inv_J(3,2) = -((m1*r1+m2*l1)*c2+m2*r2*c23)*...
                ((m1*r1+m2*l1)*s1*c2-m1*CM1y*c1+m2*r2*s1*c23);
  inv_J(3,3) =  ((m1*r1+m2*l1)*c2+m2*r2*c23)*...
                ((m1*r1+m2*l1)*c1*c2+m1*CM1y*s1+m2*r2*c1*c23);        
 %-- Determinant J --------------------------------------------------------
  det_J = m2*r2*c23*( (m1*r1+m2*l1)^2*s2*c2+(m1*m2*r1*r2+m2^2*l1*r2)*sin(2*ang2+ang3) )...
         -m2*r2*c2*s23*( (m1*r1+m2*l1)^2*c2+(2*m1*m2*r1*r2+2*m2^2*l1*r2)*c23 );
 %-- Inverse matrix -------------------------------------------------------
  inv_J = inv_J/det_J;
 %-- differential inverse kinematics --------------------------------------
  dq = inv_J*dr;
  
end

