	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%------------------------------------------------%
	
	\chapter{Infrastructure and Experimental Setup}
	
	{This chapter describes the hardware and software technologies used to develop the haptic tele-operation system. The main hardware components consist of a real humanoid robot, the Phantom Omni haptic device and the Load Cells sensors. The haptic interface consists of several modules running in the host computer and the software architecture is based on the \acs{ROS} (Robot Operating System), Fuerte version, middleware using a Linux/C/C++ programming environment.}
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%------------------------------------------------%
	
	
	\section{Hardware Technologies}
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%------------------------------------------------%	
	
	\subsection{The \acs{PHUA} Humanoid Robot}
	
{Started in 2004, the PHUA project culminated, in 2009, with the development of a highly integrated humanoid platform oriented for research in biped locomotion, navigation in real world environments and multi-modal perception for autonomous humanoids. This robot consists of a small-sized whole-body robotic platform based on standard components and open software. From a hardware point of view, this research platform has a great potential in terms of movement abilities with 27 degrees-of-freedom (\acs{DOF}), low-level \linebreak actuator control for both position and velocity, and a number of useful onboard sensors, such as video cameras, inertial sensors and load cells on both feet. A hybrid actuation system allows combining active HITEC servomotors with passive elastic components. The \linebreak communications with the humanoid robot, both for control actions and for sensory readings, is performed through an external computer using a bidirectional RS-232 protocol. The humanoid platform is represented in Figure 2.1 together with the distribution of the 27-DOFs by the robotic structure. Table 2.1 summarizes the servomotor models installed at each joint of the robotic structure. }	
	
	
	
	

%------------------------------------------------%	
		
		\begin{figure}[H]
		\centering
		\begin{minipage}{.5\textwidth}
		  \centering
		  \includegraphics[width=.4\linewidth]{imgs/Chapter2/humanoid.png}
		  \label{fig:Humanoid1}
		\end{minipage}%
		\begin{minipage}{.5\textwidth}
		  \centering
		  \includegraphics[width=.4\linewidth]{imgs/Chapter2/Degree.png}
		  \label{fig:DOF1}
		\end{minipage}
		\caption{Physical humanoid robot (left) and distribution of DOF by the full-body structure (right) \cite{Cruz}.}
		\end{figure}
		
		
		\begin{table}[h]
		\begin{center}
	\begin{tabular}{|c|c|c|}
	\hline
	Joint    & Number of DOFs & Servomotor                                                                \\ \hline
	Toe      & 1(x2)          & (none)                                                                    \\ \hline
	Ankle    & 2(x2)          & HSR-5980SG                                                                \\ \hline
	Knee     & 1(x2)          & HSr-5980SG                                                                \\ \hline
	Hip      & 3(x2)          & HSR-5980SG                                                                \\ \hline
	Trunk    & 3              & \begin{tabular}[c]{@{}c@{}}HSR-5980SG(x2)\\ HSR-8498SG(x1)\end{tabular}   \\ \hline
	Shoulder & 3(x2)          & \begin{tabular}[c]{@{}c@{}}HSR-5980SG\\ HSR-5498SG\\ HS-82MG\end{tabular} \\ \hline
	Elbow    & 1(x2)          & HSR-5498SG                                                                \\ \hline
	Neck     & 2              & \begin{tabular}[c]{@{}c@{}}HSR-5498SG\\ HS-5056MG\end{tabular}            \\ \hline
	Total    & 27             & \_\_ \\ \hline
	\end{tabular}
	\caption{\acs{PHUA} platform's degrees-of-freedom
	and installed servomotors \cite{Cruz}.}
	\label{TableServo}
	\end{center}
	\end{table}

