%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\chapter{Software for Control and Data Logging}
{ In this chapter the different processes that compose and work together to create the architecture behind this interface will be described. An important aspect that will be discussed is the data logging system that collects and records all the sensory information (\textit{e.g.,} joint positions and ground reaction forces), as well as the commands guiding the execution of a specific task. \paragraph{}  The \acs{ROS} distributed environment makes possible to divide the different modules by categories depending on its function. For example, in this work the different modules are divided as follows:}
\begin{itemize}
\item \textit{Phantom_control} module belongs to the bases category since this module allows communication with the hardware device. More precisely, it is responsible for the \linebreak connection between the joystick device and the host computer. This module subscribes the information needed to process the force feedback and publishes the data for later control of the humanoid robot.
\item \textit{Phantom_filter} module belongs to the utils category since performs data filtering. This module subscribes the \textit{phantom_control} data, handles the data (\textit{i.e.,} apply a filter to the data) and publishes it.
\item \textit{Humanoid_control} module belongs to the bases category, being responsible for the communication between the host computer and the humanoid robot. This module subscribes the data published by the \textit{phantom_filter} module and uses that data to control the servomotors of the humanoid robot.
\item \textit{Pressure_cells} module belongs to the sensory category, being responsible for collecting sensory data from the load cells, calculating the \acs{COP} and publishes it.
\item \textit{Haptic_force} module belongs to the perception, being responsible for interpreting the \acs{COP} information and transform that data in force to be used by the \textit{phantom_control} module.
\end{itemize}
{Figure \ref{Inter1} illustrates the interconnection among the different modules developed under the ROS platform. A great advantage of this distributed architecture is the decentralized execution of the several tasks involved, making it easier to detect and correct software bugs.}

		\begin{figure}[H]
		  \centering 
		  \includegraphics[width=1.0\textwidth]{imgs/Chapter4/rosgraph.png}
		 \caption{Interconnection between the different modules.}
		 \label{Inter1}
		\end{figure}
	


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
	\section{\textit{Phantom_Control} module}
	{ This is the module responsible for retrieving the different data from the joystick and control the device force feedback. This module works by using just one node. This node can be divide into 2 distinct phases. The 1 \textsuperscript{st} part begins by initializing the parameters that compose and will be published by the device : }
	
	
	\begin{itemize}
	\item \textit{position      ([1 x 3] vector)}
	\item \textit{rotation      ([1 x 3] vector)}
	\item \textit{gimbals       ([1 x 3] vector)}
	\item \textit{temp          ([1 x 3] vector)}
	\item \textit{buttons       ([1 x 2] vector)}
	\item \textit{thetas        ([1 x 7] vector)}
	\item \textit{home          (boolean)}
	\item \textit{home_position ([1 x 3] vector)}
	
	\end{itemize}

	{The \textit{position} gives the Cartesian coordinates X, Y and Z of the tip in mm. The \textit{rotation} gives the angle from the base joints, the gimbals the angles from the tip joints and all these angles are stored in the \textit{thetas} vector. The \textit{temp} gives the temperature of the motors in the base of the device and the \textit{buttons} refers to the state of the buttons from the device. Lastly, the \textit{home} and \textit{home_position} are used to set the home position. \paragraph{} Once the parameters are initialized, the device starts and an asynchronous scheduler is set to manager the servo loop. This scheduler initiates the communication with the haptic device and gets the information about the position, joint state, buttons state and temperature. If button 1 is not pressed, a force is created to send the device to the home position. This force will continue after the device is in the home_position to ensure it stays that way. If button 1 is pressed the force used is the one published by the \linebreak \textit{haptic_force} module. The thetas vector is then updated and the communication with the device stopped. In Figure \ref{Control1}, the scheme of the 1 \textsuperscript{st} part is shown.} 
	
	
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.85\textwidth]{imgs/Chapter4/Phantom1.png}
	 \caption{\textit{phantom_control} scheduler's callback scheme.}
	 \label{Control1}
	\end{figure}

	
	
	{ The 2 \textsuperscript{nd} part of the node, starts by enabling the force output and starts the scheduler. When starting the node if is unable to do it due to an error, the process is stopped. If the node starts the device does an automatic calibration, sets the\textit{ phantom_control} as advertiser (\textit{i.e.,} this module is set to publish a certain topic during the time that ROS is active), subscribes the force and sets the frequency of the main loop. Each subscriber triggers a Callback (\textit{i.e.,} function that will be evoked during the time that the ROS is active and as an argument the message of the subscribed topic), where the state of force is updated. Finally, in the main loop the new state is published and if the button 2 is pressed a new home position is set. In Figure \ref{Control2}, the scheduler's callback scheme is shown.}
	
	
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.75\textwidth]{imgs/Chapter4/EmilioPhantom.png}
	 \caption{phantom_control scheduler's callback scheme.}
	 \label{Control2}
	\end{figure}
\clearpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	 
	\section{\textit{Phantom_Filter} module}
	{ This module is based on the study conducted in the Chapter 3, Pre-processing of joystick raw-data. This module is essential for the correct operation of the humanoid robot and aims to filter the data from the joystick. \paragraph{} The node responsible for this is called \textit{PhantomFilter}. This node starts by initializing the ROS environment and advertising two markers. These markers are the ones that will be used in the \textit{Rviz} interface shown in Chapter 3. Besides publishing the markers, the node also subscribes the data from the \textit{phantom_control} and publishes the new data. Each subscribed topic triggers a callback and inside this function the \textit{phantom_control} data is updated. The variables that compose the data are:}
	 \begin{itemize}
	 \item float64[3] Position
	 \item float64[3] Rot
	 \item int16[2] Buttons
	 \item float64[3] force
	 \item float64[3] Home_pos
	 \end{itemize}
	 
	 { The above fields have the same characteristics of the data published by the \textit{phantom_control} module with the difference that this data is filtered and this process of filtering and publishing only happens after a buffer reaches a number of points defined by user.\paragraph{} In Figure \ref{Phantom3} a schematic explains the main operation of this node is shown. }
	 \clearpage
%%%%%%%%%%%%%%%%%%%%	
\begin{center} 
	\begin{figure}[H]
\begin{tikzpicture} [
    auto,
    decision/.style = { diamond, draw=gray, thick, fill=gray!20,
                        text width=6em, text badly centered,
                        inner sep=1pt},
    block/.style    = { rectangle, draw=gray, thick, 
                        fill=gray!20, text width=16em, text centered, minimum height=2em },
    line/.style     = { draw, thick, ->, shorten >=2pt },
    blockk/.style    = { rectangle, draw=gray, thick, 
                        fill=gray!20, text width=12em, text centered, minimum height=2em },
  ]
  % Define nodes in a matrix
  \matrix [column sep=5mm, row sep=10mm] {
                     \node [blockk,xshift=10mm] (T1) {\textsf{\underline{Laucher}}}; 
                    \node [below of=T1,xshift=5mm](null1) {};                                    
                    \node [block, below of=T1, yshift=-18mm] (T2) {\textsf{\underline{Setup}}\begin{itemize}
\item Initialize (subscriber, advertiser)
\item Initalize class
\end{itemize}}; 
					 \node [block, right of=T2,xshift=62mm] (T3) {\textsf{\underline{Phantom Callback}}
\begin{itemize}
\item Check device's state
\end{itemize}}; 
					 \node [below of=T2,yshift=-5mm](null2) {};                                    
					 \node [decision, below of=T2,yshift=-35mm] (T4) 
{ $Rot[3]$\linebreak $Position[3]$\linebreak $Homepos[3]$ \linebreak $force[3]$\linebreak $buttons[3]$\linebreak $==N$  points $??$  };  
					  \node[blockk, below of=T4,yshift=-30mm](T5){\textsf{Apply Filter}}; 
                    \node [blockk, below of=T5,yshift=-15mm] (T6) {\textsf{Publish State}}; & \\
  };
  
  % connect all nodes defined above
  \begin{scope} [every path/.style=line]
    \path (T1)        --    (T2);
    \path (T2)      --    (T4);
    \path (T4)      --    node [near start] {yes} (T5);
    \path (T4)     --++  (-4,0) node [near start] {no} |- (null2);
   \path (T5)    --     (T6) ;
    \path (T6)    --    (-7.8,-6.8) node [near start] {} |-(null2);
  \end{scope}

\end{tikzpicture}
\caption{Program structure of phantom_filter node.}
\label{Phantom3}
\end{figure}
\end{center}

\clearpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
	\section{\textit{Humanoid_Control} module}
	{ This module is responsible for the communication between the PC and the servomotors which control the humanoid robot.\paragraph{} First, in \cite{Cruz} a software has been implemented to control the servomotors using the \linebreak RS-232 protocol. In \cite{Emilio}, a class was created to help command these servomotors. This class has the capability to convert degrees into servomotors values and sends this information to the correct servomotor by knowing the ID. \paragraph{} The \textit{Humanoid_Control} is the node responsible for controlling the servomotors. This node starts by initializing the ROS environment, setting the serial port communication, subscribing the message from the \textit{phantom_filter} node and defining as advertiser the state of the humanoid robot. Before entering the main loop the humanoid robot is set to the home position. In the callback generated by the \textit{phantom} subscriber, the phantom data is updated and its Cartesian Coordinates are transformed into joint angles using the inverse kinematics.\paragraph{} Finally, in the main loop the new angle values are compared to the old ones, if they are different in one degree the new state is published (speed and position). Each state is composed by the following fields:}
	\begin{itemize}
	 \item int16[13] speed_wanted
	 \item int16[13] speed_now
	 \item float64[13] joint_now
	 \item float64[13] joint_wanted
	 \end{itemize}
	
	{ The speed_wanted and joint_wanted represent the desire value for the next humanoid iteration. The speed_now and joint_now the real state of the humanoid robot. \paragraph{} In Figure \ref{Human3}, the structure of this node can be viewed. }
	\clearpage

	 
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=1\textwidth]{imgs/Chapter4/HumaModule.png}
	 \caption{Program structure of humanoid_control node.\cite{Emilio}.}
	 \label{Human3}
	 \end{figure} 
	 \clearpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	 
	\section{\textit{Pressure_Cells} module}
	{ The main objective is to retrieve information from the Arduino Nano and publish it in \acs{ROS} format. This module has the function of retrieving sensory data and calculating the \acs{COP} individually and globally for each foot.\paragraph{} In order to do this, two different nodes running in parallel are needed: One is responsible for retrieving the sensory information from the load cells and the other for computing the different \acs{COP} (COPy, COPx and COP).\paragraph{} The first node is called \textit{arduino} and has the function of read the values of the ADC. This node accepts as parameters configuration the serial port of each Arduino, if display or not the markers, and the identification of the corresponding load cells. Once the configuration is set, the ADC values are retrieved, the calibration routine is triggered and the load cells information is published.\paragraph{} The second node is responsible for calculating the \acs{COP} values. This node accepts as parameter the information of which cells are communicating, sets the current frame identification and decides if it will show or not the visualization markers. After initializing the ROS environment, 3 advertises are set, one for each foot and one global. The loop starts until the variable \textit{ros::ok} is true, check if the feet are in the same plan and if a callback run in the last iteration (\textit{ros::ok1, ros::ok2}). If those parameters are true the \acs{COP} is calculated and published. The different fields published by this module can be seen in the following list:}
	\begin{itemize}
	 \item float64 COPx
	 \item float64 COPy
	 \item float64 sen1
	 \item float64 sen2
	 \item float64 sen3
	 \item float64 sen4
	 \end{itemize}
	
	
	 {COPx and COPy represent the components of the global COP. The variables sen1 to sen4 are the values retrieved from the four load cells of each foot.\paragraph{}In Figure \ref{Pressure3}, the schematic of the force_cop node can be seen. }

	
	
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=1\textwidth]{imgs/Chapter4/Emilioo.png}
	 \caption{Simplified schematic of force_cop node.\cite{Emilio}.}
	 \label{Pressure3}
	 \end{figure}
	 
\clearpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	 
	\section{\textit{Haptic_Force} module}
	{The idea of this module is to turn sensory data, already in \acs{COP} format into Force. \paragraph{} The node responsible for all of that is called \textit{force_gen}. This node has a simple structure: after starting ROS, subscribes the COP message, sets the force message as advertiser and, before starting the main loop defines the loop's rate. Then a callback is triggered and, inside this callback, all the transformation described above occurs. Everytime the callback is run the variable corresponding to the COP, updates. The new \acs{COP} value is used to calculate the force, once there are three different force generation (see Chapter 3, Haptic Force Rendering), the function that will be used is chosen by the operator. Each time the program is inside the main loop the new force values are published. The field published by this module is:}
	\begin{itemize}
	 \item float64[3] force
	 
	 \end{itemize}
	
	
	{\paragraph{} In Figure \ref{Force3}, a diagram explain the main structure of this node is shown.}

\begin{center} 
	\begin{figure}[H]
\begin{tikzpicture} [
    auto,
    decision/.style = { diamond, draw=gray, thick, fill=gray!20,
                        text width=4em, text badly centered,
                        inner sep=1pt},
    block/.style    = { rectangle, draw=gray, thick, 
                        fill=gray!20, text width=16em, text centered, minimum height=2em },
    line/.style     = { draw, thick, ->, shorten >=2pt },
    blockk/.style    = { rectangle, draw=gray, thick, 
                        fill=gray!20, text width=12em, text centered, minimum height=2em },
  ]
  % Define nodes in a matrix
  \matrix [column sep=5mm, row sep=10mm] {
                     \node [blockk,xshift=10mm] (T1) {\textsf{\underline{Laucher}}}; 
                    \node [below of=T1](null1) {};                                    
                    \node [block, below of=T1, yshift=-15mm] (T2) {\textsf{\underline{Setup}}\begin{itemize}
\item Set ROS
\item Subscribe pressure_Cells::COP
\end{itemize}}; 
					 \node [block, right of=T2,xshift=62mm] (T3) {\textsf{\underline{pressure_cells Callback}}
\begin{itemize}
\item Update COP information
\item Calculate force
\end{itemize}}; 
					 \node [below of=T2,yshift=-5mm](null2) {};                                    
					 \node [blockk, below of=T2,yshift=-15mm] (T4) {\textsf{Publish State}}; & \\
  };
  
  % connect all nodes defined above
  \begin{scope} [every path/.style=line]
    \path (T1)        --    (T2);
    \path (T2)      --    (T4);
    \path (T4)     --++  (-4,0) node [near start] {} |- (null2);;
  \end{scope}

\end{tikzpicture}
\caption{Program structure of force_gen node.}
\label{Force3}
\end{figure}
\end{center}

\clearpage
\section{\textit{Data Logging}}

{The main idea behind this work is to use a physical humanoid robot that, while tele-operated by a human subject using the haptic joystick, collects and records all sensory information and the control commands guiding the execution of a specified task. These datasets logged from the human-robot interaction can be later used for learning purposes. This task has been simplified in view of the variety of tools developed in ROS allowing to store, analyse and visualize message data. In this context, a bag is a file format in ROS created by the \textit{rosbag} tool which subscribes to one or more ROS topics. These bag files can also be played back in ROS to the same topics they were recorded from, or even remapped to new topics.\paragraph{} Using bag files within ROS is, generally, no different from having ROS nodes send the same data. In order to avoid problems with time stamped data stored inside message data, the \textit{rosbag} tool includes an option to publish a clock that corresponds to the time the data was recorded in the file. The bag file format is very efficient for both recording and playback, as messages are stored in the same representation used in the network transport layer of ROS. \paragraph{} Data logging was performed to the topics published by the five modules. All the variables are recorded using a simple node that subscribes the different topics and are saved in a .txt file. The variables of interest are the following:}
\begin{itemize}
\item Time-stamp
\item User actions on the Joystick (raw- and filtered-data)
\item Feet force sensors
\item Joint angular data
\item Synthesized forces
\end{itemize}

%{For each topic a different file is created and in all the created files the time-stamp variable is present. With this recorded data the behaviour of the humanoid robot can be studied (see Chapter 5) and used for future tests. }