ノートブック

古き良きスタイルのノートブックです。

Fetch RoboticsとORKを使った基本的な認識・制御実験

f:id:primitivem:20170929223309p:plain

Fetch Robot は倉庫などでの運搬やピッキングの作業を想定して作られたロボットで、ROSにもパッケージが用意されています。 http://wiki.ros.org/Robots/fetch
この記事では、このFetchとMoveIt!(ロボットの動きを制御することが便利にできるソフトウェア) を使って 対象物に対してFetchのアーム先端をリーチさせるという課題をGazeboのシミュレーション上で行ってみたいと思います。
また ORK(Object Recognition Kitchenという対象物をデータベースにある3次元情報と照合して認識するソフトウェア) をFetchの頭についてるカメラで使ってみたいと思います。
これは私が既存のロボットを使って一般的なトップダウン型の認識と制御を勉強する目的のために行った一連の試行錯誤をノートにしたものです。

以下の内容は Ubuntu 14.04 と ROS Indigo と Gazebo7 の環境下で行ったものです。

この記事の内容はこのURLにROSパッケージとしてあります。 https://github.com/embodmt/Gazebo_ROS/tree/master/fetch_my_experiment

環境の準備

MoveIt!のインストール

MoveIt!のインストールは私の環境では以下を実行すれば簡単に行えました。

$ sudo apt-get install ros-indigo-moveit*
$ sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controllers ros-indigo-joint-state-controller ros-indigo-effort-controllers ros-indigo-position-controllers ros-indigo-joint-trajectory-controller

この公式ドキュメント http://moveit.ros.org/install/ と このページ https://sites.google.com/site/robotlabo/time-tracker/ros/ros-manipulator を参考にしました。

Gazebo7のインストール

ROS IndigoをフルインストールするとGazebo2が同時にインストールされます。これをGazebo7に変えます。 ここを参考にして http://gazebosim.org/tutorials?tut=install_ubuntu&cat=install

$ sudo apt-get remove gazebo2
$ sudo apt-get install ros-indigo-gazebo7-ros-pkgs ros-indigo-gazebo7-ros-control
$ sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controllers

私の場合このように実行すればgazebo2がgazebo7に変わりました。しかし一度gazebo7にするとgazebo2に戻せなかったり(私の場合はそうで、詳しくはわかりません)、apt-getからインストールできなくなるROSパッケージが発生したりするので注意が必要です。

Fetch本体のセットアップ

上のようにしてgazebo7に差し替えると

$ apt-get install ros-indigo-fetch*

このようにしてfetch関連のパッケージをインストールしようとしてもできなかったので、以下のリストにある

fetch_arm_control
fetch_auto_dock_msgs
fetch_bringup
fetch_calibration
fetch_depth_layer
fetch_description
fetch_driver_msgs
fetch_ikfast_plugin
fetch_maps
fetch_moveit_config
fetch_navigation
fetch_pbd_interaction
fetch_social_gaze
fetch_teleop
fetch_tools

fetch関連のパッケージを個別にapt-getでインストールして、次に
fetch_gazebo
robot_controllers
この2つのパッケージをワークディレクトリにコピーしてcatkin_makeでビルドし、
そして以下をapt-getでインストール

$ sudo apt-get install ros-indigo-rgbd-launch
$ sudo apt-get install ros-indigo-simple-grasping

これで環境は整って、

$ roslaunch fetch_gazebo simulation.launch
$ roslaunch fetch_moveit_config move_group.launch
$ rviz rviz

この3つを立ち上げると、RViz上でFetchの操作をMoveIt!を使って行いGazeboのシミュレーション上でそのと通りに ロボットが動くということが確認できました。

fetch_my_experimentのセットアップ

今回はfetch_my_experiment というROSパッケージを新たに作り、そこにFetchを制御するスクリプトを置いたりして使います。 つまりFetch本体のパッケージには一切触れずにFetch本体をシミュレーションに使っています。したがってこのfetch_my_experimentにはurdfなどのロボット定義ファイルは存在しません。 https://github.com/embodmt/Gazebo_ROS/tree/master/fetch_my_experiment このURLにあるパッケージを手元のFetch本体のパッケージがあるところと同じcatkin workspaceにコピーすれば使えるようになります。

