ROSの勉強 第23弾：チェスセットのモデル作成

#プログラミング ROS< チェスセットのモデル作成 >

はじめに

1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第23弾として，「チェスセットのモデル作成」を扱う．

環境

仮想環境

ソフト	VMware Workstation 15
実装RAM	2 GB
OS	Ubuntu 64 ビット
isoファイル	ubuntu-mate-20.04.1-desktop-amd64.iso

コンピュータ

デバイス	MSI
プロセッサ	Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz
実装RAM	8.00 GB (7.89 GB 使用可能)
OS	Windows (Windows 10 Home, バージョン：20H2)

ROS

Distribution	noetic
プログラミング言語	Python 3.8.5
シミュレーション	gazebo

モデリング

ロボットシミュレーションにおける努力の多くが，その興味の対象である環境のモデリングに費やされるらしい．これは，一度作ってしまえば，ボタン1つで環境をリセットできたりと非常に有用なものになるため，ここで多くの時間を割くのも無駄ではないということとなるようだ．

Gazeboは，モデルをいくつかのXML形式で表現できる．新しいモデルを作るときの推奨形式はシミュレーション記述フォーマット(Simulation Description Format: SDF)とある．

SDFファイル

チェスセットには，駒とボードが必要となる．ここでは，モデル作成の術を学ぶことを目的としているため，簡易的に駒はすべて同一のブロックとしてモデル化する．こうすることで，駒に対する定義ファイルは1つで済む．以下に駒とボードのsdfファイルを示す．

ボード

chess_piece.sdf

<?xml version='1.0'?>
<sdf version = '1.4'>
    <model name = 'piece'>
        <link name = 'link'>
            <inertial>
                <mass>0.001</mass>
                <inertia>
                    <ixx>0.0000001667</ixx>
                    <ixy>0</ixy>
                    <ixz>0</ixz>
                    <iyy>0.0000000667</iyy>
                    <iyz>0</iyz>
                    <izz>0.0000001667</izz>
                </inertia>
            </inertial>
            <collision name = 'collision'>
                <geometry>
                    <box><size>0.02 0.02 0.04</size></box>
                </geometry>
                <surface>
                    <friction>
                        <ode>
                            <mu>0.4</mu>
                            <mu2>0.4</mu2>
                        </ode>
                    </friction>
                    <contact>
                        <ode>
                            <max_vel>0.1</max_vel>
                            <min_depth>0.0001</min_depth>
                        </ode>
                    </contact>
                </surface>
            </collision>
            <visual name = 'visual'>
                <geometry>
                    <box><size>0.02 0.02 0.04</size></box>
                </geometry>
            </visual>
        </link>
    </model>
</sdf>

チェスボード

chessboard.sdf

<?xml version='1.0'?>
<sdf version = '1.4'>
    <model name = 'box'>
        <static>true</static>
        <link name = 'link'>
            <collision name = 'collision'>
                <geometry>
                    <box><size>0.5 0.5 0.02</size></box>
                </geometry>
                <surface>
                    <friction>
                        <ode>
                            <mu>0.1</mu>
                            <mu2>0.1</mu2>
                        </ode>
                    </friction>
                    <contact>
                        <ode>
                            <max_vel>0.1</max_vel>
                            <min_depth>0.001</min_depth>
                        </ode>
                    </contact>
                </surface>
            </collision>
            <visual name = 'visual'>
                <geometry>
                    <box><size>0.5 0.5 0.02</size></box>
                </geometry>
            </visual>
        </link>
    </model>
</sdf>

タグの説明

＜tag＞と＜/tag＞で挟まれた内容をそのtagに収める定義として記述される．

<?xml version='1.0'?>
<sdf version = '1.4'>

この2つは始めるにあたってのおまじないと考えることにする．
tagの名前から大まかに推測はできる．modelとlinkについての定義をする際に名前を指定する．

    <model name = 'piece'>
        <link name = 'link'>