%------------------------------------------------$


	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%$------------------------------------------------$
	\subsection{Load Cells}
	
	{ The humanoid robot is equipped with four load cells placed in the corners of the feet (see Figure \ref{fig:DOF1} (Left Side)). The load cells (see Figure \ref{fig:DOF1} (Right Side)) are force transducers that measure the reaction between two plates, in this case, both feet and the ground. The cells used in this work have a range of 5 and 10 pounds of force.
		}

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

		\begin{figure}[H]
		\centering
		\begin{minipage}{.5\textwidth}
		  \centering
		  \includegraphics[width=.8\linewidth]{imgs/Chapter2/Load.png}
		\end{minipage}%
		\begin{minipage}{.5\textwidth}
		  \centering
		  \includegraphics[width=.8\linewidth]{imgs/Chapter2/P3.jpg} 
		\end{minipage}
		\caption{Bottom of the foot \cite{Sabino} (Left Side), Load Cells (Right Side) }
		\label{fig:DOF1}
		\end{figure}
				
		
%		\begin{figure}[H]
%		  \centering 
%		  \includegraphics[width=0.4\textwidth]{imgs/Chapter2/Load.png}
%		 \caption{ Bottom of the foot \cite{Sabino}}
%		\end{figure}

%------------------------------------------------%
		
		{To correctly read the values of the load cells some signal conditioning is needed. That work was done before using \acs{PDIP} (Plastic Dual Inline Package)  components and a Arduino Fio in \cite{Emilio}. The main problem is that the circuit board can not be accommodated within the humanoid robot because of its size. Another problem is the fact that the Arduino Fio needs an \acs{USB} Serial Adapter to work. It is not possible to communicate and power the board using just a \acs{USB} interface.
		Therefore, a new \acs{PCB} was designed and built to solve this problem. All components used in \cite{Emilio} were replaced by \acs{SOP} components and the Arduino used is a Arduino Nano (see Figure \ref{fig:Arduino1} and Table \ref{Arduino2}).}

		\begin{figure}[H]
		\centering
		\begin{minipage}{.5\textwidth}
		  \centering
		  \includegraphics[width=1\linewidth]{imgs/Chapter2/ArduinoNano.jpg}
		\end{minipage}%
		\begin{minipage}{.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter2/PlacaFrente.png} 
		\end{minipage}
		\caption{Arduino Nano (Left Side), New breadboard (Right Side).}
		\label{fig:Arduino1}
		\end{figure}	
	

%------------------------------------------------%		
%		\begin{figure}[H]
%		  \centering 
%		  \includegraphics[width=0.8\textwidth]{imgs/Chapter2/ArduinoNano.jpg}
%		 \caption{Arduino Nano}
%		 \label{Arduino1}
%		\end{figure}
		
		\clearpage
		\begin{table}[h]
		\begin{center}
		\begin{tabular}{|l|l|}
		Microcontroller        & ATmega328         \\
		Input Voltage          & 7-12 V            \\
		Digital I/O Pins       & 14                \\
		Analog Input Pins      & 8                 \\
		DC Current per I/O Pin & 40 mA             \\
		Flash Memory           & 32 KB (ATmega328) \\
		SRAM                   & 2 KB  (ATmega328) \\
		EEPROM                 & 1 KB  (ATmega328) \\
		Clock Speed            & 16 MHz            \\
		Dimensions             & 0.73" x 1.70"    
		\end{tabular}
		\caption{Arduino Nano Characteristics.}
		\label{Arduino2}
		\end{center}
		\end{table}
		
	%------------------------------------------------%
		
		{The Arduino Nano is a small breadboard based on the ATmega 328. This Arduino can be powered externally (6 - 20 v, pin 30) or using a mini-USB connection (pin 27). Contains 14 digital pins that can be used as input or output, and operates at 5 volts. Pins 0 and 1 are used for receiving and transmitting serial data. External interrupts are allowed in the digital pins 2 and 3 and PWM in the pins 3 to 11. The remaining pins are used to \acs{SPI} (Serial Peripheral Interface). The Arduino Nano has 8 analogue pins, providing 10 bit of resolution and able to read from ground to 5 volts.} 

		
