\chapter{Umsetzung}

Bei der Umsetzung des geplanten Systemaufbaus kam es zu mehreren Problemen, welche den geplanten Systemaufbau, sowie dessen Komplexität, negativ beeinflussen.

Die Kommunikation zwischen dem Actormodell und dem Behavior Tree musste in mehrere Komponenten aufgeteilt werden, um Konflikte innerhalb der Simulationssoftware zu vermeiden.

Zudem ist die Bewegungsplanung mit MoveIt2 deutlich komplexer als vorerst angenommen.
Diese Komplexität entsteht aus der Interaktion mehrerer Komponenten, welche das Gesamtsystem zur Ausführung benötigt.
Alle Einzelsysteme mussten hierfür konfiguriert werden, um die Kommunikation dieser zu ermöglichen.

Anhand der hier genannten Änderungen wurde die Übersicht des Systems angepasst, welche in Abbildung \ref{umsetzung_overview} zu sehen ist.

\begin{figure}[h]
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Übersicht.drawio}
\centering
\caption{Visualisierung des überarbeiteten Konzepts}
\label{umsetzung_overview}
\end{figure}

\section{Docker-Compose}
Um Docker für die Verwaltung einer ROS-Installation verwenden zu können, müssen einige Anpassungen vorgenommen werden.
Da viele Anwendungen, unter anderem auch die Simulationsumgebung, eine Desktopumgebung benötigen, muss eine Zugriffsmöglichkeit geschaffen werden.

Diese Möglichkeiten können nicht durch Docker allein gelöst werden, da Befehle auf dem Hostsystem ausgeführt werden müssen, um die gewünschten Funktionen zu aktivieren.
Um diese Modifikationen trotzdem reproduzierbar zu machen, wurde ein Shellscript geschrieben, welches zum Starten des Containers verwendet wird.

Dieses Skript erstellt zuerst die benötigten Verzeichnisse für den Container, falls diese noch nicht existieren.
Danach werden die SSH-Keys des Hosts in den Container kopiert, um eine SSH-Verbindung zu ermöglichen.
Dieser Umweg über SSH ist nötig, da die benötigten Umgebungsvariablen für ROS sonst nicht in allen Fällen gesetzt werden können.

Außerdem werden die benötigten Zugriffe auf den lokalen X-Server durch den Container anhand dessen Hostname erlaubt.
Diese Änderung erlaubt es dem Container, Fenster auf dem Desktop anzeigen zu können, solange die benötigten SysFS-Dateien hereingereicht werden.
Dies geschieht durch Einträge in der compose.yml-Datei, welche diese als ``bind mount'' in den Container hereinreicht.

Um Zugriff auf die Grafikbeschleunigung des Systems zu erhalten, muss deren Repräsentation im SysFS unter \code{/dev/dri} hineingereicht werden.
Der Zugriff auf die Desktopumgebung, welcher bereits entsperrt wurde, wird nun durch das mounten von \code{/tmp/.X11-unix} erreicht.
Dabei handelt es sich um den Unix-Socket des X11 Displayservers.

Zum Starten des Containers muss der Befehl \code{./start.sh} im Verzeichnis der Containerinstallation ausgeführt werden, welcher die obengenannten Schritte ausführt und den Container startet.
Eine Verbindung zum Container kann aufgebaut werden, nachdem die Meldung \code{ros_1 | Ready to connect.} in der Konsole erscheint.

Dafür muss ein SSH-Client eingesetzt werden, welcher eine Verbindung zu der lokalen Netzadresse des Systems mit dem Benutzer \code{ros} aufbauen kann.
Der Port des SSH-Servers wird dabei durch die Deklaration im docker-compose.yaml an das Hostsystem durchgereicht.
Hierbei ist zu beachten, dass der SSH-Server im Container auf Port 2222 ausgeführt wird, was bei der Verbindung beachtet werden muss.

Nach der Verbindung wird automatisch die ROS2-Umgebung eingerichtet, welche sofort nach Verbindungsaufbau genutzt werden kann.
Um die erstellten Pakete zu kompillieren, kann das Skript \code{build.sh} im \code{workspace}-Verzeichnis verwendet werden.

\begin{minted}[breaklines,frame=single]{bash}
#!/bin/bash
pushd "$(dirname "$0")" || exit
colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja
popd || exit
\end{minted}

Dieses Skript nutzt \code{colcon}, um alle Pakete in \code{~/workspace}-Verzeichnis zu erstellen.
Dabei wird auch eine \code{compile_commands.json}-Datei im build-Unterordner erstellt, welche von Entwicklungsumgebungen zur Syntaxvervollständigung genutzt werden.
Um eine Nutzung in allen Umgebungen zu erlauben, wurde diese in das Hauptverzeichnis gelinkt, da einige Umgebung nur dort nach dieser Datei suchen.


Da der Kompilliervorgang parallel abläuft, erscheinen Informationen zu allen Paketen gleichzeitig, was das Finden eines Fehlers erschwert.
Um trotzdem alle wichtigen Informationen zu erhalten, kommt der Event-Handler \code{console_cohesion+} zum Einsatz, welcher die Ausgaben neu formatiert.
Durch diesen werden die Ausgaben gruppiert, und erst nach einen vollständigen Kompilliervorgang eines Pakets nacheinander ausgegeben.
Dies ermöglicht es, aufgetretene Fehler einfacher auf ein bestimmtes Paket zurückführen zu können, ohne das gesamte Log zu durchsuchen.
Hierbei ist es hilfreich, dass die verbleibenden Kompilliervorgänge abgebrochen werden, falls der Kompilliervorgang eines Pakets fehlschlägt.
Dadurch befindet sich der Fehlers immer im letzten Paket der Ausgabe, da alle anderen Prozesse abgebrochen und nicht ausgegeben werden.

\section{Entwicklungsumgebung}

Ein einfacher Texteditor ist für das Schreiben von ROS-Packages ausreichend und bietet bei der Arbeit mit Containern sogar einen großen Vorteil.
Das Editieren von Dateien ist mit einem Texteditor auch von außerhalb des Containers möglich.
Jedoch besitzt ein Texteditor nur wenige Funktionen einer vollständigen Entwicklungsumgebung, welche den Prozess der Softwareentwicklung beschleunigen.
Um diese Funktionen bieten zu können, analysieren Entwicklungsumgebungen den geschriebenen Code.
Dies geschieht meißt auf eine von zwei unterschiedlichen Weisen.

Die Entwicklungsumgebung kann eine interne Repräsentation des geschriebenen Codes generieren.
Diese Repräsentation kann nun genutzt werden, um die implementierten Funktionen der Entwicklungsumgebung bereitzustellen.
Um dies zu erreichen, muss für jede unterstützte Sprache Code geschrieben werden, welcher in die Entwicklungsumgebung integriert werden muss.

