Zum Inhalt

Pfadplanung

Das Ziel der Pfadplanung ist es, den Roboterarm so zu bewegen, dass eine gegebene Abfolge von Rotationen (z.B. die vom core.solver.Solver bereitgestellte Lösung) am Würfel durchgeführt wird. Die Datengrundlage dieses Abschnitts wird durch eine Liste an Rotationen und dem Zustand sowie der aktuellen Ausrichtung des Würfels definiert (siehe core.manipulator.CubeState.CubeState).

Umsetzung

Das entwickelte Modul zur Pfadplanung wurde an die Herangehensweise eines Menschen angelehnt. Das heißt, dass ein sogenannter Cuber (diejenige Person, die den Würfel löst), mittels des Roboterarms nachgestellt wird. Dabei fallen einem folgende Aktionen ein:

  • den Würfel aufnehmen und ablegen
  • eine bestimmte Ausrichtung einnehmen beziehungsweise den Würfel im Ganzen drehen
  • eine jeweilige Seite des Würfels drehen (z.B. rechts im Uhrzeigersinn)

Um letzteres gut in einen Computeralgorithmus zu überführen können, haben wir uns dafür entschieden, dass wir keine relativen Seiten (wie zum Beispiel rechts), sondern absolute Seiten (wie zum Beispiel rot) zum Lösen verwenden möchten. Um wiederum einheitlich mit der sogenannten Cuber-Notation, welche auf relativen Seiten basiert, zu bleiben, sollten diese relativen Seiten zunächst konsistent in absolute Seiten umgewandelt werden können. Das einfache Schema blau = vorne, rot = rechts und gelb = oben hat sich als sinnvoll herausgestellt. Somit ist es nun möglich gewesen, die Pfadplanung mit Daten jedes beliebigen Lösungsalgorithmus' zu füttern, solange die einzelnen Seiten in einer von uns definierten Reihenfolge gesetzt werden können.

Um die gegebenen Aktionen so ausführen zu können, sind folgende Schritte notwendig:

Planen von einzelnen Bewegungen

Einzelne Instruktionen zeichnen sich dadurch aus, dass lediglich ein Befehl zu einem Programm hinzugefügt wird. Das kann zum Beispiel ein Bewegungsbefehl (siehe MoveInstruction) oder eine Greifinstruktion (siehe GripInstruction) sein.

Um Bewegungen durchzuführen, müssen zunächst Ziele (sogenannten Targets) generiert werden, welche dann angefahren werden können.

Erstellung von RoboDK Targets

Um mit dem Roboter bestimmte Posen einnehmen zu können, werden in RoboDK sogenannte Targets (Ziele) verwendet. Sie beschreiben je nach Konfiguration entweder eine kartesische Koordinate im 3D-Raum und die Orientierung des Flansches darin oder die Auslenkung aller sechs Roboterachsen einzeln. Diese Ziele heißen Cartesian Target beziehungsweise Joint Target. Der Unterschied wird deutlich, wenn ein bestimmtes kartesisches Ziel mehrfach angefahren wird. Die Gelenke des Roboters müssen oft nicht gleich ausgelenkt sein, da es verschiedene Möglichkeiten gibt, wie der Roboter diese Pose einnehmen kann. So kann im vorhinein auch nicht bestimmt werden, welchen Pfad der Roboter wählt, da nur die kartesischen Koordinaten an die Robotersteuerung übertragen werden und diese dann die optimale Bewegung berechnet.

Werden stattdessen Joint Targets verwendet, steht die Endposition des Roboters fest. Es entstehen somit reproduzierbare Bewegungen.

Automatische Generierung von Grip Targets

Im Gegensatz zu den per Hand definierten Joint Targets zum Einlesen des Würfels, werden die Joint Targets für die einzelnen Greifpositionen programmatisch generiert. Dies geschiet mittels des Moduls grip_target_generation beziehungsweise mittels der darin enthalten Fabrikklasse GripTargetFactory. Diese ist in der Lage, für jeden Positionstyp (GripPositionType) ein RoboDK Joint Target zu berechnen und erzeugen. Dabei gibt es hauptsächlich die drei folgenden Typen, die sich nur in ihrer Translation unterscheiden und somit die selbe Orientierung innehaben:

  • GRIP - der Greifer befindet sich im Zentrum der Halterung. Es kann nun rotiert werden.
  • PREPARE_TOP - der Greifer befindet sich oberhalb der Halterung. Es kann nun von Oben in die Halterung herangefahren werden.
  • PREPARE_SIDE - der Greifer befindet sich seitlich der Halterung. Es kann nun seitlich in die Halterung eingestochen werden.

