URDF モデリング

はじめに

株式会社Ristのロボットチームのプラダンです。ネパール出身ですが、2年前に来日しました。
タイのアジア工科大学大学院でメカトロニクスの修士号を取得。
Ristに入社する前は研究者、機械エンジニアとして働いてきました。

概要

URDFとは

URDFとは「Unified Robot Description Format」の略です。これはXML形式で書かれたコードで、'.urdf'という拡張子を付けて保存されます。ここにはロボットの物理的特性全体が記述されており、URDFモデルはROSでデータを公開している実際のロボットを仮想的に記述するために使用されます。これにより、RVIZのようなROSプログラムは、実際のジョイント値に基づいてロボットを表現することができます。

URDFコードには主にリンクとジョイントの情報が記述されています。

リンク
リンクとはシミュレートされた世界に3D空間（xyz座標）で表現されるロボットの身体・部分を指しています。

ジョイント
ジョイント（関節）は、3D空間（xyz座標）において、それぞれのリンクがどのように相互に、そして世界にどうコネクトされているかを定義するものです。

リンク

ロボットのリンクはワールドに対して以下のように記述され、定義されています：

外観（visual）： ロボット本体の外観です。ビジュアル部分には3つのタグがあります。

オリジン（origin）：実世界から見たロボット・パーツのxyz座標とrpy（roll, pitch, yaw）の向きが含まれています。
ジオメトリ（geometry）：これには、STLまたはコラダ形式の3Dメッシュが含まれています。
素材（material）：RVizで可視化する素材の色を記述します。

衝突（collision）： この部分はビジュアルの部分と非常に似ており、ロボットのパーツ同士がどのようにくっついているのか、シミュレーション環境がどのようになっているのかを示しています。

オリジン（origin）：ロボット・パーツのxyz座標とrpy（roll, pitch, yaw）方向を格納しています。
ジオメトリ(geometry)：視覚部とは異なり、ジオメトリは立方体、球体、円柱などの単純な幾何学的形状や、解像度が極端に低いメッシュなどで表現されます。

慣性（inertia）： ここでは、ボディ/ロボット部品の慣性特性の質量と運動量について説明します。ここでは、質量の中心、質量と慣性行列について説明します。

オリジン（origin）：xyz 座標とロボット・パーツの質量中心の rpy (roll, pitch, yaw) 方向。
マス（mass）：パーツ・ボディの重さ。
慣性（inertia）：パーツ・ボディの慣性行列。

<link name="base_link">
    <visual>
      <origin xyz="-0.064 0 0.0" rpy="0 0 0"/>
      <geometry>        
        <mesh filename="package://rist_tr_urdf/meshes/bases/waffle_pi_base_for_open_manipulator.stl" scale="0.001 0.001 0.001"/>        
      </geometry>
      <material name="light_black"/>
    </visual>

    <collision>
      <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
      <geometry>
        <box size="0.266 0.266 0.094"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="1.3729096e+00"/>
      <inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"
               iyy="8.6195418e-03" iyz="-3.5422299e-06"
               izz="1.4612727e-02" />
    </inertial>    

  </link>

ジョイント

ジョイント（関節）は、1 つのリンク (子リンク) を別のリンク (親リンク) に接続する方法を説明します。複数の子リンクはジョイントを介して 1 つの親リンクに接続することができます。ここでは、ジョイントの種類も記述します（リボリュート、プリズム、固定など）。また、ジョイントの限界、ダンピング、摩擦およびジョイントがどの軸で動くか（xyz）が記載されています。ジョイントが固定タイプの場合は、制限、減衰、ジョイントの移動軸などの高度なジョイント要素は必要ありません。

  <joint type="continuous" name="left_wheel">
    <origin xyz="0.1 0.15 0" rpy="0 0 0"/>
    <child link="left_wheel"/>
    <parent link="chassis"/>
    <axis xyz="0 1 0" rpy="0 0 0"/>
    <limit effort="100" velocity="100"/>
    <joint_properties damping="0.0" friction="0.0"/>
  </joint>

メモ

xmlコードを'.urdf'拡張子で保存する場合、1つのファイルにすべての要素とプロパティを含まなければならないため、コードが非常に長く、読みにくく、メンテナンスが難しく、より複雑で計算量が多いため冗長性が高いです。

これを避けるために、拡張子を'.XACRO'とし、異なるプロパティを含む複数のファイルを作成しています。これにより、様々なXACROファイルからmacrosと呼ばれる特定のプロパティをメインのロボット記述ファイルに呼び出すことができます。