Als Alternative existiert das Language Server Protocol, kurz LSP, welches eine Schnittstelle in Form eines JSON-RPC Protokolls zur Verfügung stellt, um Infomationen über den Code an die Entwicklungsumgebung zu übergeben.
Dies erlaubt einer Entwicklungsumgebung, nur noch den benötigten Server der Sprache zu starten, um Informationen über den Code in einem standartisierten Protokoll zu erhalten.

Der große Vorteil des LSP ist, dass eine Entwicklungsumgebung alle Funktionen der Programmiersprache vollständig unterstützt, solange das Protokoll vollständig implementiert wurde und ein entsprechender Server für die Sprache existiert.

Jedoch können bestimmte Funktionen, welche beim Design des LSP nicht bedacht wurden, einfacher in interner Implementation umgesetzt werden.
Deswegen haben Entwicklungsumgebungen mit interner Coderepräsentation häufig mehr Funktionen, spezialisieren sich jedoch auf bestimmte Sprachen.

In diesem Projekt wurden mehrere Entwicklungsumgebungen mit den jeweils unterschiedlichen Verfahren verwendet.
Um diese mit dem Docker-Container verwenden zu können, müssen diese Umgebungen die Entwicklung auf entfernten Systemen unterstützten, da der Container vom ausführenden System getrennt ist.

Um dies zu ermöglichen, wird ein Teil der Entwicklungsumgebung im Container ausgeführt, um mit dem inneren Kontext der Maschine arbeiten zu können.
Dazu wird beim Start einer Verbindung zu einem entfernten System dieser Server auf das Zielsystem übertragen und gestartet.

Die anfängliche Entwicklung wurde mit PyCharm und CLion durchgeführt, da diese durch ihre interne Codeanalyse die ROS-Umgebung ohne Konfiguration nutzen konnten.
Jedoch sind diese Umgebungen sehr ressourcenintensiv, was die gleichzeitige Ausführung erheblich verlangsamt.

Aus diesem Grund wurde später eine Entwicklungsumgebung mit LSP-Unterstützung, in diesem Fall Lapce, verwendet.
Dazu wurden dem Container die benötigten LSP-Server hinzugefügt und konfiguriert, um diese mit Lapce nutzen zu können.
Unter Verwendung dieser neuen Server kann Lapce nun Codevervollständigung und Codeanalyse wie PyCharm und CLion durchführen.
Dabei wird auch der Ressourcenverbrauch gesenkt, da nur ein einziger Editor benötigt wird.

\begin{figure}[hpt]
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Lapce}
\centering
\caption{Entwicklungsumgebung Lapce}
\label{lapce}
\end{figure}

\section{Verwendete Datentypen}
In diesem Projekt werden viele unterschiedliche Datentypen verwendet.
Diese Datentypen sind größtenteils aus der Programmiersprache C++ bekannt, jedoch werden auch weitere Typen aus eigenem Code oder eingebundenen Bibliotheken verwendet.
Um die Verständlichkeit der Dokumentation zu erleichtern, sind die in der Implementation verwendeten Datentypen hier zusammengefasst und beschrieben.
\begin{description}
  \item[Pose]
  ist einer der häufigsten Datentypen im Umgang mit Gazebo. Dieser Datentyp enthält die Information über die Lage und Rotation eines Objekts im Raum.
  Diese werden als relative Werte im Bezug auf das übergeornete Objekt gespeichert.
  Objekte wie der Mensch liegen in der Hierarchie der Simulation direkt unter der Welt, welche sich direkt im Nullpunkt und ohne Rotation befindet.
  Dadurch sind zum Beispiel die Koordinaten des Menschen absolut, da diese nicht durch die Welt verschoben und rotiert werden.
  \item[Area]
  ist eine Datenstruktur mit einem Vektor an Positionen, welche eine Zone im Zweidimensionalen Raum definieren.
  Jede Position ist eine einfache Datenstruktur aus 2 Gleitkommazahlen für je die X- und Y-Koordinate der Position.
  Der Verwendungszweck dieser Struktur ist die einfache Definition von Zonen, welche für Positionsgenerierungen und Postionsabfragen genutzt werden können.
  \item[ActorPluginState]
  definiert 4 Werte, welche das ActorPlugin annehmen kann. Diese 4 Werte sind SETUP, IDLE, MOVEMENT und ANIMATION.
  \item[FeedbackMessage]
  beschreibt die erste der beiden MessageQueue-Nachrichten, welche vom ActorPlugin an den ActorServer gesendet wird.
  In dieser Struktur befindet sich der aktuelle Plugin-Zustand als \code{state} Parameter vom Typ ActorPluginState.
  Außerdem ein \code{progress} Parameter in Form einer Gleitkommazahl, welche den Fortschritt der aktuellen Aktion angibt.
  Um bei Bewegungen die aktuelle Position des Menschen zu erhalten, ist auch die aktuelle Pose des Modells im Parameter \code{current} enthalten.
  \item[ActionMessage]
  ist die andere Nachricht, welche über die zweite MessageQueue vom ActorServer an das ActorPlugin gesendet wird.
  Wie in der FeedbackMessage ist ein \code{state} Parameter vom selben Typ enthalten, jedoch dient dieser hier als Vorgabe für den nächsten State.
  Ein \code{animationName} Parameter wird als ein char-Array mit einer maximalen Länge von 255 Zeichen übergeben.
  Dieser bestimmt später die Animation, welche je nach ActorPluginState während einer Bewegung oder Animation ausgeführt wird.
  Der \code{animationSpeed} Parameter beschreibt entweder die Abspielgeschwindigkeit der Animation, oder die Distanz, welche pro Animationsdurchlauf zurückgelegt wird.
  Außerdem wird im Falle einer Bewegung der Parameter \code{target} vom Typ Pose verwendet, um die Endposition und Rotation des Actors zu bestimmen.
\end{description}
\section{Simulationswelt}
Die Welt der Simulation wird im Paket \code{ign_world} zur Verfügung gestellt.
In diesem Paket sind sowohl die Geometrien der Welt, aber auch die benötigten Dateien zum Starten der Simulation enthalten.

In diesem Fall handelt es sich um den Raum, in welchem die Interaktion zwischen Mensch und Roboter stattfinden soll.
Für diesen Raum wurde zuerst ein Raumplan erstellt, welcher alle benötigten Bereiche für die Szenarien besitzt (Abbildung \ref{room-plan}).

Zuerst wird ein Stellplatz für den Roboter benötigt, welcher an einer Ecke des Raumes positioniert werden sollte.
Diese Position wurde gewählt, um die Verfahrgeschwindigkeit des Roboters höher zu halten, welche je nach Distanz zum Menschen angepasst werden soll.

