

In this chapter, the experimental infrastructure used throughout the dissertation will be explained in detail, including hardware and software specifications.

At the beginning of this dissertation, all of the hardware was connected to each other just as it was in \cite{peixoto_visual_2016} (Fig. \ref{fig:ptu}). For a more detailed explanation of how the different components were integrated with each other please refer to chapter 2 (Experimental Tools and Setup) of that work. Some changes had to be made in order to fulfill this work's objectives and they are described in section \ref{sec:modifications}. 

The next two sections describe each of the components of the \textit{final} solution.

\begin{figure}[htbp]
	\centering
	\includegraphics[width=0.7\textwidth]{./Imagens/ptu.png}
	\caption{Detailed view of the initial experimental infrastructure.\\	1 - FANUC manipulator; 2 - POLOLU-minIMU 9DOF v2; 3 - RAZOR 9DOF - SEN 10736; 4 - Arduino UNO R3; 5 - Firefly MV-03MTC - Pointgrey. Taken from \cite{peixoto_visual_2016}.}
	\label{fig:ptu}
\end{figure}



\section{Hardware}
\subsection{PointGrey Flea3 2.8 MP Color GigE Vision (Sony ICX687)}

The camera which was already mounted on the \gls{ptu} was a FFMV-03M2C-CS model made by PointGrey. However, this camera is becoming obsolete, since the FireWire 1394a communication protocol is not supported by modern day laptops anymore. As such, it was decided a new camera had to be installed. The camera chosen was the Flea 3 GigE (short name), one that was already available in \gls{lar}, therefore reducing monetary and time costs. This camera is also made by PointGrey and is the model FL3-GE-28S4C-C (Fig. \ref{fig:flea}). The main specifications of this camera are shown in table \ref{tab:flea}.

\begin{figure}[htbp]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/flea3.jpeg}
	\caption{The PointGrey Flea3  GigE camera. Taken from \cite{noauthor_flea3_nodate}.}
	\label{fig:flea}
\end{figure}

\begin{table}[H]
\centering
	\caption{PointGrey FL3-GE-28S4-C specifications. Taken from \cite{noauthor_flea3_nodate}.}
	\label{tab:flea}
\begin{tabular}{lr}
	\toprule \textbf{Specifications} & \textbf{Values} \\ \midrule
	Max. Resolution & 1928 x 1448 \\ 
	Frame rate & 15 FPS \\  
	Chroma & Color \\ 
	Sensor name & Sony ICX687 \\
	Sensor type & CCD \\
	Readout method & Global shutter \\
	Sensor format & 1/1.8" \\
	Pixel size & 3.69 $\mu$m \\
	Interface & GigE \\
	Power requirement & 12-24 V \\
	Power consumption (maximum) & 2.5 W \\
	Dimensions & 29x29x30 (mm) \\ 
	Mass & 38 grams (without optics) \\\bottomrule
\end{tabular}
\end{table}


\subsection{HSR-5498SG Digital Robot Servo}

The servomotors already present in the \gls{ptu} were two Hitec HSR-5498SG Digital Robot Servo (Fig. \ref{fig:servo}). Their specifications were thought to be sufficient for this work's needs, so they were not replaced. Their specifications can be found in table \ref{tab:servo}.

\begin{figure}[htbp]
	\centering
	\includegraphics[width=0.5\textwidth]{./Imagens/HSR-servo.jpg}
	\caption{An HSR-5498SG Digital Robot Servomotor. Taken from \cite{noauthor_hsr-5498sg_nodate}.}
	\label{fig:servo}
\end{figure}