XACRO形式

ロボット記述XACROファイルです。

I. コードは常にxmlのバージョンを記述するところから始まり、ロボット名とXACROリンク、そして呼び出されるXACRO macrosが続きます。

II. 次の重要なステップは、原点である'base_footprint'/'world'リンクを含めることです。

III. 原点が設定されると、すべてのリンクとジョイントが base_footprint を基準に記述されます。ジョイントブロックには、ジョイントの名前、ジョイントの種類（リボリュート、連続、固定、プリズムなど）、親リンク、子リンク、base_footprintを基準としたxyz座標、ジョイントの回転軸、制限（速度、力、可動域の上限、下限）が記述されます。
親リンクは本体、子リンクはジョイントを介して本体に接続されている体です。
メモ： XACROファイルでは、数学的な計算や数式を使うことができます。しかし、URDFファイルでは数値しか使えません。

IV. 次はリンクの記述です。リンクブロックには、身体の視覚、衝突、慣性が含まれています。

a. 視覚ブロックには、ボディの幾何学的/STL/DAEメッシュ、色、base_footprint/worldを基準とした座標が含まれています。

b. 衝突ブロックには、ボディの基本的な幾何学的形状と、それがシミュレートされた世界の別のボディやオブジェクトとどのように相互作用したり、衝突したりするかが含まれています。
衝突するボディのxyz軸とrpyの向きは、視覚的な側面とほぼ同じです。
c. リンクブロックの最後のブロックは、慣性ブロックです。これは、ボディの質量と慣性モーメントを記述します。
慣性は、回転軸について所望の角加速度を得るために必要なトルクです。
簡単に言えば、質量と回転軸からの垂直距離の二乗の積に等しい角加速度または減速に対する回転体の抵抗です。
ここでは、回転体の質量と3軸の慣性モーメントを記述します。

メモ： 慣性計算は、身体の幾何学的形状によって異なります。単純な形状（円柱、立方体、球体）の場合、体の慣性を記述するには ixx、ijy、izz の値だけで十分です。しかし、複雑な形状の場合は、慣性行列全体の値（ixx、ixy、ixz、yy、izy、izz）が必要となり、計算が複雑で時間がかかるため、3次元空間における身体の慣性モーメントを求めるのは面倒な作業となります。これを簡単に解決するには、MeshLabのような慣性モーメントを求めるソフトウェアを利用して、単純な形状や複雑な形状の慣性モーメントを求めることができます。
メモ: 衝突や慣性が記述されていない場合、RVizでは問題なく表示されますが、Gazeboのシミュレーションでは、ロボットは剛性がなく頑丈ではありません。Gazeboはシミュレーションのために衝突と慣性を必要とするため、ロボットは質量を持たず、物理学の法則に従わなくなります。

V. xmlファイルは以下の形式で閉じます。

</robot>

II. XACROマクロのmaterial、一般的なプロパティ

XACROファイルにはRVizで表示するときのロボットのmaterialの色が含まれています。XACROマクロは下記の通りフォーマットできます。

III. XACROマクロ

XACRO マクロには様々な使い方があります。

Properties： 記述ファイルで複数回使用される部品の幾何学的なプロパティを記述します。
Math：数学的な計算を記述します。

<xacro:property name="width" value=".2"/>
<cylinder radius="${width}" length=".1"/>

Co-ordinated： xyzの座標とrpyの向きを記述します。
Parametrized macro： パラメータを渡します。
Nested macro： 別のファイルのマクロを現在のマクロの中で使用します。

<xacro:macro name="default_origin">
  <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:macro>
<xacro:default_origin/>
  <inertial>
    <xacro:default_origin/>
    <mass value="${mass}/>"
      <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>             
  </inertial>
</xacro:macro>
<xacro:default_inertial mass="10"/>

Default macros： XACROファイルで使用するデフォルト値を設定します。
Conditional macros： 必要な条件(true/false; 0/1)を満たした場合にのみマクロのプロパティを使用します。
Command Line argument： XACRO ファイルの特定のプロパティを使用するために、コマンドラインでマクロを使用することもできます。

<xacro:macro name="pos" params="x:y=0"/>
<xacro:pos x="1"/>

<xacro:if value="<expression>">
<xacro:unless value="<expression>">

<xacro:arg name="rad" default="2"/>
<cylinder radius="$(arg rad)" length=".1"/>

