\subsection{Kinematikk}

Robotarmen ble modellert og parameterisert i tråd med Denavit-Hartenberg konvensjonene.

\begin{table}[!ht]
  \centering
  \begin{tabular}{c | c | c | c | c}
    Link & $a_i$ & $\alpha_i$ & $d_i$ & $\theta_i$ \\
    \hline
     1 & 0     & $ 90^\circ$   & 0.025m & $\theta_1^*$ \\
     2 & 0.068m & 0            &  0 & $\theta_2^*$ \\
     3 & 0.1m     & $ 0 $    & 0     & $\theta_3^*$ \\
    \end{tabular}
    \caption{DH tabell}
    \label{DH tab}
\end{table}

Følge figur illustrerer koordinatsystemet. $x_0$ går langs kjøreretningen til fartøyet som robotarmen er festet på.

\begin{figure}[H]
    \centering
    \includegraphics[width=90mm]{fig/robot.jpg}
    \caption{Koordinatsystem for robotarmen. Figur hentet fra \cite[p.~90]{robot}}
    \label{koordinatsystem}
\end{figure}

Foroverkinematikken beskriver robotarmens endepunkt som funksjon av vinklene.

\begin{align*}
    x_e = & \cos{  \theta_1} (d_2 \cos{\theta_2} + d_3 \cos{(\theta_2 + \theta_3)} )  \\
    y_e = & \sin{\theta_1}(d_2 \cos{\theta_2} + d_3 \cos{(\theta_2 + \theta_3)})  \\
    z_e = & d_1 + d_2 \sin {\theta_2} + d_3 \sin {(\theta_2 + \theta_3)}
\end{align*}


Hvor $x_e$. $y_e$ og $z_e$ definerer koordinatene til endepunktet på armen som funksjon av vinklene.

Inverskinematikk beskriver, gitt et visst koordinat, hvordan vinklene må settes
    for at enden av robotarmen skal oppnå den gitte posisjonen.
Fra ~\cite[p.~99]{robot}.

\begin{align*}
    \theta_1 = & \text{ atan2}(y_c,x_c)  \\
    \theta_2 = & \text{ atan2}(z_c - d_1, \sqrt{x_c^2 + y_c^2 }) - \text{atan2}(a_3 \sin{\theta_3}, a_2 + a_3 \cos{\theta_3} ) \\
    \theta_3 = & \text{ atan2}(\pm \sqrt{1 - D^2}, D) \\
    D = & \frac{x_c^2 + y_c^2 + (z_c - d_1)^2 - a_2^2 - a_3^2}{2 a_2 a_3}
\end{align*}

$D$ må da være mindre eller lik 1 for at løsningen skal være innenfor robotens
  gyldighetsområde.
Det er i tillegg begrensninger på vinklene basert på motordriverne sine
  begrensninger.
Verdiene for disse ligger i model.json-filen.
Dersom både $x_c$ og $y_c$ settes til null, eksisterer det uendelig antall
  løsninger til problemet da verdien for $\theta_1$ er arbitrær i løsningen.
$\theta_1$ blir da satt til null.
For alle andre inputs av $x_c$ og $y_c$ vil man få to løsninger på problemet.
Begge løsningene vil bli sjekket, og den første som finnes i løsningsrommet blir
  valgt.
