\chapter{Implementierung}\label{ch:implementation}
Die ausführliche Auseinandersetzung mit der Struktur des ROS spezifischen Arbeitsraums, dessen Pakete die nötigen Roboterbeschreibungen, Konfigurationsdateien und implementierte Algorithmen enthalten, um alle Aspekte der wissenschaftlichen Arbeit zu realisieren, ist Inhalt dieses Kapitels und in \autoref{fig:8} illustriert. Dabei initialisiert jede Quelldatei einen Node im ROS Kontext in der Programmiersprache C++. 

\begin{figure}[h!]
\centering
	\begin{tikzpicture} [ >=stealth']
		\node[rectangle, draw=black,  thick, minimum height=3em, align=center] (RD) {Roboterbeschreibung};
		\node[rectangle, draw=black,  thick, minimum height=3em, align=center] (DK) [above right = 1cm and 0.5cm of RD] {Dual Konfiguration};
		\node[rectangle, draw=black,  thick, minimum height=3em, align=center] (MK) [below right = 1cm and 0.5cm of RD]{Mono Konfiguration};
		\node[rectangle, draw=black,  thick, minimum height=3em, align=center] (I) [right= 5cm of RD]{Implementierung};
		
		\draw[-] (RD) -- (DK) -- (I);
		\draw[-] (RD) -- (MK) -- (I);

	\end{tikzpicture}
	\caption{Die Struktur des ROS Arbeitsraums ist durch dessen passive Pakete und aktive Pakete, sowie deren Beziehungen, illustriert. Diese Betrachtung partitioniert das Kapitel logisch in einen organisatorischen Teil, dessen Pakete die Prämisse des exekutiven Teils sind.} \label{fig:8}
\end{figure}

\section{Robotische Systeme und deren Konfiguration}
Die Roboterbeschreibung ist Grundlage der Generierung valider Konfigurationsdateien, welche jeweils $n \in \N_{>0}$ robotische Systeme konkretisieren und somit die Kommunikation zwischen den $n$ Robotern durch Integration der MoveGroup Schnittstelle ermöglichen. Diese ist ein Klon des öffentlich zugänglichen Pakets 'franka\_ ros' \cite{franka_ros} der Franka Emika GmbH zum August 2020, welches der Lehrstuhl für Softwaretechnologie bereitstellt. Die enthaltenen Dateien definieren einzelne Festkörpergruppen sowie deren Vereinigung zu $n$ Robotergruppen. Beispielsweise ist der Struktur aus \autoref{fig:9} zu entnehmen, dass ein Manipulator aus Hand und Arm besteht, sowie dass die Instanziierung mehrerer Manipulatoren eine zusätzliche Datei bedingt. \autoref{fig:9} b zeigt die Struktur einer Konfigurationsdatei, welche im 'config' Verzeichnis alle Festkörper der Referenzbeschreibung anhand der 'controller.yaml' Datei als Kontrollelemente spezifiziert und somit die Interaktion ermöglicht. Das Beispiel einer Modifizierten Roboterbeschreibung eines dualen Setup zeigt \autoref{lst:4}, aus dem das Muster zur Generierung einer $n$ Roboter Datei zu entnehmen ist, indem dessen Inhalt für alle Identitäten $n$ mal iteriert wird. Analog kann die entsprechende $n$\_ Konfigurationsdatei aus dem MSA erzeugt, oder mittels einer Manipulation der 'config' und 'launch' Dateien aus \autoref{fig:9} generiert werden.

\begin{figure}
\centering
\begin{minipage}{.45\linewidth}
\dirtree{%
.1 Roboterbeschreibung.
.2 ci.
.2 meshes.
.2 robots.
.3 'panda\_arm.xacro'.
.3 'hand.xacro'.
.3 'panda\_arm\_hand.xacro'.
.3 'dual\_panda\_example.xacro'.
}
\end{minipage}\hfill
\begin{minipage}{.55\linewidth}
\dirtree{%
.1 MoveIt\_Konfiguration.
.2 config.
.3 'panda\_controller.yaml'.
.3 'panda\_gripper\_controller.yaml'.
.2 launch.
.3 'move\_group.launch'.
.3 'demo.launch'.
.3 'planning\_context.launch'.
}
\end{minipage}\hfill
\caption{Visualisierung der Verzeichnisstruktur der Roboterbeschreibung und Konfiguration. Bezüglich der Übersichtlichkeit sind nur die Dateien gelistet, deren Modifikation in Hinsichtlich der Realisierung mehrerer robotischer Systeme relevant ist.}\label{fig:9}
\end{figure}

\begin{lstlisting}[language=JSON,firstnumber=1, caption={ Dual Setup Beispiel, in mit Modifikationen zur Übergabe der Roboterpositionen.}, label={lst:4}, captionpos=b]
{<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<!-- Einbindung der Roboter Komponenten-->
<xacro:include filename="panda_arm.xacro"/>
<xacro:include filename="hand.xacro"/>

<!-- Deklaration der Namensattribute der zu konkretisierenden Roboter -->
<xacro:arg name="arm_id_1" default="panda_1" />
<xacro:arg name="arm_id_2" default="panda_2" />

<!-- Deklarierung des Positionsattribute, welche aus der Kalkulation, ber die Konfigurationsdatei, in die Roboterbeschreibung bergeben wird-->
<xacro:arg name="pos_1" default="0 0 0" />
<xacro:arg name="pos_2" default="1 1 0" />
<!-- Definition des eindeutigen World-Frames, als Referenz der Roboter -->
<link name="world"/>

<!-- Initialisierung der robotischen Systeme an ihrer spezifischen Position-->
<xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="world"  xyz="$(arg pos_1)" /> 
<xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" />

<xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="world"  xyz="$(arg pos_2)" /> 
<xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8"/>
<!-- Eine n-Roboterbeschreibung erfordert Namens- und Positions- Deklarationen sowie die Initialisierung aller n Manipulatoren -->  
</robot>}
\end{lstlisting}


\subsection{MoveGroup Interface mit Dual Setup}
Eine Konfigurationsdatei konkretisiert ein robotisches System als Bewegungsgruppe 'MoveGroup', dessen Identität eindeutig durch ihr Namensattribut bestimmt ist. Demnach genügt dieses zur Instanziierung eines MoveGroup Objekts, welcher die Kommunikation zum gewählten Roboter initiiert. Eine duale Konfigurationsdatei ermöglicht analog das Instanziieren und Ansteuern zweier robotischer Systeme. Dabei ist eine Fehlfunktion dieser Instanzen während einer Operation observierbar, wobei die Planung und Exekution durch eine vermeintlich unbeabsichtigte Kollision zwischen Endeffektor und Primitiv detektiert wird, woraufhin die auszuführende Handlung negativ terminiert. Dies impliziert, dass die Funktionen des MoveGroup Interface hinsichtlich der Betrachtung mehrerer robotischer Systeme limitiert sind. Der \autoref{lst:5} zeigt einen Ansatz, der die Kollisionsdetektion definierter Kollisionsobjekte in der allowed kollision matrix 'AKM' deaktiviert, was Beispielsweise das Greifen, Halten und Abstellen eines Primitives in einem Dualen Setup ermöglicht. 


\begin{lstlisting}[language=JSON,firstnumber=1, caption={Des Endeffektors rechter und linker Finger kollidieren unweigerlich während der Exekution mit dem Primitiv. Der illustrierte Quelltext Auszug stellt einen Ansatz zur Umgehung dar und ermöglicht eine erfolgreiche Terminierung des Vorgangs.}, label={lst:5}, captionpos=b]
// Initialisierung der Szene
planning_scene::PlanningScene ps(kinematic_model);

// Generierung der Bewegungsgruppen anhand der festgelegten Namensattribute innerhalb der dualen Konfigurationsdatei
std::vector<moveit::planning_interface::MoveGroupInterface>  robots;
for(int i = 1; i <= 2; i++)
	robots.push_back(std::move(moveit::planning_interface::MoveGroupInterface("panda_arm"+ std::to_string(i))))
	
// Modifikation der Kollisionsmatrix, welche die erlaubten Kollisionen der robotischen Systeme anhand deren Adjazenzen enthaelt 
collision_detection::AllowedCollisionMatrix acm = ps.getAllowedCollisionMatrix();

for(int i = 1; i <= 2; i++){
	acm.setEntry("object" + std::to_string(i), "panda_" + std::to_string(i+1) + "_leftfinger", true);
	acm.setEntry("object" + std::to_string(i), "panda_" + std::to_string(i+1) + "_rightfinger", true);
}

// Publikation der Aenderungen als Inhalt einer Planning Scene Nachricht
moveit_msgs::PlanningScene pls;
acm.getMessage(pls.allowed_collision_matrix);
pls.is_diff = true;
planning_scene_diff_publisher.publish(pls);
\end{lstlisting}


\section{Implementierung zur Positionsermittlung}
Die tatsächliche Implementierung ist Inhalt des Reach Pakets, dessen Struktur die \autoref{fig:10} illustriert. An dieser ist ersichtlich, dass die Artefakte aus den kinematischen Berechnungen, beispielsweise der Arbeitsraumanalyse oder dessen Inversion, in den zugehörigen Ordnern persistiert werden, während anderweitig generierte Dateien, wie der Aufgabenbeschreibung und Positionsbeschreibung, an anderer Stelle vorliegen. Das Rviz Verzeichnis beinhaltet alle Konfigurationen der spezifischen Nodes im RViz Kontext, dessen Resultate anhand visueller Nachrichten publiziert werden. Dies impliziert, dass relevante Aspekte eines Nodes jederzeit durch das RViz Programm transparent observiert werden können.

\begin{figure}[h!]
\centering
\begin{minipage}{.55\linewidth}
\dirtree{%
.1 Reach.
.2 include.
.2 launch.
.2 maps.
.3 inverse.
.3 reach.
.2 rviz.
.2 src.
.2 task.
.2 position.
}
\end{minipage}\hfill
\caption{Visualisierung aller Komponenten des Reach Verzeichnisses und deren Beziehung untereinander.}\label{fig:10}
\end{figure}


\subsection{Implementierung der Arbeitsraumanalyse}
Der \autoref{lst:6} der Header Datei beschreibt alle Funktionen, die den Algorithmus der Arbeitsraumanalyse implementieren. Dazu sind gemäß der Erörterungen im 'Konzept' Kapitel, neben der kubischen Diskretisierung zusätzliche sphärische Diskretisierungen formuliert. Die Planung des Endeffektors zu einer generierten Endeffektor Pose erfolgt über das MoveGroup Interface, angewandt auf der Konfigurationsdatei eines einzelnen robotischen Systems. Dabei ist eine Implementierung für performante Rechner enthalten, die Anhand der Aufteilung der Menge $T$ in die jeweiligen Rechenkerne alle kinematischen Berechnungen $f_{kin}$ parallel operiert.

\lstinputlisting[language=C++, caption ={Implementierung zur Generierung der $\protect RM$}, label={lst:6}, captionpos=b ]{./images/publisher\_node.h}

\subsection{Implementierung des Inversen Arbeitsraums}
Der Algorithmus des Inversen Arbeitsraums kalkuliert anhand der persistierten Informationen der Arbeitsraumanalyse die Metrik D. Die Inversion aller Transformationen anhand eine Iteration über den Arbeitsraum, erfolgt durch das tf2 Objekt der gleichnamigen Bibliothek für Operationen auf homogene Transformationen. Zusätzlich erfolgt die Portierung der Metrik und Persistenz.

\subsection{Der Algorithmus zur Positionsanalyse}
Die Implementierung der Positionsanalyse erfolgt über dessen Voxelisierung durch Integration der PCL Bibliothek. Diese ist in C++ vollumfänglich Implementiert und ermöglicht performante Operationen auf 3 dimensionale Koordinaten. Ein bedeutender unterschied des Algorithmus zur Platzierung robotischer Systeme in Reuleaux \cite{Makhal2018} und dessen Implementierung in dieser Arbeit ist die differenzierte Voxelisierung. Diese Implementiert zunächst ein Gitter aus Vektoren $^{0}v \in \R^{3}$ ähnlich der kubischen Diskretisierung, und bildet jede Transformation $^{0}T_{Base_{i}} \in ^{0}T_{Base}$ eines Kettenglieds der Operationskette $g_{i} \in K$ anhand seiner räumlichen Position auf den Vektor $^{0}v$ ab. Die resultierende Struktur ist in \autoref{eq:30} gezeigt.

\begin{equation}
\begin{split}
\label{eq:30}
Task &= {(i, ^{0}T_{Base_{i}}) | g_{i} \in OK} \\
Dictionary &= {(^{0}v, Task)}
\end{split}
\end{equation}

Eine Teilkette $TK \subset K$ definiert nun anhand ihrer Glieder $g_{i}$ die Einträge, welche der Vektor $^{0}v$ mindestens enthalten muss, um eine potentielle Position des robotischen Systems zu sein, welcher $TK$ operiert. Durch die Abbildung $D_{port}({0}T_{Base}) = D_{Reach}({0}T_{Base})$ ist Analog die Struktur $Task_{Metrik}$ implementiert, welche die portierten Metriken aller Transformation $^{0}T_{Base_{i}}$ auf den Vektor $^{0}v$ abbildet

\begin{equation}
\begin{split}
\label{eq:31}
Task_{Metrik} &= {(i, D_{port}(^{0}T_{Base_{i}})) | g_{i} \in K} \\
Dictionary_{Metrik} &= {(^{0}v, Task_{Metrik})}
\end{split}
\end{equation}

Die Ermittlung der optimalen Position $^{0}v$ für eine Teilkette $TK$ erfolgt, indem das arithmetische Mittel pro Kettenglied  $TK \in g_{i}$ kalkuliert wird und in Verhältnis zur Kardinalität $\vert TK \vert$ gesetzt wird. \par 
Die resultierende Position pro $TK$ wird in einem eigenen Verzeichnis persistiert, um als Parameter der 'Pick and Place' Implementierung geladen zu werden und über jede Instanz, über die Konfigurationsdatei der Roboterbeschreibung übergeben zu werden.  