IV. GazeboのXACROファイル

Gazebo XACROファイルにはURDF.XACROロボット記述コードに基づいて、Gazeboがロボットモデルをスポーンしてシミュレートするために使用するすべてのGazeboのプロパティが含まれています。
ロボットの色を含む、ロボットの運動やセンサーに関連するすべてのものをここに記述する必要があります。

 <gazebo reference="imu_link">
    <sensor type="imu" name="imu">
      <always_on>true</always_on>
      <visualize>$(arg imu_visual)</visualize>
    </sensor>
    <material>Gazebo/Grey</material>
  </gazebo>
  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>imu_link</bodyName>
      <frameName>imu_link</frameName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0</gaussianNoise>
      <updateRate>200</updateRate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </plugin>
  </gazebo>

  <gazebo reference="base_scan">
    <material>Gazebo/FlatBlack</material>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>$(arg laser_visual)</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>base_scan</frameName>
      </plugin>
    </sensor>
  </gazebo>

V. トランスミッション XACRO file

トランスミッション XACRO はロボットモデルにギアやモーターを提供するマクロを記述します。
モーションコマンドが発行されたり、スクリプトが起動されたりしたときに、Gazeboシミュレーションでロボットの動きを可能にするため、ロボットURDFファイルの重要な部分です。
ただし、RVizはGazeboのように実世界のプロパティや物理学をシミュレートしないため、RVizではロボットのURDFは正しく機能し、動きます。


<gazebo>
    <plugin name="turtlebot3_waffle_pi_controller" filename="libgazebo_ros_diff_drive.so">
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <odometrySource>world</odometrySource>
      <publishOdomTF>true</publishOdomTF>
      <robotBaseFrame>base_footprint</robotBaseFrame>
      <publishWheelTF>false</publishWheelTF>
      <publishTf>true</publishTf>
      <publishWheelJointState>true</publishWheelJointState>
      <legacyMode>false</legacyMode>
      <updateRate>30</updateRate>
      <leftJoint>wheel_left_joint</leftJoint>
      <rightJoint>wheel_right_joint</rightJoint>
      <wheelSeparation>0.287</wheelSeparation>
      <wheelDiameter>0.066</wheelDiameter>
      <wheelAcceleration>1</wheelAcceleration>
      <wheelTorque>10</wheelTorque>
      <rosDebugLevel>na</rosDebugLevel>
    </plugin>
</gazebo>

##ロボット記述パッケージの作成

catkin_ws/srcディレクトリ（フォルダ）にあることを確認します：

     $ roscd
     $ cd ../src

rospyの依存関係を持つrobot_descriptionパッケージを作成します。

     $ catkin_create_pkg robot_description rospy

新しく作成した robot_description パッケージの中に、以下のディレクトリ/フォルダを作成します。
- launch
- URDF
- mesh
- rviz
- config

###URDF folder
このフォルダにはすべての URDF と XACRO ファイルが格納されます。

###Mesh
このフォルダにはSTLやコラダ（.dae）などのすべてのメッシュファイルが格納されます。

###RViz
このフォルダはRVizの設定を保存するためのもので、次にRVizを開いたときに保存した方法で開くように設定します。ファイルパスはlaunchコードに記述する必要があります。

###Config
このフォルダにはxyz軸とrpy方向のロボットの運動学の設定を行ったyamlファイルが入っています。

###Launch
Launchフォルダにはロボット URDF/XACRO、ジョイントステートパブリッシャーノード、ロボットステートパブリッシャーノード、rvizノード（rvizファイルのパスを含む）が記述されている起動ファイルが格納されています。

メモ：：Launchファイルに記述しなければならない重要なパラメータは、XACROを使った "robot_description "です。もし 'robot_description' パラメータが渡されなかった場合、ロボットの URDF は表示されません。

<launch>

  <arg name="model" default="$(find rist_tr_urdf)/urdf/robot.urdf.xacro"/>
  <arg name="gui" default="true" />
  
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(arg model)'" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
    <param name="use_gui" value="$(arg gui)" />
  </node>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rist_tr_urdf)/rviz/model.rviz" /> <!--long_arm_config, small_arm_config, config-->
  <param name="use_gui" value="$(arg gui)" />

</launch>

もっと複雑なノードやパラメータ、引数などは必要に応じてLaunchファイルに記述することもできますが、RVizでロボットのURDFを見るには上記の内容で十分です。