以上が環境の準備です。
*しかしながら、上記のような環境の準備は ROS Indigo で私が試行錯誤的に行った結果のメモなのでよりスマートな方法は他にあると思います。

FetchをMoveIt!で動かすテスト

MoveIt!のAPIを使ってFetchの頭に付いているカメラとアームを任意の位置まで動かす制御を pythonスクリプトで書いてみると以下のようになります。
moveit_test.py

import copy
import actionlib
import rospy

from moveit_python import (MoveGroupInterface,
                           PlanningSceneInterface,
                           PickPlaceInterface)
from control_msgs.msg import PointHeadAction, PointHeadGoal
from moveit_msgs.msg import PlaceLocation, MoveItErrorCodes, Grasp

def move():
    rospy.init_node("move")

    move_group = MoveGroupInterface("arm_with_torso", "base_link")

    #planning scene setup
    planning_scene = PlanningSceneInterface("base_link")
    planning_scene.removeCollisionObject("my_front_ground")
    planning_scene.removeCollisionObject("my_back_ground")
    planning_scene.removeCollisionObject("my_right_ground")
    planning_scene.removeCollisionObject("my_left_ground")
    planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
    planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
    planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
    planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)

    #move head camera
    client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
    rospy.loginfo("Waiting for head_controller...")
    client.wait_for_server()

    goal = PointHeadGoal()
    goal.target.header.stamp = rospy.Time.now()
    goal.target.header.frame_id = "base_link"
    goal.target.point.x = 1.2
    goal.target.point.y = 0.0
    goal.target.point.z = 0.0
    goal.min_duration = rospy.Duration(1.0)
    client.send_goal(goal)
    client.wait_for_result()

    #arm setup
    move_group = MoveGroupInterface("arm", "base_link")
    #set arm initial position
    joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
    pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
    while not rospy.is_shutdown():
     result = move_group.moveToJointPosition(joints, pose, 0.02)
     if result.error_code.val == MoveItErrorCodes.SUCCESS:
         return

if __name__ == '__main__':
 move()

それぞれの関節角を指定することでFetchを動かしています。
このスクリプトを実行するにはまず以下のようにして,

$ roslaunch fetch_my_experiment fetch.launch

fetch.lancuhファイルを起動すると今回この実験のために用意した sim.world がGazeboに読み込まれ、RVizも同時に立ち上がり、fetchロボットがスポーンされます。
そして以下で moveit_test.py の制御スクリプトを起動します。

$ rosrun fetch_my_experiment moveit_test.py

すると以下のようにFetchが指定した初期ポーズまで自動的に変形します。 f:id:primitivem:20170929223229p:plain すごく伸びるねFetch!

対象物の座標を既知として与え、そこにリーチするスクリプト

上記のようにMoveIt!が使えたろころで、今度は対象物の座標を既知のものとしてFetchの制御スクリプトに与えそこにFetchのアームのエンドエフェクターを リーチングさせてみます。

以下のスクリプトで、Gazebo上にスポーンさせた対象物となる青いボックスの座標を取得し、その座標をMoveIt!のAPIに入力することで 簡単にFetchのアームを対象物にリーチさせることができます。
reaching.py

import copy
import actionlib
import rospy

from math import sin, cos
from moveit_python import (MoveGroupInterface,
                           PlanningSceneInterface,
                           PickPlaceInterface)
from moveit_python.geometry import rotate_pose_msg_by_euler_angles

from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from control_msgs.msg import PointHeadAction, PointHeadGoal
from grasping_msgs.msg import FindGraspableObjectsAction, FindGraspableObjectsGoal
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from moveit_msgs.msg import PlaceLocation, MoveItErrorCodes, Grasp
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

from gazebo_msgs.msg import ModelStates
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from visualization_msgs.msg import MarkerArray
import tf