\begin{table}[H]
	\centering
	\caption{HSR-5498SG Digital Robot Servo specifications. Taken from \cite{noauthor_hsr-5498sg_nodate} and \cite{jun_hee_general_2016}.}
	\label{tab:servo}
	\begin{tabularx}{\textwidth}{lYY}
		\toprule \textbf{Specifications} & \multicolumn{2}{c}{\textbf{Values}} \\ \midrule
		Control system              & \multicolumn{2}{c}{Pulse width control (1500 $\mu$s neutral)}                   \\ 
		Operating voltage range     & \multicolumn{2}{c}{6.0 to 7.4 V}                                                \\ 
		Operating temperature range & \multicolumn{2}{c}{-20\textdegree \space to 60\textdegree C}                    \\ 
		Test Voltage                & 6.0 V                                  & 7.0 V                                        \\ 
		Operating speed (No load)   & 0.22s/60\textdegree                    & 0.19s/60\textdegree                          \\ 
		Stall torque                & 11.0 kgf-cm                            & 13.5 kgf-cm                                  \\ 
		Standing torque             & 10.6 kgf-cm/5\textdegree\space holdout & 14.4 kgf-cm/5\textdegree\space holdout \\ 
		Idle current                & 8 mA                                   & 10 mA                                        \\ 
		Running current (No load)   & 200 mA                                 & 240 mA                                       \\ 
		Stall current               & 1200 mA                                & 1480 mA                           \\ 
		Operating angle             & \multicolumn{2}{c}{0\textdegree\space to 180\textdegree\space (600 $\mu$s / 2400 $\mu$s respectively)}           \\ 
		Motor type                  & \multicolumn{2}{c}{Cored Metal Brush / Nd magnet}                          \\ 
		Dimensions                  & \multicolumn{2}{c}{40 x 20 x 37 mm} \\ 
		Weight                      & \multicolumn{2}{c}{59.8 g} \\ 
		Gear material               & \multicolumn{2}{c}{1 metal-karbonite \& 3 steel} \\ \bottomrule
	\end{tabularx}
\end{table}

\subsection{Arduino UNO R3}

To process the sensor readings coming from the \gls{imu}'s, an Arduino UNO R3 board (Fig. \ref{fig:arduino}) was used. Arduino is an open-source electronics platform, based on easy-to-use hardware and software. There are many different Arduino boards, but all of them are able to read inputs, make some decision based on those inputs, and then generate output signals accordingly \cite{noauthor_arduino_nodate}. These boards are based on a microcontroller that can be programmed with the Arduino programming language \cite{noauthor_arduino_nodate-1} using the Arduino Software \cite{noauthor_arduino_nodate-3}.

\begin{figure}[H]
	\centering
	\includegraphics[width=.5\textwidth]{./Imagens/arduino-uno-r3-500x500.jpg}
	\caption{An Arduino UNO R3 board. Taken from \cite{noauthor_arduino_nodate-2}.}
	\label{fig:arduino}
\end{figure}

The Arduino UNO R3 board comes with an ATmega328P microcontroller and it has the following features \cite{noauthor_arduino_nodate-2}:

\begin{itemize}
	\item 14 digital input/output pins (of which 6 can be used as PWM outputs);
	\item 6 analog inputs;
	\item 16 MHz quartz crystal;
	\item USB connection;
	\item Power jack;
	\item ICSP header;
	\item Reset button.
\end{itemize}



\subsection{Inertial Measurement Units}

With the \gls{imu}'s present in the \gls{ptu}, it is possible to estimate its spatial orientation just by analyzing the inertial data that these sensors output. \gls{phua} has a total of nine \gls{imu}'s distributed along its body. However, only two of these were used in this work, since using any more would not be necessary.

A short introduction to each \gls{imu} present in the \gls{ptu} is shown in the following pages.
\newpage
\subsubsection{RAZOR 9DOF - SEN 10736}

The 9DOF Razor \gls{imu} (Fig. \ref{fig:razor}) incorporates three sensors - an ITG-3200 MEMS triple-axis gyro, an ADXL345 triple-axis accelerometer, and a HMC5883L triple-axis magnetometer - providing a total of nine degrees of inertial measurement. All of the sensors' outputs are processed by an on-board ATmega328 microcontroller and then output via RS232 serial communication \cite{noauthor_9_nodate}. 

\bigskip\noindent\textbf{Technical specifications:} \bigskip

