\chapter{Umsetzung}

\section{Grundlegender Systemaufbau}
Der grundlegende Systemaufbau musste stark modifiziert werden, wie in Abbildung \ref{umsetzung_overview} zu sehen, um die gesetzten Aufgaben erfüllen zu können.
Dabei fallen vor allem die überarbeitete Kommunikation mit dem simulierten Menschen und die komplexere Steuerung des Roboterarms auf.

Die komplexere Steuerung des Roboters ist auf die Struktur von MoveIt zurückzuführen, welches in viele einzelne Teilmodule aufgeteilt ist.
Diese müssen durch ihren modularen Aufbau, welcher für die Vielseitigkeit verantwortlich ist, einzeln konfiguriert werden, um miteinander in Interaktion treten zu können.

Außerdem musste die Kommunikation des Modells des Menschen überarbeitet werden, da die ROS-Kommunikation in Gazebo nur mit Einschränkungen möglich ist.

\begin{figure}[]
\includegraphics[width=\textwidth]{img/Umsetzung_Overview}
\centering
\caption{Visualisierung des überarbeiteten Konzepts}
\label{umsetzung_overview}
\end{figure}
\section{Mensch}
\subsection{Übersicht}
Das angepasste Verfahren zur Menschensteuerung in der Simulation verwendet mehrere Kommunikationswege.
Als erstes wird eine Bewegungs- oder Animationsanfrage an den ROS-Action-Server im ActorServer gesendet.
Wenn die Simulation aktuell keinen Befehl ausführt, wird diese Anfrage akzeptiert, ansonsten wird sie abgebrochen.
Daraufhin werden die Daten der Anfrage über eine Posix-Message-Queue vom ActorServer an das ActorPlugin in Gazebo gesendet.

Dieses verwendet die Daten, um eine interne State-Machine in den entsprechenden Zustand zu setzen, welcher zur Ausführung des Befehls benötigt wird.

Um Feedback an den Client des ROS-Action-Servers übertragen zu können, werden bei der Ausführung von Befehlen oder Zustandswechseln des ActorPlugins Feedbackdaten über eine separate MessageQueue zurück an den ActorServer übertragen.
Diese werden durch den ActorServer aufbereitet, da nicht alle Daten für die jeweilige laufende Aktion relevant sind und an den ROS-Action-Client gesendet.

Um diese Befehle in der Simulation auch visuell umsetzen zu können, werden weitere Animationen für das Modell des Menschen benötigt, welche im Kontext der zur erfüllenden Aufgabe relevant sind.
Dafür muss dan Modell in einen animierbaren Zustand gebracht werden, in welchem dann weitere Animationen erstellt und in die Simulation eingebunden werden können.
\subsection{Modellierung}
Um neue Animationen für den Menschen in der Simulation erstellen zu können, muss ein Modell für diesen erstellt werden.
Dafür wurde eine der inkludierten Animationen in Blender geöffnet und das visuelle Modell kopiert.

Dieses Modell war auf Grund von vielen inneren Falten nur schlecht für Animationen geeignet, weshalb das Modell an diesen Stellen vereinfacht wurde.
Eine solches Vorgehen beugt Anomalien bei der Animation durch unterschiedliche Verschiebung der Strukturen vor, welche vom inneren des Modells hervortreten können.

Nun musste das visuelle Modell mit neuen Animationsknochen versehen werden, da die in der Animation vorhandenen Knochen nicht verbunden sind.
Diese Knochen bilden ein Skelett, welches zur Animation bewegt werden kann.

In Blender können sogenannte Constraints verwendet werden, um die Gelenke in Gruppen zusammenzufassen und genauer zu spezifizieren.
Dazu wurde das Plugin ``Rigify'' verwendet, welches ein komplettes Skelett generiert und konfiguriert.

Dieses generierte Skelett kann nun an das Modell angepasst werden.
Um eine bessere Übersicht zu ermöglichen, sollten als erstes alle nicht benötigten Skeletteile, wie zum Beispiel für Gesichtsanimationen, entfernt werden.
Danach werden alle Knochen dem visuellen Modell angepasst.
Dabei muss auf die Ausrichtung der Knochen zueinander geachtet werden.
Das Kreuzprodukt der Vektoren beider Knochensegmente ist die Richtung der Beugeachse, welche sich im Verbindungspunkt beider Knochen befindet.
Ist diese nicht richtig ausgerichtet, wenn zum Beispiel beide Knochen auf einer Gerade liegen, verbiegen sich Gelenke bei der Verwendung von inverser Kinematik zur Positionsvorgabe falsch.

