Bioloid Dynamixel URDF RViz tutorial

In RViz können Sensordaten visualisiert werden. Als 3D-Modell soll der Bioloid Robotor in URDF – Unified Robot Description Format erstellt werden. URDF ist ein XML-Format wobei die Endung .xml einfach in .urdf umbenannt wird, die Umbenennung jedoch nicht zwingend ist, wenn man mit .launch-Dateien oder RViz arbeitet. XML ist eine Beschreibungssprache ähnlich HTML aus der SGML-Familie. Das Hauptmerkmal von XML ist die Fehlerfreiheit im Gegensatz zu HTML, wo Fehler erlaubt sind und aufwendig vom Browser korrigiert werden. Ein XML-Dokument ist wohlgeformt, wenn jedes öffnende „Tag“ ein schließendes „Tag“ hat. Es kann nur ein einzelnes Wurzelelement in einem XML-Baum geben.

Bioloid URDF - RViz left arm

Bioloid URDF

Quellennachweis:
http://www.eclipse.org/webtools/sse/
http://www.ros.org/wiki/urdf
http://www.trossenrobotics.com/dynamixel-ax-12-robot-actuator.aspx

Voraussetzung:
-Ubuntu 12.04 LTS, ROS, Eclipse XML Editors and Tools (WST), Arbotix-Controller
-Grundkenntnisse in Linux, ROS-Fuerte, Eclipse und XML
-Alle vorhergehenden Robotik-Tutorial

Installation

apt-get install ros-fuerte-robot-model
rosmake robot_model

ROS package

Ein Paket muss erstellt werden, oder ein vorhandenes muss in manifest.xml um die Pakete arbotix_msgs, arbotix_python erweitert und kompiliert werden.

Neues Paket erstellen:

roscreate-pkg tutorial_bioloid_dynamixel_urdf image_transport roscpp std_msgs opencv2 cv_bridge gscam arbotix_msgs arbotix_python


Vorhandenes Paket erweitern in manifest.xml:

<package>
  <description brief="tutorial_bioloid_dynamixel_urdf">

    tutorial_bioloid_dynamixel_urdf

  </description>
  <author>Murat Calis</author>
  <license>BSD</license>
  <review status="draft" notes="working draft"/>
  <url>http://www.muratcalis.com/index.php/robotics</url>
  <depend package="image_transport"/>
  <depend package="roscpp"/>
  <depend package="std_msgs"/>
  <depend package="opencv2"/>
  <depend package="cv_bridge"/>
  <depend package="gscam"/>
  <depend package="arbotix_msgs"/>
  <depend package="arbotix_python"/>

</package>


Paket kompilieren, sourcen und Verzeichnisse (launch, urdf) erstellen:

rosmake tutorial_bioloid_dynamixel_urdf
echo "export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:"$(pwd) >> ~/.bashrc
. ~/.bashrc
roscd tutorial_bioloid_dynamixel_urdf
mkdir launch urdf

Dynamixel Treiber

RViz verwendet das Nachrichtenprotokoll sensor_msgs/JointState für die Adaption der Gelenkstellungen eines Roboters. Um RViz mit den nötigen Sensordaten zu versorgen, muss ein ROS-Knoten das Thema /joint_states publizieren. Der einfachste Weg ist einen Motor-Treiber zu verwenden, der die nötige Arbeit erledigt und die Motordaten an das Thema /joint_states weiterleitet. Diese Aufgabe übernimmt driver.py aus dem Paket arbotix. AX12+ Dynamixel-Motoren arbeiten in einem Winkel von 300° mit einer Auflösung von 1024 Stufen (ticks), das ergibt eine Winkelauflösung von ca. 0.29°. RViz rechnet pro Umdrehung 2*Pi von -3,14 bis +3,14. Würde man die Rohdaten eines AX12+ auslesen, bekäme man Zahlen zwischen 0 und 1023. RViz setzt nach einer Zahl, die höher als 6,28 (~ 2 * 3.14) ist, den Zeiger einfach zurück auf 0 und beginnt die Drehung von vorne – das entspräche bei einer realen Umdrehung eines AX12-Motors ungefähr 163 Drehungen im Simulator, was sich wie ein Zucken der 3D-Objekte bemerkbar macht.

Treiber konfigurieren – arbotix_driver.yaml:

roscd tutorial_bioloid_dynamixel_urdf
vi arbotix_driver.yaml

port: /dev/ttyUSB0
rate: 15
dynamixels: {
    motor2: {id: 2, invert: true},
    motor4: {id: 4},
    motor6: {id: 6}
}
controllers: {
  joint_controller: {type: follow_controller, joints: [motor2, motor4, motor6]},
}

port: der Port an dem der Arbotix-Controller über einen FTDI-USB-Adapter angeschlossen ist.
rate: Aktualisierungsrate in Hertz.
dynamixels: Motoren Bezeichnung und ID. Die IDs können mit „rosrun arbotix_pyhton terminal.py“ gesetzt werden, hier werden die Fabrikeinstellungen verwendet.
controllers: Optional für Befehlsempfang /cmd_vel.

Starter konfigurieren – arbotix_driver.launch:

roscd tutorial_bioloid_dynamixel_urdf/launch
vi arbotix_driver.launch

<launch>
  <node name="arbotix" pkg="arbotix_python" type="driver.py" output="screen">
      <rosparam file="$(find tutorial_bioloid_dynamixel_urdf)/arbotix_driver.yaml" command="load" />
  </node>
</launch>

Konfiguration testen:

roslaunch tutorial_bioloid_dynamixel_urdf arbotix_driver.launch &
rostopic echo /joint_states

Wenn der Befehl „rostopic echo /joint_states“ die Motorenbezeichnungen und Stellwerte zeigt, hat alles geklappt. Mit dem Befehl „rosrun arbotix_python ControllerGUI.py“ kann man die Motoren bewegen.

Mögliche Probleme:

-Batterie zur Versorgung des Arbotix-Controllers zu schwach.
-FTDI-Adpater nicht angeschlossen.

URDF

Als nächstes wird eine urdf-Datei erstellt. Im Ordner urdf wird die Datei bioloid.xml angelegt und mit Eclipse geöffnet. Die Dateiendung xml veranlasst Eclipse dazu, die Perspektive anzupassen, um XML-Syntax farblich korrekt darzustellen.

<?xml version="1.0" ?>
<robot name="bioloid">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.09 0.07 0.12" />
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.999"/>
            </material>
        </visual>
    </link>
</robot>

Der Prolog (<?xml version=“1.0″?>) ist immer gleich bei einfachen XML-Dokumenten, wenn keine DTD oder XLS verwendet wird – siehe XML-Spezifikation.
Der Wurzelknoten <robot> ist einmalig innerhalb der .xml-Datei.
Die URDF-Datei enthält der Einfachheit wegen nur ein visuelles Element vom Typ link.
Darin wird eine Box in Meter angegeben.

Anstelle eines <box />-Elements kann auch ein <mesh />-Element mit einem Verweis auf eine .stl- oder .dae-Datei verwendet werden. Die Modellierungsmöglichkeiten mit URDF sind im Vergleich zu VRML bescheiden, aber dafür gibt es eben die Schnittstelle zu Collada-Dateien. Die Arbeit findet dann in einem professionellen CAD-Programm statt und wird von dort als .dae-Datei exportiert. Innerhalb einer urdf-Datei werden die so erzeugten Meshes per Link eingebunden. Wenn man eine *.dae-Datei erzeugt, muss man darauf achten dass alle Objekte zu einer Einheit zusammengefasst wurden (Join), sonst stellt RViz diese falsch dar.

Prüfung einer urdf-Datei:

rosrun urdf check_urdf bioloid.xml

RViz:
Das Ergebnis kann in RViz mit dem vorinstallierten Paket urdf_tutorial angezeigt werden. Die dazugehörige .launch-Datei empfängt zwei optionale Parameter model und gui. Um eine eigene urdf-Datei als Parameter zu verwenden muss man sich im urdf-Ordner des eigenen Pakets befinden oder den Pfad mit angeben. In diesem Beispiel ist der Standort im Linux-Verzeichnis tutorial_bioloid_dynamixel_urdf/urdf. Dann lautet der Befehl für rviz mit eigener urdf-Datei:

roslaunch urdf_tutorial display.launch model:=bioloid.xml

