\chapter{Robot's Tele-operation}\label{chapter:tele-operation}
\thispagestyle{myheadings}
% -------------------------------------------------------------

% -------------------------------------------------------------

This chapter explains the developed software modules for this work and how they interact with each other; Figure \ref{fig:module_scheme} illustrates that interaction.


\begin{figure}[H]
\centering
\begin{tikzpicture}[auto, >=latex]
	
    \node [block1, name=cells] {\textbf{\textit{pressure\_cells}}};
    
    \node [block1, right of=cells, node distance=8cm, name=haptic] {\textbf{\textit{haptic\_force}}};
    
    \node [block1, below of=haptic, node distance=3cm, name=phantom, text width=4cm] {\textbf{\textit{phantom\_control}}};
    
    \node [block1, left of=phantom, node distance=8cm, name=humanoid, text width=4cm] {\textbf{\textit{humanoid\_control}}};
	
	\path [->,draw,very thick]  (cells) -- node{COP} (haptic);
	\path [->,draw,very thick]  (haptic) -- node{force} (phantom);
	\path [<-,draw,very thick]  (humanoid) -- node{kinematics} (phantom);
	
\end{tikzpicture}
\captionof{figure}{Interactions between the developed software modules.}
\label{fig:module_scheme}
\end{figure}


The \textit{pressure\_cells} module is in charge of receiving the ADC values from the serial port and convert them into forces and COP's. The COP's are then transmitted to the \textit{haptic\_force} module. This module is one of the central ideas for this work. The idea is to have a module that translates sensor information into force feedback. Load cells are used in this work but other sensors like inertial ones can be adapted into this module. After this the force values are given to the \textit{phantom\_control} module, which serves as a control and update service of the haptic device. The received forces are updated on the device and its kinematics and button status are retrieved to control the robot. The state of the device is published and the subscriber is the \textit{humanoid\_control} module. The function of this module is to control the servomotors and update its state to a message.

All messages described in this Chapter are to be recorded into a \textit{bag} (using \textit{rosbag}) for later analysis. In future works, they can be used in learning algorithms.

%\FloatBarrier
\section{Module \textit{pressure\_cells}}\label{sec:tele_pressure_cells}

The \textit{pressure\_cells} module's main purpose is to retrieve the ARDUINO Fio's information and publish it in ROS format. It also has the function of calculating the \textit{center of Pressure} (COP) of each foot and the total COP if the feet are coplanar. This module has 2 \textit{nodes} that run in parallel: one for receiving the ARDUINO's information and another one for calculating the COP's. Since there are 4 load cells for each foot, 2 ARDUINO boards are used.

As explained in Section \ref{subsec:setup_aquisition}, the four 10-bit ADC values (0 to 1023) are rearranged to to fit in 6 bytes and are sent by serial port. The first \textit{node}, named \textit{arduino}, is in charge of waiting for the termination character in the serial port and reverse the rearrangement bit process to get the original values.

The module accepts various launcher parameters for different configurations:

\begin{itemize}
	\item the serial device port that each ARDUINO is connected to
	\item to show or not the visualization markers
	\item which feet does each ARDUINO corresponds to
	\item the identification of the load cells that are installed in the foot.
\end{itemize}


After checking the parameters given by the launcher, the \textit{node} sets a publisher ready to send a message with the force values. In then enters the main loop where it continually waits for the termination byte. If found, the bytes received before are rearranged into force values and published in their message. The identification parameters for the cells trigger a calibration routine which uses the linear regression coefficients in Table \ref{table:load_linear}. This way, the published message of each \textit{node} is the 4 forces being felt by the load cells. The \textit{node} runs a loop at higher frequency than the expected one of the ADC readings ($1300 \hertz$). This way there is a continuous stream of information. 


Due to the use of two data acquisition units, two \textit{arduino} \textit{nodes} are launched. Each with their own set of parameters and published messages.


The second \textit{node} is responsible for calculating the COP of each foot and the global COP, called \textit{force\_cop}. 
The global COP is only calculated when the parameter for coplanar feet is set; this parameter is set by the user in the launcher file \textit{pressure\_cells.launch} present at the Appendix \ref{appendix_launch}. Much like the previous \textit{node}, a series of parameters were set up for global control over the \textit{force\_cop}:

\begin{itemize}
	\item which message comes from which \textit{presure\_cells} module
	\item set the correct frame identification for each foot
	\item show or not the visualization markers
	\item if the feet are coplanar.
\end{itemize}


By subscribing to the \textit{pressure\_cells}'s force message, and using the measurements in the Table \ref{table:load_coord}, the COP is calculated as in Equation \ref{equation:cop}.

\begin{equation}
	\vec{COP}=\frac{\sum\limits_{i} \vec{r_i}\times f_i}{\sum\limits_{i} f_i}
	\label{equation:cop}
\end{equation}

The $\vec{r_i}$ is the coordinates of the $ith$ load cell in the feet and $f_i$ is the correspondent force. This equation results in a $\vec{COP}$ with the coordinates in $(x, y)$ axis. A consequence is the impossibility to calculate the general COP from the COP's values of each feet. This is why the general COP needs to be calculated with all forces and coordinates in the same referential.


The operation of this \textit{node} is illustrated in Figure \ref{fig:force_cop_bin}. It begins with the setting of the cells coordinates in each foot as in Table \ref{table:load_coord}. Follow, the initialization of the ROS \textit{node} and only after this it's possible to set the ROS variables (transformations, publishers, advertisers, etc.). The ROS parameters given to the \textit{node} by its launcher are checked to see if they are at their default setting. If not, internal variables are set to the new values or names. These last variables ($ok1$, $ok2$, \textit{visualization}, etc.) control the global operation of the \textit{node} during the ROS loop. The setup ends with the setting of the advertising and subscribing variables the handle the ROS messages to be sent and received respectively. The advertisers are in charged of publishing the COP messages with the calculated vector. Three advertisers can exist in this \textit{node}: one for each foot, identified by left or right, and the global COP if the launcher gives the correct parameter.

The loop will continue while the \textit{ros::ok} variable is true, this happens while no order of shutdown is given to ROS. In this loop, 4 events take place: the first three are dependent on the global control variables.

The condition that is first checked is the \textit{level} parameter, this would mean that both feet are at the same level. In parallel with this, the $ok1$ and $ok2$ are also verified; being true means that the correspondent callback ran in the last iteration of the loop. If these three conditions are met (\textit{level}, $ok1$ and $ok2$), then the calculation for the general COP is activated. It begins with the lookup of the transformation from the right coordinate system to the left one. This way the coordinates of the right foot's load cells are transformed in to the left foot system. Now that all the cells coordinates are on the same system, using the Equation \ref{equation:cop}, the general COP is calculated and its coordinates are published in the correspondent message.

The second and third event is the publishing of the visualisation markers. The first to happen is the individual COP markers, one marker for each foot. This occurs if the \textit{visualisation} parameter is set. The other one needs the verification of both the \textit{visualisation} and \textit{level} parameters. If both true, the general COP marker is published.

The last event of the main loop checks if there are new messages from the subscribed topics. If they are, the correspondent callbacks are activated. The callbacks present in this \textit{node} are activated if a new force message is received. Since two messages are subscribed, two callbacks exist. They are almost identical, one is for the left foot and the other for the right one. Each of the callback uses Equation \ref{equation:cop} to calculate the COP for the correspondent foot. Before exiting the callback, the COP is published in its message.

After these 4 events, the loop waits the necessary time to keep a stable frequency.

\begin{figure}[H]
% The block diagram code is probably more verbose than necessary
\centering
\begin{tikzpicture}[auto, node distance=3cm,>=latex']

		\node [block1, left, name=launch] {\underline{\textbf{Launcher}}};
		
		\node [block1, below of=launch, name=setup, text width=8cm, node distance=3.5cm] {
		    \underline{\textbf{Setup}}
		    \begin{itemize}
		    	\item Set cells coordinates
		    	\item Set ROS
		    	\item Check parameters
		    	\item Subscribe force messages (set 2 callbacks)
		    	\item Set COP's publishers
		    \end{itemize}
		    };
				
		\node [block1, right of=setup, name=call1, node distance=7.5cm, text width=5cm] {
		    \underline{\textbf{Callback 1}}
		    \begin{itemize}
		    	\item Calculate COP left foot
		    	\item Publish COP left foot
		    \end{itemize}
		    $ok1=1$
		    };
				
		\node [block1, below of=call1, name=call2, text width=5.3cm, node distance=3.5cm, text width=5.5cm] {
		    \underline{\textbf{Callback 2}}
		    \begin{itemize}
		    	\item Calculate COP right foot
		    	\item Publish COP right foot
		    \end{itemize}
		    $ok2=1$
		    };
		    
		 
%		\node[block1, below of=setup, name=loop, node distance=3.5cm]{\underline{\textbf{Loop}}};
		
		\node[block?, below of=setup, name=ok, node distance=6cm, text width=1.7cm]{$ok1==1$\\\&\\$ok2==1$ ?};
		
		
		\node [block1, below of=call2, name=cop, text width=9cm, node distance=6cm] {
		    \begin{itemize}
		    	\item Get transformation from right to left foot
		    	\item Transform right cell coordinates to left foot system
		    	\item Calculate COP of all 8 cells
		    	\item Publish COP
		    \end{itemize}
		    $ok1=0 $\hspace{5mm}$ ok2=0$
		    };
		    
		\node[block?, below of=ok, name=markers, node distance=7cm, text width=1.5cm]{markers \textit{on}};
	
		\node [block1, below of=markers, name=pub, node distance=2.5cm, text width=5cm] {
		    \begin{itemize}
		    	\item Publish COP markers
		    \end{itemize}
		    };
		    
		\path[draw,->, very thick] (launch) -- (setup);
		\path [draw,->, very thick] (setup.south)  --  (ok.north);
		
		\path [draw,->, very thick] (ok.south)  -- node [near start,right]{no} (markers.north);
		
		\path [draw,->, very thick] (ok.east)  -- ++(0,0) -- node [above]{yes} ++(5.55,0)  -- (cop.north);
		
		\path [draw,->, very thick] (cop.south)  -- ++(0,-1.2) -- (markers.east);
		
		\path [draw,->, very thick] (markers.south)  -- node [right]{yes} (pub.north);
		
		\path [draw,->, very thick] (markers.west)  -- ++(0,0) -- node [above]{no} ++(-2.85,0) |-  ++(0,9.75) -- ++(4.25,0);
		
		\path [draw,-, very thick] (pub.west)  -- ++(0,0) -- ++(-1.598,0) |-  ++(0,2.65);
		
	\end{tikzpicture}
	\caption{Simplified schematic of \textit{force\_cop} node.}
	\label{fig:force_cop_bin}
\end{figure}



\FloatBarrier
\section{Module \textit{haptic\_force}}\label{sec:tele_haptic_force}

The idea of the \textit{haptic\_force} module is to translate sensory information into force feedback. In this case, the information comes from the load cells.

The used information is the calculated COP's. These are translated to force feedback with a simple linear equation represented in Equation \ref{equation:force_gen_v1}. The equation is a direct correspondence from the limits of the COP, $COP^{max}_{u}$ and $COP^{min}_{u}$, to the limits of the force feedback, $F^{max}_{Phantom}$ and $F^{min}_{Phantom}$. Where $u$ indicates the axis ,$x$ or $y$, $F_u$ is the force exerted in the $u$ axis of the haptic device and the $COP_u$ is the current value of the $u$ component of the COP. The used COP is the general one; using the information of the 8 load cells gives a wide dynamic range to the force feedback. The difference between the COP while the robot is in its home position (similar to the human's neutral anatomical position) and the current COP gives the direction and magnitude to the force feedback. The direction is from the home position COP to the current one. A dead zone is added to the generation of the haptic force to prevent positive feedback; if a component of the COP produces a force where its absolute value is lower than $d_u \newton$ that force is reduced to zero. If is higher, the Equation \ref{equation:force_gen_v1} is applied. This gives the user the unbalance that the robot is in.

\begin{equation}
	F_u=\frac{F^{max}_{Phantom} - F^{min}_{Phantom}}{COP^{max}_{u} - COP^{min}_{u}} * COP_u \mp d_u
	\label{equation:force_gen_v1}
\end{equation}

The function of the user is to correct the unbalance back to a stable position. This happens by opposing the force being exerted in the haptic device. 

A single \textit{node} handles all the above functionalities, called \textit{cells\_force\_gen} and its structure is simple. It starts with the ROS initialization and then it sets the COP message subscriber and force message advertiser. Like before, the subscriber has a callback trigger where the translation happens. The variable that holds the force values is updated in the callback, but is only sent inside the main loop. This allows a greater control over the setting of each force component. Before the main loop starts, 2 variables are set: the loop's frequency and the third component of the force. The frequency of the main loop, like in the previous module, is set higher than the origins of the message subscribed. Because the COP only sets 2 force components, the third is set to compensate gravity. Using the apparent weight of the tip, the vertical component of the force ensures that the user doesn't have to hold the device in place.

\FloatBarrier
\section{Module \textit{phantom\_control}}\label{sec:tele_phantom_control}

The module \textit{phantom\_control} uses both ROS and OpenHaptics. This is the module that controls the device's force feedback and obtains its joint values.

Due to the programming structure of a typical OpenHaptics program, all the commands for getting and setting the device's state need to be done within the same servo loop. This means that the control of the device and communication to the other modules needs to be done in the same \textit{node}; because of this, the \textit{phantom\_control} has only one \textit{node}.


The \textit{node} starts by initializing the structure that holds the state of the haptic device, which is composed of the following fields:

\begin{itemize}
	\item \textit{position} ($[1 \times 3]$ vector)
	\item \textit{rotation} ($[1 \times 3]$ vector)
	\item \textit{gimbals} ($[1 \times 3]$ vector)
	\item \textit{thetas} ($[1 \times 7]$ vector)
	\item \textit{temp} ($[1 \times 3]$ vector)
	\item \textit{buttons} ($[1 \times 2]$ vector)
	\item \textit{home} (boolean)
	\item \textit{home\_position} ($[1 \times 3]$ vector)
\end{itemize}

Visible in Figure \ref{fig:phantom_kinematics}, the \textit{position}, \textit{rotation} and \textit{gimbals} fields are taken directly from the device. The \textit{position} gives the X, Y and Z position of the tip in millimetres. The \textit{rotation} and \textit{gimbals} fields gives the angles, the first gives the 3 base joints and the second the give the tip joints. All these angles are stored in the \textit{thetas} vector; this one is used for the kinematics. The \textit{temp} holds the temperature of the motors in the base of the device. These values are used for integrity and security purposes. The \textit{buttons} hold the state of both buttons present in the device. The \textit{home} and \textit{home\_position} are used for setting a home position for the device. The boolean is only set if the device is in the home position. All these variables are published in a custom ROS message.


\begin{figure}[H]
  \centering
  \includegraphics[height=60mm]{Imagens/phantom.pdf}
  \captionof{figure}[ PHANTOM OMNI kinematics.]{ PHANTOM OMNI's kinematics.}
  \label{fig:phantom_kinematics}
\end{figure}


The device is initialized by using the HDAPI is possible to start the communication with the device and control it at a low-level. Besides initializing the device itself, an asynchronous scheduler is set to manage the servo loop: it is a high frequency and high priority thread for sending the forces to the device and retrieving its state. To have a stable force feedback it needs to run at least at $1000\hertz$. The scheduler begins by starting the communication with the device. It then gets the position, joint state, buttons state and motor temperature. It then checks if button 1 is pressed; if not, the process creates a force to take the tip to the home position. If the tip is already in the home position the process continues. If button 1 is pressed, the force feedback is generated using the force message from the \textit{haptic\_force} module. After this the communication with the device is stopped. Before the callback exits the \textit{thetas} vector is updated with the new values \citep{sensable_openhaptics_2008,sensable_openhaptics_2008-1}. Figure \ref{fig:phantom_callback} shows the schematic of the callback.


\begin{figure}
% The block diagram code is probably more verbose than necessary
\centering
\begin{tikzpicture}[auto, node distance=3cm,>=latex']
		\node [block1, name=start_com, text width=4.5cm] {
		    \underline{\textbf{Start Communication}}};
		    
		    \node [block1, below of=start_com,  name=get, node distance=2cm] {
		    \underline{\textbf{Get device state}}
%		    \begin{itemize}
%		    	\item position
%		    	\item rotation
%		    	\item gimbals
%		    	\item temperature
%		    \end{itemize}
		    };
		    
		    \node [block?, below of=get, name=button, node distance=3cm] {
		    \textit{button 1 pressed}?};
		    
		    \node [block?, right of=button, name=home, node distance=6.6cm] {
		    \textit{home}?};
		    
		    \node [block1, below of=button,  name=force_up, text width=4.5cm] {
		    \underline{\textbf{Update force feedback}}
		    };
		    
		    \node [block1, below of=home,  name=force_home, text width=6cm] {
		    \underline{\textbf{Renders force to home position}}
		    };
		    
		    \node [block1, below left of=force_home, xshift=-13mm,  name=stop, text width=4cm] {
		    \underline{\textbf{Stop communication}}
		    };
		    
		    \node [block1, below of=stop,  name=thetas, node distance=2cm, text width=4cm] {
		    \underline{\textbf{Update thetas vector}}
		    };
		    
		    
		    \path [->,draw,very thick] (start_com)  --  (get);
		    \path [->,draw,very thick] (get)  --  (button);
		\path [draw,->, very thick] (button)  -- node [right]{yes} (force_up);
		\path [draw,->, very thick] (button)  -- node [above]{no} (home);
		\path [draw,->, very thick] (home)  -- node [above]{\textit{no}} (force_home);
		    \path [->,draw,very thick] (stop)  --  (thetas);
		
		\path [draw,->, very thick] (home.east)  -- ++(0,0) -- node [above]{\textit{yes}} ++(3,0)  -- ++(0,-5.12) --(stop.east);
		
		\path [draw,->, very thick] (force_home.south)  -- ++(0,-.5) -- ++(-3.42,0)  --  (stop.north);
		
		\path [draw,-, very thick] (force_up.south)  -- ++(0,-.499) -- ++(3.2,0);
	\end{tikzpicture}
	\caption{\textit{phantom\_control} scheduler's callback scheme.}
	\label{fig:phantom_callback}
\end{figure}


After the setting of the scheduler, the force feedback is enabled and the scheduler starts. If the \textit{node} is unable to start due to an error, it stops and reports back to the launcher. The final step for the device settings is the calibration routine. This is an automatic calibration routine that checks if the device is already calibrated and if not, it takes actions to calibrate it. Usually the device is calibrated with a utility provided by OpenHapics before the \textit{node} is launched.

The structure where the state is saved is then initialized with zeros. As soon as the main loop starts, the variables present at the structure are updated.

The next step is to initialize the ROS \textit{nodes}. Like before, the subscriber, the publisher and the loop frequency are set. The published message is the state of the device. The \textit{phantom\_control} \textit{node} subscribes the force message that when received starts a callback. The callback consists of updating the structure with the required force. As in the previous modules, the frequency is set so the \textit{node} gets all the messages in the required time.

The main loop is dependent of the \textit{ros::ok} variable. This allows for the \textit{node} to exit correctly when the shutdown signal for ROS is given. The scheduler and the haptic device need to be properly stopped; this happens when the loop is stopped and before the \textit{node} ends. The main loop is where the device's state is published. This only occurs after the device's callback is executed because a continuous message publication would consume too many resources. After this, button 2 state is checked; if pressed, the current position is set as the new home position. This allows for a more intuitive user experience where one can leave the command of the robot at any time. At the end of the loop, a ROS function checks if there are new messages and runs the callback. Figure \ref{fig:phantom_control} illustrates the \textit{phantom\_control}'s program structure.

\begin{figure}[H]
% The block diagram code is probably more verbose than necessary
\centering
\begin{tikzpicture}[auto, node distance=3cm,>=latex']
		\node [block1, name=launch] {\underline{\textbf{Launcher}}};
		
		\node [block1, below of=launch, name=setup, text width=6cm] {
		    \underline{\textbf{Setup}}
		    \begin{itemize}
		    	\item Initialize device (scheduler, feedback, calibration)
		    	\item Initialize ROS (subscriber, publisher, frequency)
		    \end{itemize}
		    };
		    
		    \node [block1, right of =setup, name=schduler, node distance=6cm, yshift=12.5mm] {
		    \underline{\textbf{Schduler}}
		    };
		    
		    \node [block1, below of =schduler, name=call, node distance=2cm, text width=4cm] {
		    \underline{\textbf{Force callback}}
		    \begin{itemize}
		    	\item Update force to structure
		    \end{itemize}
		    };
		    
%		    \node [block1, below of =setup, name=loop, node distance=3.25cm] {
%		    \underline{\textbf{Loop}}
%		    };
		    
		    \node [block?, below of =setup, name=state, node distance=4.25cm] {
		    \textit{new state}?
		    };
		    
		    \node [block1, right of =state, name=pub, node distance=6cm] {
		    \underline{\textbf{Publish state}}
		    };
		    
		    \node [block?, below of =state, name=button, node distance=3.25cm] {
		    \textit{button 2}?
		    };
		    
		    \node [block1, below of =button, name=home_pos, node distance=2.5cm, text width=5cm] {
		    \underline{\textbf{Set new home position}}
		    };
		    
		    \path [->,draw,very thick] (launch) -- (setup);
		    \path [->,draw,very thick] (setup) -- (state);
		    \path [->,draw,very thick] (state) -- node[right]{no} (button);
		    \path [->,draw,very thick] (button) -- node[right]{yes} (home_pos);
		    \path [->,draw,very thick] (state) -- node[right]{yes} (pub);
		    
		    \path [->,draw,very thick] (pub.south) -- ++(0,-2.66) -- (button.east);
		    
		    \path [-,draw,very thick] (button.west) -- node[right]{no} ++(-2.75,0);
		    
		    \path [->,draw,very thick] (home_pos.south) -- ++(0,-0.5) -- ++(-4,0)-- ++(0,9)-- ++(4,0);
	\end{tikzpicture}
	\caption{\textit{phantom\_control} scheduler's callback scheme.}
	\label{fig:phantom_control}
\end{figure}

\FloatBarrier
\section{Module \textit{humaniod\_control}}\label{sec:tele_humaniod_control}

The module \textit{humaniod\_control} has the function of commanding the PHUA robot's servomotors. The idea is to have a module that can communicate with the robot. Commanding the servomotors and getting their state are two prerequisites for this module and a \textit{C++} class is developed for this purpose. This work is focused on the lower limbs and torso and, therefore, only those joints are used and commanded in this \textit{node}. The rest of the joints can be implemented using the principles explained next.

Section \ref{subsec:intro_phua} describes the \textit{Bidirectional Serial Interface} and its protocol. In the work of P. Cruz \citep{cruz_haptic_2012} some software was implemented to control the servomotors using this protocol. A low-level control was done as well as a joint-based control. Using the mentioned previous work, a new \textit{C++} class was created to integrate the servomotor control with the modular idea of the present work. This class is integrated in the \textit{humaniod\_control} module since it is not needed elsewhere. The class is capable of commanding each servomotor with just the identification number an its joint position or speed. The positions are given in degrees and converted to servomotor values before sending the byte word. The speed is set with an integer value between 0 and 255.

The main commands used in this class are the \textit{MoveJoint} and \textit{SetSpeedJoint}. They both have 2 input parameters: the identification of the joint and the value to be set. The first command is used to request a position change to a specific servomotor. The internal PD controller of the servomotor checks the current position and acts upon it. The servomotor response is a 2 byte word. In the case of \textit{MoveJoint}, the response is the speed at with the movement is being done. If the communication isn't successful, the return is \textit{0xFFFF} for both functions. The \textit{SetSpeedJoint} sets the speed at which the servomotor must make its movement. This function does not move the joint, only sets how it will move. The return response is the current position of the servomotor. In both functions is possible to command all servomotors with the right identification. This is used to take all joints to the zero position or to set the same speed for all of them.


The structure of this \textit{node} is illustrated in Figure \ref{fig:humanoid_scheme}. Like before, this \textit{node} starts with the ROS initialization. A subscriber is set to receive the the message published by the \textit{phantom\_control} \textit{node} which contains the haptic devices's state. The publisher for the the humanoid is also defined, this state is published at the end of the main loop. The frequency is set at the same value of the \textit{phantom\_control} \textit{node}. Before entering the main loop, the class for controlling the servomotors is initialized with the corrected port for serial communication. During this process the speed of all the servomotors is set to 50. It is then used to set the robot to the home position; this is the correspondent position of the humans neutral position. This happens by setting all the joints to there zero value.


\tikzstyle{for} = [draw, fill=lightgray!5, rectangle, rounded corners=7mm, thin, minimum height=6cm, text top,  minimum width=3cm, text width=2cm]

\begin{figure}[H]
% The block diagram code is probably more verbose than necessary
\centering
\begin{tikzpicture}[auto, node distance=3cm,>=latex']
		\node [block1, name=start] { \textbf{\underline{Launcher}}};
		
		\node [block1, below of=start, name=setup, text width=7.5cm, node distance=2.5cm] { \textbf{\underline{Setup}}
		\begin{itemize}
			\item Initialize ROS (subscriber, advertiser)
			\item Initialize class
			\item Set robot to Home Position
		\end{itemize}
		};

%		\node [block1, below of=setup, name=loop, node distance=2.5cm] { \textbf{\underline{Loop}}};
		
		\node [block?, below of=setup, name=check_pos, node distance=5cm] { \textit{requested == current} ?};
		
		\node [block1, right of=check_pos, name=move, node distance=4.5cm] { \textit{MoveJoint}};
		
		\node [block1, below of=check_pos, name=speed, node distance=4cm] { \textit{SetSpeedJoint}};
		
		\node [block1, below of=speed, name=pub, text width=5cm, node distance=2cm] { \underline{\textbf{Publish Humanoid State}}};
		
		\node [block1, right of=setup, name=call, text width=5cm, node distance=7.5cm] { \textbf{\underline{Haptic Callback}}
		\begin{itemize}
			\item Check device's state
			\item Calculate joint angles
			\item Update wanted position
		\end{itemize}
		};
		
		\path[->, very thick, draw] (start) -- (setup);
		\path[->, very thick, draw] (setup) -- (check_pos);
		\path[->, very thick, draw] (check_pos) --  node[above]{no} (move);
		\path[->, very thick, draw] (check_pos) --  node[above]{yes} (speed);
		\path[->, very thick, draw] (speed) -- (pub);

		\path[->, very thick, draw] (move.south) -- ++(0,-5.41) --(pub.east);
		
		\path[->, very thick, draw] (pub.west) -- ++(-3,0) -- ++(0,8.7) -- ++(5.63,0);
		
	\end{tikzpicture}
	\caption{Program structure of \textit{phantom\_control} \textit{node}.}
	\label{fig:humanoid_scheme}
\end{figure}


The main loop starts by checking all used joints to see if they are at the requested position, which is given by the haptic device callback that receives the state of the device and calculates the joint angels. These angles are the new requested position for the the joints. The calculation of the joints angles are explained further in Chapter \ref{chapter:demo}. The requested position is compared against the current position. If they are the same, the \textit{SetSpeedJoint} command is used to get the position and keep the speed updated. If not, the \textit{MoveJoint} is used to give the move command to the servomotor. All the servomotors in the line go through this process.

After this, the state of the humanoid is published; all values, requested and current, speed and positions, are placed in the message. The main loop ends with the check for new received messages for the \textit{phantom\_control} module.