Des weiteren werden eine Arbeits- und Lagersätte für den Menschen benötigt, welche in den Szenarien genutzt werden sollen.
Im Koexistenzszenario soll der Mensch nur an diesen Stellen seine Arbeit verrichten, jedoch sich selten dem Roboter nähern, um dessen Fortschritt zu begutachten.
Die Lagerstätte soll im Kooperationsszenario neben der Arbeit des Menschen auch für die fehlerhaften Teile verwendet werden, welche durch den Roboter aussortiert und durch den Menschen dorthin verbracht werden sollen.
Eine Nutzung im Kollaborationsszenario ist nicht nicht geplant, da der Mensch den Roboter überwachen und dessen Fehler korrigieren soll.

Der so geplante Raum wurde in Blender modelliert und als .stl-Datei exportiert, um sie in die Welt einbinden zu können.
Für das Kooperationsszenario wurde ein Förderband moddeliert, welches in diesem Szenario dem Raum hinzugefügt wird.

Der so erstellte Raum wird in einer .sdf-Datei als Modell referenziert, um diesen später in die Simulation einbinden zu können.
Das Förderband erhält ein eigenes Modell in einer weiteren Datei, welche zur Laufzeit in die Simulation geladen wird.
Für beide wird, wie später auch für den Roboter selbst, das Paket \code{ros_gz_sim} verwendet.
Dieses veranlasst mit dem \code{create}-Programm das Erstellen der übergebenen Datenstruktur in Gazebo.

\begin{figure}[hpt]
\begin{minipage}{.45\textwidth}
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Welt-Plan.drawio}
\centering
\caption{Geplanter Raum}
\label{room-plan}
\end{minipage}
\hspace{.09\textwidth}
\begin{minipage}{.45\textwidth}
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Welt-Blender}
\centering
\caption{Umsetzung in Blender}
\label{room-finished}
\end{minipage}
\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 in Form des \code{ActionMessage}-Typs über eine Posix-Message-Queue vom ActorServer an das ActorPlugin in Gazebo gesendet.

Anhand der \code{ActionMessage} wird die State-Machine im ActorPlugin in den richtigen Zustand für die angefragte Aktion gesetzt.
Um diese ausführen zu können, wird der gewünschte Animationsname, aber auch andere Parameter für die Aktion aus der Nachricht übernommen.

Das Feedback an den Client des ROS-Action-Servers wird bei Zustandswechseln und während laufenden Aktionen des ActorPlugins generiert.
Dabei wird der aktuelle Zustand mitsamt einigen weiteren Daten, in diesem Fall die Position des Actors und der aktuelle Fortschritt der Aktion an den ActorServer gesendet.
Dies geschieht über eine zweite MessageQueue, welche den \code{FeedbackMessage}-Datentyp überträgt.
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 späteren Anomalien bei der Animation des Modells vor.
Diese entstehen durch unterschiedliche Verschiebung der Strukturen, welche aus dem inneren des Modells hervortreten, wenn dieses bewegt wird.



An diesem Punkt könnte die Animation des Modells mit dem importierten Skelett beginnen, jedoch fehlen diesem viele Knochen, wie zum Beispiel für Finger und Zehen, aber auch Rotationsknochen für Arme und Beine.
Diese fehlenden Knochen werden jedoch für einige der gewünschten Animationen benötigt und müssten hinzugefügt werden.
Da das importierte Skelett noch andere Fehler aufwies, wurde dieses verworfen.

Um ein neues, passendes Skelett zu erstellen, wurde mit dem ``Rigify''\cite{rigify}-Plugin ein standartisiertes Menschenskelett generiert.
Dieses neue Skelett kann nun an das bereits vorhandene 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.
Nun müssen die Knochen durch Verschiebung und Skalierung an die richtigen Positionen im Modell gebracht werden.

Dabei muss auf die Ausrichtung der Knochen zueinander geachtet werden.
Das Kreuzprodukt der Vektoren beider Knochensegmente bestimmt 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.
Der Grund dafür ist das Kreuzprodukt, welches im oben genannten Fall ein Nullvektor ist, wodurch keine Beugeachse bestimmt werden kann.

Deswegen muss bei der Platzierung darauf geachtet werden, dass der Startpunkt A des ersten und der Endpunkt C des zweiten Knochens auf einer Gerade liegen.
Der Verbindungspunkt B der beiden Knochen wird nun vorerst auf dieser Gerade platziert.
Dieser muss nun senkrecht zu dieser Gerade und der gewünschten Biegeachse verschoben werden, wie in Abbildung \ref{bend} gezeigt.

\begin{figure}[hpt]
\begin{minipage}{.45\textwidth}
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Person-Bones}
\centering
\caption{Knochen des Modells}
\label{person-bones}
\end{minipage}
\hspace{.09\textwidth}
\begin{minipage}{.45\textwidth}
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Person-Armature}
\centering
\caption{Armaturen des Modells}
\label{person-armature}
\end{minipage}
\end{figure}

\begin{figure}[hpt]
\includegraphics[width=.8\textwidth]{img/MA-Umsetzung-Joint}
\centering
\caption{Visualisierung der generierten Beugeachse}
\label{bend}
\end{figure}

Das neu erstellte Skelett ist in Abbildung \ref{person-bones} visualisiert.

Um eine bessere Verformung bei der Bewegung von Knochen zu erreichen, wird das so genannte ``weight painting'' eingesetzt.
Hierfür werden für jeden Knochen entweder automatisch oder manuell Teile des Meshes mit Gewichten versehen.
Je höher die Wichtung, desto stärker ist die Verformung an dieser Stelle, wenn der Knochen bewegt oder skaliert wird.

Da das Animieren aller Knochen einzeln sehr zeitaufwändig ist, werden diese in Gruppen zusammengefasst.
Hierfür werden in Blender sogenannte Constraints eingesetzt, welche Knochen automatisch durch eingestellte Bedingungen positionieren können.

Durch das Verwenden von Rigify und einem standartisierten Skelett ist die Generierung der Constraints einfach.
Hierfür können die in dem Skelett enthaltenen Informationen vom Plugin genutzt werden, um alle häufig genutzten Constraints automatisch zu generieren.
In diesem Schritt werden auch neue Knochen eingefügt, welche das Skelett durch Constraints beeinflussen.
Das neue Animationsmodell, visualisiert in Abbildung \ref{person-armature}, kann nun für Animationen genutzt werden.

Hierfür sehen mehrere Knochengruppen zur Verfügung, welche typische Animationsmethoden abdecken.

In Rot sind hier Knochen markiert, welche für inverse Kinematik genutzt werden, in diesem Fall Arme und Beine.
Diese Knochen geben gewünschte Ausrichtung und die Zielposition des untersten Knochens vor.
Aus diesen Angaben wird die wirkliche Position der Knochen berechnet, welche durch die Constraints automatisch auf diese übertragen wird.