Mittels GripPositionType.get_type() kann bestimmt werden, welcher Positionstyp für die aktuelle Situation passend ist. Dieser Schritt ist wichtig, da mit einem gegriffenen Würfel nur von GRIP nach PREPARE_TOP und andersherum verfahren werden darf. Bei einer Bewegung von GRIP nach PREPARE_SIDE würde zwangsläufig eine Kollision mit der Halterung stattfinden. Leider ist es aber auch nicht möglich, immer nach oben hinaus zu fahren, da teils auch die Seiten in X-Richtung des Greifers (z.B. beim Umgreifen) verwendet werden (siehe GripperFace.GRIPPED_X_POSITIVE und GripperFace.GRIPPED_X_NEGATIVE). Wenn an der GRIP Position, befindet sich eine Greiferbacke unterhalb des Würfels. So würde ein Eintauchen einen sich in der Halterung befindenden Würfel zerdrücken.

Nun ist es möglich entsprechende Greiferpositionen für alle nötigen Situationen zu generieren. Mittels einer MoveInstruction kann somit eine Bewegung geplant werden.

Hinzufügen von Greiferaktionen

Ein essentieller Teil des Lösen eines Würfels mittels Roboterarm ist das Greifen und Loslassen des Würfels. Die Halterung, wie oben beschrieben, macht es möglich, den Würfel in verschiedensten Varianten zu greifen und wieder abzulegen. Hierfür müssen die Pneumatikventile des Roboterarms angesteuert werden. Dies geschieht pragmatisch mittels einer GripInstruction, welche lediglich einen Parameter erhält, der bestimmt, ob der Würfel gegriffen oder losgelassen werden soll.

Mit nur diesen zwei Grundkomponenten ist es nun möglich, komplexe Abläufe zu planen.

Planen von komplexen zusammengesetzten Aktionen

Die Manipulation des Würfels, sodass dieser am Ende den gelösten Zustand einnimmt, erfolgt hauptsächlich durch auszuführende Rotationen. Damit die zu rotierenden Seiten des Würfels jedoch nicht aufgrund der Greiferbacken oder des Flanschs unzugänglich sind, ist es wichtig, den Würfel umgreifen zu können.

Umgreifen des Würfels

Um eine gewünschte Seite zugänglich zu machen, muss der Würfel zunächst in einer gewissen Weise abgelegt und dann auf eine bestimmte Art wieder aufgenommen werden. Beim Aufnehmen des Würfels muss es eine Möglichkeit geben, die gewünschte Seite nicht wieder zu verdecken. Somit ist es relevant, in welcher Art und Weise der Würfel abgelegt wird. Dabei gibt es sehr viele Kombinationsmöglichkeiten: fünf verschiedene Seiten des Würfels können mit vier um 90° gedrehten Positionen an der Halterung gepaart werden. Diese 20 Möglichkeiten gibt es gleich zwei mal, sowohl beim Ablegen als auch beim Aufnehmen. Das ergibt in Summe 380 verschiedene Umgriffsszenarien (1 aus 20 * 1 aus 19). Die Aufgabe der Funktion core.manipulator.SimulatingCuber.SimulatingCuber.perform_regrip() ist es, hieraus die beste auszuwählen. Das heißt, dass möglichst lange kein weiteres Umgreifen notwendig ist und die Abfolge möglichst schnell durchgeführt werden kann, also kurze Pfade gewählt werden.

Wurde die gewünschte Seite nun zugänglich gemacht, kann eine Rotation ausgeführt werden.

Rotationen durchführen

Um letztendlich einzelne Schichten zu rotieren, muss der Würfel in der Halterung mit der gewünschten Seite nach unten positioniert werden. Daraufhin kann eine lineare Bewegungen zu einer um -90°, 90° oder 180° verdrehten Pose durchgeführt werden. Dadurch wird die Seite des Würfels entsprechend entgegen der Rotation des Roboters gedreht, da in der Cuber-Notation davon ausgegangen wird, dass eine Seite gedreht wird, bei unserem Ansatz allerdings eine Seite starr bleibt (festgehalten durch Kollision mit Halterung) und die anderen beiden Schichten gedreht werden. Das heißt eine Rotation von orange um 90° im Uhrzeigersinn würde den Roboter um 90° gegen den Uhrzeigersinn um die Halterung bewegen.

Die Planung einer solchen Rotation ist deutlich einfacher als die des Umgreifens, da hier nur eine Seite mit vier Rotationen kombiniert werden muss. Die Endposition ergibt sich direkt aus der Startposition: Es gibt maximal vier Möglichkeiten, die gewünschte Schicht zu rotieren. Diese Aufgabe übernimmt die Funktion core.manipulator.SimulatingCuber.SimulatingCuber.rotate_face(). Auch hier wird dabei darauf geachtet, dass keine Kollisionen stattfinden und die Bewegungen möglichst wenig Zeit benötigen.

Letztendlich kann eine komplette Lösung eines Würfels aus Gründen der Übersichtlichkeit mittels der Funktion core.manipulator.SimulatingCuber.SimulatingCuber.perform_rotations() berechnet werden.

Neben der Entwicklung der Pfadplanung erläutert der nächste Abschnitt auch, wie diese nun konkret verwendet werden können.

Historie der Pfadplanung

ArtificialCuber

