xacroを使う

諸事情により環境がUbuntu9.10になりました。

ROS1.0は普通に動いていますね。

今回はシミュレータのmakeがまだ終わらないので寄り道です。

xacroはXML macroの略です。

XMLをいちいち手書きするのは大変なのでマクロを使って楽しようという

画期的なソフトです。

urdf形式でロボットモデルを作るときに、

右手と左手をいちいち定義するよりも、腕マクロを作り、そこに引数として「右」、「左」を入れたほうが楽ですよね。

そんな感じで使っているみたいです。

まさにその例が以下です。

<xacro:macro name="pr2_arm" params="suffix parent reflect">
  <pr2_upperarm suffix="${suffix}" reflect="${reflect}" parent="${parent}" />
  <pr2_forearm suffix="${suffix}" reflect="${reflect}" parent="elbow_flex_${suffix}" />
</xacro:macro>

<xacro:pr2_arm suffix="left" reflect="1" parent="torso" />
<xacro:pr2_arm suffix="right" reflect="-1" parent="torso" />

これが↓のように展開されます。

<pr2_upperarm suffix="left" reflect="1" parent="torso" />
<pr2_forearm suffix="left" reflect="1" parent="elbow_flex_left" />
<pr2_upperarm suffix="right" reflect="-1" parent="torso" />
<pr2_forearm suffix="right" reflect="-1" parent="elbow_flex_right" />

この例ではむしろ書くコード量が増えていますが、不要なコピペはしなくて済みますね。

パラメータ

<xacro:parameter name="the_radius" value="2.1" />
<xacro:parameter name="the_length" value="4.5" />

<geometry type="cylinder" radius="${the_radius}" length="${the_length}" />

上記のようにしてparameterをセットすることができます。

要するに

the_radius = 2.1

the_length = 4.5

と代入し、${the_radius}, ${the_length}で参照することができます。

このパラメータ節はXML中の好きな場所に置くことができます。

もうひとつのパラメータがパラメータブロックで、以下のようにして使います。

<xacro:parameter name="front_left_origin">
  <origin xyz="0.3 0 0" rpy="0 0 0" />
</xacro:parameter>

<pr2_wheel name="front_left_wheel">
  <xacro:insert_block name="front_left_origin" />
</pr2_wheel>

パラメータブロックは<xacro:insert_block>を使って挿入します。

計算
先ほど書いた${}の中では算術演算することができます。
簡単な計算（四則演算？）が使えるみたいです。

<xacro:parameter name="pi" value="3.1415926535897931" />

<circle circumference="${2.5 * pi}" />

Rospackコマンド
${}の中ではrospackコマンドと同じことができます。
ここにあるオプションが使えます。

<foo value="$(find xacro)" />

マクロ

xacroのメイン昨日です。パラメータとマクロのタグを使ってマクロを定義します。
パラメータはスペースなどで区切ります。

以下に例を示します。

<xacro:macro name="pr2_caster" params="suffix *origin">
  <joint name="caster_${suffix}_joint">
    <axis xyz="0 0 1" />
  </joint>
  <link name="caster_${suffix}">
    <xacro:insert_block name="origin" />
  </link>
</xacro:macro>

<xacro:pr2_caster suffix="front_left">
  <pose xyz="0 1 0" rpy="0 0 0" />
</xacro:pr2_caster>

suffixとoriginという２つのパラメータを受け取っています。

Cでいうと

#define PR2_CASTER(suffix, origin) ...

という感じでしょうか。

ただし、originのほうにはスターがついていて、*originになっています。

これはoriginが単なる変数ではなくパラメータブロックであることを示します。

先ほどfront_left_originと言う名前ででてきました。


pr2_casterを実際に使っているところを見ると、suffixについてはfront_leftと直接指定しています。一方で二つめの引数であるはずのoriginは定義していません。
originは<xacro:pr2_caster>の中に含まれるすべてがoriginとしてマクロに渡されます。
今回は<pose .. >の部分になりますね。