\begin{minipage}[c]{0.7\textwidth}

\begin{itemize}
	\item 3DOF gyroscope - ITG3200 \cite{noauthor_itg-3200_2010}:
		\subitem Measures up to $\pm$ 2000\textdegree/s;
		\subitem 16 bit resolution;
		\subitem operating frequency of 30 kHz.
	\item 3DOF accelerometer - ADXL345 \cite{noauthor_adxl345_2009}:
		\subitem Measures up to $\pm$ 16g;
		\subitem 13 bit resolution;
		\subitem Operating frequency of 3.2 kHz.
	\item 3DOF magnetometer - HMC5883L \cite{noauthor_3-axis_2011}:
		\subitem operating until $\pm$ 8 gauss;
		\subitem 12 bit resolution;
		\subitem operating frequency of 400 kHz.
	\item ATmega328 incorporated micro-controller;
	\item RS232 communication;
	\item Dimensions: 28 x 41 mm;
	\item 3.5-16 VDC input.
\end{itemize}
\end{minipage}
\begin{minipage}[c]{0.29\textwidth}
\includegraphics[width = \textwidth]{./Imagens/razor.jpg}
\captionof{figure}{A RAZOR 9DOF - SEN 10736 IMU. Taken from \cite{noauthor_9_nodate}.}
\label{fig:razor}

\end{minipage}
\newpage

\subsubsection{Pololu MinIMU-9 v2}

The Pololu MinIMU-9 v2 (Fig. \ref{fig:minimu}) is an even more compact \gls{imu} which contains an L3GD20 3-axis gyroscope and a LSM303DLHC 3-axis accelerometer and magnetometer. The outputs of these sensors can be obtained via an I\textsuperscript 2C interface and there are a total of 8 of these modules distributed along \gls{phua}'s body, in order to obtain the orientation of each main body part separately \cite{noauthor_pololu_nodate}.

\bigskip \noindent\textbf{Technical specifications:}\bigskip

\begin{minipage}[c]{0.7\textwidth}
\begin{itemize}
	\item 3DOF gyroscope - L3GD20 \cite{noauthor_l3gd20_2013}:
		\subitem operating until $\pm$ 2000/s;
		\subitem 16 bit resolution;
		\subitem operating frequency of 0.76 kHz.
	\item 3DOF accelerometer -LSM303DLHC \cite{noauthor_lsm303dlhc_2013}:
		\subitem operating until $\pm$ 16g;
		\subitem 12 bit resolution;
		\subitem operating frequency of 400 kHz.
	\item 3DOF magnetometer - LSM303DLHC \cite{noauthor_lsm303dlhc_2013}:
		\subitem operating until $\pm$ 8 gauss;
		\subitem 12 bit resolution;
		\subitem operating frequency of 400 kHz.
	\item I\textsuperscript 2C communication.
	\item Dimensions: 20.32 x 12.7 mm;
	\item 2.5-5.5 VDC input.

\end{itemize}
\end{minipage}
\begin{minipage}[c]{0.29\textwidth}
\includegraphics[width = \textwidth]{./Imagens/minimu.jpg}
\captionof{figure}{A Pololu MinIMU-9 v2 IMU. Taken from \cite{noauthor_pololu_nodate}.}
\label{fig:minimu}

\end{minipage}

\subsection{FANUC M-6iB/6S robotic manipulator}

In order to perform experiments with high repeatability and great precision, a robotic manipulator present in \gls{lar} was used. The manipulator used is a FANUC M-6iB/6S (Fig. \ref{fig:fanuc}) and is part of the M-6iB series, comprised of small robots which are designed to approximate the reach of an operator. The variant present in \gls{lar} (6S) features six rotational joints (Fig. \ref{fig:robotjoints}), each of them providing a different \gls{dof}, up to 6 kg of payload at wrist, 951 mm of horizontal reach and a $\pm$0.08 mm repeatability interval \cite{noauthor_m-6ib_2007}.