そのあと，inertiaとあるが，これは各方向に対する慣性モーメントの値だが，質量とその物体の形状および寸法から求められるので，あらかじめそれらのパラメータ設定を行い計算しておく必要がある．その際に使えるプログラムを過去に学んだので，後述する．それを基に計算すると，以下のような結果が得られた．

形状はbox（六面体）で幅・奥行・高さに関する慣性が出力される．なお，慣性行列において，対角行列が基本的に得られるため，多くの場合，ixx, iyy, izz以外は0になる．また，ixyとiyxは同じとみなせるので，定義のときにも9個ある要素の内いくつかが省略されていることが分かる．それにより，Iw, Id, Ihはそれぞれixx, izz, iyyに対応させる．
collisionはそのままで衝突（当たり判定の定義）である．geometryにより衝突する範囲を形状と大きさで指定し，surface（表面）については，friction（摩擦）をodeの中でmu（摩擦係数）を指定する．また，contact（他との接触）の中で，max_vel（最大速度）とmin_depth（最小接触深度）を定義する．物理的な定義はこんな感じで，次にvisual（見た目）についての定義を説明する．visualタグの中で，geometryにより，見た目の形状と大きさを定義する．

簡単にタグを説明すると，全てを説明できているわけではないが，大まかには以上のようになる．

慣性モーメント計算プログラム

ソースコード

calculate_inertia.py

#!/usr/bin/env python3
import math


class InertialCalculator(object):

    def __init__(self):
        print("InertialCalculator Initialised...")

    def start_ask_loop(self):

        selection = "START"

        while selection != "Q":
            print("#############################")
            print("Select Geometry to Calculate:")
            print("[1]Box width(w)*depth(d)*height(h)")
            print("[2]Sphere radius(r)")
            print("[3]Cylinder radius(r)*height(h)")
            print("[Q]END program")
            selection = input(">>")
            self.select_action(selection)

        print("InertialCaluclator Quit...Thank you")

    def select_action(self, selection):
        if selection == "1":
            mass = float(input("mass>>"))
            width = float(input("width>>"))
            depth = float(input("depth>>"))
            height = float(input("height>>"))
            self.calculate_box_inertia(m=mass, w=width, d=depth, h=height)
        elif selection == "2":
            mass = float(input("mass>>"))
            radius = float(input("radius>>"))
            self.calculate_sphere_inertia(m=mass, r=radius)
        elif selection == "3":
            mass = float(input("mass>>"))
            radius = float(input("radius>>"))
            height = float(input("height>>"))
            self.calculate_cylinder_inertia(m=mass, r=radius, h=height)
        elif selection == "Q":
            print("Selected Quit")
        else:
            print("Usage: Select one of the give options")


    def calculate_box_inertia(self, m, w, d, h):
        Iw = (m/12.0)*(pow(d,2)+pow(h,2))
        Id = (m / 12.0) * (pow(w, 2) + pow(h, 2))
        Ih = (m / 12.0) * (pow(w, 2) + pow(d, 2))
        print("BOX w*d*h, Iw = "+str(Iw)+",Id = "+str(Id)+",Ih = "+str(Ih))

    def calculate_sphere_inertia(self, m, r):
        I = (2*m*pow(r,2))/5.0
        print("SPHERE Ix,y,z = "+str(I))

    def calculate_cylinder_inertia(self, m, r, h):
        Ix = (m/12.0)*(3*pow(r,2)+pow(h,2))
        Iy = Ix
        Iz = (m*pow(r,2))/2.0
        print("Cylinder Ix,y = "+str(Ix)+",Iz = "+str(Iz))

if __name__ == "__main__":
    inertial_object = InertialCalculator()
    inertial_object.start_ask_loop()

式の原理については，以下のものを参考にするといいかもしれない．
https://physics-school.com/moment-of-inertia/
※参考文献ではなく，原理について分かりやすいものを貼り付けただけである．

環境に出現

