%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\chapter{The Haptic Tele-operation Paradigm}

{This chapter describes the haptic tele-operation approach for teaching balance skills to humanoid robots through force feedback. The intention is to include the human teacher in-the-loop by providing him/her with state feedback on the robot's performance. Based on the evaluation of performance, the human teacher may provide functional guidance and corrections on the robot's behaviour. In this context, haptics tele-operation refers to an active and bidirectional interface between the human teacher and the robot learner. The bilateral nature of the implemented interface enables the human teacher to provide motion commands to the robot and, simultaneously, to receive force feedback about the robot's current state. \paragraph{} Figure \ref{tudo1} illustrates the control architecture of the haptic tele-operation system and the interconnection among the main hardware components. One of the main computational components of the interface is responsible for the kinematic mapping between the DOFs of the haptic device to those of the humanoid robot. The second important component is related with the strategy for translating information about the robot's state into force feedback, usually referred as the force rendering problem. It becomes challenging to adopt the best strategies for translating information about the state of the robot into force feedback. Here, the goal is to provide the human teacher with the right perception of what is happening to the robot in terms of balance state.\paragraph{} Another dimension of the problem is the need for a training phase. The human teacher needs training with respect to what force rendering algorithms are most effective, what sensorial information accurately reflects the real-word dynamics and how the robot interprets the human intention. Force feedback from the environment, performance metrics to help the human teacher understand the level of improvement achieved and the repetition over time are fundamental factors that will contribute to simplify the learning process and/or to improve the overall system's performance. Despite the importance of a training phase with multiple users, the study of the interface usability is missing since it is beyond the scope of this work.\paragraph{} In summary, this chapter is divided into two distinct parts. The first part presents the data processing algorithms implemented to transform commands from the human teacher into control actions applied to the robot. The second part is dedicated to analyze the data obtained from the force sensors located in the feet of the humanoid robot and how they are transformed into force feedback.}
	
%-------------------------------------------------------%	
	\begin{figure}[H]
		  \centering 
		  \includegraphics[width=0.85\textwidth]{imgs/Chapter3/InterCon1.png}
		 \caption[The haptic loop of the tele-operation system]{The haptic loop of the tele-operation system. The haptic device senses the human operator input, such as position and orientation, and the system applies the input to the tele-operated humanoid robot. The response of the humanoid robot is computed through haptic rendering and relayed to the human operator. The ideal result is that the human operator feels that he/she is interacting directly with the humanoid robot. Depending on the haptic interface and application, this loop must be repeated a few hundreds of Hertz or higher frequencies around 1 kHz.}
		 \label{tudo1}
		\end{figure}
\clearpage
\section{Pre-processing of joystick raw-data}

\subsection{Haptic Workspace and Force Limits}

	
	%{The Phantom Omni device is composed by 6 \acs{DOF} (see chapter 2 section 2.1.3) where the first 3 \acs{DOF} give the Cartesian position and each position component has its own limit.
		% Using the Open Haptics library\cite{Phantom} is possible to discover the limits of the device. This limits and the corresponding axis in the Phantom Omni device are shown in Table \ref{Dim2} and Figure \ref{Dim1} respectively.} 
	{The haptic device used in this work is a SensAble PHANToM Omni (more recently Geomagic Touch) joystick popularized by haptic applications in virtual reality \cite{ThomasHMassie}. The Omni is a pen-style haptic device, with sensors and actuators, based on a 6-DOF serial mechanism, providing information about the position (the first 3-DOF) and the orientation (the last 3-DOF) of its end-effector. In addition to sense the stylus motion in six DOF, this haptic device can apply forces in the x, y and z directions to the stylus tip (Figure \ref{Dim1} shows the reference coordinate system). In the bilateral interface, motion becomes the output from the user and force becomes the input to the user (\textit{i.e.,} the teleoperation system provides information to and requires commands from the user).}
%------------------------------------------------%
		
		\begin{figure}[H]
		  \centering 
		  \includegraphics[width=0.6\textwidth]{imgs/Chapter2/PhantomAxis.png}
		 \caption{Phantom Omni device with the Cartesian axis}
		 \label{Dim1}
		\end{figure}
		
		{ Given the physical limits of the rotational joints and the link lengths of this haptic device, the workspace available for the operator is also limited. Tables \ref{Dim2} and \ref{PhantomOri2} depict the limits in the translational and rotation motions, respectively. The limits can also be found using the function \textit{hdgetDoublev} from the Open Haptics library \cite{Phantom}.}
		
		\begin{table}[htbp]
		\small
		  \begin{center}%
		    \begin{tabular}{|c|c|c|c|}\hline
		       $$ & $x (mm)$ & $y(mm)$ & $z(mm)$\\ \hline
		       $Maximum$ & $210$  & $205$ & $130$ \\
		        $Minimum$ & $-210$ & $-110$ & $-85$\\ \hline
		  	 \end{tabular}$\qquad$%$
		    \caption{Maximum workspace dimensions}%
		    \label{Dim2}%
		  \end{center}%
		\end{table}%
		
		\clearpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
%------------------------------------------------%	
		
		%{The last 3 \acs{DOF} represent the orientation (see Figure \ref{Phantom1}), and each one controls a separate orientation. Table \ref{PhantomOri2} show the limits of this device orientation.}
		
%-----------------------------------------------%
		
		\begin{table}[htbp]
		\small
		  \begin{center}%
		    \begin{tabular}{|c|c|c|c|}\hline
		       $$ & $J4 (mm)$ & $J5(mm)$ & $J6(mm)$\\ \hline
		       $Maximum $ & $-0,765$  & $-1,864$ & $-0,562$ \\
		        $Minimum $ & $-5,534$ & $-4,186$ & $-5,723$\\ \hline 
		  	 \end{tabular} $\qquad$%$
		    \caption{Maximum Orientation workspace}%
		    \label{PhantomOri2}%
		  \end{center}%
		\end{table}%
		
%-----------------------------------------------%

%      {To ensure a better understanding and control of the joystick for a future work, an offset of PI was added to the initial values (see Table \ref{PhantomOri3} and \ref{PhantomOri2}). The new values were those considered for the rest of the work. }
%  
% %----------------------------------------------%
%      
%      \begin{table}[htbp]
%	  \begin{center}%
%	    \begin{tabular}{|c|c|c|c|}\hline
%	       $$ & $J4 (mm)$ & $J5(mm)$ & $J6(mm)$\\ \hline
%	       $Maximum$ & $2,384$  & $0,072$ & $-2,628$ \\[3mm]
%	       $Medium$ & $-0.009$ & $0,001$ & $-0,012$\\ [3mm]
%	        $Minimum$ & $2,578$ & $-0,083$ & $-2,578$\\ \hline 
%	  	 \end{tabular}$\qquad$%$
%	    \caption{Orientation maximum workspace with offset}%
%	    \label{PhantomOri3}%
%	  \end{center}%
%	\end{table}%
%      
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


{Besides the 6-DOF input, the Omni device provides 3D force feedback in position. To accurately render the robot's state, the haptic device must be able to render forces over a large dynamic range, in both frequency and magnitude. The maximum and the continuous force allowed for the Omni device are presented in Table \ref{PhantomForce1} (values available by the same function \textit{hdgetDoublev}). Although the device allows the application of forces of 3.3 N, it can only support a continuous exertable force (24 hrs.) of 0.88 N (a permanent damage may occur if the value is exceeded).}  
%This device has a peculiar feature, force feedback. Using the library \texttt{Open Haptics} is possible to know the maximum force and maximum continuous force allowed by the device (see Table.\ref{PhantomForce1}).}

%------------------------------------------------%
	
		\begin{table}[htbp]
		\small
		  \begin{center}%
		    \begin{tabular}{|c|c|c|}\hline
		       $$ & $Maximum (N)$ & $Continuous(N) (24 hrs)$\\\hline
		       $Force$ & $3.3$  & $0.86$ \\ \hline
		  	 \end{tabular}$\qquad$%$
		    \caption{Maximum and Continuous force permitted by the device }%
		    \label{PhantomForce1}%
		  \end{center}%
		\end{table}%
		
%------------------------------------------------%

		
		%{Despite the fact that the device allows the application of 3.3 N of force, it can only support a continuous force of 0.86 N. The application of more than that can lead to permanent damage.}
		{In addition to the large difference in values, another dimension of the problem is the lack of uniformity throughout the target workspace. As a result, the usable workspace for the haptic device with force feedback is reduced to the values shown in Table \ref{PhantomForce2}}
		
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%{The effect of the force feedback has its limitations. Although the force values are the ones shown in table \ref{PhantomForce1}, these values are not true to all the workspace. The usable workspace (\textit{i.e.,} space where the force can be guaranteed) is considerably smaller (see Table \ref{PhantomForce2})}
	
%------------------------------------------------%
	
	\begin{table}[htbp]
	\small
	  \begin{center}%
	    \begin{tabular}{|c|c|c|c|}\hline 
	       $$ & $x (mm)$ & $y(mm)$ & $z(mm)$\\ \hline
	       $Maximum$ & $ 80$ & $ 60$ & $ 35$ \\
	       $Minimum$ & $-80$ & $-60$ & $-35$\\ \hline 
	  	 \end{tabular}$\qquad$%$
	    \caption{Usable workspace of the device}%
	    \label{PhantomForce2}%
	  \end{center}%
	\end{table}%
	
%---------------------------------------------------------------------------------%


	
\subsection{Data Filtering}
%{The humanoid robot is a complex machine,so the noise attached to the data from the haptic joystick can result in severe loss of control and irreversible damage. Another problem is the fact that human hand is not completely accurate. After a tele-operation example all the data was stored using the rosbag tool and utilized for filtering tests using Matlab.}

{Using a haptic joystick as the input device, there are two different modes for describing the desired robot motion: position control and velocity control. Position control mode means that the joystick displacement is interpreted as a desired position. This is typical for controlling stationary robots with a limited workspace. In velocity mode, the joystick data is interpreted as a desired velocity imposed to the robot. Since the intention is to control a stationary robot with a limited workspace, position control makes more sense. Thus, the user operation on the joystick is transmitted to the humanoid robot in the form of a position command.\paragraph{} However, the joystick raw-data may be unsuited if directly transferred into control actions to be applied to the humanoid robot. Therefore, user control in position must be filtered to smooth the actions sent to the robot controller. For the experiments carried out, two types of data filtering were implemented. First, low-pass filtering was used to remove human's hand vibrations or instability when holding the joystick. 
Figure \ref{dataFil1} illustrates the result of applying a moving average filter on the data acquired with the joystick and after processed in Matlab. 
}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	

	%\subsubsection*{Position }
	
	%{The Cartesian position is the principal set of data published by the device. This position is divided into 3 distinct components (x, y and z). Firstly, a filter was applied to remove the noise. The chosen filter was a moving mean filter. The filter was tested in Matlab and the results can be seen in Figure \ref{dataFil1}. }
	
%-------------------------------------------------------%

	
%	\renewcommand\lstlistingname{Algorithm}
%	\begin{lstlisting} 
%	 function MA=moving_average(V,winSize) %winsize= lag for moving average
%	   		MA=zeros(size(V));
%	   		for n=1:size(V,2)
%	   			MA(:,n)=mean(V(:,max(1,n-winSize):n),2);
%	   		end
%	  	 end
%	  	 
%	\end{lstlisting}
	
	\begin{figure}[H]
		  \centering 
		  \includegraphics[width=1.2\textwidth]{imgs/Chapter3/Filtro/DataM.eps}
		 \caption{Original raw-data vs. filtered-data.}
		  \label{dataFil1}
		\end{figure}
		
{Second, path filtering was used to impose specific paths such as up-down, front-back or left-right. This apparent constrain reflects, in general, more precisely the human intention in teleoperation applications, especially when a hybrid solution is considered (\textit{i.e.,} the filter is only applied to certain components of the movement, but not others). A second advantage can be envisioned during operation whenever the user is exposed to abrupt movements of the humanoid platform. Whenever these perturbations are passed to the driver's hand via force feedback, they could cause unintentional control pulses that can be mitigated by filtering the joystick raw-data.\paragraph{} In this work, this second part of the data processing consists of applying an arctangent function to the data in each individual plane (\textit{i.e.}, x-y, x-z and y-z planes). Once the quadrant is known, the movement is modified according to an external parameter provided a priori by the user. For example, accepting from the user only limited slope resolutions in multiples of 90$\degree$. In this particular case, the final movement is restricted to occur along the x-, y- and z-axis. Figure \ref{DataFilt1} shows the result of several tests performed in 3D space with different resolutions of slopes in the trajectories. It should be noted that for resolution angles lower than $\frac{\pi}{6}$ the difference between the original raw-data and the filtered-data is negligible. }
	%{The problem of the noise in each component was eliminated with the filter above, but the second problem remains. Although there is no noise in the data, there is no certainty that the data published to control the robot results in fact in the desire movement. The main reason for this to happen is that the human operator cannot make a straight move along principal directions (x, y and z). \paragraph{} The second part of data processing consists in passing the data into an arctangent function and thus know in which quadrant the movement is being done. Once the quadrant is known, this movement will be changed to the closest possible angle. This angle value is an external parameter in radians chosen by the operator. For example, if the parameter as the value of $\frac{PI}{2} (90\degree)$ the final movement will be done only vertically and horizontally, each component will have $90\degree$ relationship. If the chosen parameter is $45\degree$ for example, each component will have a relationship of $90\degree$ or $45\degree$ and so on.\paragraph{} In Figure \ref{DataFilt1} , three examples of how this filter works will be presented. }
	
%------------------------------------------------------%
	
	\begin{figure}[H]
		\centering
		\begin{subfigure}{0.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Filtro/normalpi2.eps}
		  \caption{3D Plan for a parameter of $\frac{PI}{2} (90\degree)$}
		  \label{sub1}
		\end{subfigure}%
		\begin{subfigure}{0.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Filtro/normalpi2plano.eps}
		  \caption{XY Plan for a parameter of $\frac{PI}{2} (90\degree)$ }
		  \label{sub2}
		\end{subfigure}
		\begin{subfigure}{0.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Filtro/normalpi4.eps}
		  \caption{3D Plan for a parameter of $\frac{PI}{4} (45\degree)$}
		  \label{sub3}
		\end{subfigure}%
		\begin{subfigure}{0.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Filtro/normalpi4plano.eps}
		  \caption{XY Plan for a parameter of $\frac{PI}{4} (45\degree)$}
		  \label{sub4}
		\end{subfigure}
		\begin{subfigure}{0.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Filtro/normalpi6.eps}
		  \caption{3D Plan for a parameter of $\frac{PI}{6} (30\degree)$}
		  \label{sub5}
		\end{subfigure}%
		\begin{subfigure}{0.5\textwidth}
		  \centering
		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Filtro/normalpi6plano.eps}
		  \caption{XY Plan for a parameter of $\frac{PI}{6} (30\degree)$}
		  \label{sub6}
		\end{subfigure}
		\caption{Original Data Vs Filtered Data , XY and 3D plan}
		\label{DataFilt1}
		\end{figure}
	
	{Another perspective of the same problem is depicted in Figure \ref{DataTime} where the time courses of the three coordinates are represented. The filtered-data (dotted curves) are now limited to principal directions, being visible that they are affected by time delay.}
	
	\begin{figure}[H]
		\centering
		\begin{subfigure}{1\textwidth}
		  \centering
		  \includegraphics[width=0.76\linewidth]{imgs/Chapter3/Filtro/Time1.eps}
		  \caption{Coordinate variation over time for a parameter of $\frac{PI}{2} (90\degree)$}
		\end{subfigure}
		\begin{subfigure}{1\textwidth}
		  \centering
		  \includegraphics[width=0.76\linewidth]{imgs/Chapter3/Filtro/Time2.eps}
		  \caption{Coordinate variation over time for a parameter of $\frac{PI}{4} (45\degree)$.}
		\end{subfigure}
		\begin{subfigure}{1\textwidth}
		  \centering
		  \includegraphics[width=0.76\linewidth]{imgs/Chapter3/Filtro/Time3.eps}
		  \caption{Coordinate variation over time for a parameter of $\frac{PI}{6} (30\degree)$.}
		\end{subfigure}
		\caption{Original Data Vs Filtered Data over time.}
		\label{DataTime}
		\end{figure}
	
	%{This filter made possible to move in all directions, depending on the desire of the operator. For angle values smaller than $\frac{}{6} (30\degree) $ the differences between the original data and filtered data are nearly invisible. With this filter is now possible to guarantee that the information that will be sent to control the humanoid robot is the desirable one.}
	
	
	% Aqui comeca a parte on line %%%%%%
	
	{After the \texttt{MATLAB} experiments, the next step is to convert and test all the code in ROS/C++ environment. A Rviz application was developed to test all the code before using it in ROS/C++ environment. The Rviz tool has the ability to create markers (see Chapter 2 section Robot Operating System). In this case, the marker chosen was a Cube whose position is directly linked to the information coming from the Phantom Omni joystick (see Figure \ref{RvizT1}). It was possible to understand that the movement was properly modified by changing the joystick's position and analyzing the outcome in Rviz. }

	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.9\textwidth]{imgs/Chapter3/Filtro/RvizT.png}
	 \caption{Example of the used Rviz interface.}
	 \label{RvizT1}
	\end{figure}
	
	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
	%\subsubsection*{Orientation}
	%{Besides the 3 \acs{DOF} corresponding to the Cartesian Position, the Phantom Omni device has 3 additional \acs{DOF} that can be used if necessary to control the orientation of the Humanoid Robot.\paragraph{} Similarly to the Cartesian Coordinates, the orientation coordinates also have noise. Therefore, a moving mean filter was applied. Is visible in Figures \ref{JFilt1}, \ref{JFilt2} and \ref{JFilt3}, the filter effect in the original data.} 
	{This data filtering is mostly concerned with position data, but is also used with the orientation as well. The result of applying a moving average filter to the raw-data from the last three DOF (orientation of the tip) is shown in Figures \ref{JFilt1} to \ref{JFilt3}. The orientation was tested using the Rviz application. After testing the different rotations of the cube, it has been proven that the use of all the 3 \acs{DOF}s at the same time is difficult and can lead to continued instability. Therefore, for further work only one \acs{DOF} can be active at the same time. The active one is determined by calculating the Standard deviation of 200 orientation points, the degree in the highest value is considered the dominant. }

	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.7\textwidth]{imgs/Chapter3/Filtro/j4all.eps}
	 \caption{Joint 4 original vs filtered data.}
	 \label{JFilt1}
	\end{figure}
	
	% Filter J5
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.7\textwidth]{imgs/Chapter3/Filtro/j5all.eps}
	 \caption{Joint 5 original vs filtered data.}
	 \label{JFilt2}
	 \end{figure}
	
	% Filter J6
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.7\textwidth]{imgs/Chapter3/Filtro/j6all.eps}
	 \caption{Joint 6 original vs filtered data.}
	 \label{JFilt3}
	\end{figure}
	
	%{Although in these examples the differences between the original data and filtered data may not be so obvious, if the noise is more significant the attenuation will be noticeable.}
	