Orange gefärbte Knochen werden für generelle Einstellungen von Fingern, Handfläche und Zehen genutzt.
Die Handfläche kann so gekrümmt werden, wodurch sich alle Finger mit der Krümmung mitbewegen.
Ein Abknicken aller Zehen kann durch die Rotation des Hilfsknochens an den Zehen erreicht werden.
Die Finger können auch einzeln rotiert und eingeknickt werden.
Hierbei wird der Grad des Einknickens nicht durch eine Rotation des Knochens ausgedrückt, da diese bereits durch die Rotation des Fingers selbst benutzt wird.
Anstatt der Rotation wird die Skalierung des Knochens eingesetzt, wobei ein kleinerer Knochen eine stärkere Knickung aller Fingerglieder bewirkt.

Die gelben Knochen beeinflussen die generelle Pose des Modells.
Der Quader in der Hüfte gibt die gewünschte Höhe des Modells vor, welche auch für die inverse Kinematik benutzt wird.
Die anderen Knochen beeinflussen die Rotation des Beckens, der Wirbelsäule, der Schultern und des Kopfes.

Das hier erstellte, verbesserte Rigify-Skelett kann nun durch den Einsatz der neuen Constraints einfacher animiert werden.
Jedoch ist das Exportieren eines solchen Rigs ist schwierig, da viele Grafikengines verschachtelten Skelette und Constraints nicht verwenden können.

\subsection{Export der Modellanimationen}

Um aus einem existierenden, vollständig verbundenen Skelett einzelne Knochen zu extrahieren, exisitiert ein weiteres Plugin mit dem Namen ``GameRig''\cite{gamerig}.
Dieses separiert die neuen Steuerknochen wieder vom ursprünglichen Modell, wodurch in diesem nur noch die deformierenden Knochen enthalten sind.

Alle erstellten Animationen der Steuerknochen müssen nun in direkte Bewegungen der deformierenden Knochen des Modells umgewandelt werden.
Um dies zu erreichen wird der in Abbildung \ref{export-prepare} visualisierte Arbeitsablauf verwendet.

Im ersten Schritt wird das zu exportierende Skelett ausgewählt, um diesem später die neue Animation zuweisen zu können.
Dann muss im zweiten Schritt die gewünschte Animation der Liste der bekannten Animationen hinzugefügt werden.

Danach muss die durch die Constraints abgebildete Animation auf das ausgewählte Skelett übertragen werden, was mit dem ``Bake Action Bakery''-Knopf ausgelöst wird.
Dabei wird die Position aller deformierenden Knochen zu jedem Zeitpunkt der Animation bestimmt, und in einer neuen Animation mit modifiziertem Namen abgespeichert.

Diese neu erstellte Animation kann nun im vierten Schritt dem ausgewählten Skelett zugewiesen werden.
Nach dieser Veränderung kann die diese Animation als Collada-Animation exportiert werden.
Dazu muss noch das visuelle Modell der Selektion hinzugefügt werden, da Gazebo dieses für jede Animation benötigt.

Nun kann der Export über die in Blender integrierte Exportoption ausgelöst werden.
Hierfür müssen die in Abbildung \ref{export-settings} verwendeten Exporteinstellungen verwendet werden, damit Gazebo die exportierte Animation nutzen kann.
Alle in der Blender-Version 3.5 nicht standartmäßigen Einstellungen wurden dabei durch Punkte hervorgehoben.

Zuerst müssen im Reiter ``Main'' die globalen Exporteinstellungen angepasst werden.
Der markierte Punkt 2. bewirkt, dass nur die vorher ausgewählten Modellteile exportiert werden.
Dies verkleinert das Modell, da alle Steuerknochen von Gazebo nicht verwendet werden können, was die Ladezeit der Simulation verbessert.

Punkt 3. und 4. bewirken, dass das exportierte Modell in Gazebo verwendete Vorwärtsachse erhält.
In Blender ist Y als Vorwärtsachse üblich, jedoch verwendet Gazebo die X-Achse für diesen Zweck.
Wird diese Modifikation nicht vorgenommen, sind die Modelle um 90 Grad verdreht.

Nun muss im mit 5 markierten Animations-Reiter noch eine letzte Einstellung vorgenommen werden.
Die mit 6 markierte Einstellung bewirkt, dass alle Bewegungen der exportierten Knochen mit gespeichert werden, auch wenn diese sich nicht bewegen.
Da sich einige Knochen nur am Anfang der Animation in einer bestimmten Pose befinden, exportiert Blender diese nicht, was in Gazebo zu verdrehten Knochen führt.

\begin{figure}[hpt]
\begin{subfigure}[b]{\textwidth}
\includegraphics[width=.5\textwidth]{img/MA-Umsetzung-Animation-Prepare}%
\hfill
\includegraphics[width=.5\textwidth]{img/MA-Umsetzung-Animation-Prepare2}
\end{subfigure}
\caption{Vorbereitung zum Export mit GameRig}
\label{export-prepare}
\end{figure}

\begin{figure}[hpt]
\begin{subfigure}[b]{\textwidth}
\includegraphics[height=.50\linewidth]{img/MA-Umsetzung-Animation-Save}%
\hfill
\includegraphics[height=.50\linewidth]{img/MA-Umsetzung-Animation-Save2}
\end{subfigure}
\caption{Benötigte Exporteinstellungen in Blender}
\label{export-settings}
\end{figure}

\subsection{Programmierung}
\subsubsection{Message Queue}
Nach der ersten Implementation des ActorPlugins kam es zu Kollisionen mit \code{ros_control}, welche beide \code{rclcpp} zur Kommunikation benutzen.
Diese Kollisionen führt zu einem Crash, wenn beide Plugins in der Simulation geladen werden.
Dies geschieht, da beide Plugins rclcpp, eine Bibliothek zur Kommunikation mit ROS-Topics, 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.

Nach dem Debuggen des Crashes wurden einige Problemlösungen ausprobiert, jedoch ließ sich der Konflikt durch Modifikation des ActorPlugins allein nicht verhindern, weshalb ein anderer Weg zur Problembehebung gewählt werden musste.

Eine Anpassung beider Plugins auf die gemeinsame Nutzung einer Ressource ist möglich, erfordert jedoch potentiell weitreichende Neuimplementationen, welche zeitlich nur schwer planbar sind.
Die Nutzung eines separaten Nachrichtendienstes, welcher keinen globalen Kontext benötigt, ist die sicherste und schnellste Lösung des Problems.
Jedoch erfordert diese Implementation zusätziliche Logik, um die beiden Dienste ineinander übersetzen zu können.

Die Auswahl eines Dienstes wurde dabei aus einer Reihe an unterschielichen Möglichkeiten getroffen.
Webdienste, wie zum Beispiel REST-API's und Websockets werden von vielen Sprachen nativ untersützt, was sie zu guten Kanidaten für solche Kommunikation macht.

