\chapter{Inverse kinematics}
%l0=0.050; m0=0.444;    % Foot/ankle link's length 
l0: Foot/ankle link's length;\paragraph{}
% l1=0.139; m1=1*0.343;  % Ankle/knee link's length 
l1: Ankle/knee link's length; \paragraph{}
 %l2=0.143; m2=1*0.409;	% Knee/hip link's length 
 l2: Knee/hip link's length;\paragraph{}
 %l21 = 0.063; l22 = 0.080;
 %l3 = 0.119; m3 = 0.75;       
l21=l2+l1;\paragraph{}
l3=l0+l1+l2;\paragraph{}

$xh = X$
\paragraph{}
$yh = Y-l3/2$
\paragraph{}
$zh = Z$
 
  
\begin{equation}
\theta 1 = atan2(-yh,zh-l0)
\end{equation}

$k1 =  2*l1*xh$
\paragraph{}   
$ k2 = -2*l1*A$
\paragraph{}
$ k3 = xh^2+A^2+l1^2-l2^2$
\paragraph{}
$AUX\_var = k1^2+k2^2-k3^2$
  
\begin{equation}
  \theta 2 = atan2(k1,k2)+atan2(\sqrt{AUX\_var},k3)
\end{equation}

$k2 = 2*l1*l2$
\paragraph{}
$k3 = xh^2+A^2-l1^2-l2^2$
\paragraph{}
$AUX\_var  = k2^2-k3^2; $

\begin{equation}
 \theta 3 = -atan2(\sqrt{AUX\_var},k3);
\end{equation}

$k1 = cos(\theta 1)*sin(\theta 2+\theta 3)*cos(\theta 5)$
\paragraph{}
$k2 = sin(\theta 1)*cos(\theta 5)$
\paragraph{}
$k3 = cos(\theta 1)*cos(\theta 2+\theta 3)*sin(\theta 5)$
\paragraph{}
$AUX_var = k1^2+k2^2-k3^2$

\begin{equation}
\theta 4 = atan2(k1,k2)+atan2(\sqrt{AUX_var},k3);
\end{equation}

\begin{equation}
\theta 5 = asin(-sin(\theta 1)*cos(\theta 2+\theta 3));
\end{equation}

$ k1 = ( -cos(\theta 2+\theta 3)*sin(\theta 4)*sin(\theta 5)-sin(\theta 2+\theta 3)*cos(\theta 5) )$
\paragraph{}
$k2 = cos(\theta 2+\theta 3)*cos(\theta 4)$

\begin{equation}
\theta 6 = atan2(k1,k2);
\end{equation}

