\chapter{Umsetzung}

Bei der Umsetzung des geplanten Systemaufbaus kam es zu mehreren Problemen, die 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, die 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, die 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, ist der Zugriffs auf eine solche Umgebung erforderlich.

Diese Probleme können nicht durch Docker allein gelöst werden, da die Virtualisierungsumgebung eine Trennung der Systeme vorsieht.
Die Isolation des Containers von der Benutzeroberfläche des Systemskann durch eine Kombination aus zwei Komponenten aufgehoben werden.

Um diese Modifikationen trotzdem reproduzierbar zu machen, wurde ein Shellscript geschrieben, dass 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, die 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, der im vorherigen Schritt 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.
Dieser führt die obengenannten Schritte aus und den Container startet.
Eine Verbindung zum Container ist möglich, nachdem die Meldung \code{ros_1 | Ready to connect.} in der Konsole erscheint.

Dafür wird ein SSH-Client eingesetzt, der 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 antwortet, was nicht dem standartmäßigen Port 22 entspricht.

Nach der Verbindung wird automatisch die ROS2-Umgebung eingerichtet.
Diese kann ohne weitere Befehle nach Verbindungsaufbau genutzt werden.
Um die erstellten Pakete zu kompilieren, wurde das Skript \code{build.sh} im \code{workspace}-Verzeichnis erstellt.

\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, die 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 Kompiliervorgang 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, der die Ausgaben neu formatiert.
Durch diesen werden die Ausgaben gruppiert, und erst nach einen vollständigen Kompiliervorgang 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 Kompiliervorgänge abgebrochen werden, falls der Kompiliervorgang 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 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, die 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, der in die Entwicklungsumgebung integriert werden muss.

Als Alternative existiert das Language Server Protocol, kurz LSP, dass 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, die beim Design des LSP nicht bedacht wurden, einfacher in interner Implementation umgesetzt werden.
Deswegen besitzen 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, deren Pose sich direkt im Nullpunkt und ohne Rotation befindet.
  Durch diesen Umstand sind 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, die 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, die für Positionsgenerierungen und Postionsabfragen genutzt werden können.
  \item[ActorPluginState]
  definiert 4 Zustände, die das ActorPlugin annehmen kann. Diese 4 Werte sind SETUP, IDLE, MOVEMENT und ANIMATION.
  \item[FeedbackMessage]
  beschreibt die erste der beiden MessageQueue-Nachrichten, die 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, die 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, die ü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, die je nach ActorPluginState während einer Bewegung oder Animation ausgeführt wird.
  Der \code{animationSpeed} Parameter beschreibt entweder die Abspielgeschwindigkeit der Animation, oder die zurückgelegte Distanz pro Animationsdurchlauf.
  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 dem die Interaktion zwischen Mensch und Roboter stattfinden soll.
Für diesen Raum wurde zuerst ein Raumplan erstellt, der alle benötigten Bereiche für die Szenarien besitzt (Abbildung \ref{room-plan}).

Zuerst wird ein Stellplatz für den Roboter benötigt.
Dieser sollte an einer Ecke des Raumes positioniert werden, was zu höheren Verfahrgeschwindigkeiten führt.
Das ist durch die dynamische Verfahrgeschwindigkeit bedingt, die bei geringerer Distanz zum Menschen abnimmt.

Des weiteren werden eine Arbeits- und Lagersätte für den Menschen benötigt, die 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, die 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, das nur 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, die 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, die 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, die im Kontext der zur erfüllenden Aufgabe relevant sind.
Dafür muss dan Modell in einen animierbaren Zustand gebracht werden, in dem 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, die 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, die 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, dass 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, die 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, die 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, die typische Animationsmethoden abdecken.

In der Farbe Rot sind im Modell Knochen markiert, die 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, die 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, die 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}, die 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, die 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, die zeitlich nur schwer planbar sind.
Die Nutzung eines separaten Nachrichtendienstes, der 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, der 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, der 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, dass es Anwendungen erlaubt, Zugriff auf diesen zu erlangen.
Ein Programm kann in diesem Bereich Daten ablegen, die durch andere Programme gelesen und geschrieben werden können.
Die Koordinierung der Zugriffe erfolgt dabei durch das Betriebssystem, was gleichzeitige Zugriffe, wie bei shared memory, aussschließt.
Durch diesen Umstand kommt es zu einem Anstieg an Latenzzeit.

Die Wahl des Dienstes fiel auf eine MessageQueue, da die integrierten Funktionen die Entwicklung erleichtern und 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 Kompiliervorgang 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 Kompilantionsreihenfolge 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, die 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, die 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, die 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, die mit einer Antwort quittiert werden muss.
Hierbei sind die Antworten ACCEPT_AND_DEFER, ACCEPT_AND_EXECUTE und REJECT möglich.

