View on GitHub

introduction-to-ros

ROS (Robot Operating System) 入門

ロボットモデルの作成:URDF

目次

前のページ

概要

本記事ではロボットモデルの作成方法について説明します.

ロボットモデルはURDF (Unified Robotics Description Format) という形式で記述されます.

まず,URDFの考え方について記述します. 次に,コードの書式を実際にモデルを作成しながら確認します.

URDFの考え方

図1

図2

ROS wikiより引用

URDFによるモデルの記述は,リンクジョイントに分けて考えます.

URDFでモデル作成


注意:本記事では,混乱を避けるためこれから作成したいモデルに必要最小限なもののみ紹介します. それ以外にも詳しく知りたい方は以下をご覧ください.

link elements

joint elements


本講座では,最終的に以下のルンバのようなモデルを作成することを目指します.このモデルを作成するには,円柱と球のリンクを作成し,ジョイントで適切に接続することが必要です.また,接続には,車体とキャスター間のような固定のものと,車体と車輪の間のような回転するものがあります.

そこで,本記事では

の作成について練習します.

1. 円柱と球リンクの作成および固定ジョイント

以下に示す銀魂のジャスタウェイ的なものの作成を通して円柱,球リンクおよび固定ジョイントの作成方法を学びます.ただし,手は省略します.

ジャスタウェイ

作成するリンク

作成するジョイント

URDF作成準備

以下をインストール

sudo apt update
sudo apt install -y liburdfdom-tools ros-noetic-urdf-tutorial ros-noetic-joint-state-publisher-gui

パッケージ作成

cd ~/catkin_ws/src/
catkin_create_pkg my_urdf_tutorial std_msgs rospy roscpp

URDFファイル作成

my_urdf_tutorial/urdfディレクトリ内に以下を作成

justaway.urdf

<robot name="justaway">

  <link name="base_link"/>

  <link name="body_link">
    <visual>
      <geometry>
        <cylinder radius="0.2" length="0.6"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
  </link>

  <link name="head_link">
    <visual>
      <geometry>
        <sphere radius="0.15"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
  </link>
  
  <joint name="body_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="body_link"/>
    <origin xyz="0 0 0.30" rpy="0 0 0" />
  </joint>

  <joint name="head_joint" type="fixed">
    <parent link="body_link"/>
    <child  link="head_link"/>
    <origin xyz="0 0 0.30" rpy="0 0 0" />
  </joint>

</robot>

説明

roslaunchと同様にXML表記です.

<robot>タグ

urdfに必須で,ロボットの名前を記述します

<link>タグ

head_linkもbody_linkと同様の表記なので,上記を見て解読してみましょう.

<joint>タグ

  <joint name="body_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="body_link"/>
    <origin xyz="0 0 0.30" rpy="0 0 0" />
  </joint>

  <joint name="head_joint" type="fixed">
    <parent link="body_link"/>
    <child  link="head_link"/>
    <origin xyz="0 0 0.30" rpy="0 0 0" />
  </joint>

今回は,base_linkから真上に30cmのところにbody_linkの重心. body_linkの重心から真上に30cmのところにhead_linkの重心があるため,上記のようなoriginになります.

確認

文法チェック

cd ~/catkin_ws/src/my_urdf_tutorial/urdf
check_urdf justaway.urdf

urdfの存在するディレクトリでcheck_urdf urdfファイルとすると文法チェックできます. うまくいくと以下のように表示されます.

robot name is: justaway
---------- Successfully Parsed XML ---------------
root Link: base_link has 1 child(ren)
    child(1):  body_link
        child(1):  head_link

ロボットモデル可視化

roslaunch urdf_tutorial display.launch model:=justaway.urdf

先程aptで入れたurdf_tutorialパッケージのlaunchを用いることでurdfモデルの可視化ができます.model:=urdfファイルとして引数を与えます.

成功すると以下のように表示されます.

2. 回転ジョイントの作成

ジャスタウェイの頭を回転させてもよくわからないので,以下のように直方体に円柱を回転ジョイントで接続した手持ち扇風機的なものを作成します.

モノタロウ

作成するリンク

作成するジョイント

コード

handyfan.urdf

<robot name="handyfan">

  <link name="base_link"/>

  <link name="body_link">
    <visual>
      <geometry>
        <box size="0.04 0.02 0.20"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
  </link>

  <link name="fan_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.02"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
  </link>
  
  <joint name="body_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="body_link"/>
    <origin xyz="0 0 0.10" rpy="0 0 0" />
  </joint>

  <joint name="fan_joint" type="continuous">
    <parent link="body_link"/>
    <child  link="fan_link"/>
    <origin xyz="0.0 0.02 0.06" rpy="1.570796326794897 0 0" />
    <axis xyz="0 0 1" />
  </joint>

</robot>

回転ジョイント以外一緒なので,回転ジョイントについてのみ記述します.

  <joint name="fan_joint" type="continuous">
    <parent link="body_link"/>
    <child  link="fan_link"/>
    <origin xyz="0.0 0.02 0.06" rpy="1.570796326794897 0 0" />
    <axis xyz="0 0 1" />
  </joint>

ロボットモデル可視化

roslaunch urdf_tutorial display.launch model:=handyfan.urdf

付属のGUIで色々角度を変えることができます.

次のページで,本記事で練習したことを活かしてルンバのモデリングを演習形式で行います!

リンク

次のページ

目次


余談

base_linkについて

今回はロボット座標系の原点としてbase_linkを置くとしましたが,本来はbase_linkとはbaseとなるlinkの座標系であるべきです.

以下のサイトの図解がわかりやすいですが,より厳密にやる場合,ロボットのbaseとなるlinkの座標系をbase_linkとし,base_link座標系を地面に投射した座標系(今回で言うbase_link)をbase_footprintという座標系にします.

ROSの座標変換TFについて

ROSのnavigation stackというロボットの自律移動に関するパッケージの集合が上記の座標系設定を採用しているためこの座標系はよく用いられており,私も普段はこちらを用いています.ただ,自律移動ロボットの場合,base_footprintは大体base_linkのz座標をずらしただけの位置に存在することが多いため,本記事のようにbase_footprint=base_linkとする記法もよく見ます.

base_link vs base_footprint