\documentclass[a4paper,11pt]{article}

% Zeichencodierung für deutsche Umlaute
\usepackage[utf8]{inputenc}
\usepackage[T1]{fontenc}

% Grafiken einbinden
\usepackage{graphicx}

% Deutsche Sprache (Datum, Trennungen, etc.)
%\usepackage[ngerman]{babel}

% Optional: bessere Schrift
\usepackage{lmodern}

\usepackage{caption}

% Then in your document preamble
\captionsetup[figure]{font=smaller}


\title{How to get  a robot position  from video  images}
\author{Christoph Kendel}
\date{\today}



\begin{document}

\maketitle

\begin{center}
\begin{minipage}{0.89\linewidth}
\section{Info}

My Robot-Arm does not have homing switches. I  use WebCams to get the position of each joint. This way
I can get the initial position without movement as well as contiuously check the position while working under load. 

\vspace{2mm}
\hspace{5mm}
I work with Aruco markers. With budget  low resolution WebCams I can get reliable positions (when using two cams). But the angular recognizion of the markers  turned out  
not to be reliable. Thus
 I have to calculate the angle of each element from the relative positions of the markers. Here I explain how I these calculations are done.

\end{minipage}
\end{center}

\pagebreak
\section{Angles}

The linear $x$ Position is easy to get: the markers 201, 204, 200, 198, 229, 243 have all  a fixed relative $x$ position.  Thus the average
can be taken, wich is saved as $x_{242}$ the x-position of the hand-rotation-joint.

All the other values are the angles {\tt y, z, a, b, c} and {\tt e}. % (e for the opening-of the hand).

\subsection{Bizeps {\bf \tt   y}}

To find the angle of the bizeps (upper arm) there are different options, depending on the angle-of-view and which
ArUcos are visable.



\begin{figure}[h!]
\includegraphics[width=\linewidth]{pic/robot_image_b.png}
\caption{Video Capture with recognized Aruco markers}
\end{figure}


\noindent
The calculation is based on two approaches: Position of the Markers 243 etc on the one hand, and relative position 198  $\leftrightarrow$ 229 on the other hand.


\vspace{2mm}

\noindent
\begin{minipage}{0.39\linewidth}
Several ways to calculate {\tt y}: 
\begin{itemize}
\item 229 $\leftrightarrow$ 198 $\tan(y) = \frac{\Delta z}{\Delta y} $
\item From $y_{\rm Axis}$ and the positions of 243, 229, 198 (each position on its own) we can calculate  $\tan(y+\delta)$
	with a known $\delta$ from the geometry.

Thus we get {\tt y}
\end{itemize}
\end{minipage}\hfill
\begin{minipage}{0.6\linewidth}
\includegraphics[width=\linewidth]{pic/robot_sideView_measurements.pdf}
\end{minipage}

\vspace{2mm}

\noindent
Each availiable (if marker is visible) approach to calculate {\tt y} is done, and the mean is calculated. We can check if they deviate too much, and give
warnings.

\subsection{Ellbow -- Rotation}

At the ellbow there is a motor that turns the forearm arround. Depending on which Arucus are visible, the position {\tt a} of the motor can be calculated:

\begin{itemize}
\item {\tt x} of 223  or  ... (only the $x$ Position, in the graphics in green). It is indipendent of all other angles. 
\end{itemize}
%
%
the
{\tt y} and {\tt z} position of 223 etc can't be used, as it depends on the angle of the forearm and the angle of the bizeps. Thus is it less reliable.  (Although it would be a nice
thing to use as a double--check). With the $x_{223}$ compared to  the $x_{226}$ i can calculate the ellbow turning {\tt a} value:


$$
\sin({\tt a} + \alpha_{\rm })  = \frac{\Delta x}{35\,\rm mm} = \frac{x_{223}  - x_{226}}{35\,\rm mm}
$$


\noindent
as I know the angle $\alpha$ (yellow triangle) from the printed geometry of the arm. The $x_{226}$ is calculated from all visible markers of the 
sled and bizeps, as their markers are fixed in $x$ position. This $\alpha$ is calculated for all markers of the forearm. 


\begin{figure}[h!]
\hspace*{-4.5mm}
\includegraphics[height=0.3\linewidth]{pic/robot_frontView_forearm.pdf}\hspace{6mm}
\includegraphics[height=0.3\linewidth]{pic/robot_sideView_forearm.pdf}
\caption{Front view and sideview of the forearm with calculation variables.}
\end{figure}

\noindent
As the rotation {\tt a} may change for different markers, the median is taken.


\subsection{Forearm}

The position  of the forearm motor (controlling motor in the bizeps) can be calculated from the difference between the bizeps 
position $y$ and the angle of the forearm. Thus we need the forearm--angle. That can be calculated in two ways
%
%
\begin{itemize}
\item relative position of two markers on the forearm, that are on one line.
\item relative $y, z$ position of one marker on the forearm and one marker on the ellbow, e.g. 242 or 222.
\end{itemize}
%
Again:
As I might get different results, I have to find a median value. 


\vspace{2mm}

\hfill $\Rightarrow$  Hand-Joint $P$ is known.

\subsection{Hand}

To get the hand position, the easiest way I see is to calculate the middle position between both finger-markers%
\footnote{The 
other way to calculate the hand position without the markers orientation would be: calculate the middle-hand-plane from 
the forearm rotation, with the distance of the finger-point to that plane, you'd also get the finger-opening and the projectet position of the finger-marker
to that plane, you'd get the angle.} 
($F_1$, $F_2$ here shown in green). 
The direction $P \to M$  in combination with the forearm-direction results in the angle {\tt b}.


\begin{figure}[h!]
\hfill
\includegraphics[height=0.3\linewidth]{pic/robot_hand_sideView.pdf}
\hfill
\includegraphics[height=0.26\linewidth]{pic/robot_hand_topView.pdf}
\hfill {}
\caption{ Hand with points to calculate the values for {\tt b, c, e}.}
\end{figure}


\noindent
The Distance between the hand markers  is a result of  the  opening of the fingers  {\tt e}. 
So I can calculate {\tt e} from that distance.

Furthermore the direction of the distance between the grippers gives me the rotation of the hand {\tt c}. 

\paragraph{Limitations with the Hand:}  The results depend heavily on the accuracy of $P$ and $M$. Both are never measured directly. So have to 
double check if they make sense. Only if the distance $\overline {PM}$ checks up to be the mechanical correct, that can be used to evaluate the correctnes of $P$ and $M$. 
And only if the point $M$ lies in the plane of the rotation {\tt a} arround the forearm. 


\section{Program}

The NodeJS Program calculates the angles and checks, if they are the ones at the moment represented by the system.

First try is to just hard-code all the relevant ArucoPoints. That seems to be ok, but it gets impossible to maintain. 
After a week I dont recognize my own code. It's hard to make improvements, when you first have to re-think all those
small steps, when you have constantly to look up all positions of the markers.

It seems to be a solution to save the geometry and all markers in one file. That can be edited, that can be improved. 
And here I can save the relation between x,y, and the position of the Aruco Markers. Of course, at the moment 
the geometry is still proitty much hard-coded. But i have some flexibility. And mainly: I can loop over {\sl all
Arucos on the Board} to find the coordinate system. Or I can loop over all Arucos that
attache to the ``arm1''.

\begin{figure}[h!]
\includegraphics[width=\linewidth]{pic/code_robot_definition.pdf}
\caption{Robot Geometry in a {\tt .json} File}
\end{figure}

\end{document}