Das hier erstellte, verbesserte Rigify-Skelett kann nun einfacher animiert werden.
Dies liegt vor allem an neuen Markierungen in Blender, welche durch mehrere Constraints viele andere Knochen beeinflussen.
Beispielswise können Fuß- und Handmarkierungen gesetzt werden, welche die Rotation des Fußes oder der Hand, aber auch gleichzeitig die inverse Kinematik des gesamten Beins oder Arms automatisieren.
Selbiges gilt für neue Markierungen, welche zum Beispiel Hüft- und Kopfbewegungen vereinfachen.

Das Exportieren eines solchen Rigs ist jedoch schwierig, da viele Grafikengines keine verschachtelten Skelette verstehen.
Dies ist auch der Grund, warum die Skelette beim initialen Import mitgelieferter Animationen nicht verbunden waren.

Um aus einem existierenden, vollständig verbundenen Skelett einzelne Knochen zu extrahieren exisitiert ein weiteres Plugin mit dem Namen ``GameRig''.
Dieses separiert die Knochen wieder, um die Animationen für Grafikengines exportieren zu können.

Nach dieser Veränderung kann die Animation als Collada-Animation exportiert werden.
Dabei muss darauf geachtet werden, dass die vorwärts gerichtete Achse auf die richtige Achse gestellt ist.
Außerdem ist es später einfacher, wenn nur eine Animation in jeder Datei vorhanden ist.
Dies ist dem Fakt geschuldet, dass diese anderen Animationen zwar verfügbar sind, jedoch aber nur durch einen Index unterscheidbar sind, der von der Reihenfolge der exportierten Animationen abhängig ist.

\subsection{Programmierung}
\subsubsection{Message Queue}
Bei der Implementierung des ActorPlugins stellte sich heraus, dass die nun im ActorServer ausgelagerten Befehle mit den Befehlen im ros_control-Plugin kollidieren.
Dies geschieht, da beide Plugins rclcpp, eine Bibliothek zur Kommunikation mit ROS, verwenden.

In dieser Bibliothek wird eine globale Instanz angelegt, welche den Zustand des Kommunikationsprotokolls abbildet.
Da jedoch von beiden Plugins auf diesen Zustand zugegriffen wird, kommt es zur Problemen, da kein Synchronisationsmechanismus existiert.
Die dadurch entstehenden gleichzeitigen Zugriffe auf die selben Ressourcen führen zur Terminierung des Programms.

Eine Anpassung beider Plugins auf die gemeinsame Nutzung einer Ressource ist möglich, erfordert jedoch weitere Anpassungen, welche zeitlich nur schwer planbar sind.
Die Nutzung eines separaten Dienstes, welcher keinen globalen Kontext benötigt, ist die sicherste Lösung des Problems.
Durch einen solchen Dienst werden auch in Zukunft keine Plugins gestört, auch wenn sie selbigen Dienst zur Kommunikation verwenden.

Die Auswahl eines Dienstes wurde dabei aus einer Reihe an unterschielichen Möglichkeiten getroffen.
Eine REST-API hat den Vorteil, dass sie durch fast jede Programmiersprache genutzt werden kann, die Sockets unterstützt, hat jedoch keinen einheitlichen Feedbackmechanismus.
Die neueren Websockets bieten die Möglichkeit, bidirektional Daten zu übertragen und erlauben somit Feedback an das aufrufende Programm.
Beide Technologien basieren jedoch auf einem Webserver, welcher auf einem bestimmten Port des Systems ausgeführt werden muss, was Kollisionen mit anderen Serveices ermöglicht.
Die Portnummer kann zwar geändert werden, ist jedoch nicht einfach mit einer Komponente assoziierbar, was sie zu einer ``Magischen Zahl'' macht.
Dies sorgt für schlechte Lesbarkeit in einem wichtigen Teil des Kontrollflusses.
Außerdem besitzen beide Terchnologien durch TCP oder UDP und HTTP relativ großen Protokolloverhead, welcher bei den hohen Updateraten der Gazebo-Simulation zu Problemen führen könnte.