Eine REST-API hat den Vorteil, dass sie durch fast jede Programmiersprache genutzt werden kann, die Sockets unterstützt, besitzt jedoch keinen einheitlichen Feedbackmechanismus.
Dieser müsste entweder durch kontinuierliche Abfragen des aktuellen Status oder eine lang laufende Request mit zerstückelter Antwort realisiert werden.
Eine Verwendung von kontinuierlichen Abfragen ist zwar mit fast jeder Programmiersprache möglich, jedoch ist die Reaktionszeit des Systems an die Antwortzeit des Servers gebunden.
Der Einsatz von zerstückelten Antworten umgeht dieses Problem, jedoch müssen der eingesetzte Webserver und Client dieses Feature unterstützen.

Die neueren Websockets bieten die Möglichkeit, bidirektional Daten zu übertragen.
Dadurch können Aktionsanfragen und Feedback auf dem gleichen Kanal übertragen werden, was das Protokoll übersichtlicher macht.
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 gleichen Speicherbereich zugreifen können.
Alle Zugriffe auf den Bereich sind extrem schnell, da sie den Arbeitsspeicher des Systems nie verlassen, was diese Technik ideal zur Datenübertragung zwischen Prozessen macht.
Durch das erlauben gleichzeitiger Zugriffe kann es hierbei vorkommen, dass beide Programme gleichzeitig versuchen, Änderungen am Speicher vorzunehmen.
Diese gleichzeitige Modifikation kann zu Fehlern führen, wenn die Zugriffe auf den Bereich nicht kontrolliert werden.

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, da die integrierten Funktionen die Entwicklung erleichtern ud die Geschwindigkeit ausreichend für das Einsatzszenario ist.
Jedoch existieren unter Linux 2 unabhängige Implementationen von MessageQueues mit unterschidelichen Funktionen.
Die erste Implementation ist die System V MessageQueue, und verwendet zur Identifikation einfache Ganzzahlen.
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 Zeichenketten 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 entwickelt wurde.
\subsubsection{ROS-Nachrichten}
Die verwendeten Nachrichten für den ActionServer, als auch für die Message Queue sind in den entsprechenden Paketen \code{ros_actor_action_server_msgs} und \code{ros_actor_message_queue_msgs} organisiert.
Sie sind absichtlich nicht in den nutzenden Paketen untergebracht, da sie durch ein externes Programm in den benötigten Code umgewandelt werden.
Dieser Schritt muss vor dem Kompilliervorgang der nutzenden Pakete geschehen, was so am einfachsten realisiert werden kann.
Dazu werden diese Nachrichtenpakete als Dependency der nutzenden Pakete angegeben, was eine automatische Anpassung der Kompillantionsreihenfolge bewirkt.

Eine Action des \code{ros_action}-Pakets besteht immer aus 3 einzelnen Nachrichten, deren Inhalt frei definiert werden kann.

Zum Start der Action wird die erste Nachricht vom Client an den Server gesendet.
In dieser Startnachricht befinden sich alle Informationen, welche zur Ausführung der Action benötigt werden.
Nach dieser Nachricht kann der Server später entscheiden, ob die Aktion ausgeführt werden soll und sie gegebenenfalls abbrechen.

Nach dem Beginn der Action werden vom Server Feedbacknachrichten an den Client gesendet, welche den Fortschritt der Action beschreiben.
Dabei ist es die Aufgabe des Programmierers, diese an sinnvollen Punkten des Ablaufs zu senden.

Am Ende der Action wird die Endnachricht an den Client gesendet, welche weitere Rückgabewerte liefern kann.
Die Endnachricht enthält standartmäßig eine Erfolgsangabe, weshalb diese nicht angegeben werden muss.

Ein ActionServer innerhalb eines Programmes definiert 3 Funktionen, welche die Handhabung einer Aktion definieren.
Die erste Funktion übergibt den Wert der Startnachricht, welche mit einer Antwort quittiert werden muss.
Hierbei sind die Antworten ACCEPT_AND_DEFER, ACCEPT_AND_EXECUTE und REJECT möglich.

ACCEPT_AND_EXECUTE signalisiert die sofortige Ausführung des gewünschten Befehls und ACCEPT_AND_DEFER steht für eine spätere Ausführung der gewünschten Aktion.
Die Option REJECT weist die Ausführung der Aktion vorzeitig ab.

Die zweite Funktion übergibt Abbruchanfragen an den Server, falls die Aktion durch den Client abgebrochen werden soll.
Auch diese Anfrage kann entweder mit ACCEPT akzeptiert werden, oder mit REJECT zurückgewiesen werden.

Um Feedback während der Ausführung der Aktion geben zu können, exisitiert die dritte Funktion.
Diese erhält als Parameter ein Objekt, mit welchem Feedback- und Endnachrichten an den Client gesendet werden können.
\subsubsection{ActorPlugin}
Das ActorPlugin steuert anhand von empfangenen Nachrichten aus der eingehenden Message Queue den Menschen in der Simulationsumgebung.
Der Code des Plugins ist dabei im Paket \code{ign_actor_plugin} organisiert, welches im Gazebo-Modell der Welt referenziert werden muss, um das Plugin zu laden.

Das Plugin wird durch den Startvorgang und später von den empfangenen Nachrichten in mehrere Zustände versetzt, welche im folgenden erläutert werden:
\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[Idle]
  ist der Zustand, welcher nach erfolgreicher Ausführung eines Befehls angenommen wird.
 \item[Movement]
  bedeutet die Ausführung einer Bewegung in potentiell mehreren Schritten.
 \item[Animation]
  entspricht der Ausführung einer Animation an der aktuellen Position des Actors.
  Diese kann durch einen Skalierungsfaktor beschleunigt oder verlangsamt werden.
\end{description}

In diesen Zuständen muss das ActorPlugin die Simulationsumgebung beeinflussen, in dem die simulierte Person bewegt und animiert wird.
Dies erfordert den Zugriff auf simulationsinterne Daten, welche durch das Plugin gelesen und verändert werden sollen.
Der Zugriff auf diese Daten wird durch ein EntityComponentManager-Objekt ermöglicht.

Durch den EntityComponentManager kann auf das so genannte ``Entity Component System'' zugegriffen werden, in welchem alle Simulationsobjekte organisiert sind.
Ein Entity Component System besteht aus einer oder mehr Entities, welche die sich in der Simulation befindlichen Objekte abbilden.
Objekte können beliebig viele Components besitzen, bei welchen es sich um zusätzliche Eigenschaften und Erweiterungen der Funktionen des betroffenen Objekts handelt.
Die wichtigsten Komponenten für die Funktion des Plugins sind dabei:
\begin{description}
 \item[components::Actor]
  Dieses Objekt beschreibt die simulierte Person mit allen weiteren Daten, wie zum Beispiel dessen Animationen.
 \item[components::AnimationName]
  In dieser Komponente wird der aktuell verwendete Animationsname abgelegt, welcher von Gazebo zur Auswahl der aktuell laufenden Animation verwendet wird.
 \item[components::AnimationTime]
  enthält den aktuellen Zeitpunkt innerhalb der Animation, um die richtigen Positionsdaten aus dieser auswählen zu können. Dabei wird zwischen einzelnen Positionen linear interpoliert, falls der Zeitpunkt zwischen diesen liegt.
 \item[components::Pose]
  entspricht einer bereits beschriebenen Pose, jedoch verwendet Gazebo dafür diesen neuen Datentyp.
 \item[components::TrajectoryPose]
  beschreibt eine Pose während einer Bewegung. Ist diese nicht gesetzt, werden keine Animationen abgespielt.