\begin{figure}[H]
	\centering
	\includegraphics[width=0.5\textwidth]{./Imagens/fanuc.png}
	\caption{A FANUC M-6iB/6S robotic manipulator. Taken from \cite{noauthor_fanuc_nodate}.}
	\label{fig:fanuc}
\end{figure}

\begin{figure}[H]
	\centering
	\includegraphics[width=0.5\textwidth]{./Imagens/robotjoints.png}
	\caption{A schematic representing the 6 rotational joints of the FANUC M-6iB/6S. Taken from \cite{noauthor_m-6ib_2007}.}
	\label{fig:robotjoints}
\end{figure}

More details about the communication between the computer controlling the experiments and the robotic manipulator are present in section \ref{sec:robCOMM}.

\section{Software}

\subsection{Development platform and programming languages}

All of this dissertation's work has been developed under the Ubuntu 16.04 LTS Open Source Operative System, a Linux distribution based on the Debian architecture \cite{noauthor_ubuntu_nodate} and most of the programming was done using the C and C++ programming languages.



%\begin{figure}[!htb]
%	\centering
%	\begin{minipage}[t][2cm][t]{.5\textwidth}
%		\centering
%		\includegraphics[width=.5\textwidth]{./Imagens/ubuntu-logo.png}
%		\vfill
%		\caption*{(a)}
%	\end{minipage}%
%	\begin{minipage}[c]{0.5\textwidth}
%		\centering
%		\includegraphics[width=0.5\textwidth]{./Imagens/c_c++.png}
%		\caption*{(b)}
%	\end{minipage}
%			\caption{a)The Ubuntu logo. b) The C and C++ programming languages. Taken from \cite{noauthor_ubuntu_nodate-1} and \cite{bolinches_advanced-c-cpp-programming-tutorial:_2017}.}
%\end{figure}

\subsection{ROS - Robot Operating System}

The Robot Operating System (ROS) is an open source framework for writing robot software created in 2007. Through its collection of tools, libraries, conventions and even built-in applications, it aims to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms \cite{noauthor_ros.org_nodate}.

This allows for research groups around the world to participate in collaborative projects, which are all built on the same environment, making the resulting applications more robust. Applications created in projects can be packaged and used by other \gls{ros} users, allowing the community to save time costs developing what has already been done. \gls{ros} applications can even be written in different programming languages, such as C++, Python and Lisp, with experimental libraries being developed in Java and Lua \cite{noauthor_ros/introduction_nodate}.

It is also a distributed and modular framework, which means that users can use as much or as little of \gls{ros} as they desire \cite{noauthor_ros.org_nodate-1}.

What follows is a quick explanation of some keywords and concepts associated with this framework, followed by a simple diagram (Fig. \ref{fig:ros_diagram}) which will be useful when trying to understand the rest of this work \cite{noauthor_ros/introduction_nodate}:

\begin{itemize}
\item \textbf{Packages}

Packages are the main units for organizing software in \gls{ros}. It is the smallest thing you can build and release and in it there can be \gls{ros} runtime processes (\textit{nodes}), \gls{ros}-dependent libraries, datasets, configuration files, launch files, message files, or anything else that is usefully organized together.

\item \textbf{Metapackages}

Metapackages are a specialized type of Package which only serve to represent a group of related other packages.

\item \textbf{Nodes}

Nodes in \gls{ros} are the equivalent of processes in normal operating systems. As was stated before, \gls{ros} is designed to be as modular as possible; Considering the example of a typical robot control system, it is made of many nodes. One node may control a laser range-finder, while another controls the wheel motors, another performs localization, another performs path planning, and so on. A \gls{ros} node is written with the use of a \gls{ros} client library, such as roscpp (for C++) or rospy (for Python).
\\\\
\item \textbf{Messages}

Nodes communicate with each other by publishing messages to topics. A message is a simple data structure, which is comprised of fields with certain data types.

\textbf{msg} files are simple text files for specifying the data structure of a message sent in \gls{ros}.