Arbeiten mit URDF:
Änderungen in der urdf-Datei werden nicht sofort in RViz übernommen, denn die Quelldaten stammen vom Parameter-Server ab, der wiederum die urdf-Daten beim Start der launch-Datei erhalten hat. Die urdf-Daten in /robot_description können mittels eigener launch-Datei aktualisiert werden. Leider übernimmt RViz dann nur Geometriedaten und keine Änderungen an Motoren (joints), vielmehr muss man RViz komplett neu starten damit die Änderungen übernommen werden. Abfragen kann man die aktuell geladene urdf-Datei mit:

rosparam get /robot_description

URDF-Daten aktualisieren:
Hat man nur kleine Änderungen gemacht und möchte diese in RViz sehen, kann eine launch-Datei mit Verweis auf die zu ladende urdf-Datei hilfreich sein. Gestartet wird die launch-Datei mit „roslaunch <pfad>/<dateiname.launch>“. Um die Aktualisierung in RViz zu sehen nimmt man das Häkchen bei Robot Model raus und wieder rein. Wie bereits erwähnt lassen sich Änderungen an Geometriedaten von <link>-Elementen auf diese Weise aktualisieren jedoch nicht die von <joint>-Elementen.

<launch>
    <param name="robot_description" command="cat $(find tutorial_blabla)/urdf/bioloid.xml" />
</launch>

URDF verstehen:
In einem URDF-Baum gibt es immer Eltern-Kind-Knoten. Das bedeutet ein Kind-Knoten hat immer ein Elternelement. Der Bezug im Koordinatensystem ist dementsprechend: parent-Element steht im Raum vor dem child-Element. Das bedeutet soviel wie, das child-Element bewegt sich im Koordinatensystem relativ zum parent-Element entfernt. Das Verschieben eines der obersten parent-Elemente impliziert eine Verschiebung aller darunter befindlichen child-parent-Elemente.

Ausführlich erklärt wurde URDF auf der ROSCon 2012:


bioloid.xml:

Die vollständige URDF-Datei für den linken Arm:

<?xml version="1.0" ?>
<robot name="bioloid">
	<link name="base_link">
		<visual>
			<geometry>
				<box size="0.09 0.07 0.12" />
			</geometry>
			<material name="white">
				<color rgba="1 1 1 0.999"/>
			</material>
		</visual>
	</link>

  <joint name="motor2" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <!-- OFFSET relative to x0y0z0 -->
    <origin xyz="0.02 0 0.04" rpy="1.57 0 0"/>
    <axis xyz="-1 0 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0.5"/>
  </joint>

	<link name="link1">
    <visual>
      <geometry>
        <mesh filename="file:///home/username/fuerte_workspace/tutorial_blabla/urdf/bioloid_shoulder_join.dae" scale="0.01 0.01 0.01"/>
      </geometry>
      <!-- OFFSET relative to motor0 -->
      <origin xyz="0.0425 0 0" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="1 1 1 0.999"/>
      </material>
    </visual>
  </link>

	<joint name="motor4" type="revolute">
		<parent link="link1" />
		<child link="link2" />
		<origin xyz="0.05 0 0.01" rpy="0 1.57 0" />
		<axis xyz="0 -1 0" />
		<limit effort="0" lower="-3.14" upper="3.14" velocity="0.5"/>
	</joint>

	<link name="link2">
		<visual>
      <geometry>
        <mesh filename="file:///home/username/fuerte_workspace/tutorial_blabla/urdf/bioloid_upper_arm_link_joint.dae" scale="0.01 0.01 0.01"/>
      </geometry>
      <origin xyz="0 0 0.025" rpy="0 0 0" />
      <material name="green">
				<color rgba="1 0 0 0.999"/>
			</material>
		</visual>
	</link>

	<joint name="motor6" type="revolute">
		<parent link="link2" />
		<child link="link3" />
		<origin xyz="0.0 0 0.06" rpy="0 0 0" />
		<axis xyz="0 -1 0" />
		<limit effort="0" lower="-3.14" upper="3.14" velocity="0.5"/>
	</joint>

	<link name="link3">
		<visual>
      <geometry>
        <mesh filename="file:///home/username/fuerte_workspace/tutorial_blabla/urdf/bioloid_hand_link_joint.dae" scale="0.01 0.01 0.01"/>
      </geometry>
      <origin xyz="0 0 0.035" rpy="0 0 0" />
      <material name="white">
				<color rgba="0 1 0 0.999"/>
			</material>
		</visual>
	</link>

</robot>

Schreibe einen Kommentar