ROS入門 (27) - Xacroによるロボットモデルの作成

npaka

2021年11月27日 07:07

「Xacro」によるロボットモデルの作成の手順をまとめました。

・Melodic

前回

1. Xacro

「Xacro」は、「URDF」を生成するためのマクロ言語です。次の3つの機能を持っています。

・定数
・数式
・マクロ

これまで、URDFでR2D2を作成してきましたが、設定が膨大になり、うんざりしているかと思います。「Xacro」を使うことで、URDFの全体的なサイズを縮小し、読みやすく、保守しやすくなります。

2. Xacroによるロボットモデルの作成

前回作成した「R2D2」のXacroファイル版を作成し、URDFに変換し、rvizで表示してみます。

◎ Xacroファイルの作成
・~/catkin_ws/src/urdf_tutorial/urdf/08-macroed.urdf.xacro

<?xml version="1.0"?>
<robot name="macroed" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:property name="width" value="0.2" />
  <xacro:property name="leglen" value="0.6" />
  <xacro:property name="polelen" value="0.2" />
  <xacro:property name="bodylen" value="0.6" />
  <xacro:property name="baselen" value="0.4" />
  <xacro:property name="wheeldiam" value="0.07" />
  <xacro:property name="pi" value="3.1415" />

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>

  <xacro:macro name="default_inertial" params="mass">
    <inertial>
      <mass value="${mass}" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </xacro:macro>

  <link name="base_link">
    <visual>
      <geometry>
        <cylinder radius="${width}" length="${bodylen}"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="${width}" length="${bodylen}"/>
      </geometry>
    </collision>
    <xacro:default_inertial mass="10"/>
  </link>

  <xacro:macro name="wheel" params="prefix suffix reflect">

    <link name="${prefix}_${suffix}_wheel">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
        <geometry>
          <cylinder radius="${wheeldiam/2}" length="0.1"/>
        </geometry>
        <material name="black"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
        <geometry>
          <cylinder radius="${wheeldiam/2}" length="0.1"/>
        </geometry>
      </collision>
      <xacro:default_inertial mass="1"/>
    </link>
    <joint name="${prefix}_${suffix}_wheel_joint" type="continuous">
      <axis xyz="0 1 0" rpy="0 0 0" />
      <parent link="${prefix}_base"/>
      <child link="${prefix}_${suffix}_wheel"/>
      <origin xyz="${baselen*reflect/3} 0 -${wheeldiam/2+.05}" rpy="0 0 0"/>
    </joint>

  </xacro:macro>

  <xacro:macro name="leg" params="prefix reflect">
    <link name="${prefix}_leg">
      <visual>
        <geometry>
          <box size="${leglen} 0.1 0.2"/>
        </geometry>
        <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
        <material name="white"/>
      </visual>
      <collision>
        <geometry>
          <box size="${leglen} 0.1 0.2"/>
        </geometry>
        <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
      </collision>
      <xacro:default_inertial mass="10"/>
    </link>

    <joint name="base_to_${prefix}_leg" type="fixed">
      <parent link="base_link"/>
      <child link="${prefix}_leg"/>
      <origin xyz="0 ${reflect*(width+.02)} 0.25" />
    </joint>

    <link name="${prefix}_base">
      <visual>
        <geometry>
          <box size="${baselen} 0.1 0.1"/>
        </geometry>
        <material name="white"/>
      </visual>
      <collision>
        <geometry>
          <box size="${baselen} 0.1 0.1"/>
        </geometry>
      </collision>
      <xacro:default_inertial mass="10"/>
    </link>

    <joint name="${prefix}_base_joint" type="fixed">
      <parent link="${prefix}_leg"/>
      <child link="${prefix}_base"/>
      <origin xyz="0 0 ${-leglen}" />
    </joint>
    <xacro:wheel prefix="${prefix}" suffix="front" reflect="1"/>
    <xacro:wheel prefix="${prefix}" suffix="back" reflect="-1"/>
  </xacro:macro>
  <xacro:leg prefix="right" reflect="-1" />
  <xacro:leg prefix="left" reflect="1" />

  <joint name="gripper_extension" type="prismatic">
    <parent link="base_link"/>
    <child link="gripper_pole"/>
    <limit effort="1000.0" lower="-${width*2-.02}" upper="0" velocity="0.5"/>
    <origin rpy="0 0 0" xyz="${width-.01} 0 0.2"/>
  </joint>

  <link name="gripper_pole">
    <visual>
      <geometry>
        <cylinder length="${polelen}" radius="0.01"/>
      </geometry>
      <origin xyz="${polelen/2} 0 0" rpy="0 ${pi/2} 0 "/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${polelen}" radius="0.01"/>
      </geometry>
      <origin xyz="${polelen/2} 0 0" rpy="0 ${pi/2} 0 "/>
    </collision>
    <xacro:default_inertial mass="0.05"/>
  </link>

  <xacro:macro name="gripper" params="prefix reflect">
    <joint name="${prefix}_gripper_joint" type="revolute">
      <axis xyz="0 0 ${reflect}"/>
      <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
      <origin rpy="0 0 0" xyz="${polelen} ${reflect*0.01} 0"/>
      <parent link="gripper_pole"/>
      <child link="${prefix}_gripper"/>
    </joint>
    <link name="${prefix}_gripper">
      <visual>
        <origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
        </geometry>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
        </geometry>
        <origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0 0 0"/>
      </collision>
      <xacro:default_inertial mass="0.05"/>
    </link>

    <joint name="${prefix}_tip_joint" type="fixed">
      <parent link="${prefix}_gripper"/>
      <child link="${prefix}_tip"/>
    </joint>
    <link name="${prefix}_tip">
      <visual>
        <origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0.09137 0.00495 0"/>
        <geometry>
          <mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
        </geometry>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
        </geometry>
        <origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0.09137 0.00495 0"/>
      </collision>
      <xacro:default_inertial mass="0.05"/>
    </link>
  </xacro:macro>

  <xacro:gripper prefix="left" reflect="1" />
  <xacro:gripper prefix="right" reflect="-1" />

  <link name="head">
    <visual>
      <geometry>
        <sphere radius="${width}"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="${width}"/>
      </geometry>
    </collision>
    <xacro:default_inertial mass="2"/>
  </link>

  <joint name="head_swivel" type="continuous">
    <parent link="base_link"/>
    <child link="head"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 ${bodylen/2}"/>
  </joint>

  <link name="box">
    <visual>
      <geometry>
        <box size="0.08 0.08 0.08"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="-0.04 0 0"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.08 0.08 0.08"/>
      </geometry>
    </collision>
    <xacro:default_inertial mass="1"/>
  </link>

  <joint name="tobox" type="fixed">
    <parent link="head"/>
    <child link="box"/>
    <origin xyz="${.707*width+0.04} 0 ${.707*width}"/>
  </joint>