なので展開結果は以下になります。

<joint name="caster_front_left_joint">
  <axis xyz="0 0 1" />
</joint>
<link name="caster_front_left">
  <pose xyz="0 1 0" rpy="0 0 0" />
</link>

また、マクロは入れ子にすることができて、外側から順に展開されます。
以下のように定義すると、

<a>
  <xacro:macro name="foo" params="x">
    <in_foo the_x="${x}" />
  </xacro:macro>

  <xacro:macro name="bar" params="y">
    <in_bar>
      <xacro:foo x="${y}" />
    </in_bar>
  </xacro:macro>

  <xacro:bar y="12" />
</a>

以下のように展開されます。

<a>
  <in_bar>
    <in_foo the_x="12.0"/>
  </in_bar>
</a>

また、xacroコマンドは以下のようにして使います。

$ rosrun xacro xacro.py hoge.xacro
とすると展開されたものがプリントされます。
-oをつけるとファイルに出力できます。


また、実際に使うときは、xmlns:xacroを指定しないといけません。
では前回作ったurdfをxacroを使って書き直して見ます。



<robot xmlns:xacro="http://ros.org/wiki/xacro"
       name="simple_box">
  <xacro:macro name="my_joint" params="name parent child z">
    <joint name="${name}" type="revolute" >
      <limit lower="-1" upper="1" effort="1" velocity="1" />
      <axis xyz="0 1 0"/>
      <parent link="${parent}" />
      <child link="${child}" />
      <origin xyz="0 0 ${z}" rpy="0 0 0" />
    </joint>
  </xacro:macro>

  <xacro:macro name="my_link" params="name mass color z *box">
    <link name="${name}">
      <inertial>
        <mass value="${mass}" />
        <!-- center of mass (com) is defined w.r.t. link local coordinate system  -->
        <origin xyz="0 0 ${z}" />

        <inertia  ixx="${mass}" ixy="0.0"  ixz="0.0"  iyy="${mass}"  iyz="0.0"  izz="${mass}" />
      </inertial>
      <visual>
        <!-- visual origin is defined w.r.t. link local coordinate system -->
        <origin xyz="0 0 ${z}" rpy="0 0 0" />
        <geometry name="${name}_visual_geom">
          <xacro:insert_block name="box" />
        </geometry>
      </visual>
      <collision>
        <!-- collision origin is defined w.r.t. link local coordinate system  -->
        <origin xyz="0 0 ${z}" rpy="0 0 0 " />
        <geometry name="${name}_collision_geom">
          <xacro:insert_block name="box" />
        </geometry>
      </collision>
    </link>
    <gazebo reference="${name}">
      <material>Gazebo/${color}</material>
      <turnGravityOff>false</turnGravityOff>
    </gazebo>
  </xacro:macro>

  <xacro:my_joint name="my_box_joint1" parent="my_box" child="my_box2" z="0.06"/>
  <xacro:my_joint name="my_box_joint2" parent="my_box2" child="my_box3" z="0.12"/>
  <xacro:my_link name="my_box" mass="3.0" z="0.0" color="Blue">
    <box size="0.1 0.10 0.10" />
  </xacro:my_link>
  <xacro:my_link name="my_box2" mass="1.0" z="0.05" color="Red">
    <box size="0.05 0.05 0.10" />
  </xacro:my_link>
  <xacro:my_link name="my_box3" mass="1.0" z="0.05" color="Green">
    <box size="0.05 0.05 0.10" />
  </xacro:my_link>
</robot>


となります。

$ rosrun xacro xacro.py object.urdf.xacro > test.urdf
などとしてurdfを出力できます。


$ wc object.urdf.xacro
  54  185 1952 object.urdf.xacro
$ wc object.urdf
 103  355 3281 object.urdf

となり、行数的には半分になりました。

編集もしやすいですね。

これは画期的！と思ったんですがどうでしょうか？