ROSの勉強 第36弾：ロボットアームの作成（4）

#プログラミング ROS< ロボットアームの作成（4） >

はじめに

1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第36弾として，「ロボットアームの作成（4）」を扱う．

環境

仮想環境

ソフト	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
シミュレーション	gazebo

ロボットアームの作成

ROSを使って，ほとんどの新しいロボットを制御する手順は次のようである．

1. ROSのメッセージインタフェースを決める．
2. ロボットのモータ用ドライバを書く．
3. ロボットの物理構造を書く．
4. Gazeboのシミュレーションで使用できるようにモデルに物理的特性を追加する．
5. tfを介して座標変換データを配信し，rvizでそれを可視化する．
6. センサを追加する．ドライバとシミュレーションのサポートも必要．
7. ナビゲーション等の標準的なアルゴリズムを適用する．

ロボットアームを例にその流れを確認していく．
今回は，5, 6についてまとめてロボットアームの作成（4）として扱うこととする．

手順5:座標変換を確認する

CougarBotが自分で経路計画をするまでには，もう少し作業を続ける必要がある．

座標変換

rvizも座標系間の関係を表す座標変換情報を必要とする．
この座標変換情報はtfトピックのtf2_msgs/TFMessageメッセージで提供される．
このメッセージを提供するのは簡単で，次の2つのステップだけ．

• ロボット全ての関節に関するsensor_msgs/JointStateメッセージを/joint_statesトピックに配信する
• robot_state_publisherで/joint_statesメッセージを対応する/tfメッセージに変換する

実装

関節状態を配信するには，Gazeboプラグインが使える．
URDFファイルにそのプラグインを追加することで，関節状態を配信できるようになる．
ここでは，ros_joint_state_publisherプラグインを使って，joint_statesインタフェースを提供し，
アームの現在の状態を送信する．追加は非常に簡単で，状態データを配信する関節リストを指示するだけ．

ソースコード

cougarbot.urdf