class Reaching:
    def __init__(self):
        rospy.init_node('reaching', anonymous=True)
        self.model_pose = [0.0,0.0,0.0]

        self.set_init_pose()
        #self.set_forbidden()

    def set_forbidden(self):
        #set forbidden erea
        self.planning_scene = PlanningSceneInterface("base_link")
        self.planning_scene.removeCollisionObject("my_front_ground")
        self.planning_scene.removeCollisionObject("my_back_ground")
        self.planning_scene.removeCollisionObject("my_right_ground")
        self.planning_scene.removeCollisionObject("my_left_ground")
        self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
        self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
        self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
        self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
        self.planning_scene.removeCollisionObject("demo_cube")
    
    def set_init_pose(self): 
        #set init pose
        move_group = MoveGroupInterface("arm_with_torso", "base_link")
        #set arm initial position
        joints = ["torso_lift_joint", "shoulder_pan_joint",
                    "shoulder_lift_joint", "upperarm_roll_joint",
                    "elbow_flex_joint", "forearm_roll_joint",
                    "wrist_flex_joint", "wrist_roll_joint"]
        #pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0]
        #pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
        result = move_group.moveToJointPosition(joints, pose, 0.02)

    def target(self):
        rate = rospy.Rate(1)
        rospy.sleep(1)
        ls = tf.TransformListener()
        while not rospy.is_shutdown():
         print self.model_pose

         try:
             (trans,rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0))
             print "tf base_link to test",trans
         
             group = moveit_commander.MoveGroupCommander("arm_with_torso")
             #print group.get_current_pose().pose
             pose_target = geometry_msgs.msg.Pose()
             pose = group.get_current_pose().pose
             pose_target = geometry_msgs.msg.Pose()
             pose_target.orientation.x = pose.orientation.x
             pose_target.orientation.y = pose.orientation.y
             pose_target.orientation.z = pose.orientation.z
             pose_target.orientation.w = pose.orientation.w
             pose_target.position.x = trans[0]
             pose_target.position.y = trans[1]
             pose_target.position.z = trans[2]+0.3

             group.set_pose_target(pose_target)
             group.go()

         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
             print "e"

         rospy.sleep(1)

if __name__ == '__main__':
 r = Reaching()
 r.target()

target()メソッド内の処理で対象物の座標を"object"という名前のtfとして受け取りその座標をMoveIt!のAPIに投げているだけです。 ちなみに対象物の座標は move_base.py という 別のスクリプトから発行されているものです。 実行するには、先ほどと同じように以下でGazeboとRvizを起動させます

$ roslaunch fetch_my_experiment fetch.launch

そして次に以下のようにmove_base.pyスクリプトの実行でFetchの土台を目的地まで移動させ、さらに既知のデータとして受け取った対処物の座標をtfとして発行して座標変換を 容易に行えるようにする。

$ rosrun fetch_my_experiment move_base.py

ちなみにこの move_base.py も reaching.py に負けず劣らずのアドホックなコードなので、この実験の環境でしか機能しませんがそのほうがミニマルに書けて後から見た時に学習用としてわかりやすいです。
次に以下でリーチするスクリプトを実行。Fetchのアームで対象物まで真上からリーチングします。

$ rosrun fetch_my_experiment reaching.py

実行結果が以下のようになります。 まずFetchが対象物が乗っている白いボックスまで動いていって f:id:primitivem:20170929223304p:plain そしてアームを対象物の青い小さいボックスの上にリーチします f:id:primitivem:20170929223309p:plain Gazebo上で青いボックスを移動させても、Fetchのアームは青いボックスについてきます。 f:id:primitivem:20170929223320p:plain f:id:primitivem:20170929223333p:plain

対象物をORKで認識する

ORK(Object Recognition Kitchen) を導入することにより簡単に深度カメラからの3次元情報とデータベースに登録しておいた対象物の3次元情報を照合して対象物を認識しその座標を取得することができます。

このようにしてORKをインストールして

$ sudo apt-get install ros-indigo-object-recognition-*

この公式チュートリアルの 通りにやれば簡単に物体のデータベースへの登録とカメラからの認識を使うことができます。

*メモ(チュートリアルに補足して)

$ rosrun object_recognition_core push.sh

*Counch DBでwebクライアントから登録されている3Dモデルを参照するにはこれを実行する必要がある。

そこでここでは公式チュートリアルに登場したコーク缶の3DモデルをDBに登録して、同じものをGazeboのシミュレーション上にもスポーンしてテーブルの上に置き Fetchロボットの頭に付いている深度カメラを使って認識してみます。

基本的には公式のチュートリアルのやり方と同じなのですが .ork という拡張子のついたORK用の設定ファイルである detection.object.ros.ork と detection.table.ros.ork を Fetchロボットの深度カメラが出力するROS Topicに合わせて書き換える必要があります。公式チュートリアル内では