Die erste mögliche Antwort ACCEPT_AND_EXECUTE signalisiert die sofortige Ausführung des gewünschten Befehls.
Als Alternative existiert die Antwort ACCEPT_AND_DEFER, die für eine verspätete Ausführung der gewünschten Aktion steht.
Die REJECT-Antwort 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 dem 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, dass 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, die 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, der 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, die 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 dem alle Simulationsobjekte organisiert sind.
Ein Entity Component System besteht aus einer oder mehr Entities, die Objekte innerhalb der Simulation abbilden.
Objekte können beliebig viele Components besitzen, bei denen 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, der 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 der das Plugin als Component hinzugefügt wurde.
 Außerdem wird eine Referenz auf die Konfiguration des Plugins in der geladenen Welt übergeben.
 Für den Zugriff auf die Simulationsumgebung werden zuletzt noch eine aktuelle Instanz des EntityComponentManagers und EventManagers von Gazebo übergeben.
 \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, die 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, die 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, dass 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, der 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, wird ein Skalierungsfaktor genutzt.
Dieser optionale Faktor kann mit der abgelaufenen Zeit multipliziert werden.
Dies verursacht eine Beschleunigung oder verlangsamung der Ausführung.
Nach jedem Update-Schritt wird eine Feedback-Nachricht gesendet, die den aktuellen Fortschritt der Animation enthält.
Wurde die Animation vollständig durchlaufen, wird der Zustand des ActorPlugins auf Idle gesetzt.

\begin{figure}[hpt]
\includegraphics[width=\textwidth]{uml/out/plugin_states.eps}
\caption{Zustandsübergänge im ActorPlugin}
\label{plugin_states}
\end{figure}
Alle Zustandsübergänge sind in Abbildung \ref{plugin_states} zusammengefasst.

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, der 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, die 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, die 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, der 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.

Nach dem Start des ActorServers werden zwei ROS-ActionServer gestartet.
Diese können jeweils zum Abspielen von Animationen oder zum Starten von Bewegungen des simulierten Menschen genutzt werden.
Wenn ein Client eine dieser Aktionen startet, überträgt er die Zieldaten an den entsprechenden ActionServer.
Beide ActionServer prüfen bei dem Empfang eines neuen Ziels als erstes, ob bereits eine andere Aktion ausgeführt wird.
Wird bereits eine andere Aktion ausgeführt, kommt es zur Ablehnung der aktuellen Anfrage.
Im anderen Fall wird die Anfrage 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, der 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, die in ihrer Gesamtheit das vollständige Robotermodell in der Simulation verwendbar machen.

Zuerst muss ein Modell des Roboters erstellt werden, dass 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.
Diese kann vor dem Export in FreeCAD eingestellt werden.
Durch diese Einstellung bleiben feine Strukturen erhalten, die später in Blender wieder reduziert werden können.
Ein solches Vorgehen erlaubt es Blender, bessere Entscheidungen über die Reduktion von Strukturen zu treffen.
Das Resultat ist ein besseres Endmodell des Roboters.

\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, die 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, die 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, die den Arm bewegen.

Die Gelenkpositionen sind dabei relative Angaben, die sich auf das Glied beziehen, an dem ein weiteres Glied über das Gelenk verbunden werden soll.
Alle kontrollierbaren Gelenke benötigen auch eine Gelenkachse, die 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, der das Modell für MoveIt anpasst.
Dabei wird das Modell mit weiteren Parametern versehen, die durch MoveIt genutzt werden.

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, die 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, die 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, die 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, dass 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, der 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, die 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, dass 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, die 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, die ü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, die 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, die 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, der 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, die 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, der 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, der im \code{animation_name} Parameter definiert wird.
  Jedoch wird für die Synchronisation zur Bewegung ein Disztanzwert benötigt, der 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, die nur bei der erfolgreiche Ausführung der Bewegung mit dem SUCCESS-Status beendet wird.
  \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.
Dies erhöht die Lesbarkeit der Bäume und vereinfacht die Verwendung gleicher Aktionen in mehreren Bäumen.

Für das Arbeiten an der Werkbank ist eine Bewegung zu dieser erforderlich.
Außerdem muss der Mensch sich zur Werkbank ausrichten, um die späteren Animationen korrekt auszuführen.
Dazu wird eine Bewegung zu einem Punkt vor der Werkbank mit vorgegebener Zielrichtung genutzt.
Nach dieser Bewegung soll ein Arbeitsprozess simuliert werden.
Da dieser Prozess weit vom Roboter entfernt ist, ist eine genaue Animation nicht erforderlich.
In diesem Fall wird die Arbeit durch ausstrecken und zurücknehmen der Hand in Richtung der Werkbank simuliert.
Dies wird durch zwei Animations-Nodes mit entsprechenden Parametern gesteuert.
Die Ausführung der Nodes dieser Gesamtaktion soll Sequenziell erfolgen, weshalb die Nodes unterhalb einer Sequence-Node gruppiert werden.
Der vollständige resultierende Baum ist in Abbildung \ref{subtree_work} visualisiert.