<?xml version="1.0"?>
<robot name="cougarbot">
	<!--作業台に固定されているようにするための架空のリンク"world"-->
	<link name="world"/>
	

	<!--台座のモデリング-->
	<link name="base_link">
    <!--外観-->
		<visual>
			<geometry>
				<cylinder length="0.05" radius="0.1"/>
			</geometry>
			<material name="silver">
				<color rgba="0.75 0.75 0.75 1"/>
			</material>
			<origin rpy="0 0 0" xyz="0 0 0.025"/>
		</visual>
    <!--物理的特性-->
    <!--干渉-->
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.025"/>
    </collision>
    <!--慣性-->
    <inertial>
      <mass value="1.0"/>
      <origin rpy="0 0 0" xyz="0 0 0.025"/>
      <inertia ixx="0.0027" iyy="0.0027" izz="0.005" ixy="0" ixz="0" iyz="0"/>
    </inertial>
	</link>
	<!--台座と作業台（world）をつなぐ関節-->
	<joint name="fixed" type="fixed">
		<parent link="world"/>
		<child link="base_link"/>
	</joint>

	
	<!--胴体のモデリング-->
  	<link name="torso">
    <!--外観-->
  		<visual>
  			<geometry>
  				<cylinder length="0.5" radius="0.05"/>
  			</geometry>
  			<material name="silver">
  				<color rgba="0.75 0.75 0.75 1"/>
  			</material>
  			<origin rpy="0 0 0" xyz="0 0 0.25"/>
  		</visual>
      <!--物理的特性-->
      <!--干渉-->
      <collision>
        <geometry>
          <cylinder length="0.5" radius="0.05"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0.25"/>
      </collision>
      <!--慣性-->
      <inertial>
        <mass value="1.0"/>
        <origin rpy="0 0 0" xyz="0 0 0.25"/>
        <inertia ixx="0.02146" iyy="0.02146" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
      </inertial>
  	</link>
  	<!--胴体と台座をつなぐ関節（腰）-->
  	<joint name="hip" type="continuous">
  		<axis xyz="0 0 1"/>
  		<parent link="base_link"/>
  		<child link="torso"/>
  		<origin rpy="0 0 0" xyz="0.0 0.0 0.05"/>
  	</joint>
  	

  	<!--上腕のモデリング-->
  	<link name="upper_arm">
    <!--外観-->
  		<visual>
  			<geometry>
  				<cylinder length="0.4" radius="0.05"/>
  			</geometry>
  			<material name="silver"/>
  			<origin rpy="0 0 0" xyz="0 0 0.2"/>
  		</visual>
      <!--物理的特性-->
      <!--干渉-->
      <collision>
        <geometry>
          <cylinder length="0.4" radius="0.05"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
      </collision>
      <!--慣性-->
      <inertial>
        <mass value="1.0"/>
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
        <inertia ixx="0.01396" iyy="0.01396" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
      </inertial>
  	</link>
  	<!--上腕と胴体をつなぐ関節（肩）-->
  	<joint name="shoulder" type="continuous">
  		<axis xyz="0 1 0"/>
  		<parent link="torso"/>
  		<child link="upper_arm"/>
  		<origin rpy="0 1.5708 0" xyz="0.0 -0.1 0.45"/>
  	</joint>


    <!--前腕のモデリング-->
    <link name="lower_arm">
    <!--外観-->
      <visual>
        <geometry>
          <cylinder length="0.4" radius="0.05"/>
        </geometry>
        <material name="silver"/>
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
      </visual>
      <!--物理的特性-->
      <!--干渉-->
      <collision>
        <geometry>
          <cylinder length="0.4" radius="0.05"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
      </collision>
      <!--慣性-->
      <inertial>
        <mass value="1.0"/>
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
        <inertia ixx="0.01396" iyy="0.01396" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
      </inertial>
    </link>
    <!--前腕と上腕をつなぐ関節（肘）-->
    <joint name="elbow" type="continuous">
      <axis xyz="0 1 0"/>
      <parent link="upper_arm"/>
      <child link="lower_arm"/>
      <origin rpy="0 0 0" xyz="0.0 0.1 0.35"/>
    </joint>


    <!--手のモデリング-->
    <link name="hand">
    <!--外観-->
      <visual>
        <geometry>
          <box size="0.05 0.05 0.05"/>
        </geometry>
        <material name="silver"/>
      </visual>
      <!--物理的特性-->
      <!--干渉-->
      <collision>
        <geometry>
          <box size="0.05 0.05 0.05"/>
        </geometry>
      </collision>
      <!--慣性-->
      <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.00042" iyy="0.00042" izz="0.00042" ixy="0" ixz="0" iyz="0"/>
      </inertial>
    </link>
    <!--手と前腕をつなぐ関節（手首）-->
    <joint name="wrist" type="continuous">
      <axis xyz="0 1 0"/>
      <parent link="lower_arm"/>
      <child link="hand"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.425"/>
    </joint>


    <!--トランスミッションの追加-->
    
    <!--腰関節用-->
    <transmission name="tran0">
      <type>transmission_interface/SimpleTransmission</type>
      <!--関節の指定-->
      <joint name="hip">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
      <!--アクチュエータの定義（ギア比1の減速機）-->
      <actuator name="motor0">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

    <!--肩関節用-->
    <transmission name="tran1">
      <type>transmission_interface/SimpleTransmission</type>
      <!--関節の指定-->
      <joint name="shoulder">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
      <!--アクチュエータの定義（ギア比1の減速機）-->
      <actuator name="motor1">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
    
    <!--肘関節用-->
    <transmission name="tran2">
      <type>transmission_interface/SimpleTransmission</type>
      <!--関節の指定-->
      <joint name="elbow">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
      <!--アクチュエータの定義（ギア比1の減速機）-->
      <actuator name="motor2">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
    
    <!--手首関節用-->
    <transmission name="tran3">
      <type>transmission_interface/SimpleTransmission</type>
      <!--関節の指定-->
      <joint name="wrist">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
      <!--アクチュエータの定義（ギア比1の減速機）-->
      <actuator name="motor3">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>


    <!--ros_controlプラグインをロード-->
    <gazebo>
      <plugin name="control" filename="libgazebo_ros_control.so"/>
    </gazebo>
    
    <!--関節の状態データを配信するプラグイン-->
    <gazebo>
      <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
        <jointName>hip, shoulder, elbow, wrist</jointName>
      </plugin>
    </gazebo>


</robot>

結果

rostopic echo /joint_statesにより，確認すると...

他の関節情報も配信されていることが分かる．
ここで．robot_state_publisherをlaunchファイルに追加する．