ここまでの作業でモデルの定義まではできたが，まだGazeboなどの環境に呼び出すことはしていない．そのためには，プログラムを構築する必要がある．以下では，環境にモデルを出現させるプログラムについて，ソースコードとそのときの環境における様子を示す．

ソースコード

spawn_chessboard.py

#! /usr/bin/env python3

import sys, rospy, tf
from gazebo_msgs.srv import *
from geometry_msgs.msg import *
from copy import deepcopy

if __name__ == '__main__':
    rospy.init_node("spawn_chessboard")     #ノード初期化
    rospy.wait_for_service("gazebo/delete_model")       #delete_modelのサービスを待つ
    rospy.wait_for_service("gazebo/spawn_sdf_model")    #spawn_sdf_modelのサービスを待つ
    delete_model = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)   #delete_modelを使えるようにする
    delete_model("chessboard")  #"chessboardという名前のオブジェクトを削除"
    s = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel)    #spawn_sdf_modelを使えるようにする
    orient = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) #spawn_sdf_modelを使えるようにする
    board_pose = Pose(Point(0.25, 1.39, 0.90), orient)  #ボードの状態を座標と姿勢で設定
    unit = 0.05 #ユニット；後の駒の配置に使う
    with open("/home/yuya/catkin_ws/src/simple_model/src/chessboard.sdf", "r") as f:    #読み取り指定でchessboard.sdfをfとして開く
        board_xml = f.read()    #board_xmlに読み取った情報を格納
    with open("/home/yuya/catkin_ws/src/simple_model/src/chess_piece.sdf", "r") as f:   #読み取り指定でchess_piece.sdfをfとして開く
        piece_xml = f.read()    #piece_xmlに読み取った情報を格納

    print(s("chessboard", board_xml, "", board_pose, "world"))  #ボードの出現

    #順番にチェスの駒を配置するようにforを2回使用
    for row in [0, 1, 6, 7]:    #行番号：相手と自分2行ずつ
        for col in range(0, 8): #列番号：0~7
            piece_name = "piece_%d_%d" % (row, col) #駒の名前をpiece_行_列というように指定する
            delete_model(piece_name)    #もうすでにその駒がある場合，削除する
            pose = deepcopy(board_pose) #boardの状態を取得
            pose.position.x = board_pose.position.x - 3.5 * unit + col * unit   #x座標をボードの情報と列情報を使って指定
            pose.position.y = board_pose.position.y - 3.5 * unit + row * unit   #y座標をボードの情報と行情報を使って指定
            pose.position.z += 0.02     #z座標は少しボードよりも上にして埋まらないようにしている
            s(piece_name, piece_xml, "", pose, "world") #駒の出現

どうやら，openのところで，相対パスではエラーが出てしまい，絶対パスで指定している．原因まではまだ追究できていない．

様子

少し見づらいが，ソースコードの説明にもあるように，ボードが出現した後，駒が順に並べられていく様子が確認できる．

感想

今回は，本来Robonaut2というNASAがオープンに提供しているロボットを通して，多関節リンク機構をもつロボットの制御について学習するはずだったが，そのRobonaut2の環境構築を幾度やってもできず，エラーを追っていくと，その構築していく中でファイルを一部いじらなければならないようで，途中でどうしようもなくなり，とりあえずとどまり行くわけにはいかず，この多関節リンク機構をもつロボットの制御については，またの機会にすることにした．その環境構築の様子と，その時のエラー，そして解決できたものについては，その解決した手順についてをおまけとして後述する．
したがって，今回はRobonaut2を使わない部分だけ学習を進めることにした．それは，チェスセットのモデル作成であった．sdfファイルについて学び，Gazebo上でそのモデルを呼び出す術についてまで学習することができた．

参考文献

プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　 　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行

おまけ

Robonaut2の構築手順

私が試した構築手順を以下に示す．

必要なパッケージのインストール

yuya@ubuntu:~$ sudo apt-get install ros-noetic-ros-control ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller ros-noetic-moveit* ros-noetic-octomap* ros-noetic-object-recognition-*