Eine andere Möglichkeit ist die Nutzung von ``shared memory'', einem geteilten Speicherbereich zwischen beiden Programmen.
Dieser kann zur bidirektionalen Kommunikation genutzt werden, da beide Programme auf den Speicher zugreifen können.
Alle Zugriffe auf den Bereich sind extrem schnell, was diese Technik ideal zur schnellen Datenübertragung zwischen Prozessen macht.
Durch das erlauben gleichzeitiger Zugriffe kann es hierbei vorkommen, dass die selbe Adresse gleichzeitig von einem Programm gelesen und von einem anderen geschrieben wird.
Die dabei gelesenen Daten können Schäden aufweisen, weswegen Zugriffe auf den Speicherbereich koordiniert werden müssen.

Die letzte betrachtete Methode ist die Verwendung einer Message Queue.
Hier wird im Betriebssystem ein Speicherbereich mit bestimmter Größe für den Datenaustauch reserviert.
Dieser Bereich besitzt ein Identifikationsmerkmal, mit welchem Anwendungen Zugriff auf diesen erlangen können.
Ein Programm kann in diesem Bereich Nachrichten ablegen, welche durch das andere Programm gelesen werden können.
Die Koordinierung der Zugriffe erfolgt dabei durch das Betriebssystem, was gleichzeitige Zugriffe, wie bei shared memory, aussschließt.
Hierdurch kommt es zu einem Anstieg an Latenzzeit, jedoch ist dieser ausreichend gering.

Die Wahl des Dienstes fiel auf eine MessageQueue, jedoch existieren unter Linux 2 unabhängige Implementationen.
Die erste Implementation ist die System V MessageQueue, und verwendet zur Identifikation einfache Integer.
Eine Spezialität dieser alten Implementation ist das Sortieren der Nachrichten nach Nachrichtentyp in der gleichen Warteschlange.
Die neuere Implementation der POSIX MessageQueue bietet einige weitere Funktionen, wie zum Beispiel aynchrone Benachrichtigungen bei neuen Nachrichten, Quality of Service und nutzt bis zu 256 Zeichen lange Strings zur Identifikation.

Die ausgewählte Implementation ist die neuere POSIX-Implementation einer Message Queue, da diese basierend auf den Erfahrungen mit der System V Implementation verbessert wurde.
\subsubsection{Nachrichten}
Die versendeten Nachrichten für den ActionServer, als auch für die Message Queue sind in den Paketen \code{ros_actor_action_server_msgs} und \code{ros_actor_message_queue_msgs} abgelegt.
Die beiden ActionServer definieren jeweils 3 Nachrichten, welche eine Startnachricht, das Feedback während der Laufzeit und eine Endnachricht beschreiben.

In der Startnachricht werden alle Daten, welche der Server für die Bearbeitung einer Anfrage benötigt, übergeben.
Dies geschieht, damit der Auftrag schon beim Start abgebrochen werden kann, sollte dieser nicht erfüllbar sein.

Die Feedbacknachrichten werden vom Server an den Client zurück gesendet, solange das Programm ausgeführt wird.
Dabei ist es Aufgabe des Programms, diese in beliebigen Abständen an den Client zu senden.

Die Endnachricht kann Rückgabewerte für die ausgeführte Aufgabe enthalten, falls diese benötigt werden.
Sie werden in diesem Projekt nicht genutzt, da das Beenden eines Auftrags immer mit einer einfachen Erfolgs- oder Misserfolgsmeldung quittiert wird.
\subsubsection{ActorServer}
Der ActorServer ist die Brücke zwischen ROS und dem ActorPlugin.
Es werden zwei ActionServer angeboten, welche jeweils Bewegungen oder Animationen des simulierten Menschen auslösen können.
Beide ActionServer prüfen zuerst, ob bereits eine andere Aktion ausgeführt wird.
Sollte dies der Fall sein, wird die Anfrage abgelehnt.
Im anderen Fall wird die Aufgabe akzeptiert und in das MessageQueue-Format übersetzt und an das ActorPlugin gesandt.
Hier kommt es zu einer forcierten Warteschleife, welche die Bestätigung der Aktion vom ActorPlugin in der Feedback-Queue wartet, um das Starten mehrerer gleichzeitiger Aktionen zu unterbinden.
Parallel werden alle eingehenden Feedback-Nachrichten der Message Queue des ActorPlugins in Feedback für die aktuell laufende Action umgewandelt.