\end{description}

Durch das Verändern dieser Komponenten kann nun die gewünschte Funktionalität des Plugins erreicht werden.
Diese Veränderungen müssen jeden Simulationschritt erneut vorgenommen werden, um einen flüssigen Ablauf zu gewährleisten.
Um dies zu erreichen, können im Plugin bestimmte Interfaces mit entsprechenden Funktionen implementiert werden.\cite{ignPlugin}
Diese Funktionen werden später in der Simulation aufgerufen.
Folgende Funktionen werden von Gazebo unterstützt:
\begin{description}
 \item[Configure] wird aufgerufen, sobald das Plugin geladen wurde.
 Alle benötigten Informationen über dessen Umgebung werden als Parameter übergeben.
 Darunter fallen die Entity, zu welcher das Plugin als Component hinzugefügt wurde, eine Referenz auf die Konfiguration des Plugins in der geladenen Welt und eine aktuelle Instanz des EntityComponentManagers und EventManagers von Gazebo.
 \item[PreUpdate] wird verwendet, um den nächsten Update-Schritt vorzubereiten.
 Diese Funktion erlaubt die Modifikation von Entities und Components und wird vor der eigentlichen Physiksimulation aufgerufen.
 \item[Update] wird genutzt, um die in der Simulation abgelaufene Zeit zu simulieren.
 Auch hier ist die Modifikation von Entities und Components möglich.
 In dieser Funktion werden Beispielsweise die Physiksimulation der Objekte durchgeführt.
 \item[PostUpdate] bietet die Möglichkeit, die Ergebnisse der Simulation vor dem nächsten Simulationsschritt auszulesen.
 Dies wird zum Beispiel für die simulierten Sensoren in Gazebo genutzt, welche hier ihren Status prüfen.
 In dieser Funktion kann nur lesend auf Entities und Components zugegriffen werden.
\end{description}
Jede dieser Funktionen ist in einem Interface mit dem Präfix \code{ISystem} definiert, welche vom Plugin genutzt werden können.
Für Gazebo muss das Plugin noch registriert werden, was mit dem \code{IGNITION_ADD_PLUGIN}-Makro geschieht.
Mit diesen Vorbereitungsschritten wird das Plugin von Gazebo verwendbar gemacht.

Das ActorPlugin benötigt für seine Funktion das übergeornete Simulationsobjekt, welches durch das Plugin beeinflusst werden soll.
Dies erfordert die Implementation des \code{ISystemConfigure}-Interfaces für das ActorPlugin.
Außerdem soll die simulierte Person bewegt werden. Da dieser Vorgang während der Simulationszeit ablaufen soll, muss hierfür das \code{ISystemUpdate}-Interface genutzt werden.

Als erstes wird durch das Laden die Configure-Funktion aufgerufen.
Da das Plugin erst durch Gazebo geladen werden muss, kann davon ausgegangen werden, dass alle benötigten Message Queues bereits durch den ActorServer erstellt wurden.
Trotz dieses Umstandes wartet das Plugin auf deren Erstellung, um bei Konfigurationsfehlern auffälliges Verhalten zu zeigen.
Im Log wird diese Aktion vermerkt, um Debugging zu erleichtern.
Nach dem erfolgreichen Aufbau der Verbindung wird ein Thread gestartet, welcher die eingehenden Nachrichten verarbeitet.
Dabei wird zuerst ein Mutex gesperrt, um parallele Zugriffe durch die Update-Funktion zu verhindern.
Alle in der Anfrage gespeicherten Daten werden nun übernommen und der gewünschte State gesetzt.
Nach dem Entsperren des Mutex wird im nächsten Simulationschritt die gewünschte Aktion durchgeführt.

Das Setzen aller obrigen Zustände ist dabei möglich, jedoch werden nur Idle, Animation und Movement genutzt.
Der Idle-Zustand kann zum Beenden eines anderen Zustands eingesetzt werden, falls dieser abgebrochen werden soll.
Die Zustände Animation und Movement werden gesendet, falls neue Ziele gesetzt werden sollen.
Da beide Zustände Animationen verwenden, wird bei deren erster Ausführung in der Update-Funktion die gewünschte Animation gesetzt.
Um dies zu ermöglichen, werden alle in der Actor-Komponente gespeicherten Animationen durchsucht.
Sollte die Animation nicht gefunden werden, wird versucht, diese anhand ihres Namens zu laden.
Wurde eine Animation gefunden, wird deren Länge gespeichert, um diese später für das Feedback nutzen zu können.

Ab diesem Zeitpunkt unterscheiden sich die Implementation von Animation und Movement.
Im Zustand der Animation wird nun die Komponente AnimationTime jedes Update aktualisiert.
Um die Ausführungsgeschwindigkeit einer Animation anpassen zu können, kann ein Skalierungsfaktor genutzt werden, welcher mit der abgelaufenen Zeit in der Simulation verrechnet wird.
Nach jedem Update-Schritt wird eine Feedback-Nachricht gesendet, welche den aktuellen Fortschritt der Animation enthält.
Wurde die Animation vollständig durchlaufen, wird der Zustand des ActorPlugins auf Idle gesetzt.

Soll über den Movement-Zustand eine Bewegung abgespielt werden, müssen noch weitere Parameter der Bewegung berechnet werden.
Dies geschieht nach dem Setzen der Animation während des ersten Update-Aufrufs.
Zuerst wird ein Vektor zur Zielposition berechnet, welcher später zur Bewegung genutzt wird.
Sollte dieser über einer gewissen Mindestlänge liegen, wird eine Rotation des Menschen in Bewegungsrichtung ausgeführt.
Nach dieser Rotation wird die Bewegung an die Zielposition umgesetzt.
Dabei wird die Bewegungsanimation abgespielt, welche durch einen Faktor an die zurückgelegte Distanz angepasst wird.
Dies vermeidet, dass die Beine der Person über den Boden gleiten.
Wurde eine Endrotation angegeben, welche nicht dem Nullvektor entspricht, wird nach der Bewegung noch eine Rotation in diese Richtung ausgeführt.
Dabei werden auch die Beine wieder in die Ausgangslage bewegt.
Wenn sowohl die Zielrotation ausgeführt wurde und die Beine wieder in Ausgangslage sind, wird der Zustand auf Idle gesetzt.