%-----------------------------------------------%
		
		{The new board is smaller: the new size is 4.37 cm x 2,195 cm instead of 6.051 cm x 2.712 cm and the route and clearance are now 0.2032 mm (8 mil) and 0.1524 mm (6 mil) instead of 0.25 mm (10 mil) and 0.2032 mm (8 mil).\paragraph{} The board's new size is beneficial to pack the boards with the Arduino Nano in the humanoid robot. This will permit a better control of space and will decrease the number of outside cables.  }
		

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

	
	\subsection{Haptic Device}
	
	
	{The idea behind an haptic device is to provide the users the force feedback information that represents a "feeling of the situation". The haptic device will be useful for tele-operation tasks where visual information may not be sufficient. In this work, it is used the Phantom Omni haptic device manufactured by SensAble Technology. This is a pen-style haptic device based on serial mechanisms (see Figure \ref{Phantom1}) providing 6-\acs{DOF} motion (3-\acs{DOF} translational motion and 3-\acs{DOF} orientational motion) and capable of rendering three dimensional forces(\textit{i.e.,}, 3-\acs{DOF} force feedback device). The communication with the host computer is achieved through an IEEE 1394a FireWire port. Table \ref{PhantomData}} summarizes the most important technical specifications of the haptic device Phantom Omni.
	
	
	\begin{figure}[H]
		  \centering 
		  \includegraphics[width=0.45\textwidth]{imgs/Chapter2/PhantomDof.png}
		 \caption{Phantom Omni DOF's.}
		 \label{Phantom1}
		\end{figure}
	
	
	
%------------------------------------------------%	
	
\begin{table}[h]
\small
\begin{tabular}{|c|c|}
\hline
\textbf{Model} & \textbf{The PHANTOM Omni Device} \\ \hline
\textbf{Force feedback workspace} & \begin{tabular}[c]{@{}c@{}}~6.4 x 4.8 H x 2.8 D in\\ \textgreater 160 W x 120 H x 70 D mm\end{tabular} \\ \hline
\textbf{\begin{tabular}[c]{@{}c@{}}Footprint\\ Physical area the base of device\\  occupies on the desk\end{tabular}} & \begin{tabular}[c]{@{}c@{}}6 5/8 " x 8 D in\\  ~168 W x 203 D mm\end{tabular} \\ \hline
\textbf{Weight (device only)} & 3 lb 15 oz \\ \hline
\textbf{Range of motion} & Hand movement pivoting at wrist \\ \hline
\textbf{Nominal position resolution} & \begin{tabular}[c]{@{}c@{}}\textgreater450 dpi\\ ~0.055 mm\end{tabular} \\ \hline
\textbf{Backdrive friction} & \textless1 oz (0.26 N) \\ \hline
\textbf{\begin{tabular}[c]{@{}c@{}}Maximum exertable force \\ at nominal position\end{tabular}} & 0.75 lbf. (3.3 N) \\ \hline
\textbf{Continuous exertable force} & \textgreater 0.2 lbf. (0.88 N) \\ \hline
\textbf{Stiffness} & \begin{tabular}[c]{@{}c@{}}X axis \textgreater 7.3 lb/in (1.26 N/mm)\\ Y axis \textgreater13.4 lb/in (2.31 N/mm)\\ Z axis \textgreater 5.9 lb/in (1.02 N/mm)\end{tabular} \\ \hline
\textbf{Inertia (apparent mass at tip)} & ~0.101 lbm. (45 g) \\ \hline
\textbf{Force feedback} & x, y, z \\ \hline
\textbf{\begin{tabular}[c]{@{}c@{}}Position sensing \\ -------------------\\ {[}Stylus gimbal{]}\end{tabular}} & \begin{tabular}[c]{@{}c@{}}x, y, z(digital encoders) \\ ------------------\\ {[}Pitch, roll, yaw ( 5\% linearity potentiometers){]}\end{tabular} \\ \hline
\textbf{Interface} & \begin{tabular}[c]{@{}c@{}}IEEE-1394 FireWire\textregistered\\  port: 6-pin to 6-pin\end{tabular} \\ \hline
Supported platforms & Intel or AMD-based PCs \\ \hline
\textbf{OpenHaptics SDK compatibility} & Yes \\ \hline
\end{tabular}
\caption{Phantom Omni by Sensable\texttrademark{} characteristics \cite{Sensable}.}
\label{PhantomData}
\end{table}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	

	
	
	\section{Software Tools}

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	\subsection{Robot Operating System (ROS)}
	{This project is developed under the Robot Operating System (\acs{ROS}) software that helps to create robot applications by providing a wide range of libraries and computational tools.\linebreak It is open-source software that provides functionalities such as hardware abstraction, device drivers, graphic tools, debugging tools, libraries of software commonly used and message passing between processes. The \acs{ROS} software allows to split the problem in different parts. This point is important to the Humanoid project because it allows to divide each part of the robotic control in separated modules (or packages) that communicate with each other but can work independently. To do so, each module is programmed recurring to C++ or python and some executable/nodes are created.\paragraph{} \acs{ROS} is distributed under BSD license (permissive free software license) and has the advantage of having  a large on-line community, a very well documented code and solutions to help developers to create their code faster. This makes \acs{ROS} a very flexible platform, capable of being implemented in any robot \cite{RosO}. A brief description about the different elements that compose the \acs{ROS} environment is given below.
}  
		
	\subsubsection*{Nodes}
	{
	In \acs{ROS}, a node is a process which performs computation. Each one has a specific function, for example, a node may control the haptic joystick, another may control the ServoMotors and so on. A node can have subscribers and publishers and communicate with another using topics, services, and parameter server.
Therefore, the use of nodes brings two important benefits: the ability to isolate potential problems and reduce the complexity \cite{Node}.
	}

%----------------------------------------------%
	
	\subsubsection*{Topic}
	{
	Each node exchanges messages through a communication channel/bus that, in ROS, is known as a topic \cite{topic}. On the one hand, whenever a node is ready to receive some data subscribes a specific topic. On the other hand, whenever a node generates new data, it publish it on a certain topic. Each topic can have multiple publishers and subscribers. Using a tool called \textit{rostopic} is possible to view specific information about a topic. In the list below, examples of the use of this tool are shown.
	
	%--------------------------------------------%
	\begin{itemize}
	\item \textit{rostopic bw}     Display bandwidth used by topic list
	\item \textit{rostopic echo}   Print messages to screen 
	\item \textit{rostopic find}   Find topics 
	\item \textit{rostopic hz}     Display publishing rate of topic
	\item \textit{rostopic info}   Print information about active  topics
	\item \textit{rostopic list}   Show active topics
	\item \textit{rostopic pub}    Publish data to the topic
	\item \textit{rostopic type}   Print topic type 
	\end{itemize}
	
	}
	
	\subsubsection*{Messages}		
	{
	A message is a data structure with different fields that supports standard and array primitive types (\textit{e.g.,} integer, floating point, boolean, etc). The class that implements the message is automatically generated from a file with the .msg extension.
Each message consists of a header with a time stamp and a frame ID. In the following list an example of a .msg file is shown.
	\begin{itemize}
	\item \textit{Header header}
	\item \textit{int16 speed_g_static}
	\item \textit{int16[13] speed_wanted}
	\item \textit{string data}
	\item \textit{float64[13] joint_now}
	\end{itemize}
	
	}
	
	
	\subsubsection*{Launch file}
	{
	 An important part of \acs{ROS} are the launch files. Instead of launching each node individually, an \acs{XML} file with a .launch extension can be created to launch all the required nodes. An example of a .xml file is shown below :
	 
	 %_--------------------------------------%
	 
	 \renewcommand\lstlistingname{Algorithm}
	 \begin{lstlisting} 
	 <?xml version="1.0"?>
<launch>
      
    <!-- This is a launches Humanoid node-->
    <node name="humanoid_node" pkg="humanoid_control" type="humanoide">
<!--
        <remap from="/device" to ="/device_mapper/serial_com1"/>
-->
        
        <param name="op_mode" value="6"/>
        <param name="port" value="/dev/ttyUSB0"/>
    
    </node>
</launch>    
	 \end{lstlisting}
	 	
	}
	
%-----------------------------------------------%	
	
	\subsubsection*{Rviz}
	{
	Rviz is one of the most used tools for debugging in \acs{ROS}. Rviz is a graphic tool to see what is happening in a \acs{ROS} system. It is possible to subscribe topics and see data inside them in a graphical way. In Figure \ref{Rv}, an example of this tool \cite{Rviz} can be seen.
	
%-----------------------------------------------%	
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=1\textwidth]{imgs/Chapter2/Marker.png}
	 \caption{Rviz example.}
	 \label{Rv}
	 \end{figure}
	}
	
	
%------------------------------------------------%	
	\subsubsection*{Bag}
	{
	 An important tool when working with the \acs{ROS} environment is the rosbag \cite{Bag}. Rosbag is a tool that can subscribe one or more \acs{ROS} topics and record the data of each subscribed \acs{ROS} topic. Besides recording, the rosbag tool also plays the data, making it possible to test new algorithms even without the robot. The following list shows examples of how to use this tool.
	 \begin{itemize}
	\item \textit{rosbag record} Record a bag file with the contents of the specified topics
	\item \textit{rosbag info} Display a summary of the contents of the bag files.
	\item\textit{ rosbag play} Play back the contents of the given bags
	\item \textit{rosbag check} Determine if the a bag is playable in the current system
	\item \textit{rosbag fix} Repairs a bag using registered rules
	\item \textit{rosbag filter} Convert a bag file using the given Python expression
	\item \textit{rosbag compress} Compress the bag files
	\item \textit{rosbag decompress} Decompress the bag files
	\item \textit{rosbag reindex} Reindex the bag files
	\end{itemize}
	
	}
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	%---------------------------------------%
	\subsection{OpenHaptics Toolkit}
	{ The OpenHaptics Toolkit is a programming solution to build haptical applications developed in C/C++ by SensAble\texttrademark{}. This toolkit is available for different operating systems and includes: }
	\begin{itemize}
	\item QuickHaptics micro API, allows to quickly and easily write applications or adds haptics to existent applications. The fact that has a built-in geometry parsers and intelligent parameters makes possible to create haptic scenes with small code.
	
	\item Haptic Device API (HDAPI), provides low-level access to the haptic device and enables haptic programmers to render forces directly.
	
	\item Haptic Library (HLAPI), provides high-level haptic rendering and is designed to be familiar to the OpenGL API programmers, simplifying haptics and graphics threads.
	
	\item Device Drivers (PDD), support all shipping Phantom devices.
		
	\item Source Code Examples, several examples to illustrate commonly used functionalities of Open Haptics.
	
	\item Programmer's Guide, explains the OpenHaptics toolkit and introduces the architecture of the toolkit.
	
	\item API Reference, contains references pages to all the QuickHaptics, HDAPI and HLAPI functions.
	
	\end{itemize}		

	{%The QuickHaptics quick and easily writes applications or adds haptics to existent applications. The fact that has a built-in geometry parsers and intelligent parameters makes possible to create haptic scenes with small code. The HDAPI provides low-level access to the haptic device and enables haptic programmers to render forces directly. Finally, the HLAPI provides high-level haptic rendering and is designed to be familiar to the OpenGL API programmers, simplifying haptics and graphics threads.}
	\paragraph{} The typical application structure under the OpenHaptics is illustrated in Figure \ref{Quick}. The relation between the QuickHaptics micro API and the existing HD and HL layers is shown in Figure \ref{Monitor}.}

	\begin{figure}[H]
	\centering 
	\includegraphics[width=0.9\textwidth]{imgs/Chapter2/Quick.png}
	\caption{OpenHaptics overview \cite{ProgrammersGuide}.}
	\label{Monitor}
	\end{figure}
	
	\begin{figure}[H]
	\centering 
	\includegraphics[width=0.7\textwidth]{imgs/Chapter2/Monitor.png}
	\caption{Relationship between QuickHaptics, HD and HL layers of OpenHaptics \cite{ProgrammersGuide}. }
	\label{Quick}
	\end{figure}