cougarbot.launch

<launch>
	<!--CougarBotのURDFモデルをパラメータサーバにロードする-->
	<param name = "robot_description" textfile = "$(find make_robot2)/urdf/cougarbot.urdf"/>

	<!--空のワールドでGazeboを開始する-->
	<include file = "$(find gazebo_ros)/launch/empty_world.launch"/>

	<!--GazeboでCougarBotを生成し，パラメータサーバからのその記述を受ける-->
	<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model cougarbot"/>

	<!--コントローラをROSのパラメータサーバに読み込む-->
	<rosparam file = "$(find make_robot2)/yaml/controllers.yaml" command = "load"/>

	<!--コントローラ作成-->
	<node name = "controller_spawner" pkg = "controller_manager" type = "spawner" args = "arm_controller"/>

	<!--robot_state_publisher: 順運動学を計算し，tfメッセージを生成する-->
	<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher"/>

</launch>

さらにrvizでロボットモデルを反映させると...

ここで，rqt_plotも起動させて，各関節位置についてのグラフを用意．

rqt_plotの起動

rqt_plot '/joint_states/position[0]' '/joint_states/position[1]' '/joint_states/position[2]' '/joint_states/position[3]'

このとき，次のコマンドを実行して，ロボットが移動するときの挙動をロボットとグラフから見る．

実行コマンド1

rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory '{joint_names: ["hip", "shoulder", "elbow", "wrist"], points: [{positions: [0.1, -0.5, 0.5, 0.75], time_from_start: [1, 0]}]}' -1

実行コマンド2

rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory '{joint_names: ["hip", "shoulder", "elbow", "wrist"], points: [{positions: [0.0,0.0, 0.0, 0.0], time_from_start: [1, 0]}]}' -1

実行の様子

以上により，座標変換が正しく扱われていることを確認できた．

手順6:MoveIt設定する

MoveItはモーションプランニングと制御用のツールからなるライブラリである．

以下にその手順を動画像で示す．

それぞれの項目を簡単に列挙しておく．

• Self-Collision（自己干渉）
  • ロボットのモデルを調べ，可能な姿勢をランダムにたくさんサンプリングし，干渉判定する方法をするべきと，そうでないときを決定することを手助けする．
• visual Joints（仮想関節）
  • ここですることはない
• Planning Groups（プランニングループ）
  • 腕全体を扱うことのできるモーションプランニングのグループを1つ作成する必要がある．ここでは，armという名前にしているが，任意である．運動学ソルバーには，kdl_kinematics_plugin/KDLKinematicsPluginを選択している．これは，汎用的な逆運動学のソルバーを提供する．最も効率が良いわけではないが，ここでは十分．
• Robot Poses（ロボットの姿勢）
  • ここですることはない
• End Effectors（エンドエフェクタ）
  • ロボットのどのリンクを経路計画に使うのかを指示する必要がある．
• Passive Joints（受動関節）
  • ここですることはない
• Configuration Files（コンフィギュレーションファイル）
  • ROSのパッケージをどこに作成するかを指示する．このパッケージの中に新しいコンフィギュレーションファイルが書き込まれる．

以下では，これを使うために必要な処理を記述する．

YAMLファイルの作成

MoveItがCougarBotアームのコントローラを使用できるように設定するためのYAMLファイルをcougarbot_moveit_config/configの中に新たに作成する．

controllers.yaml

# Hand-made
controller_manager_ns: /
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - hip
      - shoulder
      - elbow
      - wrist

このファイルは，MoveItにros_controlプラグインによって提供されるfollow_joint_trajectoryアクションサーバに見つけられるようにし，またどの関節がコントロールされるかを伝える．

launch.xmlファイルの作成

先ほどのYAMLファイルを読み込むなどのいくつかのパラメータを設定している．

cougarbot_moveit_controller_manager.launch.xml

<launch>  
	<rosparam file="$(find cougarbot_moveit_config)/config/controllers.yaml"/>  
	<param name="use_controller_manager" value="true"/>  
	<param name="controller_manager_name" value="/"/>  
	<param name="moveit_controller_manager"  value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>  
</launch>

以上がMoveItの設定である．

感想

今回は，CougarBotに経路計画させるための準備を行い，MoveItについても知ることができた．次回では，実際にロボットを動かすということで楽しみである．

参考文献

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