Das ActorPlugin besitzt kein Konzept eines ROS-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 eine zweiten Message Queue zu jedem Simulationsschritt gesendet.
Um Zustandsübergänge erkennen zu können, werden auch diese als Feedback an den ActorServer gesendet.
\subsubsection{ActorServer}
Der ActorServer ist die Brücke zwischen ROS und dem ActorPlugin.
Er ist als das Programm \code{ros_actor_action_server} im gleichnamigen Paket enthalten.
Dieser weitere Dienst bindet das ActorPlugin an ROS an.

Es werden dafür zwei ROS-ActionServer gestartet, welche jeweils Bewegungen oder Animationen des simulierten Menschen auslösen können.
Beide ActionServer prüfen bei dem Empfang eines neuen Ziels zuerst, ob bereits eine andere Aktion ausgeführt wird.
Wird bereits eine andere Aktion ausgeführt, kommt es zur Ablehnung weiterer Anfragen.
Im anderen Fall wird die Aufgabe akzeptiert und in das MessageQueue-Format übersetzt und an das ActorPlugin gesandt.

Um das Starten mehrerer gleichzeitiger Aktionen zu unterbinden, muss der Empfang einer neuen Anfrage bestätigt werden, bevor weitere Befehle über den ROS-ActionServer entgegen genommen werden können.
Hierzu wird ein Mutex verwendet, welcher die Auswertung neuer Nachrichten verhindert, so lange der aktuelle Befehl noch nicht durch das Plugin bestätigt wurde.
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.
\section{Roboter}
\subsection{Übersicht}
Der Roboter besteht aus vielen interagierenden Systemen, welche in ihrer Gesamtheit das vollständige Robotermodell in der Simulation verwendbar machen.

Zuerst muss ein Modell des Roboters erstellt werden, welches in Gazebo geladen werden kann.
Dieses Modell muss dann für die Bewegungsplanung mit MoveIt erweitert werden.
Hierbei werden Controller von ros_control mit dem Modell verbunden, um den aktuellen Zustand der Achsen zu überwachen und diese steuern zu können.

Um diese Vielfalt an Daten standartisiert an andere Software in ROS weitergeben zu können, wird eine MoveGroup in ROS gestartet, welche die Planung von Bewegungen durch andere Programme zulässt.
\subsection{Modellierung}
Für den Kuka LBR iisy existiert kein Simulationsmodell für Gazebo und ROS, weswegen dieses Modell aus Herstellerdaten generiert wurden.
Hierzu stand eine .stl-Datei des Herstellers zur Verfügung.
Aus dieser Datei wurden mit FreeCAD\cite{freecad} alle Glieder des Roboters als Collada-Dateien exportiert.
Dabei muss darauf geachtet werden, dass die exportierten Daten eine ausreichende Meshgröße haben, welche vorher in FreeCAD eingestellt werden muss.
Dies erlaubt das Erhalten der feinen Strukturen des Arms, auch visualisiert in Abbildung \ref{robot_raw}, welche erst später in Blender reduziert werden sollen.

\begin{figure}[]
\includegraphics[width=\textwidth/2]{img/MA-Roboter-Rohdaten}
\centering
\caption{Rohdaten aus .stl-Datei}
\label{robot_raw}
\end{figure}

Diese Dateien können dann in Blender bearbeitet werden, um sie für die Simulation tauglich zu machen.
Hierfür 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.
Das vollständige visuelle Modell ist in Abbildung \ref{robot_visual} zu sehen.

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.
Das resultierende Modell, welches in Abbildung \ref{robot_collision} dargestellt wird, wird später zur Kollisionserkennung verwendet.

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.
\begin{figure}[hpt]
\begin{minipage}{.49\textwidth}
\includegraphics[width=\textwidth]{img/MA-Roboter-Visuell}
\centering
\caption{Visuelles Modell}
\label{robot_visual}
\end{minipage}
\begin{minipage}{.49\textwidth}
\includegraphics[width=\textwidth]{img/MA-Roboter-Kollision}
\centering
\caption{Kollisionsmodell}
\label{robot_collision}
\end{minipage}
\end{figure}
\subsection{MoveIt 2 Konfiguration}
Das somit erstellte Paket kann nun mit dem neu implementierten MoveIt Configurator um die benötigten Kontrollstrukturen erweitert werden.
Dazu wurde der neue Setupassistent von MoveIt2 verwendet, welcher das Modell analysiert, um es mit für MoveIt benötigten Parameter zu erweitert.

Die Erstellung des erweiterten Modells mit dem Assistenten funktionierte komplett fehlerfrei, jedoch ließen sich die generierten Dateien nicht nutzen.

Um die generierten Dateien nutzen zu können, mussten diese weiter angepasst werden.
Dies beinhaltete die korrekte Integration der Roboterdefinitionen im Paket, aber auch zahlreiche Pfadreferenzen auf verwendete Dateien, welche nicht korrekt generiert wurden.
Diese können standartmäßig nicht in Gazebo, RViz und MoveGroup gleichzeitig geladen werden, da diese Programme unterschiedliche Pfadangaben verlangen, was die Nutzung verhindert.

