Im vorherigen Tutorial haben wir unseren Roboter in Gazebo simuliert und festgestellt, dass wir eine Regelung für unsere Jointmotoren benötigen um ihn zu bewegen. Ros_control ist ein Package, welches sowohl bei der Reglerimplementierung als auch bei der Hardwareabstraktion unterstützt. Damit ist es einfach, Regler für unsere Joints (zu deutsch: Gelenke) zu implementieren. In diesem Tutorial werden wir ros_control installieren, das URDF mit den Positionsreglern für jeden Joint ergänzen und die Regler in einer separaten Konfigurationsdatei parametrieren. Wir lernen wie wir Positionsbefehle an die Regler senden können und zusätzlich sehen wir uns eine einfache Methode an um die Performance der Regler zu tunen. Hört sich gut an? Dann legen wir los!
Notiz: Falls nur den UR5 Roboterarm in Gazebo mit Hilfe von ros_control für die Positionsregelung steuern willst, ohne zu lernen wie die Regler mit ros_control hinzugefügt, parametrisiert und anschließend getunt werden können, kannst du direkt weiter zur Kurzanleitung springen. Dort findest du alle nötigen Befehle zusammengefasst. Ansonsten fangen wir damit an unsere Positionsregler hinzuzufügen!
Ros_control setup
Klone zuerst das Git-Repository mit der modifizierten URDF, der Konfigurationsdatei für unsere Regler und den Launch files in den src Ordner in deinem Catkin Workspace.
~/catkin_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bashWenn du nicht weißt was ein Catkin Workspace ist und wie man ROS Packages baut dann starte am besten mit diesem Tutorial. Wenn du mit Git nicht vertraut bist kannst du auch die Zip-Datei direkt von Github unter diesem Link heruterladen. Danach musst du die Datei nur in den src Ordner in deinem Workspace entpacken. Danach kannst du den Workspace bauen.
Beachte: Wenn du den source Befehl nicht zu deiner ./bashrc Datei hinzufügt musst du den letzten Befehl in jedem neu geöffneten Linux-Terminal ausführen damit ROS die installierten Packages findet.
Um ros_control und einige Regler zu installieren führe den folgenden Befehl in deinem Terminal aus:
$ sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllersDie URDF Datei modifizieren
Nun schauen wir uns an, was wir zu unserer URDF hinzufügen müssen damit wir sie zusammen mit ros_control benutzen können. Wenn du das Repository heruntergeladen hast, findest du die Änderungen bereits in der Datei ur5_jnt_pos_ctrl.urdf im urdf Ordner. Als erstes fügen wir in unserem URDF ein ros_control plugin direkt nach dem öffnenden <robot> Tag ein. Dieses extrahiert die informationen aus dem URDF.
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>Zweitens, fügen wir die Positionsregler nach den <link> und <joint> Spezifikationen hinzu. Dazu dient der <transmission> Tag der hier beispielhaft für einen der Schulterjoints abgebildet ist:
<transmission name="shoulder_lift_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_lift_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_lift_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>Im <type> Tag können wir den Typ unseres Aktuators (Motors) definieren. Wir wählen SimpleTransmission als Übertragung was einfach bedeutet dass wir einen Motor haben dessen Bewegung über ein Getriebe auf die Ausgangsseite übertragen wird.
Um unsere Regler in Gazebo zu verwenden fügen wir ein EffortJointInterface für jeden Joint hinzu. Letztlich ist nämlich die Ausgangsgröße unseres Reglers, also die Größe die wir an den Motor weitergeben, “effort” (also Kraft bzw. Drehmoment). Wir tun außerdem so als sei das Übersetzungsverhältnis unseres Getriebes zwischen Motor und Joint 1:1, daher setzten wir <mechanicalReduction> auf den Wert 1.
Die Konfigurationsdatei
Zusätzlich zu unserem erweiterten URDF brauchen wir eine Konfigurationsdatei in der wir die Reglerparameter definieren die anschließend in den Parameter Sever geladen werden. Unser Positionsregler wird ein PID-Regler sein was heißt, dass wir einen proportionalen, einen integralen und einen differentiellen Anteil haben. Wenn du mit diesen Begriffen nichts anfangen kannst dann keine Sorge, wir werden uns mit dem Regleraufbau im letzten Abschnitt näher beschäftigen wenn es ans Tuning geht.
Vorerst ist es nur wichtig zu wissen, dass wir diese Konfigurationsdatei brauchen um ros_control bestimmte Parameter für unsere Regler mitzuteilen. Die Konfigurationsdatei ist im Repository in dem Unterordner config gespeichert. Und trotzdem habe ich sie hier für dich nochmal abgedruckt:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
shoulder_pan_joint_position_controller:
type: effort_controllers/JointPositionController
joint: shoulder_pan_joint
pid: {p: 500.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
shoulder_lift_joint_position_controller:
type: effort_controllers/JointPositionController
joint: shoulder_lift_joint
pid: {p: 500.0, i: 100.0, d: 30.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
elbow_joint_position_controller:
type: effort_controllers/JointPositionController
joint: elbow_joint
pid: {p: 10000.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
wrist_1_joint_position_controller:
type: effort_controllers/JointPositionController
joint: wrist_1_joint
pid: {p: 200.0, i: 10.0, d: 20.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
wrist_2_joint_position_controller:
type: effort_controllers/JointPositionController
joint: wrist_2_joint
pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
wrist_3_joint_position_controller:
type: effort_controllers/JointPositionController
joint: wrist_3_joint
pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}Wie wir sehen sind für jeden Regler (ein Regler pro Joint) p, i, und d Werte definiert. Der robot_state_publisher am Anfang der Datei gewährt Zugang zu den Zuständen der Joints, ist aber im Moment für uns nicht wichtig.
Die Launch-Datei
Werfen wir zu Schluss noch einen kurzen Blick auf die sogenannte launch-Datei mit der wir unsere controller starten.
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find ur5-joint-position-control)/config/ur5_jnt_pos_ctrl.yaml" command="load"/>
<param name="robot_description" textfile="$(find ur5-joint-position-control)/urdf/ur5_jnt_pos_ctrl.urdf"/>
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="shoulder_pan_joint_position_controller shoulder_lift_joint_position_controller elbow_joint_position_controller wrist_1_joint_position_controller wrist_2_joint_position_controller wrist_3_joint_position_controller joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/ur5/joint_states" />
</node>
</launch>Zuerst laden wir die Reglerparameter von unserer ur5_config.yaml Datei zusammen mit der URDF in den Parameterserver. Dann spezifizieren wir das URDF für unseren Roboter. Im nächsten Schritt starten wir die einzelnen Regler mit ros_control’s controller_spawner. Dann starten wir den robot_state_publisher der die Joint-Zustände des joint_state_controller tranformiert und in /tf publisht um es für andere Programme wie Rviz zugänglich zu machen (momentan nicht wichtig).
Zuletzt, bevor wir alles starten können, müssen wir das universal_robot package vom ROS Industrial Projekt herunterladen welches die visuellen und geomatrischen Berschreibungen des UR5 Roboters enthält. Wenn du schon eines der vorherigen Tutorials gemacht hast dann sind die bereits installiert. Ansonsten, führe bitte die folgenden Befehle aus während du dich im src Ordner in deinem Workspace befindest und ersetze <distro> mit deiner installierten ROS Version.
~/catkin_ws/src$ git clone -b <distro>-devel https://github.com/ros-industrial/universal_robot.git $ cd $HOME/catkin_ws
$ rosdep update
$ rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths srcDann kannst du deinen Workspace bauen:
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bashJetzt starten wir Gazebo:
$ roslaunch gazebo_ros empty_world.launchFühre die folgenden zwei Befehle in einem neuen Terminal aus um den Roboter zu laden und die Regler zu starten (vergiss nicht dass du eventuell den Source-Befehl source devel/setup.bash noch einmal ausführen musst):
$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5
$ roslaunch ur5-joint-position-control ur5_joint_position_control.launchDer Roboterarm wir in seiner Nullkonfiguration geladen was für den UR5 bedeutet dass er am Boden liegt. Wenn wir von dort aus versuchen die Regler zu tunen kann das umständlich werden. Wir müssen den Roboter zuerst in eine Position bringen in der wir die Bewegungen befehligen können die wir für das Reglertuning benötigen. Mit zu Beginn schlechten Reglerparametern kann das nervtötend sein weswegen wir die Startpose unseres Roboters im Startbefehl definieren. Allerdings gibt es in diesem Feature von Gazebo anscheinend noch einen Bug. Im Folgenden beschreibe ich den Ablauf um den Roboter in einer bestimmten Pose zu starten der für mich funktioniert:
Zuerst, starte Gazebo wie davor:
$ roslaunch gazebo_ros empty_world.launchDann drücke auf den pause Button in der unteren Leiste um die Simulation anzuhalten. Danach, lade den Roboter mit dem erweiterten Befehl:
$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5 -J shoulder_lift_joint -1.5 -J elbow_joint 1.0Die zusätzlichen Argumente -J shoulder_lift_joint -1.5 und -J elbow_joint 1.0 bestimmen die Startwinkel für den Schulter und den Ellbogenjoint. Dann starten wir die Regler genau wie zuvor:
$ roslaunch ur5-joint-position-control ur5_joint_position_control.launchJetzt können wir die Simulation wieder starten indem wir in der unteren Leiste wieder auf unpause (den Playbutton) klicken. Der Roboter ist jetzt in einer komfortableren Position und die Regler versuchen, den Roboter in dieser Position zu halten. Alternativ kannst du auch die folgende Launch-Datei ausführen die die vorherigen Schritte für dich übernimmt (außer dass du die Simulation mit dem Playbutton wieder starten musst):
$ roslaunch ur5-joint-position-control ur5_gazebo_position_control.launchPositionsbefehle senden
Nun können wir Befehle an unseren Positionsregler senden um den Roboter zu bewegen. Wenn dir das Konzept von Publisher, Subscriber und ROS-Topics nicht bekannt ist dann ließ dir dazu nochmal eines der ROS Tutorials durch. Der Positionsregler hört auf die folgende Topic: ur5/<controller_name>/command/. Rqt_gui ist die einfachste Art, Befehle unter dieser Topic zu senden und lässt sich mit dem folgenden Befehl von unserem Terminal aus starten:
$ rosrun rqt_gui rqt_guiIn der Menüleiste unter dem Reiter Plugins wählen wir Topics aus.
Nun können wir verschiedene Topics auswählen die wir versenden (publishen) möchten, indem wir das Suchfeld verwenden und unsere Auswahl mit dem grünen Pluszeichen hinzufügen. Wir werden uns den Tuningprozess am Beispiel des wrist_1 Joint ansehen, deswegen füge bitte die /wrist_1_joint_position_controller/command Topic hinzu. Wir können einen bestimmten statischen Wert festlegen indem wir die Topic mit dem kleinen Pfeil links erweitern und den Wert in der Spalte expression in der data Zeile eintragen. Alternativ können wir Funktionen definieren und Rqt eine Sequenz von Werten für uns berechnen zu lassen. Anstatt einem statischen Werte könnten wir etwas wie sin(0.1*i) schreiben, wobei i eine Zeitvarible ist die Rqt zur Verfügung stellt. Außerdem können wir konfigurieren, mit welcher Frequenz wir die Topic publishen wollen wobei 100 ein guter Wert dafür ist. Sobald wir einen Haken in die kleine Checkbox auf der linken Seite machen sollten usere Regler auf den Befehl reagieren.
Die Reglerperformance kann je nach Joint sehr unterschiedlich sein, was daran liegt das wir die gleichen Standardparameter für unterschiedliche Joints definiert haben die unterschidliche Massen bewegen müssen.
PID-Regler tunen
Unten sehen wir den Unterschied zwischen einem Regler mit schlechten und einem Regler mit guten Parametern die auf den gleichen Positionsbefehl reagieren. Ganz offensichtlich müssen wir die Reglerparameter tunen damit sich der Roboter insgesamt geschmeidig bewegt. Dabei hilft uns wiederum die Rqt_gui.
Position control only with proportional gain 
Position controller with tuned PID gains
Wie zu vor beim Message Publisher füge nun das Plot plugin aus dem Plugins-Menü zu Rqt hinzu.
Ähnlich wie beim Message Publisher können wir nun Topics, deren Werte wir uns ansehen wollen, zu unserem Graphen hinzufügen indem wir in der Leiste danach suchen und dann auf das grüne Plus klicken. Für das Tuning möchten wir uns neben dem Befehl den wir zum Regler senden auch ansehen wie gut unser Roboter darauf reagiert. Füge als Beispiel die Topics /wrist_1_joint_position_controller/state/process_value – das ist die (virtuell) gemessene aktuelle Jointposition – und /wrist_1_joint_position_controller/command/data – das ist der Reglerbefehl – zu unserem Graphen hinzu.
Ein üblicher Ansatz um Reglerparameter zu tunen ist, sich die Sprungantwort des Systems anzusehen. Das heißt, wie reagiert der Regler zusammen mit dem System auf eine plötzliche Änderung des Positionsbefehls. Das können wir testen indem wir den Wert für /wrist_1_joint_position_controller/command ändern, z.B. von 0 auf -1 (Im falle des writs_1 Joints bedeutet das eine Bewegung nach oben). So können wir beobachten wie gut die echte Position unserem Stellwert folgt und anhand dessen können wir unsere Parameter tunen.
Rqt erlaubt uns, die Reglerparameter unmittelbar anzupassen. Füge das Plugin Dynamic Reconfigure aus dem Plugins-Menü unter dem Ordner Configuration hinzu und wähle den wrist_1_joint_positon_controller aus der Liste auf der linken Leiste aus indem du den entsprechenden pid auswählst.
Der proportionale Anteil
Im p-Anteil wird die Abweichung (den Fehler) \( p_{error}\) zwischen der gewünschten Position und der aktuellen Position mit dem Parameter \(K_p\) multipliziert. Wenn wir also \(K_p\) erhöhen erreicht der Regler die gewünschte Position schneller und genauer, aber die Schwingungen werden größer. Wenn wir nur den proportionalen Anteil haben ist außerdem ein relativ hohes \(K_p\) nötig um exakt die gewünschte Position zu erreichen. Um den Einfluss der verschiedenen Parameter zu demonstrieren, setze p = 20, i = 0.0 und d = 0.0 im Dynamic Reconfigure Plugin. Jetzt können wir uns die Sprungantwort ansehen.
Der differentielle Anteil
Der d-Anteil ist für die Dämpfung im Regler zuständig. Hier wird die Geschwindigkeit \(v\) unserer Joints mit dem \(K_d\)-Parameter multipliziert und vom Reglerausgang abgezogen. Wenn wir \(K_d\) z.B. auf 5 erhöhen machen wir den Regler wieder etwas langsamer, aber wir dämpfen damit auch die Schwingungen. Eine gute Daumenregel ist, dass die Überschwingung weniger als 10% des Sprungs der Regelgröße betragen soll (hier also 0.1), wobei der Regler dabei so Reaktionsschnell wie möglich bleiben sollte.
Der integrale Anteil
Für den i-Anteil wird lediglich der Fehler über die Zeit aufaddiert (\(\int_{0}^{t} p_{error}(t)\)), sodass auch ein kleiner Fehler der über längere Zeit besteht Einfluss auf das Reglerverhalten hat. Damit werden wir den bleibenden Positionsfehler los, allerdings keine Schwingungen.
Die weiteren Parameter, i_clamp_min and i_clamp_max definieren Grenzen für den Wert auf den unser aufsummierter Fehler im i-Anteil anwachsen kann, damit keine zu hohen Werte am Reglerausgang entstehen. Vorerst können wir die Werte wie folgt definieren: i = 0.1, i_clamp_min = -200 and i_clamp_min = 200.
Das schaut schon ganz gut aus! Du kannst weiter mit den Parametern der unterschiedlichen Jointregler herumprobieren bis du die optimalen Einstellungen für unseren Roboter gefunden hast. Das kann zum Teil recht herausfordernd sein aber damit bekommt man ein gutes Gefühl dafür, welchen Einfluss die Eigenschaften des Roboters und die Reglerparameter auf die Performance haben. Wir werden die Parameter im nächsten Tutorial benutzen wo wir die Spitze (tcp-Position) unseres Roboters im Kartesischen Raum mit Hilfe von Vorwärts- und Rückwärtskinematik steuern wollen. Dies ist eine Voraussetzung für fast alle Anwendungen eines Roboterarms. Wenn du bereit dafür bist dann geht’s gleich weiter mit dem nächsten Tutorial.
Kurzanleitung
Um den UR5 roboter zusammen mit ros_control in Gazebo zu laden, klone zuerst das folgende Repository in den src Ordner in deinem Catkin Workspace:
~/catkin_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.gitDann installiere ros_control:
$ sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllersWenn nicht bereits von einem der vorigen Tutorials vorhanden, installiere das universal_robot Package:
~/catkin_ws/src$ git clone -b <distro>-devel https://github.com/ros-industrial/universal_robot.git
$ rosdep update
$ rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths srcDann baue deinen Workspace:
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bashDanach, starte eine leere Umgebung in Gazebo:
$ roslaunch gazebo_ros empty_world.launchÖffne ein neues Terminal, lade den UR5 Roboter und starte die Positionsregler für die Joints:
$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5
$ roslaunch ur5-joint-position-control ur5_joint_position_control.launchWenn du den Roboter in einer aufrechteren Position starten möchtest kannst du auch einfach diese Launch-Datei benutzen die Gazebo startet und sowohl den Roboter als auch die Regler lädt. Du musst danach nur die Simulation mit dem Play-Button in der unteren Leiste in Gazebo starten:
$ roslaunch ur5-joint-position-control ur5_gazebo_position_control.launchNun kannst du Positionsbefehle z.B. für den Schulterjoint über die Topic /shoulder_lift_joint_controller/command publishen. Am einfachsten geht das mit der Rqt_gui. Oder du lernst wie das programmiertechnisch geht in dem nächsten Tutorial in dem wir die Spitze (tcp-Position) unseres Roboters steuern.