Zunächst gab es den sogenannten ArtificialCuber, welcher, wie beschrieben, einen menschlichen Cuber nachstellen sollte. Deshalb arbeitet er auf der höchsten Abstraktionsebene und bietet somit Funktionen zum Verdrehen und Umgreifens des Würfels. Genauer sind dies folgende (pseudocode):

  • move(pos a) - Bewegt den Arm in eine gewünschte Pose.
  • enter_mount(pos a) - Bewegt den Arm in einer gewissen Pose an die Halterung und fährt dabei über eine sichere prepare Position.
  • exit_mount() - Fährt den Arm aus der Halterung heraus und nimmt eine sichere prepare Position ein.
  • rotate(n) - Führt n 90°-Rotationen aus. Geht dabei davon aus, dass der Arm sich mit gegriffenen Würfel an der Halterung befindet.

Sowie die drei highest-level Methoden, die auf den Seiten des Würfels arbeiten (pseudocode):

  • rotate_face(face, n) - Führt eine gewünschte Rotation aus, in dem es die richtige Greiferstellung berechnet und dann den entsprechenden rotate(n)-Befehl ausführt. Verwendet dabei die schnellste Möglichkeit, falls mehrere Start-/Endpositionen zur Auswahl stehen.
  • perform_regrip(future_faces) - Führt einen Umgriff durch, sodass möglichst viele aufeinanderfolgende Rotationen durchgeführt werden können, ohne erneut umzugreifen.
  • perform_rotations(rotations) - Führt eine Liste an Rotationen aus. Stellt davor jeweils sicher, dass die entsprechende Seite aktuell zugänglich (nicht durch die Greiferbacken oder den Flansch verdeckt) ist.

Abbildung 1. Abstraktionsebenen der Pfadplanung

Alle Aktionen wurden direkt auf dem Roboter ausgeführt und konnten somit den aktuellen Zustand des UICubes zugreifen, um den nächsten Schritt zu planen. Somit war es allerdings nicht möglich, alle Instruktionen im Voraus zu planen. Nachfolgende Instruktionen konnten erst geprüft werden, sobald die vorherige Instruktion endete und damit ein statischer Zustand erreicht wurde. Einen Schritt zurück zu gehen, sobald eine Kollision entdeckt wurde, wäre nicht nur zeitintensiv sondern mit diesem Ansatz auch schlicht nicht möglich.

Wie sich dieses Problem lösen lies, wird im nächsten Abschnitt erklärt.

SimulatingCuber und Co.

Wir entschlossen uns, ein Klassenmodell mit Würfelzustand und Instruktionen zu entwerfen, um das genannte Problem zu kompensieren. So wurde der ArtificialCuber durch den SimulatingCuber abgelöst und es entstanden der CubeState, das SimulationProgram sowie verschiedene SimulationInstructions:

Jede Anweisung kann mittels apply_to_cube_state() auf den CubeState angewandt werden und liefert den resultierenden Zustand zurück. Ein SimulationProgram fasst nun mehrere SimulationInstructions zusammen und kann somit den Endzustand des Würfels durch iteratives Anwenden der einzelnen Instruktionen berechnen.

So ist es nun möglich python-seitig alle Schritte durchzuplanen, ohne mit RoboDK in Kontakt zu stehen, was die Ausführungszeit sowie das Tight-Coupling reduziert.

Abbildung 2. Vergleich einer Umgreifprozedur unter Verwendung der beiden Ansätze. Links: Der ArtificialCuber führt die Bewegungen aus und merkt an einer gewissen Stelle, dass dieser Umgriff in der gewählten Variante nicht funktioniert. Rechts: Der SimulatingCuber simuliert die Bewegungen stattdessen und erkennt dabei an selbiger Stelle, dass diese Umgriffsvariante nicht durchführbar ist. Daraufhin kann er direkt die nächste Möglichkeit überprüfen.

Im folgenden Abschnitt wird erklärt, wie eine solche Kollisionsprüfung nun durchgeführt wird.

Schnittstelle zu RoboDK

Soll nun ein Programm bezüglich Kollisionserkennung oder Reachabilityzwecken überprüft werden, muss RoboDK angefragt werden. Dafür ist die Klasse RoboDKProgram entworfen worden. Sie erlaubt das Umwandeln eines SimulationPrograms in ein RoboDK Programm, welches dann in Gänze inklusive Kollisionserkennung simuliert werden kann. Diese Simulation liefert ein SimulationResult zurück, welcher folgende Werte beinhaltet:

  • num_valid_instructions - Anzahl der gültigen Instruktionen des Programs.
  • time - Geschätzte Dauer des Programmablaufs.
  • distance - Die Distanz, die der TCP im Raum zurückgelegt.
  • path_validity_percentage - Prozentangabe des gültigen Pfades, dass heißt unter Einhaltung von Achsendanschlägen und Vermeidung von Kollisionen.
  • message - Eine Nachricht, die dem Anwender angezeigt werden kann.

Wie der physische Würfel im Sinne des erstellten SimulationPrograms gelöst wird, erläutert das nächste Kapitel.