Eine Möglichkeit zur Lösung des Problems ist die Verwendung von \code{file://}-URIs, welche von allen dieser Programme gelesen werden können.
Jedoch ist hier als Nachteil zu verzeichnen, dass der Moveit Setup Assistent diese URIs nicht lesen kann, was zu Fehlern bei einer Rekonfiguration führen kann.

Das so erstellte Modell kann ber den Aufruf von \code{ros2 launch iisy_config demo.launch.py} in RViz getestet werden.
Hierfür erscheint das Robotermodell mit mehreren Markern und Planungsoptionen, welche genutzt werden können, um beliebige Bewegungen zu planen und auszuführen.
\subsection{Integration mit Gazebo}
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 Werkzeug 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.

In diesem Fall wird das Modell als String geladen, welcher durch \code{xacro} erstellt wurde.
Dies ist nötig, um Informationen aus anderen Dateien in das generierte XML übernehmen zu können.

Im Modell sind auch die verwendeten Gazebo-Plugins deklariert, welche für die Integration mit \code{ros_control} verantwortlich sind.
Das Gazebo-Plugin läd dabei die verwendeten Controller und versorgt diese mit Informationen über den Roboter.

\section{Behavior Trees}
Alle Behavior Trees wurden im Paket \code{btree} organisert, welches die Bäume im XML-Format und die Implementation der Nodes und umliegenden Infrastruktur enthält.


Für die Umsetzung des Szenarios wurden neue Nodes für den BehaviorTree erstellt.
Diese lassen sich nach Nutzung in verschiedene Gruppen einordnen.

\subsubsection{Allgemein nutzbare Nodes}
\begin{description}
  \item[GenerateXYPose]
  generiert eine Pose in einem durch den \code{area} Parameter angegebenen Bereich.
  Um dies zu ermöglichen, wird zuerst die Fläche aller Dreiecke berechnet, welche den Bereich definieren.
  Diese werden durch den Gesamtinhalt geteilt, um eine Wichtung der Dreiecke zum Gesamtinhalt zu erreichen.
  Nun wird eine Zufallszahl zwischen 0 und 1 gebildet.
  Von dieser werden nun die Wichtungen der Dreiecke abgezogen, bis dies Zufallszahl im nächsten abzuziehenden Dreieck liegt.
  Nun wird in diesem Dreieck eine zufällige Position ermittelt, welche ber den Ausgabeparameter \code{pose} ausgegeben wird.
  \item[InAreaTest]
  prüft, ob eine Pose, vorgegeben durch den \code{pose} Parameter, in einer durch den \code{area} Parameter definierten Zone liegt.
  Hierfür wird überprüft, ob die X und Y-Werte der Pose in einem der Dreiecke liegen, welche die Area definieren.
  Der Rückgabewert ist das Ergebnis dieser Überprüfung, wobei SUCCESS bedeutet, dass sich die Pose in der Area befindet.
  \item[OffsetPose]
  wird genuzt, um eine Pose im Raum zu bewegen und/oder deren Orientierung zu verändern.
  Falls der \code{offset} Parameter gesetzt ist, wird dieser mit dem \code{input} Parameter summiert.
  Außerdem wird die Orientierung der Pose auf den \code{orientation} Parameter gesetzt, falls dieser vorhanden ist, was den ursprünglichen Wert überschreibt.
  \item[InterruptableSequence]
  stellt eine Sequence dar, welche auch nach ihrem Abbruch ihre Position behält.
  Dies ist von Nöten, wenn bestimmtes Verhalten unterbrechbar ist, aber zu einem späteren Zeitpunkt fortgesetzt werden soll.
  Hierzu wird der Iterator der unterlegenden Sequenz nur erhöht, wenn die untergeordnete Node SUCCESS zurück gibt.
  Außerdem wird der Iterator nicht zurückgesetzt, wenn die Ausführung dieser modifizierten Sequenz abgebrochen wird.
  \item[WeightedRandom]
  ist eine Steuerungsnode, welche mehrere untergeordnete Nodes besitzt.
  Dabei werden diese nicht, wie bei anderen Steuerungsnodes üblich, sequentiell ausgeführt.
  Anhand einer vorgegebenen Wichtung im \code{weights} Parameter wird eine der untergeordneten Nodes zufällig ausgewählt.
  Diese Node wird nun als einzige Node ausgeführt, bis diese den SUCCESS-Status zurück gibt.
  Nach dem dieser Status erreicht wurde, wird bei dem nächsten Durchlauf eine neue Node ausgewählt.
  Der Rückgabewert ist der Rückgabewert der ausgewählten untergeorneten Node.
  \item[IsCalled]
  fragt den aktuellen Called-Status des Actors ab, welcher in einigen Szenarien vom Roboter verwendet wird, um den simulierten Menschen zu rufen.
  Der Rückgabewert der Node ist dabei SUCCESS, falls der Mensch gerufen wird, oder FAILURE, wenn kein RUF durchgeführt wird.
  \item[SetCalledTo]
  setzt den aktuellen Called-Status auf den Wert des übergebenen \code{state} Parameters.
  Da diese Aktion nicht fehlschlagen kann, liefert diese Node immer SUCCESS als Rückgabewert.
  \item[RandomFailure]
  generiert eine Zufallszahl von 0 bis 1, welche mit dem \code{failure_chance} Parameter verglichen wird.
  Der Rückgabewert ist das Ergebnis des Vergleichs, FAILURE, wenn die Zufallszahl kleiner als der \code{failure_chance} Parameter ist, oder im anderen Falle SUCCESS.
\end{description}

\subsubsection{Menschenspezifisch}
\begin{description}
  \item[ActorAnimation]
  wird verwendet, um dem simulierten Menschen eine auszuführende Animation zu senden.
  Die Node benötigt zur Ausführung einen Animationsname, welcher im \code{animation_name} Parameter angegeben wird.
  Ein optionaler \code{animation_speed} Parameter gibt die Ausführungsgeschwindigkeit vor.
  Der Rückgabewert ist SUCCESS, wenn die komplette Ausführung gelang, oder FAILURE, falls diese abgebrochen oder abgelehnt wurde.
  \item[ActorMovement]
  funktioniert wie eine ActorAnimation, sendet jedoch eine Bewegungsanfrage.
  Auch für diese wird ein Animationsname benötigt, welcher im \code{animation_name} Parameter definiert wird.
  Jedoch wird für die Synchronisation zur Bewegung ein Disztanzwert benötigt, welcher in einem Animationsdurchlauf zurückgelegt wird.
  Dieser wird im \code{animation_distance} Parameter übergeben.
  Eine Zielpose wird im \code{target} Parameter gesetzt.
  Eine Besonderheit dieses Paramerters ist die Verwendung des Null-Quaternions als Richtungsangabe, was die Endrotation auf die Laufrichtung setzt.
\end{description}

\subsubsection{Roboterspezifisch}
\begin{description}
  \item[RobotMove] gibt dem Roboter eine neue Zielposition über den \code{target} Parameter vor.
  Bei dieser Node handelt es sich um eine asynchrone Node, welche nur bei der erfolgreiche Ausführung der Bewegung SUCCESS zurück gibt.
  \item[SetRobotVelocity] setzt eine neue maximale Geschwindigkeit des Roboters, vorgegeben durch den \code{velocity} Parameter.
  Der Rückgabewert ist immer SUCCESS.
\end{description}
Diese beiden roboterspezifischen Nodes beeinflussen im Hintergrund das gleiche System, da Bewegungen bei Geschwindigkeitsänderungen neu geplant und ausgeführt werden müssen.
Dazu wird das letzte Ziel bis zu dessen erreichen vorgehalten, um die Ausführung neu zu starten, fall eine Geschwindigkeitsänderung durchgeführt werden muss.
Um die RobotMove-Node nicht zu unterbrechen, wird diese nur über einen Callback über den Erfolg oder Misserfolg der gesamten Bewegung und nicht über den Erfolg einzelner Teilbewegungen informiert.

\subsection{Subtrees}
Um die Wiederverwendung von bestimmten Verhaltensweisen zu erleichtern, wurden diese in Subtrees ausgelagert.
In diesem Fall wurden die Aktionen ``Arbeiten an der Werkbank'' und ``Ablegen eines Objekts im Lagerregal'' des Menschen ausgelagert, da diese in mehreren Fällen verwendet werden und die Lesbarkeit der Bäume verbessert wird.

\subsection{Verhalten des Roboters}

\subsection{Verhalten des Menschen}