% Aqui começa a parte online  %%%%%%%%%%
	
	%{The orientation was tested using the Rviz application. After testing the different rotations of the cube, it has been proven that the use of all the 3 \acs{DOF}s at the same time is difficult and can lead to continued instability. Therefore, for further work only one \acs{DOF} can be active at the same time. The active one is determined by calculating the Standard deviation of 200 orientation points, the degree in the highest value is considered the dominant. }
	

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Kinematic Mapping between Haptic Device and Humanoid Robot}
	%{The humanoid robot is controlled from the filtered data arriving from the haptic device. In order to provide a natural and intuitive interaction with the real humanoid robot, it is necessary to define the best way to map the DOFs of the haptic device to those of the robotic system. The implemented mode assumes that the human operator controls the robot's hip position and there is a correspondence between the 0 position of the joystick and the robot's initial position (legs straight), due to that all positive values from the haptic joystick in the Z axis are ignored. In other words, the location of the inverse kinematics algorithm is used for translating the haptic commands into robot actions (joint angles). The kinematics equations describing the relation between the Cartesian and the joint spaces of the lower limbs are presented in Appendix C.\paragraph{} The angle values obtained by the kinematics algorithm need to be transformed into a range of values between 600 and 2400, before being sent to the HITEC servomotors that include internal PID position controllers that assures a maximum position error of 1$\degree$. The communications between the host computer and the servomotors are performed via a RS-232 bidirectional serial interface. The RS-232 communication parameters and the structure of the data frame are depicted in Table \ref{Comm1} and \ref{Comm2}, respectively. The communication data frame is composed by seven bytes, where the first five are the start byte, the command, two command's parameters and the checksum, and the last two are used to get a response from the servomotor, such as the current joint angular position \cite{Ribeiro}. }
	{Assuming the humanoid robot is controlled from the filtered data arriving from the haptic device, this section considers the mapping from the haptic device to the humanoid robot (two robots). In order to provide a natural and intuitive human-robot interaction, it is necessary to define the best way to map the DOFs of the haptic device to those of the robotic system. In this context, it is important to remember that the haptic device moves in its own workspace, while the robot moves in another workspace somewhat different. Before discussing how the haptic and the robot are connected, it must be highlighted that the two devices are not always coupled, \textit{e.g.,} before the system is turned on.\paragraph{} Typically, the humanoid robot starts from an initial standing position (upright). When the system is turned on, the haptic's tip is moved to the zero position and the connection is established by pressing a keyboard key in the host computer. One of the joystick's buttons can be pressed to define a new "zero" position allowing the user to rest without affecting the humanoid robot behaviour. In that case, the beginning of the movement requires that the stylus tip be placed in the last configuration to avoid a shift between the two robots (\textit{i.e.,} to assure that the two workspaces perfectly overlap). This means the haptic mechanism and the humanoid robot are constrained to start at the same location and the system does not allow offsets between them.\paragraph{} In order to define the connection between the two robots, it is worth noting that the haptic device and the humanoid robot are kinematically dissimilar, both from the point of view of number of DOF and link lengths. As result, connecting them at a joint level does not seem to be appropriate or even feasible; instead, it was decided to connect them at their tips (Cartesian tip positions) in a position-position mode. Then, the hip position can be defined by: }
	
	\begin{equation}
	\tau_{hip}= \mu . \tau_{haptics}
	\end{equation}
{where $\mu$ represents the motion scale, $\tau haptic$ is the haptic position measured relative to the haptic device (see Figure \ref{Dim1}) and $\tau hip$ is the hip position measured relative to the robot view (discussed in Chapter 5). In this work, the scale μ was set to map the two workspaces as best as possible. However, it is expected that nonlinear or time-varying mappings may be useful to best utilize the humanoid robot workspace. \paragraph{} In the implemented mode, the human operator controls the robot's hip position what implies that an inverse kinematics algorithm should be used for translating the haptic commands into robot actions (joint angular displacements). The kinematics equations describing the relation between the Cartesian and the joint spaces of the lower limbs are presented in \cite{Emilio}. The angle values obtained by the kinematics algorithm need to be transformed into a range of values between 600 and 2400 before being sent to the HITEC servomotors. Each individual servomotor includes a local internal PID position controller that assures a position error around 1$\degree$. The communications between the host computer and the servomotors are performed via a RS-232 bidirectional serial interface. The RS-232 communication parameters and the structure of the data frame are depicted in Table \ref{Comm1} and \ref{Comm2}, respectively. The communication data frame is composed by seven bytes, where the first five are the start byte, the command, two command's parameters and the checksum, and the last two are used to get a response from the servomotor, such as the current joint angular position \cite{Ribeiro}.  }
	 \begin{table}[h]
	 \small
	 \centering
\begin{tabular}{|c|c|}
\hline
Parameter & Value \\ \hline
Baudrate  & 19200 \\ \hline
Data bits & 8     \\ \hline
Stopbits  & 2     \\ \hline
Parity    & none  \\ \hline
Handshake & none  \\ \hline
\end{tabular}
\caption{Communication Parameters \cite{Cruz}.}
\label{Comm1}
\end{table}

	\begin{table}[h]
	\small
\begin{tabular}{|l|l|l|l|l|l|l|l|}
\hline
 byte & 1 & 2 & 3 & 4 & 5 & 6 & 7 \\ \hline
 Controller & StartByte &Command & Param1 & Param2 & Checksum & 0x00 & 0x00 \\ \hline
Servomotor & \multicolumn{5}{l|}{           High-Impedance} & Return1 & Return2 \\ \hline
\end{tabular}
\caption{Communication Frame \cite{Cruz}.}
\label{Comm2}
\end{table}

\section{Haptic Rendering}

\subsection{Source of feedback}
%{A key feature in the haptic tele-operation approach is the transmission of some of the proprioception of the robot to the user. By taking into account that balance is a major concern, it is assumed that the most relevant source of feedback are the ground reaction forces measured from the four load cells installed on the feet corners. The normal reaction forces allow estimate the center-of-pressure (\acs{COP}) that determines the dynamic stability conditions of the humanoid robot.\paragraph{}The \acs{COP} will determine the condition in which the humanoid robot is. When the robot is placed in the home position (\textit{i.e.,} all servos have a angle of 0\degree) the \acs{COP} has a value near of zero in both components (X and Y). During the movement, this \acs{COP} will be changing. The differences between the actual and the initial \acs{COP} will determinate the state of balance of the humanoid robot. The greater this difference,, higher will be the imbalance that the robot feels. The \acs{COP} is calculated from the information obtained from the Load Cells, Figure \ref{LoadCell1}, and using the equation \ref{COP1} : }
{A key feature in the haptic tele-operation approach is the transmission of some of the proprioception of the robot to the user. Haptic rendering is the process of computing the force feedback based on measurements of the robot's state to be applied on the human's hand. By taking into account that balance is a major concern, it is assumed that the most relevant sources of feedback are the ground reaction forces measured from the four load cells installed on the feet corners. The normal reaction forces allow estimate the centre-of-pressure (CoP) that determines the dynamic stability conditions of the humanoid robot. \linebreak The COP is calculated based on the information obtained from the load cells using the following equation:}

		\begin{equation} \label{COP1}
		C\bar{O}P = \frac{\sum_{i}\bar{r_i} \times f_i}{\sum_{i}f_i} 
		\end{equation}
		{ where $\bar{r_i}$ is the coordinate of the load cell in the feet and $f_i$ is the correspondent force.
		}
		
		\begin{figure}[H]
		  \centering 
		  \includegraphics[width=1\textwidth]{imgs/Chapter2/LoadCells.eps}
		 \caption{Information generated by the Load Cells example.}
		 \label{LoadCell1}	
		\end{figure}
		
% {The \acs{COP} information resulting from the above equation can be presented in two \linebreak different ways: the global \acs{COP} and the \acs{COP} of each coordinate (X and Y). In Figure \ref{COP2} and \ref{COP3} an example of the 3 generated COPs is shown.}

{Figure \ref{LoadCell1} illustrates the information generated by the load cells of one foot for a simple example motion. From these data, the coordinates of the COP can be calculated through Eq. \ref{COP1} and represented in two different ways: the global spatial evolution of the COP (see Figure \ref{COP2}) and the time course of the coordinates along the x and y axis (see Figure \ref{COP3}). As can be noted, when the robot is placed in the home position (\textit{i.e.,} all servos with a zero angle), the x and y components of the COP is close to zero.}
 
 	\begin{figure}[H]
		  \centering 
		  \includegraphics[width=1\textwidth]{imgs/Chapter2/COPexample.eps}	
		 \caption{Generated \acs{COP} example for both feet.}
		 \label{COP2}
		\end{figure}
		
		\begin{figure}[H]
		  \centering 
		  \includegraphics[width=1\textwidth]{imgs/Chapter3/Forcas/COPXY.eps}	
		 \caption{Generated COPX and COPY example.}
		 \label{COP3}
		\end{figure}
		
\subsection{Force Generation}
%{Sometimes using only vision to perform tele-operation may not be sufficient. The force feedback aimed at creating a contrary force to assist the operator in balance. So, the force that will be subscribed by the device can be generated into 3 components (x, y and z). The force is applied only to the axis x and z and is obtained from COPx and COPy. The force of the y component is constant and serves to counteract the force of gravity. It was important to choose the best function for this purpose. First of all it was important not to use a linear function. With a linear function the small incremental alteration of force could be no longer felt due to habituation. So, 3 non linear functions were chosen, an exponential, an inverse function and a sigmoid function. In the next 3 subsections will be given an explanation of these 3 forces.}
{Beyond the sensing step, the computational process of haptic rendering involves, in each cycle, the force calculation that is then fed back (actuation) on the human's hand. As mentioned before, the teleoperation system provides the user with force feedback based on what the robot is feeling in terms of COP coordinates (a balance index), informing him/her on the proximity of the limits of stability by means of a repulsive force (pushing the end-effector away). In line with this, the joystick synthetizes forces in the x and z components, while the force in the y component is constant aiming to counteract the force of gravity. \paragraph{} Aiming to provide the human operator with the sense of robot's balance, force feedback can be calculated in many different ways, using linear and nonlinear functions. First of all, it seems a good rule to avoid linear functions since small incremental alterations could be no longer felt due to user's habituation. Second, a deadzone should be considered to allow small displacements of the COP can be eliminated. Having this in mind, a sigmoid-type function (Richard's curve) is chosen, allowing for an easy parametrization of the S-shaped curve:}