</robot>

URDFは400行ほどでしたが、Xacroファイルは200行ほどになっています。

◎ XacroファイルをURDFに変換
Xacroファイル「08-macroed.urdf.xacro」をURDF「model.urdf」に変換するコマンドは、次のとおりです。

$ cd ~/catkin_ws/src/urdf_tutorial/urdf
$ xacro 08-macroed.urdf.xacro > model.urdf

◎ rvizでの確認
rvizでURDFを表示するコマンドは、次のとおりです。
（見た目は前回と同じ）

$ cd ~/catkin_ws/src/urdf_tutorial
$ roslaunch urdf_tutorial display.launch model:=urdf/model.urdf

3. Xacroファイルの先頭の記述

「URDF」の先頭は、次のように記述してきました。

<?xml version="1.0"?>
<robot name="XXXX">
    :
</robot>

「Xacroファイル」の先頭は、次のように記述します。

<?xml version="1.0"?>
<robot name="XXXX" xmlns:xacro="http://ros.org/wiki/xacro">
    :
<robot>

4. 定数

「URDF」では「base_link」は次のように記述します。

  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
  </link>

この情報は少し冗長です。円柱の長さと半径を2回指定します。さらに悪いことに、それを変更したい場合は、2つの異なる場所で変更する必要があります。

「Xacro」では「定数」を使って、次のように記述することができます。

<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />
<link name="base_link">
  <visual>
    <geometry>
      <cylinder radius="${width}" length="${bodylen}"/>
    </geometry>
    <material name="blue"/>
  </visual>
  <collision>
    <geometry>
      <cylinder radius="${width}" length="${bodylen}"/>
    </geometry>
  </collision>
</link>