\begin{figure}[]
\includegraphics[width=\textwidth]{img/MA-subtree-work}
\centering
\caption{BehaviorTree für das Arbeiten an der Werkbank}
\label{subtree_work}
\end{figure}

Das Ablegen von Objekten in einem Schrank ist ein komplizierterer Prozess, da der Schank in mehreren Ebenen befüllt werden kann.
Außerdem sind mehrere Fächer verfügbar, die genutzt werden können.
Wie bereits bei der Werkank wird zuerst eine Bewegung zum Schrank ausgeführt.
Diese hat ein einzelnes Schrankteil als Ziel, wobei drei Schrankteile existieren.
Die Auswahl des Ziels erfolgt zufällig durch eine WeightedRandom-Node, die eine der drei Bewegungs-Nodes ausführt.
Nachdem erfolgt eine weitere zufällige Auswahl.
Diese entscheidet, ob ein oberes oder unteres Fach genutzt werden soll.
Im Falle eines oberen Faches werden nur zwei Animationen benötigt.
Die Hand wird zu einem oberen Fach ausgestreckt und zurückgezogen.
Für den Fall eines unteren Fachs werden vier Animationen genutzt.
Zuerst kniet die Person auf den Boden nieder, um das untere Fach erreichen zu können.
Danach wird ein Objekt durch inspizieren und ablegen in das Fach gelegt.
Nun kann die Person wieder aufstehen, was die Person wieder zum gleichen Lage wie am Anfang der Aktion bringt.
Für die beiden Fälle werden auch Sequenz-Nodes genutzt, da die Aktionen nacheinander ausgeführt werden sollen.
Die beiden zufälligen Auswahlverfahren werden durch eine Sequenz verbunden.
Der resultierende Ablauf wurde als Baum in Abbildung \ref{subtree_deposit} visualisiert.

\begin{figure}[]
\includegraphics[width=\textwidth]{img/MA-subtree-deposit}
\centering
\caption{BehaviorTree für das Ablegen von Objekten im Schrank}
\label{subtree_deposit}
\end{figure}

\subsection{Verhalten des Roboters}
Das grundlegende Verhalten des Roboters ist in jedem Anwendungsfall gleich.
Dies hängt mit der automatischen Geschwindigkeitsanpassung zusammen.
Der Roboter soll bei annäherndem Mensch zuerst seine Geschwindigkeit reduzieren, wenn dieser eine Warnzone betritt.
Falls sich der Mensch weiter bis in die Sicherheitszone begibt, ist ein vollständiger Stopp des Roboters vorgesehen.

Um diese Funktionen zu erlauben, ist eine Reaktion auf Statusänderungen von vorherigen Nodes nötig.
Dies wird durch eine übergeordnete ReactiveSequence erreicht.
Eine solche Node verhält sich ähnlich einer normalen Sequence, jedoch prüft sie vor jedem Tick noch einmal vorherige Nodes.
Wenn eine vorherige Node Failure zurückgibt, wird die aktuell laufende spätere Node abgebrochen.

Als erste Node in der ReactiveSequence wird getestet, ob sich der Mensch in der Sicherheitszone aufhält.
Wenn dies nicht der Fall ist, kann die Ausführung weiter fortgesetzt werden.
Dies wird durch eine über dem Test angeordnete Inverter-Node erreicht.

Nach diesem Schritt kann die Geschwindigkeitsanpassung vorgenommen werden, falls diese nötig ist.
Dies wird durch eine If-Then-Else Node erreicht.
Als Bedingung wird ein Test auf den Aufenthalt des Menschen in der Warnzone verwendet.
Wenn der Mensch sich in dieser befindet, wird die Geschwingigkeit auf 10\% gesetzt.
Sonst wird die Geschwindigkeit wieder auf 100\% erhöht.

Nach dieser Sequenz kann der spezifische Teil für die gewünschte Applikation hinzugefügt werden.
In Abbildung \ref{tree_base_robot} wurde dies durch eine Wolke repräsentiert.

\begin{figure}[]
\includegraphics[width=\textwidth]{img/MA-tree-base-robot}
\centering
\caption{BehaviorTree für das Ablegen von Objekten im Schrank}
\label{tree_base_robot}
\end{figure}

\begin{figure}[]
\includegraphics[width=\textwidth]{img/MA-tree-robot-coex}
\centering
\caption{BehaviorTree für das Ablegen von Objekten im Schrank}
\label{tree_base_robot}
\end{figure}

\begin{figure}[]
\includegraphics[width=\textwidth]{img/MA-tree-robot-coop}
\centering
\caption{BehaviorTree für das Ablegen von Objekten im Schrank}
\label{tree_base_robot}
\end{figure}

\begin{figure}[]
\includegraphics[width=\textwidth]{img/MA-tree-robot-colab}
\centering
\caption{BehaviorTree für das Ablegen von Objekten im Schrank}
\label{tree_base_robot}
\end{figure}

\subsection{Verhalten des Menschen}
