function inv_J = InverseJacobianCOG( q,wleg )

% Computes the differential inverse kinematics using the Jacobian ---------
  l0 = 47;  l1 = 140;  l2 = 146;
  m0 = 0.444;  m1 = 0.343;  m2 = 0.409;
  CM1y = 15.86; r1 = 88.52; r2 = 101.28;
  q1 = q(1);  q2 = q(2);  q3 = q(3);
  
  s1 = sin(q1); s2 = sin(q2); s3 = sin(q3);
  c1 = cos(q1); c2 = cos(q2); c3 = cos(q3);
  s23 = sin(q2+q3); c23 = cos(q2+q3);
% Jacobian inverse --------------------------------------------------------
  inv_J(1,1) =  0;
  inv_J(1,2) =  wleg*m2*r2*c1*s3*(m1*r1+m2*l1);
  inv_J(1,3) =  wleg*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) =  wleg*m2*r2*c23*( (m1*r1+m2*l1)*s1*c2-m1*CM1y*c1+m2*r2*s1*c23 );
  inv_J(2,3) = -wleg*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*q2+q3)+m2^2*r2^2*s23*c23;
  inv_J(3,2) = -wleg*((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) =  wleg*((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 = wleg*m2*r2*c23*( (m1*r1+m2*l1)^2*s2*c2+(m1*m2*r1*r2+m2^2*l1*r2)*sin(2*q2+q3) )...
         -wleg*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;
%--------------------------------------------------------------------------  
end

