\chapter{Introduction}\label{chapter:intro}
\thispagestyle{myheadings}

The idea of an autonomous human-like robot has been a dream of some for a long time. Introduced by science fiction, the idea that mankind can some day create a fully functional lookalike of humans to do any task too dangerous or too monotonous has inspired many people. It is thought that this achievement will give mankind a chance to pursue other endeavours otherwise not attainable. So far, no such robotic piece has come together despite all the scientific advances in the last 50 years.



The humanoid robotics field gives engineers a great challenge in an wide range of areas, such as biomechanics, physics, electronics and mechanical. With each area, comes an array of problems. One of the main problems is the motion control. Due to the parallel robotics, there no model for motion of a humanoid robot and the found solutions are based in pre-programmed routines. Contrasting with these, the robot learning from demonstration algorithms give the possibility to have robotic systems with learning capabilities and able to adapt to new environments. To study and implement this methods a structured framework is needed. The work here presented tries to introduce a solution for these problems with a haptic interface combined with a modular programming structure. 

%Although there has been very incredible advances in humanoid robots, it is still a problem to implement a mathematical model to control so many degrees of freedom. This makes controlling this type of robots very hard and computational time consuming. Some robotic models were applied to different robots with various success rates in controlled environment with pre-programmed routines.

%!!!!! GIVE EXAMPLES !!!!!

% -------------------------------------------------------------
% motivation
% -------------------------------------------------------------
\section{Motivation}\label{ssec:intro_motivation}

This work is the preliminary stage of a long term goal to teach auto-balancing and walking motion with a learning from demonstration algorithm. The idea is to move the robot in different stages of equilibrium while using haptics to transmit its state back to the operator. The force feedback can be generated by different sensory data (inertial, center of pressure, tilting,etc.), just like humans. The robot's stability is deduced with this data and felt by the operator with the force feedback. 

By recording all the robot's information (state of joints, center of pressure, haptic device kinematics, etc.) during those episodes, the PHUA project believes that it is possible to use a learning from demonstration algorithm. After which the robot knows how to perform such equilibrium and motion operations without the operator's control.



% -------------------------------------------------------------
% objectives
% -------------------------------------------------------------
\section{Objectives}\label{ssec:intro_objectives}

The objectives of this work are to create a software and hardware basis to work on the PHUA robot's equilibrium and stability.

This is achieved by sensing the floor in with the robot is standing and with that information calculate the vertical projection of the center of gravity , also know as center of pressure (COP). The state of the COP is then transformed into force feedback to be used in the haptic device that also controls the movement of the robot.

The main objectives of this work are presented in the bullet points below.

\begin{itemize}
 \item Implement the ROS (Robot Operating system) platform into the project
 \item Develop a data acquisition unit for the load cells
 \item Link the load cells data with the haptic force feedback
 \item Develop a control system for the robot with haptics
 \item Validate the methodology by performing balancing experiences
 \item Record all relevant data from the experiences
\end{itemize}

The diagram in Figure \ref{fig:main_problem} shows the main problem for this project.


%%% ******** TIKZ DEFINITIONS ******** %%%
% Define styles for the objects
\tikzstyle{block1} = [draw, fill=lightgray!40, rectangle,thick, minimum height=3em, minimum width=6em, text centered, text width=3cm]
\tikzstyle{block2} = [draw, fill=lightgray!40, circle ,minimum size=2.5cm, thick, text centered]
\tikzstyle{block?} = [draw, fill=lightgray!20, diamond, thick, minimum height=3em, minimum width=3em, text centered, text width=1.25cm]
\tikzstyle{bit} = [minimum width=0.5cm, minimum height=1.5em, outer sep=0pt, draw=black, semithick]
\tikzstyle{textonly} = [minimum width=0.5cm, minimum height=1.5em, semithick]
%%% ******************************** %%%


