ロボットモデル作成ツール

ROSではロボットのモデルをURDFで簡単（？）に記載できますが，間違えやすいので表を使ってリンクごとのパラメータを一括して入力することで全体のURDFモデルを出力するpythonのプログラムを紹介します．

URDFでは，リンクの親子関係を接続する関節（ジョイント）とリンクの位置と大きさを入力していくことで木（ツリー）構造でつながりを表現していきます．

この親子関係のツリー構造は，「ヒューマノイドロボット」の本で推奨（？）されている二分木の構造（１つのリンクに１つのジョイントが最大）ではなく，一つのリンクに複数のジョイントがつながる方（本では非推奨？）の構造で表現することになります．

この1つのリンクに複数のジョイントを繋げた状態を表す表を作成します．その表を読み込んだモデル生成のツールでURDFファイルを生成します．

（２分木構造でURDFファイルを作れなくはなかったのですが，結局URDFでは２分木構造じゃないのでやめました．）

まず，URDFの基本のリンク記載例を確認します．

１つのリンクの記載の例は下記の通りです．

リンクの記載例

  <!-- Link 1 -->

  <link name="link1"> #リンクのパラメータ設定開始 nameでリンクの名前指定

    <collision> #collisionで接触判定を行うリンクのサイズ指定

      <origin rpy="0 0 0" xyz="0 0 0.075"/>  #サイズ指定を行うときの原点座標の位置(xyz)と姿勢(rpy)を固定角で指定

      <geometry>

        <box size="0.05 0.05 0.15"/> #原点座標の位置・姿勢を中心に直方体のサイズ(x,y,z)を指定

      </geometry>

    </collision> #接触判定の記載はここまで

    <visual> #rvizなどで表示するリンクのパラメータ：普通はcollisionと同じ

      <origin rpy="0 0 0" xyz="0 0 0.075"/>

      <geometry>

        <box size="0.05 0.05 0.15"/>

      </geometry>

      <material name="red"/> #collisionと違うのは色指定があること

    </visual> #リンクの描画はここまで

    <inertial> #リンクの質量や慣性モーメントを定義

      <origin rpy="0 0 0" xyz="0 0 0.075"/> #リンクの質量中心位置と座標の姿勢を指定

      <mass value="1"/> #リンクの質量を指定

      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> #リンク座標における慣性モーメントのパラメータ指定

    </inertial>

  </link> #リンクのパラメータ設定ここまで

次に，関節の記載例は下記の通りです．

関節の記載例

  <joint name="joint1" type="revolute"> #関節のパラメータ記載開始 nameで名前を指定，typeで回転"revolute"か固定"fix"を指定

    <parent link="link1"/> #ジョイントが繋ぐ親のリンク（接続元）の名前を指定

    <child link="link2"/>#ジョイントが繋ぐ子のリンク（接続先）の名前を指定

    <origin rpy="0 0 0" xyz="0 0 0.15"/> #ジョイントを指定する座標系の位置・姿勢を指定

    <axis xyz="0 0 1"/> #指定した座標系のどこに関節軸があるか指定

    <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/> #関節のトルク(effort)，関節角(rad指定）の下限(lower)，関節角の上限(upper)を指定

  </joint> #関節のパラメータ記載修了

上記のリンクと関節のパラメータ設定例から下記のパラメータを表で管理することとする．

◯パラメータ

• link リンク名

• child_link リンク(link)の子供のリンク名：ジョイント作成時に現在のリンクと接続するリンクの名前．複数あるときは「 , 」区切りでスペースなしで記載．(ex.link1,link2,link3)