Im Falle des Bewegungs-ActionServers werden mehrere Parameter benötigt.
Zuerst werden Animationsname und -diztanz benötigt, um die richtige Animation auszuwählen und die Bewegung mit der Animation zu synchronisieren.
Als Feedbacknachricht erhält der Client die aktuelle Pose des Actors im Simulationsraum.

Soll eine Animation über den Action Server abgespielt werden, wird auch hier ein Animationsname, jedoch auch eine Animationsgeschwindigkeit benötigt.
Die Feedbacknachricht enthält den Fortschritt der Animation als Gleitkommazahl.
\subsubsection{Gazebo Plugin}
Das ActorPlugin nutzt die Daten aus der Message Queue für Befehle, um diese in der Simulation umzusetzen.
Es werden dabei mehrere Zustände unterschieden.
\begin{description}
 \item[Setup]
  wird ausschließlich zu Simulationsbeginn verwendet, um alle benötigten Referenzen aus der Simualtionumgebung im Plugin zu hinerlegen, so dass diese in den anderen Zuständen genutzt werden können.
 \item[Movement]
  bedeutet die Ausführung einer Bewegung in potentiell mehreren Schritten.
  Zuerst wird die Distanz zum Zielpunkt geprüft.
  Ist diese ausreichend gering, wird nur eine Bewegung in die gewünschte Endausrichtung durchgeführt.
  Ist diese größer, dreht sich der Actor in Richtung des Ziels und bewegt sich anschließend dorthin.
  Falls die gewünschte Endrotation nicht einem Null-Quaternion entspricht, wird anschließend noch eine Rotation in die Zielrichtung durchgeführt.
 \item[Animation]
  entspricht der Ausführung einer Animation an der aktuellen Position des Actors.
  Diese kann durch einen Skalierungsfaktor beschleunigt oder verlangsamt werden.
 \item[Idle]
  ist der Zustand, welcher nach erfolgreicher Ausführung eines Befehls angenommen wird.
\end{description}

Das ActorPlugin besitzt kein Konzept des ActionServers und verlässt sich auf den ActorServer, welcher die Feedbacknachrichten in das richtige Format bringt.
Feedback wird in den Zuständen Movement und Animation in jedem Simulationsschritt gesendet.
Um auch Zustandsübergänge erkennen zu können, werden auch diese als Feedback an den ActorServer gesendet.
\section{Roboter}
\subsection{Übersicht}
\subsection{Modellierung}
Für den Kuka LBR iisy existiert kein Simulationsmodell für Gazebo und ROS, weswegen dieses Modell aus Herstellerdaten generiert wurden.
Die Maße und Form des Roboters wurden aus einer .stl-Datei des Arms generiert.

Diese Datei wurde dazu in die unterschidlichen Glieder aufgeteilt, welche danach in Blender weiter bearbeitet wurden.
Hiebei wurde die hohe Auflösung der Modelle reduziert, was sich in kleineren Dateien und Startzeiten der Simulation, aber auch in der Renderzeit der Simulation, auswirkt.
Außerdem wuden die Glieder so ausgerichtet, dass der Verbindungspunkt zum vorherigen Glied im Nullpunkt des Koordinatensystems befindet.

Um die Simulation weiter zu beschleunigen, wurden die Kollisionsboxen des Arms noch weiter vereinfacht, was die Kollisionsüberprüfung dramatisch beschleunigt.
Dabei werden stark simplifizierte Formen verwendet, welche das hochqualitative visuelle Modell mit einfachen Formen umfassen.

Diese Herangehensweise ist nötig, da Kollisionserkennung auf der CPU durchgeführt wird, welche durch komplexe Formen stark verlangsamt wird.

Um aus den generierten Gliedermodellen ein komplettes Robotermodell erstellen zu können, wurde eine .urdf-Datei erstellt.
In dieser werden die verwendeten Gelenktypen zwischen den einzelnen Gliedern, aber auch deren Masse, maximale Geschwindigkeit, maximale Motorkraft, Reibung und Dämpfung hinterlegt.
Diese Daten können später zur Simulation der Motoren genutzt werden, welche den Arm bewegen.

Die Gelenkpositionen sind dabei relative Angaben, welche sich auf das Glied beziehen, an welchem ein weiteres Glied über das Gelenk verbunden werden soll.
Alle kontrollierbaren Gelenke benötigen auch eine Gelenkachse, welche je nach Gelenktyp die mögliche Beweglichkeit bestimmt.

