function Q = InvKin3dof(XYZ_target,L)

 global qi % Register that holds the current angular positions

 %-- Constants & variable atribution --------------------------------------
  l0 = L(1); l1 = L(2); l2 = L(3);
  x3 = XYZ_target(1); y3 = XYZ_target(2); z3 = XYZ_target(3)-l0;
  
 %-- Ankle joint (lateral plane) ------------------------------------------
  ang1 = atan2(-y3,z3);  
  
  if (ang1 > pi/4 && ang1 < 3*pi/4) || (ang1 < -pi/4 && ang1 > -3*pi/4)
      A = y3/sin(ang1);
  else
      A = -z3/cos(ang1);
  end
  
 %-- Ankle joint (sagittal plane)------------------------------------------
  k1_2 =  2*l1*x3;
  k2_2 = -2*l1*A;
  k3_2 = x3^2+A^2+l1^2-l2^2;
  kk_aux = k1_2^2+k2_2^2-k3_2^2;
  if kk_aux < 0, disp('ERROR MSG: function InvKin3dof'), pause, return, end
  ang2 = atan2(k1_2,k2_2)+atan2(sqrt(k1_2^2+k2_2^2-k3_2^2),k3_2);
 %-- Angkle in the interval -pi to pi -------------------------------------
  if ang2 >  pi,    ang2 = ang2-2*pi;   end
  if ang2 < -pi,    ang2 = ang2+2*pi;   end
 
 %-- Knee joint -----------------------------------------------------------
  k1_3 = 0;
  k2_3 = 2*l1*l2;
  k3_3 = x3^2+A^2-l1^2-l2^2;
  kk_aux = k2_3^2-k3_3^2;
  if kk_aux < 0, disp('ERROR MSG: function InvKin3dof'), pause, return, end
  ang3 = -atan2(sqrt(k2_3^2-k3_3^2),k3_3);
 %-- Joint vector Q -------------------------------------------------------
  Q = [ ang1 ang2 ang3 ]';
%-----------------------------------------------------------------------end