• link_rpy リンクの姿勢をxyz固定角(roll, pitch, yaw）で指定

• link_xyz リンクの原点位置をxyz座標系で指定（ただし，rpyでの回転前の座標系で）

• link_size リンクの原点位置を中心に直方体のサイズを（x y z）で指定．（こちらはrpyで回転後の座標系）

• link_color リンクのカラー指定．（今回はテンプレートにデフォルトのカラーを記載することにする）

• link_mass リンクの質量

• link_ixx リンクの回転モーメント xx成分（以下略）

• link_ixy

• link_ixz

• link_iyy

• link_iyz

• link_izz

• joint linkとchild_linkを繋ぐ関節の名前（ただし，今回はジョイントの名前は自動的に決定することにする）

• joint_type* 関節の種類（fix：固定，revolute：回転で指定）

• joint_rpy* 関節の姿勢をxyz固定角(roll, pitch, yaw）で指定

• joint_xyz* 関節の原点位置をxyz座標系で指定（ただし，rpyでの回転前の座標系で）

• joint_axis* 関節軸の方向を指定（x軸に回転軸があるときは(1 0 0) )

• joint_effor*t 関節の最大トルク

• joint_lower* 関節が動くことのできる最小角度

• joint_upper* 関節が動くことのできる最大角度

• joint_velocity* 関節角の最大速

*には子供の数だけ複数定義する．記載方法は例を参照

３自由度の下図のマニピュレータのパラメータの表をcsvファイルで記載したものを示す．

3dofarm.csvは，このページの一番下のリンクからダウンロードしてください．

このcsvファイルを読み込んで，URDFの基本の型をテンプレートファイルとして用意したものに流し込んでいくことを考えます．

pythonでテンプレートファイルから目的とするurdfファイルを生成するために

jinja2を利用します．

jinja2のインストールはコマンドプロンプトで下記の通り入力すればOK.

------------

$ sudo pip install jinja2

------------

pipのコマンドが見つからないときは下記コマンドでインストールしておく．

------------

$ sudo apt-get install python-pip

------------

jinja2の詳しい使い方はおいていおて（自分もよくわかってないので）

いきなりurdf生成のためのテンプレートを下記に示します．

まずは，pythonでjinja2を呼び出すプログラム．

create_robot_csv.py

#!/usr/bin/env python

# -*- coding: utf-8 -*-

from jinja2 import Environment, FileSystemLoader

import sys

#CSVモジュール

import csv

import subprocess

args = sys.argv

#テンプレートファイルを指定

env = Environment(loader=FileSystemLoader('./', encoding='utf8'))

tpl = env.get_template('template3.urdf')

#CSVファイル読み込み 引数指定がないときはデフォルトでrobot_param.csv

if len(args) > 1 :

  f = open(args[1], 'rb')

else :

  f = open('robot_param2.csv', 'rb')

if len(args) > 2 :

  robot_name = args[2]

elif len(args) > 1 :

  name = args[1].rsplit(".", 1)

  robot_name = name[0]

else :

  robot_name = u"sample_robot"

dataReader = csv.reader(f)

#テンプレートへ挿入するデータの作成

#robot_name = u"six_dof_arm"

sample_list = []

count = 0

for raw in dataReader:

  if count != 0 :

    if 'base' in raw[0] : base_link=raw[0]

    children_num = 0

    if ',' in raw[1] :

      children_num = 1

      children=raw[1].split(",")

      for child in children :

        sample_list.append({'link':raw[0], 'child_link':child, 'link_rpy':raw[2], 'link_xyz':raw[3], 'link_size':raw[4], 'link_color':raw[5], 'link_mass':raw[6],'link_ixx':raw[7], 'link_ixy':raw[8], 'link_ixz':raw[9], 'link_iyy':raw[10], 'link_iyz':raw[11], 'link_izz':raw[12], 'joint_type':raw[13+8*(children_num-1)], 'joint_rpy':raw[14+8*(children_num-1)], 'joint_xyz':raw[15+8*(children_num-1)], 'joint_axis':raw[16+8*(children_num-1)], 'joint_effort':raw[17+8*(children_num-1)], 'joint_lower':raw[18+8*(children_num-1)], 'joint_upper':raw[19+8*(children_num-1)], 'joint_velocity':raw[20+8*(children_num-1)], 'children_num':children_num})

        children_num += 1

    else :

      sample_list.append({'link':raw[0], 'child_link':raw[1], 'link_rpy':raw[2], 'link_xyz':raw[3], 'link_size':raw[4], 'link_color':raw[5], 'link_mass':raw[6],'link_ixx':raw[7], 'link_ixy':raw[8], 'link_ixz':raw[9], 'link_iyy':raw[10], 'link_iyz':raw[11], 'link_izz':raw[12], 'joint_type':raw[13], 'joint_rpy':raw[14], 'joint_xyz':raw[15], 'joint_axis':raw[16], 'joint_effort':raw[17], 'joint_lower':raw[18], 'joint_upper':raw[19], 'joint_velocity':raw[20], 'children_num':children_num})

  count +=1

print "%d links data read" % (count-1)

#テンプレートへの挿入

urdf = tpl.render({'robot_name':robot_name, 'base_link':base_link, 'sample_list':sample_list})

#ファイルへの書き込み

tmpfile = open(robot_name+".urdf", 'w') #書き込みモードで開く

tmpfile.write(urdf.encode('utf-8'))

tmpfile.close()

pythonのcsvモジュールを用いてファイルを読み込んで，テンプレートファイルにデータを送っています．

テンプレートファイルは下記の通り．

template.urdf

<?xml version="1.0" ?>

<robot name="{{robot_name}}">

  <material name="red">

    <color rgba="1.0 0.0 0.0 1.0"/>

  </material>

  <material name="green">

    <color rgba="0.0 1.0 0.0 1.0"/>

  </material>

  <material name="blue">

    <color rgba="0.0 0.0 1.0 1.0"/>

  </material>

{% for sample in sample_list %}

{% if 'world' in sample.link %}

  <!-- {{sample.link}} -->

  <link name="{{sample.link}}"/>

{% elif sample.children_num < 2 %}

  <!-- {{sample.link}} -->

  <link name="{{sample.link}}">

    <collision>

      <origin rpy="{{sample.link_rpy}}" xyz="{{sample.link_xyz}}"/>

      <geometry>

        <box size="{{sample.link_size}}"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="{{sample.link_rpy}}" xyz="{{sample.link_xyz}}"/>

      <geometry>

        <box size="{{sample.link_size}}"/>

      </geometry>

      <material name="{{sample.link_color}}"/>

    </visual>

    <inertial>

      <origin rpy="{{sample.link_rpy}}" xyz="{{sample.link_xyz}}"/>

      <mass value="{{sample.link_mass}}"/>

      <inertia ixx="{{sample.link_ixx}}" ixy="{{sample.link_ixy}}" ixz="{{sample.link_ixz}}" iyy="{{sample.link_iyy}}" iyz="{{sample.link_iyz}}" izz="{{sample.link_izz}}"/>

    </inertial>

  </link>

{% endif %}

  {% if '0' not in sample.child_link  %}

  <joint name="{{sample.link}}_2_{{sample.child_link}}" type="{{sample.joint_type}}">

    <parent link="{{sample.link}}"/>

    <child link="{{sample.child_link}}"/>

    <origin rpy="{{sample.joint_rpy}}" xyz="{{sample.joint_xyz}}"/>

    {% if sample.joint_type != 'fixed' %}

    <axis xyz="{{sample.joint_axis}}"/>

    <limit effort="{{sample.joint_effort}}" lower="{{sample.joint_lower}}" upper="{{sample.joint_upper}}" velocity="{{sample.joint_velocity}}"/>

    {% endif %}

  </joint>

  {% endif %}

{% endfor %}

</robot>

リンクとジョイントのテンプレートに数値が流し込まれていく課程で

if文で条件を判断してテンプレートと実際に必要な部分との整合性を取っていきます．

jointの名前は自分でつけたいかもしれませんが，今回のテンプレートでは

親リンクの名前と子供のリンクの名前をつなげた名前を自動的に割り振っています．

ex. Link1とLink2の関節名：Link1_2_Link2

それでは，２つのファイルを使って3dofarm.csvからurdfを生成したいと思います．

下記のコマンドで，csvファイルを指定して実行します．

-------------

$ python create_robot_csv.py 3dofarm.csv

6 links data read  <-というメッセージが出るはず．

-------------

出力されるファイルはcsvファイルの名前と同じ名前で拡張子が「.urdf」になります．

下記コマンドで，問題なくurdfファイルが生成されているか確認します．

-------------

$ check_urdf 3dofarm.urdf <-このコマンドを打つと下記のように表示されるはず

robot name is: 3dofarm

---------- Successfully Parsed XML ---------------

root Link: world has 1 child(ren)

    child(1):  base_link

        child(1):  Link1

            child(1):  Link2

                child(1):  Link3

                    child(1):  Link4

-------------

では，実際にrvizで表示してみましょう．

表示するまでの課程は．．．省略．他のページを見てください．

この程度のロボットの記述を一々こんなツール使う必要がないと思いますので

複数のリンクが一つのリンクから生えている例を示します．

２足歩行ロボットの構成例（bipedrobot.csv）をHPの下のリンクに置いてあるのでダウンロードしてください．

bipedrobot.csvをダウンロード後に下記のようにコマンドを打つとbipedrobot.urdfが生成されます．

-------------

$python create_robot_csv.py bipedrobot.csv

-------------

CSVファイルで記載した２足歩行ロボットのモデルは下図のとおりです．

このモデルではbody_linkから右足と左足が２つ生えているので，一つ目のリンクから２つジョイントを記載する必要があります．

csvファイル内で一つ目のリンク（base_link）のchild_linkに右足のリンク（RLeg1）と左足のリンク（LLeg1）をカンマ「 , 」で連続的に記載し，base_linkとRLeg1，base_linkとLLeg1を繋ぐ関節のパラメータを右列にセット毎に記載していきます．

生成されたbipedrobot.urdfをrvizに表示すると下記のようになります．

片足６自由度の脚を２つ取り付けていますが右足RLegと左足LLegはcsvを見るとわかりますが

リンクの名前と連結先の名前以外はデータをコピーすれば構成できます．

今回の例では，質量や慣性モーメント，関節角度の限界などは適当に入力しているので

gazeboまでのシミュレーションは考えていませんので修正の必要があります．

（そのうちやるかも．．．）

このツールを使えば，４脚や６脚も簡単に作れます．

ただし，urdfの欠点で角度の指定を計算式による指定ができないのでxacroのテンプレートにするべきです．

xacroは次の節で紹介します．

XACROの利用（2017年8月18日追加）

xacroを利用するときは，先程用意したテンプレート(template.urdf)の最初にある"robot name"を指定する場所の後ろにxacroを利用する宣言"xmlns:xacro="http://www.ros.org/wiki/xacro"を記載するだけで利用できるようになります．

変更前：

<robot name="{{robot_name}}">

 変更後：

<robot name="{{robot_name}}" xmlns:xacro="http://www.ros.org/wiki/xacro">

template.urdfのままではおかしいのでtemplate.xacroとして下記のようなxacroのテンプレートファイルを用意します．

xacroの宣言をしておくと<property>のタグを使って変数を用意することができます．

よく使う円周率をpiという変数として<proterty>で用意した例を下記に示します．

今回の修正では，geometryのリングサイズの指定がbox限定だったのを

csvファイル内で形状を指定して記述できるように変更しました．

template.xacro

<?xml version="1.0" ?>

<robot name="{{robot_name}}" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <property name="pi" value="3.14159265359"/>

  <material name="red">

    <color rgba="1.0 0.0 0.0 1.0"/>

  </material>

  <material name="green">

    <color rgba="0.0 1.0 0.0 1.0"/>

  </material>

  <material name="blue">

    <color rgba="0.0 0.0 1.0 1.0"/>

  </material>

{% for sample in sample_list %}

{% if 'world' in sample.link %}

  <!-- {{sample.link}} -->

  <link name="{{sample.link}}"/>

{% elif sample.children_num < 2 %}

  <!-- {{sample.link}} -->

  <link name="{{sample.link}}">

    <collision>

      <origin rpy="{{sample.link_rpy}}" xyz="{{sample.link_xyz}}"/>

      <geometry>

        <{{sample.link_size}}/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="{{sample.link_rpy}}" xyz="{{sample.link_xyz}}"/>

      <geometry>

        <{{sample.link_size}}/>

      </geometry>

      <material name="{{sample.link_color}}"/>

    </visual>

    <inertial>

      <origin rpy="{{sample.link_rpy}}" xyz="{{sample.link_xyz}}"/>

      <mass value="{{sample.link_mass}}"/>

      <inertia ixx="{{sample.link_ixx}}" ixy="{{sample.link_ixy}}" ixz="{{sample.link_ixz}}" iyy="{{sample.link_iyy}}" iyz="{{sample.link_iyz}}" izz="{{sample.link_izz}}"/>

    </inertial>

  </link>

{% endif %}

  {% if '0' not in sample.child_link  %}

  <joint name="{{sample.link}}_2_{{sample.child_link}}" type="{{sample.joint_type}}">

    <parent link="{{sample.link}}"/>

    <child link="{{sample.child_link}}"/>

    <origin rpy="{{sample.joint_rpy}}" xyz="{{sample.joint_xyz}}"/>

    {% if sample.joint_type != 'fixed' %}

    <axis xyz="{{sample.joint_axis}}"/>

    <limit effort="{{sample.joint_effort}}" lower="{{sample.joint_lower}}" upper="{{sample.joint_upper}}" velocity="{{sample.joint_velocity}}"/>

    {% endif %}

  </joint>

  {% endif %}

{% endfor %}

</robot>

変更箇所は赤色の文字だけです．

create_robot_csv.pyをtemplete.xacroを読み込んで.xacroファイルを出力するためのpythonのスクリプトcreate_robot_xacro.pyへ下記のように書き換えておきます．

create_robot_xacro.py

#!/usr/bin/env python

# -*- coding: utf-8 -*-

from jinja2 import Environment, FileSystemLoader

import sys

#CSVモジュール

import csv

import subprocess

args = sys.argv

#テンプレートファイルを指定

env = Environment(loader=FileSystemLoader('./', encoding='utf8'))

tpl = env.get_template('template.xacro')

#CSVファイル読み込み 引数指定がないときはデフォルトでrobot_param.csv

if len(args) > 1 :

  f = open(args[1], 'rb')

else :

  f = open('robot_param2.csv', 'rb')

if len(args) > 2 :

  robot_name = args[2]

elif len(args) > 1 :

  name = args[1].rsplit(".", 1)

  robot_name = name[0]

else :

  robot_name = u"sample_robot"

dataReader = csv.reader(f)

#テンプレートへ挿入するデータの作成

#robot_name = u"six_dof_arm"

sample_list = []

count = 0

for raw in dataReader:

  if count != 0 :

    if 'base' in raw[0] : base_link=raw[0]

    children_num = 0

    if ',' in raw[1] :

      children_num = 1

      children=raw[1].split(",")

      for child in children :

        sample_list.append({'link':raw[0], 'child_link':child, 'link_rpy':raw[2], 'link_xyz':raw[3], 'link_size':raw[4], 'link_color':raw[5], 'link_mass':raw[6],'link_ixx':raw[7], 'link_ixy':raw[8], 'link_ixz':raw[9], 'link_iyy':raw[10], 'link_iyz':raw[11], 'link_izz':raw[12], 'joint_type':raw[13+8*(children_num-1)], 'joint_rpy':raw[14+8*(children_num-1)], 'joint_xyz':raw[15+8*(children_num-1)], 'joint_axis':raw[16+8*(children_num-1)], 'joint_effort':raw[17+8*(children_num-1)], 'joint_lower':raw[18+8*(children_num-1)], 'joint_upper':raw[19+8*(children_num-1)], 'joint_velocity':raw[20+8*(children_num-1)], 'children_num':children_num})

        children_num += 1

    else :

      sample_list.append({'link':raw[0], 'child_link':raw[1], 'link_rpy':raw[2], 'link_xyz':raw[3], 'link_size':raw[4], 'link_color':raw[5], 'link_mass':raw[6],'link_ixx':raw[7], 'link_ixy':raw[8], 'link_ixz':raw[9], 'link_iyy':raw[10], 'link_iyz':raw[11], 'link_izz':raw[12], 'joint_type':raw[13], 'joint_rpy':raw[14], 'joint_xyz':raw[15], 'joint_axis':raw[16], 'joint_effort':raw[17], 'joint_lower':raw[18], 'joint_upper':raw[19], 'joint_velocity':raw[20], 'children_num':children_num})

  count +=1

print "%d links data read" % (count-1)

#テンプレートへの挿入

urdf = tpl.render({'robot_name':robot_name, 'base_link':base_link, 'sample_list':sample_list})

#ファイルへの書き込み

tmpfile = open(robot_name+".xacro", 'w') #書き込みモードで開く

tmpfile.write(urdf.encode('utf-8'))

tmpfile.close()

上記のスクリプトファイルを使って下記のコマンドでxacroファイルを試しに出力してみます．

今回geometryの行の指定を形状も含めて記載するように修正したので，サンプルのcsvファイルを

bipedrobot_xacro.csvとして用意しました．一番下からダウンロードして確認してみてください．

-------------

$python create_robot_xacro.py bipedrobot_xacro.csv

-------------

bipedrobot_xacro.xacroというファイルが出力されているのが確認できるはずです．

xacroファイルができればそのままモデルに出力するためのlaunchファイルを記載することができます．

ここでは，一度urdfに変換してみることで，xacroにすることによって，設定パラメータを数式を記載しても計算してくれるようになることを確認してみたいと思います．

出力したxacroファイルの中のある変数に数式を書き込んでurdfに変換したときに計算してくれていることを確認する例を下記に示します．

先程出力したbipedrobot_xacro.xacroのbase_linkのピッチ角をpi/2だけ回転させることを考えてみます．

下記のように修正．

bipedrobot_xacro.xacro（一部抜粋）

<?xml version="1.0" ?>

<robot name="bipedrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <property name="pi" value="3.14159265359"/>

  <material name="red">

    <color rgba="1.0 0.0 0.0 1.0"/>

  </material>

  <material name="green">

    <color rgba="0.0 1.0 0.0 1.0"/>

  </material>

  <material name="blue">

    <color rgba="0.0 0.0 1.0 1.0"/>

  </material>

  <!-- base_link -->

  <link name="base_link">

    <collision>

      <origin rpy="0 ${pi/2} 0" xyz="0 0 0.0"/>

      <geometry>

        <box size="0.2 0.5 1.0"/>

      </geometry>

    </collision>

上記は最初の数行を抜粋しただけなのですが，修正したところは赤い文字のところだけです．

ピッチ角をpi/2だけ変更するために計算式を記載しています．

urdfでは計算式を書くとエラーになってしまいますが，

xacroでは${数式}と書くと{}で囲んだ中の数式を計算してくれます．

urdfファイルに出力して該当部分が計算されているか確認します．

-------------

$rosrun xacro xacro bipedrobot_xacro.xacro > bipedrobot_xacro.urdf

$check_urdf bipedrobot_xacro.urdf

-------------

２行目のurdfのチェックは別に必要ありませんが，一応urdfファイルとして問題ないということを確認する例です．

下記に該当箇所が分かる部分を抜粋した例を示します．

bipedrobot_xacro.urdf（一部抜粋）

<?xml version="1.0" ?>

<!-- =================================================================================== -->

<!-- |    This document was autogenerated by xacro from bipedrobot.xacro               | -->

<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->

<!-- =================================================================================== -->

<robot name="bipedrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <material name="red">

    <color rgba="1.0 0.0 0.0 1.0"/>

  </material>

  <material name="green">

    <color rgba="0.0 1.0 0.0 1.0"/>

  </material>

  <material name="blue">

    <color rgba="0.0 0.0 1.0 1.0"/>

  </material>

  <!-- base_link -->

  <link name="base_link">

    <collision>

      <origin rpy="0 1.5707963268 0" xyz="0 0 0.0"/>

      <geometry>

        <box size="0.2 0.5 1.0"/>

      </geometry>

    </collision>

赤色で記載された部分が計算された結果になっています．

今回は例なので，rvizを表示しても変更した箇所がcollisionタグのところだけなので見た目ではわかりません．

<visual>タグなどの姿勢も全て合わせて出力するのは面倒なので，csvファイルを変更して，数式を記載したものがモデルに反映されていることを示します．

今回の例では，本研究室の６脚ロボットASTERISKのモデルで確認してみます．

xacroで使える関数はJade以降であればsin(), cos()などの関数も利用可能になりました．

また，上記の例ではpiを自分で定義しましたが，Jade以降は標準で用意されています．

Indigo用のファイルをasterisk.csv，Jade以降のファイルをasterisk_jade.csvとして用意しました．確かめたい人は，ページの一番下からダウンロードしてください．

ASTERISKは，各足に３つの関節があるモデルですが，IKを求めるときは６自由度があると楽なので，足の先端に仮想的に３軸直交の関節を用意しています．

１本の脚のモデルができれば，他の５本は表にコピーでリンクの名前を変えれば作れます．

Jade以降の環境であれば，60度毎に脚を配置することが簡単にできます．

-------------

$python create_robot_xacro.py asterisk.csv

$rosrun xacro xacro asterisk.xacro > asterisk.urdf

$check_urdf asterisk.urdf

-------------

xacroのファイルから直接launchファイルで呼び出すときの記載例は下記の通り．

asterisk.launch

<launch>

  <arg name="gui" default="True" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(find asterisk)/urdf/asterisk.xacro" />

  <param name="use_gui" value="$(arg gui)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <node name="rviz" pkg="rviz" type="rviz"/>

</launch>

asteriskというパッケージの下にurdfフォルダを作って，その中にasterisk.xacroファイルを保存している例なので，このまま動くと思わないでください．

出力結果のurdfをrvizで表示した様子を下記に示します．

上記は3自由度バージョンのASTERISKでしたが，最近開発した6自由度のASTERISKのモデルも下記に示します．

このロボットのモデルのcsvファイルは，ページ最後のファイルのasterisk_sixdof_r.csvです．

*注意事項

表中のリンク名に数字のゼロ「０」を含んだ名前にすると，ジョイントを生成してくれません．

ゼロ「０」だけ記載の時にスキップするようにしたかったのですが，すぐに対応できていません．

リンク名には「０」を含まないように記載をしてください．(2017/08/21)

Gazeboへの対応（2017年８月２３日追加）

Gazeboへ対応するためにテンプレートとかパラメータに関する微調整をしました．

パラメータ表でうまいことモデルを作ってrvizで表示もできてmoveitもうまく行っているのに，なぜかGazeboで表示できずに実機でやらざるを得なかったら意味が無いので若干修正しました．

モデルをきれいに表で記載したいために，質量ゼロの仮想リンクを繋げてモデルを作成すると，その後ろにあるリンク数がつながるとGazeboで表示すると突然崩壊します！

リンクのフレームとfixedの固定関節だけなら，いままでロボットアーム土台で１つだけ使っていたときは大丈夫だったのですが，多脚ロボットの足すべての土台にそのようなリンクやジョイントを用意するとだめなようです．

この現象はUbuntu14.04とUbuntu16.04のROSに標準で入れられるGazeboの両方で同じ現象でした．

まえがきは以上で，早速修正したファイルを紹介すると下記のとおりです．

・create_robot_gazebo_xacro.py  

下記テンプレートを使ってxacroとurdfの２つのファイルを出力するプログラム．

注意事項として，自分で「rosrun xacro xacro ファイル名」で実行したときは記述ミスしたときのエラーがわかりますが，このコマンドではエラーが表示されません．urdfファイルの容量がゼロのときはxacroの記述が間違っていて出力できていません．

・template_gazebo.xacro

Gazeboのタグを追加してGazeboへspawnできるようにした．関節毎にtransmissionとmotorを追加してあるが，ギア比は「１」としています．

・asterisk_sixdof_r2.csv

Gazeboでも表示できるように修正したcsvファイル．asterisk_sixdof.csvとほぼ同じ構成で出力されますが，記載をわかりやすくするために，最初のロボットの胴体（base_link）に60度毎に回転した固定関節を用意してから各足を配置する記載にしています．各足のパラメータはすべて同じ構成なのでパラメータをコピーすれば簡単に記載可能です．

このファイルを使ってgazeboに表示させたいと思います．

上記３つのファイルをダウンロードして下記のコマンドを実行してxacroファイルを生成します．

--------------

$python create_robot_gazebo_xacro.py asterisk_sixdof_r2.csv

--------------

csvファイルと同じ名前のxacroファイル(asterisk_sixdof_r2.xacro)とurdfファイル(asterisk_sixdof_r2.urdf)が生成されていることがわかると思います．

表示するために下記のようにrosのノードを作ってみます．基本はgazeboでロボットアームを作ったときと同じです．

$ cd ~/catkin_ws/src

$ catkin_create_pkg asterisk urdf

生成したパッケージにlaunchフォルダとファイルを作成する．

$ cd ~/catkin_ws/src/asterisk

$ mkdir launch

$ mkdir urdf

$ cp ~/create_robot/asterisk_sixdof_r2.xacro ./urdf  （~/create_robot/というディレクトリにxacroファイルを作成していると仮定）

$ cd launch

$ gedit asterisk_gazebo.launch

asterisk_gazebo.launchは下記のように記載．

asterisk_gazebo.launch

<launch>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">

    <arg name="world_name" value="$(find asterisk_t)/worlds/asterisk.world"/>

  </include>

  <arg name="gui" default="True" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(find asterisk)/urdf/asterisk.xacro" />

  <param name="use_gui" value="$(arg gui)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- param name="robot_description" type="str" command="$(find xacro)/xacro.py $(find asterisk)/urdf/asterisk.xacro" / -->

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model asterisk" />

</launch>

gazeboの環境を作るためにlaunchファイルで指定している場所に下記のようにディレクトリとファイルを用意．

$ cd ~/catkin_ws/src/asterisk

$ mkdir worlds

$ cd worlds

$ gedit asterisk.world

asterisk.world

<launch>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">

    <arg name="world_name" value="$(find asterisk_t)/worlds/asterisk_t.world"/>

  </include>

  <arg name="gui" default="True" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(find asterisk_t)/urdf/asterisk.xacro" />

  <param name="use_gui" value="$(arg gui)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- param name="robot_description" type="str" command="$(find xacro)/xacro.py $(find asterisk_t)/urdf/asterisk.xacro" / -->

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model asterisk_t" />

</launch>

ここまで準備できれば後は下記のコマンドを実行するとgazeboの画面が立ち上がります．

 ---------

$ roslaunch asterisk asterisk_gazebo.launch

---------

上記のように表示されますが，関節に割り当てたアクチュエータへトルクの指示を行っていないので放置すると徐々に崩れていきます．

Gazeboのcontrollerへの対応（2017年８月２３日追加）

ということで，コントローラを自動でつける事ができたのでコントローラを立ち上げればOKということだったのですが．．．

URDFの作法を忘れており，上記までに作成したrootのリンク（一番最初のリンク）に質量を与えているとcontrollerがうまく立ち上がらないことがわかりました．

ということで，また仕様を変更します．

根本(root)のリンクだけを質量ゼロにすればいいので１行増やすか，現在の根本のリンクの質量をゼロにして，次のリンクから質量を与えればOKです．

今後の予定（は未定）

moveitで各脚の制御を追加してトライポッド歩容まで導入予定．

２足歩行のモデルでもやりたい．