Alle hier erstellten Dateien wurden im Paket \code{iisy_config} zusammengefasst, um diese einfacher wiederauffinden zu können.
\subsection{MoveIt 2 Konfiguration}
Das somit erstellte Paket kann nun mit dem neu implementierten MoveIt Configurator um die benötigten Kontrollstrukturen erweitert werden.
\subsection{Details}
Das so erstellte Modell kann nun zur Laufzeit in Gazebo geladen werden.
Dafür wird das Paket \code{ros_gz_sim} verwendet, welches das \code{create} Programm beinhaltet.
Mit diesem Werkzeuk kann ein Modell unter einem bestimmten Namen anhand einer Datei oder eines übergebenen Strings in Gazebo importiert werden.
Das Modell kann dabei über Argumente im Raum verschoben und rotiert werden, falls diese Funktionalität benötigt wird.

Im Modell sind auch die verwendeten Gazebo-Plugins deklariert, welche für die Integration mit dem \code{ros_control} verantwortlich sind.

\section{Behavior Trees}
Alle Behavior Trees wurden im Paket \code{btree_trees} organisert, welche durch das Paket \code{btree_nodes} gelesen werden.
BehaviorTrees werden in \code{behaviortree_cpp_v3} als .xml-Dateien gespeichert.
Diese Dateien enthalten die Anordnung der Nodes selbst, aber auch weitere Konfigurationsmöglichkeiten in Form von Ein- und Ausgabeschnittstellen.

Solche Schnittstellen können verwendet werden, um Nodes generischer gestalten zu können.
So kann auf feste Parameter in den Nodes selber verzichtet werden, was das erstellen mehrerer Nodes für ähnliche Funktionalitäten verhindert.
Diese Daten können sowohl aus einem String ausgelesen werden, falls die ensprechende Funktion, welche diese in den Zieltyp übersetzt, implementiert wurde.
Aber sie können auch aus dem sogenannten Blackboard entnommen werden.

Das Blackboard ist ein System, welches die Nutzung von Variablen als Parameter für Ein- und Ausgänge erlaubt.
Diese werden im Hintergrund als eine Referenz auf den eigentlichen Wert gespeichert.
Eine solche Funktion erlaubt das weitere Zerlegen von Vorgängen innerhalb des BehaviorTrees.
Solche kleineren Nodes sind durch ihren limitierten Umfang universeller einsetzbar, da sie nur kleinere Teilprobleme betrachten, welche zu komplexeren Strukturen zusammengesetzt werden können.

Um die dadurch wachsenden Strukturen besser überblicken zu können, lassen sich Nodes als sogenannte SubTrees abspeichern.
Diese bilden dann in ihrer Gesamtheit eine neue Node, welche im BehaviorTree eingesetzt werden kann.
Um den Einsatz von Variablen innerhalb eines SubTrees zu ermöglichen, besitzt jeder SubTree ein separates Blackboard.
Dadurch kann ein Eingriff in äußere Funktionalität verhindert werden.

Natürlich sollte es auch möglich sein, Variablen an solche SubTrees zu übergeben.
Diese können, wie auch bei normalen Nodes, einfach als Parameter an den SubTree übergeben werden.
Die Bibliothek \code{behaviortree_cpp_v3} verbindet dann diese Werte und erlaubt die Datenübergabe zu und von dem SubTree.

\subsection{Asynchrone Nodes}
Da nicht jeder Prozess in einem einzigen Durchlauf des BehaviorTrees abgebildet werden kann, muss die Möglichkeit geschaffen werden, lang anhaltende Prozesse abzubilden.
Dies geschieht in in \code{behaviortree_cpp_v3} durch asynchrone Nodes.

Eine asynchrone Node besitzt neben den Zuständen SUCCESS und FAILURE auch noch die beiden anderen Zustände RUNNING und IDLE.

Der Zustand RUNNING steht dabei für eine Node, welche sich noch in der Ausführung befindet.
So lang dieser Zustand anhält, wird die Node nicht noch ein weiteres Mal gestartet, sondern nur der Zustand abgefragt.

Der IDLE-Zustand ist ein besonderer Zustand, welcher nur durch eine vollständige Ausführung erreichbar ist.
Er wird von der Node angenommen, nachdem ein RUNNING Zustand durch entweder SUCCESS oder FAILURE beendet wurde und darf sonst nicht verwendet werden.