\item \textbf{Topics}

In \gls{ros}, messages are routed via a transport system which uses publishing/subscribing semantics. Nodes \textbf{publish} messages into a given topic. The topic is simply a name that is used to identify the content of the message. A node which is interested in a certain kind of data may \textbf{subscribe} to the appropriate topic. There may be multiple concurrent publishers and subscribers for a single topic, and a single node may publish and/or subscribe to multiple topics.

\item \textbf{Bags}

Bags are a format for saving and playing back \gls{ros} message data. Bags are an important mechanism for storing data, such as sensor data, that can be difficult to collect but is necessary for developing and testing algorithms.
\end{itemize}

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/publish_subscribe_concept.png}
	\caption{Two nodes subscribe the \texttt{/example} topic which contains a message of the type \texttt{std\_msgs/String} and was published by a third node. Taken from \cite{noauthor_exchange_nodate}.}
	\label{fig:ros_diagram}
\end{figure}

\subsection{OpenCV}

\gls{opencv} is an open source computer vision and machine learning software library, first released to the public in 2000. It is written on the C++ language but can work with C++, C, Python, Java and MATLAB and supports Windows, Linux, Android and Mac OS. It contains functions and algorithms that can help in many applications, like for example detecting and recognizing faces, identifying objects, tracking camera movements and moving objects, extracting 3D models of objects, and much more \cite{noauthor_about_nodate}.

\gls{opencv} has a modular structure, which means that it includes several shared or static libraries. The following modules are available: \texttt{core, imgproc, video, calib3d, features2d, objdetect, highui, gpu} and some other helper modules \cite{noauthor_introduction_nodate}.

The modules used in this work were the following:

\subsubsection{\texttt{core} module}

A compact module defining basic data structures, including the dense multi-dimensional array \texttt{Mat} and basic functions used by all other modules.

\subsubsection{\texttt{imgproc} module}

An image processing module that includes linear and non-linear image filtering, geometrical image transformations (resize, affine and perspective warping, generic table-based remapping), color space conversion, histograms, and so on.

\subsubsection{\texttt{calib3d} module}

Basic multiple-view geometry algorithms, single and stereo camera calibration, object pose estimation, stereo correspondence algorithms, and elements of 3D reconstruction.

\subsubsection{\texttt{highui} module}

An easy-to-use interface for video capturing, image and video codecs, as well as simple UI capabilities.

\subsection{ViSP - Visual Servoing Platform}


The Visual Servoing Platform (ViSP) is a modular cross platform C++ library, which allows the prototyping and developing of applications using visual tracking and servoing techniques, and was developed by the Inria Lagadic team. With this library, it is possible to compute control laws that can be applied to robotic systems. It provides a set of visual features that can be tracked using real time image processing or computer vision algorithms \cite{noauthor_visp_nodate}.

It is divided into several modules as can be seen in figure \ref{fig:ViSP_architecture}. The modules which were most used in this work were the \texttt{\textbf{me}} (moving edges) and \texttt{\textbf{vision}} modules, which are described below \cite{noauthor_software_nodate}.

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/soft_archi_v5.png}
	\caption{The ViSP software architecture. Taken from \cite{noauthor_software_nodate}.}
	\label{fig:ViSP_architecture}
\end{figure}

\subsubsection{\texttt{me} module}

This module provides real-time tracking of points normal to the object's contours. It allows for the tracking of lines and ellipses in pre-recorded video, or even a live feed coming from a video camera \cite{noauthor_moving-edges_nodate}. The first attempt at estimating verticality through visual data was made using this module at its base.

\subsubsection{\texttt{vision} module}

This module is able to compute a pose estimation (transformation matrix) or an homography from points using a robust scheme. For this work the feature that was used was the pose estimation feature \cite{noauthor_computer_nodate}.
By giving at least four points from an image plane and their corresponding 3D coordinates, \gls{visp} is able to estimate the relative pose between the camera and the object frame. This pose is returned as an homogeneous transformation matrix \cite{noauthor_visual_nodate}. An application using this module was the second method used to estimate verticality through visual data.