%\subsubsection*{Exponential Function}\label{ForceCh1}
%	{The first function is an exponential function. This function is calculated using the \acs{COP} variation. This function is presented in the equation \label{eq:F1}.}
%	
%	\begin{center}
%	\begin{equation} \label{eq:F1}
%	 F_u=\pm \alpha ^{(Cop_u * K)}  \pm 1;   	 
%		\end{equation}
%		    {   where $ F_u=0, if |F_u|\leq M$ and $ |F_u|=L, if |F_u|\leq L$ }
%	\end{center}
%	
%	
%	{ \paragraph{} In the function above, one constant K is used to normalize the value of the \acs{COP} so this value is small. The variable $\alpha$ is parametrized, and the higher this value, the more pronounced is the force curve. The constants L and M were chosen to define the maximum and minimum force allowed. In Figure \ref{sub7} and \ref{sub8} the different generated curves, for a starting $\alpha$ at 5 is shown.}
%	
%	
%	\begin{figure}[H]
%		\centering
%		\begin{subfigure}{1\textwidth}
%		  \centering
%		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Forcas/F1N.eps}
%		  \caption{Generated force for the z component($F_z$) using the y component of the \acs{COP} (COPy)}
%		  \label{sub7}
%		\end{subfigure}
%		\begin{subfigure}{1\textwidth}
%		  \centering
%		  \includegraphics[width=0.8\linewidth]{imgs/Chapter3/Forcas/F1xN.eps}
%		  \caption{Generated force for the x component($F_x$) using the x component of the \acs{COP} (COPx) }
%		  \label{sub8}
%		\end{subfigure}
%		\caption{Force exponential function for different $\alpha$}
%		\label{ExpF1}
%		\end{figure}
%	
%	{The use of this function presents a major limitation, although it can be parametrized and have a higher impact to the operator. After some experiments the variation of the force will begin to cease to be felt, since the variation has a close to linear behaviour. }
%\clearpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%	\subsubsection*{Inverse function}
%	
%	
%	{The second function is an inverse function. This function is calculated using the \acs{COP} variation and its extreme values. This function is presented in Equation \ref{eq:F2} and \ref{eq:F3}.}
%	
%	\begin{center}
%	\begin{equation} \label{eq:F2}
%	Fz = \frac{K+1}{|COPy|-0.07} - \frac{1}{0.07} \pm 0.5 \pm 0.2*K;
%	\end{equation}
%	{    where $Fz=0$, If $|Fz|\leq M $ and $|Fz|=L$, If $|Fz|\leq M $}
%	\end{center}
%	
%	\begin{center}
%	\begin{equation}\label{eq:F3}
%	 Fx = \frac{K+1}{|COPx|-0.046} - \frac{1}{0.046} \pm 0.5 \pm 0.25 \times K ;
%	\end{equation}
%	{where $Fx=0$ , If $|Fx|\leq M$ and $|Fx|=L$ , If $|Fx|\leq L$}
%	\end{center}
%	 
%	 
%	 { \paragraph{} The variable $K$ can be parametrized, and the higher this value the more pronounced the force curve will be. The constants L and M were chosen to define the maximum and minimum force allowed. To ensure that after the deadzone the starting force for all the different parameters is near to $|0.5|$, a small incrementation of $\pm 0.25 \times K$ for equation \ref{eq:F3} and $\pm 0.2 \times K$ for equation \ref{eq:F2} was added. In Figure \ref{sub9} and \ref{sub10}, the different generated curves for different $K$ is shown.}
%	 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
%	 	\begin{figure}[H]
%		\centering
%		\begin{subfigure}{0.9\textwidth}
%		  \centering
%		  \includegraphics[width=1\linewidth]{imgs/Chapter3/Forcas/F2N.eps}
%		  \caption{Generated force for the z component($F_z$) using the y component of the \acs{COP} (COPy)}
%		  \label{sub9}
%		\end{subfigure}
%		\begin{subfigure}{0.9\textwidth}
%		  \centering
%		  \includegraphics[width=1\linewidth]{imgs/Chapter3/Forcas/F2xN.eps}
%		  \caption{Generated force for the x component($F_x$) using the x component of the \acs{COP} (COPx) }
%		  \label{sub10}
%		\end{subfigure}
%		\caption{Force inverse function for different $K$}
%		\label{ExpF1}
%		\end{figure}
%		
%		
%	 {This force function unlike the previous one (see Section Exponential function), has a non linear behaviour but this non linear behaviour only starts for higher \acs{COP} values. This makes this function only useful in cases where the unbalance (\textit{i.e.,} \acs{COP} variation) is extreme. When the \acs{COP} variation is small the generated force will not have any impact in the operator.}
%	 
%	\clearpage
%	\subsubsection*{Sigmoid function}
%{The third function is a sigmoid function. This function was chosen due to its easy parametrization. This function is presented in the equation \ref{Sigm}.}	
	
	
	\begin{equation}\label{Sigm}
	F=A + \frac{(K-A)}{(1+0.5*\mathrm{e}^{-B*(COP_u - M)})^{\frac{1}{0.5895}}}
	\end{equation}
	