\begin{center}
\begin{tikzpicture}[auto, node distance=2.5cm,>=latex']
    % We start by placing the blocks
    \node [block1, left, name=user] {\textbf{User}};
    
    \node [block1, right of=user, node distance=4.5cm, name=device] {\textbf{Haptic Device}};
    
    \node [block1, below of=device, name=robot] {\textbf{Robot}};
    
    \node [block1, right of=robot, node distance=2cm, below of=robot, xshift=9mm, name=environment] {\textbf{Environment}};
    
    \node [block1, below of=robot,node distance=4cm, name=cells] {\textbf{Load Cells}};

    	
    \path[draw,<->, very thick] (user.east) -- (device.west);
    
    \path[draw,->, very thick] (device.south) -- (robot.north);
    
    \path[draw,->, very thick](cells.west) edge [bend left=35] (device.south west)
    
    					(environment.south) edge [bend left=20] (cells.east)
    					 
   						(environment.north) edge [bend right=20] (robot.east)
    
    					(robot.south) -- (cells.north);
        
\end{tikzpicture}
\captionof{figure}[Main schematic]{Schematic of the main parts for this work.}
\label{fig:main_problem}
\end{center}

The user handles the haptic device to control the robot while is sensing the force feedback. The external interactions (gravity, friction, external forces, tilted planes, etc.) influence the robot and the load cells. These give the information about the robot's equilibrium and in turn activate the force feedback.

\FloatBarrier

\section{Thesis guide}\label{ssec:intro_walkthrough}

The thesis is organized by chapters in the fallowing manner: the rest of this Chapter gives the reader an explanation about the past work done on the PHUA and its robot since the beginning of the project. It also shows how the haptic device is incorporated in that work. The Chapter \ref{chapter:setup} has two parts: the first describes the chosen approach for the signal acquisition from the pressure cells and all the other hardware requirements for this work. The second explains the structure for the software structure used in this work and its possibilities. The Chapter \ref{chapter:tele-operation} explains how the modules created work and how they interact with the materials presented in Chapter \ref{chapter:setup}. Chapter \ref{chapter:demo} presents the demonstrations done to validate this work and its results. The final chapter discuss the main points of this thesis, presents its conclusions and proposes future work hypothesis.

\FloatBarrier
\section{Framework}\label{sec:intro_framework}

% +++++++++++++ HAPTIC INTERFACES +++++++++++++
\subsection{Haptic device}\label{subsec:phua_haptic}

The haptic device this work uses is the PHANTOM OMNI by Sensable, shown is Figure \ref{fig:phantom_1}. The OMNI device is a 6 degrees of freedom (DOF) impedance-based joystick able to render three-dimensional forces at high frequencies. This device uses a proprietary library provided by the manufacturer, explained in Section \ref{subsec:setup_haptic}. 

Even though the device has 6 DOF, due to it's mechanical construction only the first three joints are actuated. This means that it is only possible to render force vectors and not toques since the last three DOF's are not actuated. These last joints possess potentiometers that give out their positions resulting in the gimbals angles of the tip. The position is given by high precision optical encoders that are coupled at the base's actuators.

The specifications of the PHANTOM OMNI device are shown in Table \ref{table:phantom_specs}. The workspace where it's capable of exerting its nominal maximum force is quite small, so when using the force feedback to command the robot a scale factor needs to be used. Outside of this workspace, the forces magnitude and directions are not guaranteed to be the ones sent to the device. The apparent mass at the tip is a useful data for gravity compensation.

\begin{figure}
  \centering
  \includegraphics[height=50mm]{Imagens/LargePHANTOMOmniImage.pdf}
  \captionof{figure}[ PHANTOM OMNI by Sensable]{ PHANTOM OMNI by Sensable haptic device \citep{sensable_phntm_2008}.}
  \label{fig:phantom_1}
\end{figure}


\begin{table}
\caption[PHANToM OMNI device specifications]{PHANToM OMNI device specifications \citep{sensable_phntm_2008}.}
\label{table:phantom_specs}
\centering
\begin{tabular}{c|c}
\begin{tabular}{c}
\textbf{Force Feedback}\\
\textbf{Workspace}\\
\end{tabular}
&
\begin{tabular}{c}
160 W X 120 H X 70 D [mm]\\
\end{tabular}\\ 
\hline
\begin{tabular}{c}
\textbf{Footprint}\\
(Physical area the base\\
 of the device occupies)\\
\end{tabular}
&
\begin{tabular}{c}
168 W x 203 D [mm]\\
\end{tabular}\\
\hline
\begin{tabular}{c}
\textbf{Weight}\\
(device only)\\
\end{tabular}
&
\begin{tabular}{c}
3 lb 15 oz\\
(aprox. 1.786 kg)\\
\end{tabular}\\
\hline
\textbf{Range of Motion} & 
\begin{tabular}{c}
Hand movement\\
pivoting at wrist\\
\end{tabular}\\
\hline
\begin{tabular}{c}
\textbf{Nominal}\\
\textbf{Position Resolution}\\
\end{tabular}
& 
\begin{tabular}{c}
450 dpi\\
(aprox. 0.055 mm)\\
\end{tabular}\\
\hline
\begin{tabular}{c}\textbf{Backdrive}\\\textbf{Friction}\end{tabular} & 0.26 N\\
\hline
\begin{tabular}{c}
\textbf{Maximum}\\
\textbf{Exertable Force}\\
(at nominal position)\\
\end{tabular}
&
\begin{tabular}{c}
3.3 N\\
\end{tabular}\\
\hline
\begin{tabular}{c}
\textbf{Continuous}\\
\textbf{Exertable Force}\\
(24 hrs.)\\
\end{tabular}
&
\begin{tabular}{c}
0.88 N\\
\end{tabular}\\
\hline
\textbf{Stiffness} &
\begin{tabular}{c}
X axis : 1.26 N/mm\\
Y axis : 2.31 N/mm\\
Z axis : 1.02 N/mm\\
\end{tabular}\\
\hline
\begin{tabular}{c}
\textbf{Inertia}\\
(apparent mass at tip)\\
\end{tabular} &
\begin{tabular}{c}
45 g\\
\end{tabular}\\
\hline
\begin{tabular}{c}\textbf{Force}\\\textbf{Feedback}\end{tabular} & x, y, z\\
\hline
\begin{tabular}{c}
\textbf{Position Sensing}\\
(with stylus gimbal)\\
\end{tabular}
&
\begin{tabular}{c}
- x, y, z\\
(digital encoders)\\
- roll, pitch, yaw\\
($\pm$5\% linearity potentiometers)\\
\end{tabular}\\
\hline
\textbf{Interface} &
\begin{tabular}{c}
IEEE-1394 FireWire port\\
(6-pin to 6-pin)\\
\end{tabular}
\end{tabular}
\end{table}

% +++++++++++++ END OF HAPTIC INTERFACES +++++++++++++

\FloatBarrier

% +++++++++++++ PHUA +++++++++++++
\subsection{Humanoid Project of the University of Aveiro (PHUA)}\label{subsec:intro_phua}

This project was started in 2003/2004 in a joint effort between the Mechanical Engineering Department (DEM) and the
Electronics, Telecommunications and Informatics Department (DETI) to create a humanoid platform suitable for research in control, perception, navigation and behaviour. It's main objective is to develop and integrate both software and hardware in a functional low-budget platform.

The first model was finish in 2008 with several thesis and publish papers during those 4 years of development. The platform consisted in a humanoid robot with 22 DOF weighting 6 Kg with its 9600 mA 7.4 V batteries and with an approximated height of 60 cm. Assisted with a PC-104 connected with a CAN network by RS-232 protocol, this platform was capable of identifying and following objects with its head and performing several exercises with its legs \citep{ferreira_desenvolvimento_2007, rodrigues_unidade_2008, silva_desenvolvimento_2006}.



In 2009, the project took another important step and a new platform was developed. The mechanical structure was redesigned and new actuators and sensors were chosen. The new platform has 27 DOF, visible in Figure \ref{fig:phua_front}. \textit{HITEC\textsuperscript{\textregistered}} digital and analogue servomotors actuate 25 of the joints which are arranged as in Figure \ref{fig:phua_dof} with the servomotors assembled as in Table \ref{table:phua_dofs}. When assembled it weights about 6$\kg$ and has an approximated height of 65$\cm$ \citep{ cruz_haptic_2012, godinho_desenvolvimento_2011, lage_algoritmos_2011, ribeiro_desenvolvimento_2010, sabino_estrutura_2009}.

%\begin{figure}[H]
%\begin{minipage}{.45\textwidth}
%    \centering
%  \includegraphics[height=100mm]{Imagens/phua_front_p.pdf}
%  \captionof{figure}{PHUA robot, front view \citep{ cruz_haptic_2012}.}
%  \label{fig:phua_front}
%\end{minipage}
%
%\begin{minipage}{.45\textwidth}
%    \centering
%  \includegraphics[height=100mm]{Imagens/phua_dof.pdf}
%  \captionof{figure}{PHUA robot degree's of freedom \citep{ cruz_haptic_2012}.}
%  \label{fig:phua_dof}
%\end{minipage}
%\end{figure}

\begin{figure}[H]
\begin{minipage}[b]{0.45\linewidth}
\centering
\includegraphics[width=0.92\textwidth]{Imagens/phua_front_p.pdf}
\captionof{figure}{PHUA robot, front view \citep{ cruz_haptic_2012}.}
\label{fig:phua_front}
\end{minipage}
\hspace{0.5cm}
\begin{minipage}[b]{0.45\linewidth}
\centering
\includegraphics[width=0.92\textwidth]{Imagens/phua_dof.pdf}
\captionof{figure}{PHUA robot degree's of freedom \citep{ cruz_haptic_2012}.}
\label{fig:phua_dof}
\end{minipage}
\end{figure}

\renewcommand{\arraystretch}{1.1}
\begin{table}[H]
    \centering
	\captionof{table}{PHUA platform's degrees-of-freedom and installed servomotors.}
	\label{table:phua_dofs}
	\begin{tabular}{c|c|c}
	\hline
	\textbf{Joint}&\begin{tabular}{c}\textbf{Number of}\\\textbf{DOFs}\end{tabular}&\textbf{Servomotor}\\
	\hline
	Toe & $1 (\times2)$ & (none)\\
	%\hline
	Ankle & $2 (\times2)$ & \textit{HSR-5980SG}\\
	%\hline
	Knee & $1 (\times2)$ & \textit{HSR-5980SG}\\
	%\hline
	Hip & $3 (\times2)$ & \textit{HSR-5980SG}\\
	%\hline
	Trunk & $3$ & \begin{tabular}{c}\textit{HSR-5980SG}$(\times2)$\\
	\textit{HSR-8498SG}$(\times1)$\end{tabular}\\
	%\hline
	Shoulder & $3 (\times2)$ & \begin{tabular}{c}\textit{HSR-5980SG}\\
	\textit{HSR-5498SG}\\
	\textit{HS-82MG}\end{tabular}\\
	%\hline
	Elbow & $1 (\times2)$ & \textit{HSR-5498SG}\\
	%\hline
	Neck & $2$ & \begin{tabular}{c}\textit{HSR-5498SG}\\
	\textit{HS-5056MG}\end{tabular}\\
	%\hline 
	Total & 27 & ---\\ \hline
	\end{tabular}
\end{table}



The servomotors possess a proprietary protocol that make use of RS-232 called \textit{Bidirectional Serial Interface}. This works with a 7 byte frame, represented in the Table \ref{table:setup_servocomm_bsi2}, where the first 5 are the command, parameter and checksum. The last 2 bytes are placed by the servomotor responding to the command \citep{ cruz_haptic_2012}.


\begin{table}[H]
\centering
\caption[The HITEC BSI RS-232 7 byte word]{The HITEC \textit{BSI} RS-232 7 byte word \citep{ribeiro_desenvolvimento_2010}.}
\begin{small}
\begin{tabular}{c|c|c|c|c|c|c|c}
\hline
\textbf{byte} & 1 & 2 & 3 & 4 & 5 & 6 & 7\\ 
\hline
\textbf{Controller} & Startbyte & Command & Param1 & Param2 & Checksum & 0x00 & 0x00\\
\hline
\textbf{Servomotor} & \multicolumn{5}{|c|}{High-Impedance} & Return1 & Return2\\
\hline
\end{tabular}
\end{small}
\label{table:setup_servocomm_bsi2}
\end{table}

Each servomotor has a PD controller based in position, ranging from 600 to 2400, with speed input, integer values between 1 and 255. Any one of the servomotors is identified by a 2 digit number setted with a HITEC supplied software \citep{ cruz_haptic_2012}.

Part of the locomotion system of the robot is the elastic elements present in the lower limbs and torso. These help store potential energy to be dispensed in a later phase of the gait. Rubber bands are placed in some of the lower limbs's joints. Although they keep the servomotors in strain these elastic elements ensure the stability and return to the initial pose \citep{cruz_haptic_2012, santos_development_2010, santos_mechatronic_2012}.




In order to provide the correct binary to the different joints and reduce the strain on the actuators's transmission axis belted transmission were used to transmit power to the joints of the legs and torso. Belt tensioners keep the transmission stable and allow for adjustments.

The PHUA platform is also equipped with 8 pressure sensors in its feet, that will be further analized in chapter \ref{chapter:setup}, and 2 \textit{FireFly\textsuperscript{\textregistered}} \textit{FireWire} cameras for the artificial vision system.


The last work done in this project introduced the concept of tele-kinesthethic teaching. The idea is junction between teleoperation and learning by demonstration. The developed haptic interface used force feedback for the operation of the robot. This interface tries to improve kinesthethic teaching through an haptic environment of the robot's surroundings and it's own restrictions \citep{ cruz_haptic_2012}. 

The data recorded during such operations can be ran through a robot learning from demonstration algorithm, without the need to use complex dynamic models. This methods usually relies on a teleoperation system or a vision or motion sensors with the user's recording of the task. A haptic interface can change the paradigm on this issue. With the ability to 'feel' the robot's state, the user can dynamically adapt to changes in the environment or to the robot's restrictions. Using the user's reactions to the force feedback allows the creation model-free control system \citep{cruz_tele-kinesthetic_2012}. Before the robot learning from demonstration can be applied, a number of steps have to be taken and this work is the start of those steps.




\FloatBarrier