\subsection{Arduino IDE and language}

The Arduino IDE (Fig. \ref{fig:arduino_ide}) was the software used to write and upload code to the Arduino UNO R3 board. The Arduino language is merely a set of C/C++ functions that can be called from the code. It basically acts as a simplified language adequate to all levels of programmers. In every Arduino program there are two functions. The \texttt{setup()} function is called when a program starts. It is used to initialize variables, pin modes, start using libraries, etc. It will only run once, after powering up the board or pressing the reset button. There is also a \texttt{loop()} function which continuously runs the code inside it in each loop cycle.

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/arduino_ide.png}
	\caption{A screenshot of the Arduino IDE window.}
	\label{fig:arduino_ide}
\end{figure}

Like other languages, the Arduino language can be extended by a number of libraries. The library that was primarily exploited in this work was the \texttt{Servo} library \cite{noauthor_arduino_nodate-4}. With it, it is simple to set a servomotor's shaft position and speed through the PWM outputs on the Arduino board. However, it is not possible to read a servomotor's current position through the use of this library, as was thought to be possible at the beginning of this work.



\subsection{robCOMM - A Robot-Computer Interface}

\label{sec:robCOMM}

In order to control the FANUC M-6iB/6S manipulator remotely from a computer via the TCP/IP protocol, the robCOMM server present in the manipulator's controller was used. This server allows the user to send and receive information between both devices. This communication is made using a language developed in \gls{lar}, called robCOMM \cite{cancela_extensao_2007}, which for example allows the user to move the robotic manipulator to a given set of joint/cartesian coordinates, read the current values of each joint, etc.

For a full list and explanation of the instructions available in this language, please refer to \cite{cancela_extensao_2007} and \cite{cancela_extensao_2007-1}. The instructions used in this dissertation were the RUNTPP and GETCRJPOS functions, which are described below.

\subsubsection{RUNTPP function}

This function allows a remote device to command the manipulator to run a previously created program which is present in the controller's memory.\\

\noindent\textbf{Syntax:} RUNTPP<CR>tpProgramName<CR>\\

\noindent where <CR> stands for the carriage return character and tpProgramName is the name of the program the user wishes to execute.

This function returns either ``0<CR>'' if unsuccessful or ``1<CR>'' if successful.

\subsubsection{GETCRJPOS function}

This function returns the current joint configuration of the robotic manipulator to the remote device.\\

\noindent\textbf{Syntax:} GETCRJPOS\\

\noindent This function returns <CR>...<CR> where ``...'' are the joint values followed by either ``0<CR>'' if unsuccessful or ``1<CR>'' if successful.

\subsection{Glade and GTK+ toolkit}

Glade is a Rapid-Application Development (RAD) tool to enable quick \& easy development of user interfaces for the GTK+ toolkit and the GNOME desktop environment. These interfaces are saved as an Extensible Markup Language (XML) file, and are dynamically loaded by applications using the GtkBuilder GTK+ object. By doing so, Glade XML files are programming-language independent and the interfaces they contain are generated at runtime \cite{noauthor_glade_nodate}.

In the semester previous to the one this work took place in, there was an assignment for the \textit{``Projecto em Automa\c{c}$\tilde{a}$o e Rob{\'o}tica''} class whose purpose was the familiarization with the hardware used in this dissertation. The assignment was to create a simple graphical interface (Fig. \ref{fig:glade_interface}) which would allow a user to control the position of the servomotors in the already existing \gls{ptu} and visualize the image obtained with the camera placed on that said \gls{ptu}.

\begin{figure}[hbtp]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/Interface.png}
	\caption{A screenshot of the created user interface.}
	\label{fig:glade_interface}
\end{figure}

As such, Glade and the GTK+ toolkit were the tools used to create this interface, along with the Arduino code which controlled the servomotors and code in the C language which was used to mainly handle the interface's callbacks and to send the user's desired motor position to the Arduino board.