%{As said in the beginning of this subsection , this function is easily parametrized. The parameter B will decide the curvature of the function. The constants A and K will define the minimum and maximum force and the value M the dead-zone. This function is then calculated with the \acs{COP} variation. In Figure \ref{SigmI} the generated curves, for different B parameters is shown.}	
	{The six parameters are defined as follows: A/(K) is the lower/(upper) asymptote that defines the minimum/(maximum) force exerted on the user, B affects the growth rate and M the deadzone. Figure \ref{SigmI} depicts different curves showing the effect of varying the growing rate B and \ref{Sigdead} shows the effect of applying different deadzones for a fixed B, in this case 1.5. After a few experiments, it was found that the shape of this curve is particularly appropriate for the intended applications ( described in detail in Chapter 5). In order to maintain a desired (and constant) COP, growth should be fastest at the start, while the maximum force feedback value is approached much more gradually. }
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.9\textwidth]{imgs/Chapter3/Forcas/Sig.eps}
	 \caption{Sigmoid function for different B parameters.}
	 \label{SigmI}
	\end{figure}
	
	\begin{figure}[H]
	  \centering 
	  \includegraphics[width=0.9\textwidth]{imgs/Chapter3/Forcas/deadzones.eps}
	 \caption{Sigmoid function for different M parameters.}
	 \label{Sigdead}
	\end{figure}
	
	{This function shows a non-linear behaviour in the whole movement. Even with a small \acs{COP} variation the operator can feel a higher or a small counterforce, simply by reducing or increasing the parameter B. Due to this characteristic, this function was the chosen one to be used during the experiments performed in this work.}