Robonaut2の構築

yuya@ubuntu:~$ mkdir -p ~/chessbot/src
yuya@ubuntu:~$ cd ~/chessbot/src/

yuya@ubuntu:~/chessbot/src$ git clone https://bitbucket.org/nasa_ros_pkg/deprecated_nasa_r2_simulator.git
yuya@ubuntu:~/chessbot/src$ git clone https://bitbucket.org/nasa_ros_pkg/deprecated_nasa_r2_common.git

yuya@ubuntu:~/chessbot/src$ cd ..
yuya@ubuntu:~/chessbot$ catkin_make

yuya@ubuntu:~/chessbot$ source devel/setup.bash

エラーと解法

ERROR1: path問題

エラーメッセージ

ERROR: cannot launch node of type [gazebo/gazebo]: gazebo
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/yuya/chessbot/src
ROS path [2]=/home/yuya/catkin_ws/src
ROS path [3]=/opt/ros/noetic/share
ERROR: cannot launch node of type [gui/gui]: gui
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/yuya/chessbot/src
ROS path [2]=/home/yuya/catkin_ws/src
ROS path [3]=/opt/ros/noetic/share

解決

launchファイルの書きかえ
pkg="gazebo"→pkg="gazebo_ros"
参考 (https://answers.ros.org/question/130656/roslaunch-cannot-launch-node-of-type-gazebo/ )
guiに関しては，pkg="gazebo_ros"にするだけでなく以下の操作も行う．
type="gui"→type="gzclient"
どうやらバージョンによって，guiではくgzclientとなってしまっているようだ．
参考 (https://answers.ros.org/question/270662/unable-to-find-gui-node-in-gazebo-ros-package/ )

ERROR2: パラメータ問題

エラーメッセージ

RLException: Invalid <param> tag: Cannot load command parameter [robot_description]: no such command [['/opt/ros/noetic/share/xacro/xacro.py', '/home/yuya/chessbot/src/deprecated_nasa_r2_common/r2_description/robots/r2c_full_body.sim.urdf.xacro']]. 

Param xml is <param name="robot_description" command="$(find xacro)/xacro.py '$(find r2_description)/robots/r2c_full_body.sim.urdf.xacro'"/>
The traceback for the exception was written to the log file

解決

xacro.py → xacro
の変更後さらに，以下に示す
ERROR2_*とERROR2_**を解決することで，解決した

ERROR2_*: xacro問題

エラーメッセージ

name 'robot_name' is not defined 
when evaluating expression 'robot_name'

解決

<property name="robot_name"~> → <xacro:property name="robot_name"~>というように書き換えた
参考 (https://answers.ros.org/question/245707/parse-error-when-using-a-xacro-property-loaded-through-a-yaml-file/ )

ERROR2_**: xacro問題2

エラーメッセージ

unknown macro name: xacro:robot_world_origin

解決

  <xacro:robot_world_origin>
    ～
  </xacro:robot_world_origin>

を次のように書き換える

  <xacro:macro name="robot_world_origin">
    ～
  </xacro:macro>

参考 (https://qiita.com/srs/items/9ac7f2f6e47732bb7535)

ERROR3: リンク問題

エラーメッセージ

[ERROR] [1615438526.491255002]: Failed to build tree: child link [robot_nameworld_ref] of joint [robot_namefixed/world/world_ref] not found

未解決

ERROR4: spawn_model問題

エラーメッセージ

spawn_model: error: unrecognized arguments: -paused=true

解決?

  <node name="spawn_r2c_body" pkg="gazebo_ros" type="spawn_model" 
args="-urdf -param robot_description -model r2c_body -paused=true" 
respawn="false" output="screen" />

の-paused=trueを消すことでエラーは消えた

以上のようにいくつかは解決できたものもあるが，リンク問題において，ファイルではしっかりと定義されていること確認したが，実行するとうまくいかず，どうしても解決できなかった．