rosrun object_recognition_core detection -c  `rospack find object_recognition_tabletop`/conf/detection.object.ros.ork

このようにしてORKを起動しており、ここで使われている .orkファイルをコピーしてきてそれを土台にFetchロボット用に書き換えました。(ちなみに私の環境ではORKをapt-getでインストールしたのでこのファイルは /opt/ros/indigo/share/object_recognition_tabletop/conf という場所に存在しました)
さらにこの Romeo というロボットのGitHubレポジトリにORKが使えるようにセットアップされていたので、この .ork ファイルも参考になりました。
結果Fetch用のORKのconfigが以下のようになりました。
detection.table.ros.ork
detection.object.ros.ork
特に pareameters: の部分で正確にROS Topicを受け取れるように書き換える必要がありました。
以下 detection.object.ros.ork の一部。

source1:
  type: RosKinect
  
  〜〜省略〜〜

  parameters:
    rgb_frame_id: '/head_camera_depth_optical_frame'
    rgb_image_topic: '/head_camera/rgb/image_raw'
    rgb_camera_info: '/head_camera/rgb/camera_info'
    depth_image_topic: '/head_camera/depth_registered/image_raw'
    depth_camera_info: '/head_camera/depth_registered/camera_info'
    
 〜〜省略〜〜

これさえできればORKの起動も簡単でチュートリアルの通りにやればすんなり実行できました。 深度カメラからのデータを受け取れるgazeboシミュレーションとそのRVizを起動した状態で 以下を実行するとテーブルの認識ができ、

$ rosrun object_recognition_core detection -c `rospack find fetch_my_experiment`/config/detection.table.ros.ork

公式チュートリアルの通りにコーク缶の3DモデルをDBに登録して、シミュレーション上でもFetchの目の前にコーク缶を置いた状態で 以下を実行すると、

$ rosrun object_recognition_core detection -c `rospack find fetch_my_experiment`/config/detection.object.ros.ork

RvizでORKのトピックをaddすればちゃんとテーブルとコーラが認識されます。

しかしたくさんのターミナルを立ち上げるのは面倒なので上のようにrosrunで走らせたORKの起動コマンドとシミュレーション環境をまとめて以下のようにlaunchファイルにします。
fetch_ork.launch

<launch>
  <env name="GAZEBO_MODEL_PATH" value="$(find fetch_my_experiment)/models:$(optenv GAZEBO_MODEL_PATH)" />
  
  <include file="$(find fetch_my_experiment)/launch/simulation.launch" />

  <!-- Start MoveIt -->
  <include file="$(find fetch_moveit_config)/launch/move_group.launch" >
    <arg name="info" value="true"/><!-- publish grasp markers -->
  </include>

  <!--<node name="table_top" pkg="object_recognition_core" type="detection" args="-c $(find fetch_my_experiment)/config/detection.table.ros.ork" />-->
  <node name="table_top_object" pkg="object_recognition_core" type="detection" args="-c $(find fetch_my_experiment)/config/detection.object.ros.ork" />

  <!--<node name="rviz" pkg="rviz" type="rviz" />-->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find fetch_my_experiment)/launch/fetch_ork.rviz" />
</launch>

ではこれを実行してみましょう。

$ roslaunch fetch_my_experiment fetch_ork.launch

これでシミュレーションとRVizと一緒にORKのノードも一緒に起動する。 そして以下でカメラヘッドの角度を調節する

$ rosrun fetch_my_experiment set_position.py

f:id:primitivem:20170929223436p:plain カメラがテーブルを向きました。そして
gazebo上にはデータモデルとして登録したコーク缶と同じメッシュファイルのオブジェクト(青色だけど)を配置してあるので、 ORKのデータベースと照合してこれをコーク缶として認識している様子がわかります。 f:id:primitivem:20170929223445p:plain 水色で囲まれた四角がテーブルとしてORKに認識されマーカーがパブリッシュされたもので、青色のコーク缶が認識され RViz上に "coke" と名前付きでマーキングされているのがわかります。
さらにここからFetchのアームを使ったリーチング動作をしたいところですが、Fetchのシミュレーションが私のコンピュータの環境では重くて面倒くさいので割愛することにします。

以上、Fetch RoboticsとORKを使った基本的な実験でした。