XacroでURDFを簡略化して記述 - アプリとサービスのすすめ

前回書いた記事のURDFファイルをXacroで書き直した。

trafalbad.hatenadiary.jp

かなり簡単に書けたので、おすすめ。

目次
1. xacroファイル
2. xacroの特徴
3. collisionタグとinertialタグの挿入
4. 表示

1. xacroファイル

car_robot.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="bodylen" value="0.6" />
  <xacro:property name="baselen" value="0.4" />

  <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>

  <xacro:macro name="default_wheel" params="name parent">
    <link name="${name}">
      <visual>
        <origin rpy="1.57075 0 0" xyz="0 0 0"/>
        <geometry>
          <cylinder length="0.1" radius="0.035"/>
        </geometry>
        <material name="black"/>
      </visual>
    </link>
    <joint name="${name}_joint" type="continuous">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
    </joint>
  </xacro:macro>

  <xacro:macro name="default_leg" params="legname parent jointname xyz">
    <link name="${legname}">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
    </link>

    <joint name="${jointname}" type="fixed">
      <parent link="${parent}"/>
      <child link="${legname}"/>
      <origin xyz="${xyz}"/>
    </joint>
  </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:default_leg legname="right_leg" parent="base_link" jointname="base_to_right_leg" xyz="0 -0.22 0.25"/>
  
  <link name="right_base">
    <visual>
      <geometry>
        <box size="0.4 0.1 0.1"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>

  <joint name="right_base_joint" type="fixed">
    <parent link="right_leg"/>
    <child link="right_base"/>
    <origin xyz="0 0 -0.6"/>
  </joint>

  <xacro:default_wheel name="right_front_wheel" parent="right_base"/>
  <xacro:default_wheel name="right_back_wheel" parent="right_base"/>


  <xacro:default_leg legname="left_leg" parent="base_link" jointname="base_to_left_leg" xyz="0 0.22 0.25"/>

  <link name="left_base">
    <visual>
      <geometry>
        <box size="0.4 0.1 0.1"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>

  <joint name="left_base_joint" type="fixed">
    <parent link="left_leg"/>
    <child link="left_base"/>
    <origin xyz="0 0 -0.6"/>
  </joint>

  <xacro:default_wheel name="left_front_wheel" parent="left_base"/>
  <xacro:default_wheel name="left_back_wheel" parent="left_base"/>
</robot>

2. xacroの特徴

書き始め

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

タグのパラメーター化

inertialタグのデフォルトパラメータ

<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"/>

linkタグのパラメータ化

<xacro:macro name="default_leg" params="legname parent jointname xyz">
    <link name="${legname}">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
    </link>

    <joint name="${jointname}" type="fixed">
      <parent link="${parent}"/>
      <child link="${legname}"/>
      <origin xyz="${xyz}"/>
    </joint>
</xacro:macro>

<!-- 簡略化してかく-->
<xacro:default_leg legname="left_leg" parent="base_link" jointname="base_to_left_leg" xyz="0 0.22 0.25"/>

定数

<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />


<!-- 簡略化してかく-->
<geometry>
    <cylinder radius="${width}" length="${bodylen}"/>
</geometry>

まだまだいっぱいあるけど簡単で便利なのはここら辺。

3. collisionタグとinertialタグの挿入

ちなみに

collisionタグ：衝突要素
inertialタグ：慣性モーメント

を入れてみた。

どちらもvisualと同列、linkの中に書く。

<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"/>