2つの定数を、先頭の2行で定義しています。これは、使用前後どこでも、任意のレベルで定義できますが、通常は先頭に記述します。そして、<geometry>で実際の半径を指定する代わりに、${定数名}で指定します。

定数では、値だけでなくテキストと置き換えることも利用できます。

<xacro:property name=”robotname” value=”marvin” />

<link name=”${robotname}s_leg” />
    ↓
<link name=”marvins_leg” />

5. 数式

$ {}では、4つの基本演算子(+, -, *, /)、単項マイナス、括弧、sin/cosを使用して、任意の数式を埋め込むこともできます。

<cylinder radius="${wheeldiam/2}" length="0.1"/>
<origin xyz="${reflect*(width+.02)} 0 0.25" />

すべての計算はfloatを使用するため、次のように評価されます。

<link name="${5/6}"/>
    ↓
<link name="0.833333333333"/>

6. マクロ

「マクロ」とは、複数の操作をまとめて必要に応じて呼び出せるようにする機能のことです。

◎ シンプルなマクロ
はじめに、シンプルなマクロの例を紹介します。

<xacro:macro name="default_origin">
  <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:macro>

<xacro:default_origin />
    ↓
<origin rpy="0 0 0" xyz="0 0 0"/>

<xacro:マクロ名 />が<xacro:macro name="マクロ名">の内容に置き換えられます。完全に同じではありませんが（2つの属性の順序が逆）、生成されたXMLは同等です。

◎ パラメータ化したマクロ
次に、パラメータ化したマクロの例を紹介します。

  <xacro:macro name="default_inertial" params="mass">
    <inertial>
      <mass value="${mass}" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0" />
    </inertial>
  </xacro:macro>
  
  <xacro:default_inertial mass="10"/>

パラメータは定数と同じように機能し、数式で使用できます

ブロック全体をパラメータとして使用することもできます。


<xacro:macro name="blue_shape" params="name *shape">
  <link name="${name}">
    <visual>
      <geometry>
        <xacro:insert_block name="shape" />
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <xacro:insert_block name="shape" />
      </geometry>
    </collision>
  </link>
</xacro:macro>

<xacro:blue_shape name="base_link">
  <cylinder radius=".42" length=".01" />
</xacro:blue_shape>

ブロックパラメータを指定するには、パラメータ名の前に*を指定します。「insert_block」を使用してブロックを挿入します。ブロックは何度でも挿入できます。

7. 実用的な使用法

Xacroの実用的な使用法を紹介します。

◎ Legマクロ
多くの場合、異なる場所に複数の似たようなオブジェクトを作成する必要があります。場所にはある程度の対称性があります。R2D2の2つの足も、マクロを使うことで、コード量を減らしています。

<xacro:macro name="leg" params="prefix reflect">
  <link name="${prefix}_leg">
    <visual>
      <geometry>
        <box size="${leglen} 0.1 0.2"/>
      </geometry>
      <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <box size="${leglen} 0.1 0.2"/>
      </geometry>
      <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
    </collision>
    <xacro:default_inertial mass="10"/>
  </link>
  <joint name="base_to_${prefix}_leg" type="fixed">
    <parent link="base_link"/>
    <child link="${prefix}_leg"/>
    <origin xyz="0 ${reflect*(width+.02)} 0.25" />
  </joint>
  <!-- 省略 -->
</xacro:macro>

<xacro:leg prefix="right" reflect="1" />
<xacro:leg prefix="left" reflect="-1" />

・名前プレフィックスを使用して、2つの同様の名前のオブジェクトを取得。
・数式を使用して関節の原点を計算。
ロボットのサイズを変更する場合、関節オフセットを計算するためにいくつかの数式を使用してプロパティを変更すると、多くの問題を回避できます。
・reflectパラメータを使用し、脚をbase_to _ $ {prefix} _leg原点の体の両側に配置。

8. ランチャーファイルでのXacroの使い方

「ランチャーファイル」で、「Xacroファイル」を「URDF」に変換することもできます。これは、常に最新の状態に保たれ、ハードディスク容量も消費しないため便利です。ただし、生成に時間がかかる場合があります。

display.launchもXacroに対応しています。

  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />

そのため、URDFと同じ方法で、Xacroファイルを開くことができます。

$ roslaunchurdf_tutorial display.launch model:=urdf/08-macroed.urdf.xacro

9. 参考

次回

この記事が気に入ったらサポートをしてみませんか？