ノートブック

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

Universal Robot で簡単な実験

f:id:primitivem:20170929222206p:plain

Universal Robot は人と協働することができる6自由度の産業用ロボットで、ROSにもパッケージが用意されています。 http://wiki.ros.org/universal_robot
この記事では、このUR(Universal Robot)とロボットの動きを制御することが便利にできるソフトウェア MoveIt! と 深度カメラとOpenCVを使って、対象物に対してURの先端をリーチさせるという課題をGazeboのシミュレーション上で行ってみたいと思います。
これは私が既存のロボットを使って一般的なトップダウン型の認識と制御を勉強する目的のために行った一連の試行錯誤をノートにしたものです。

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

この記事の内容はこのURLにROSパッケージとしてあります。 https://github.com/embodmt/Gazebo_ROS/tree/master/ur_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 を参考にしました。

UR本体のセットアップ

UR本体はこのレポジトリ https://github.com/ros-industrial/universal_robot
を自分の手元にクローンしてcatkin_makeを実行すれば使えるようになりました。

ur_my_experimentのセットアップ

今回は ur_my_experiment というROSパッケージを新たに作り、そこにURを制御するスクリプトを置いたり、UR本体のURDFをインクルードしてきて深度カメラを取り付けたりする実験用のパッケージです。 つまりUR本体のパッケージには一切触れずにUR本体をシミュレーションに使っています。 https://github.com/embodmt/Gazebo_ROS/tree/master/ur_my_experiment このURLにあるパッケージを手元のUR本体のパッケージがあるところと同じcatkin workspaceにコピーすれば使えるようになります。

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パッケージが発生したりするので注意が必要です。

これで一通りの準備は完了して、
UR本体のパッケージと、今回解説する ur_my_experimentの2つを同じcatkin_workspaceに置いた状態で

$ roslaunch ur_my_experiment ur10_moveit.launch

を実行すれば gazebo と rviz が同時に立ち上がり moveitのplanningをguiで操作して実行すればgazeboでも指示通りにURが動きます。つまり一通りRVizを使ってMoveIt!のモーションプランニングが行えて、それが ちゃんとシミュレーション上にあるURの動きとして反映されるようになりましす。

既知として与えられた対象物の座標へのリーチング

Gazeboのシミュレーション上でURの近くに青色の四角い箱を置き、その座標を既知のものとして取得することで URのアームの先端部分をその箱の真上にリーチングするという実験をしてみたいと思います。
以下のようなMoveIt!のAPIを使うpythonスクリプトでURを制御します。
reaching.py

import rospy
import random,math
import numpy as np
import copy
from moveit_python import (MoveGroupInterface,
                           PlanningSceneInterface,
                           PickPlaceInterface)
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import PoseStamped

class Reaching:
    def __init__(self):
        rospy.init_node('reaching', anonymous=True)
        self.robot = moveit_commander.RobotCommander()
        self.model_pose = [0.0,0.0,0.0]
        self.obstacle_pose = [0.0,0.0,0.0]
        self.set_forbidden()
        self.set_init_pose()

    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") 
        self.planning_scene.removeCollisionObject("obstacle")
        print dir(self.planning_scene)
        import inspect
        print "addBox's variable is ",inspect.getargspec(self.planning_scene.addBox)
        print "attachBox's variable is ",inspect.getargspec(self.planning_scene.attachBox)
        print "addCube's variable is ",inspect.getargspec(self.planning_scene.addCube)
        print "setColor's variable is ",inspect.getargspec(self.planning_scene.setColor)

    def set_init_pose(sefl): 
        #set init pose
        move_group = MoveGroupInterface("manipulator","base_link")
        planning_scene = PlanningSceneInterface("base_link")
        joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
                        "elbow_joint", "wrist_1_joint",
                        "wrist_2_joint", "wrist_3_joint"]
        pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]      
        move_group.moveToJointPosition(joint_names, pose, wait=False)
        move_group.get_move_action().wait_for_result()
        result = move_group.get_move_action().get_result()
        move_group.get_move_action().cancel_all_goals()


    def callback(self,data):
        x = data.pose[1].position.x
        y = data.pose[1].position.y
        z = data.pose[1].position.z
        self.model_pose = [x,y,z]
        
        x = data.pose[2].position.x
        y = data.pose[2].position.y
        z = data.pose[2].position.z
        self.obstacle_pose = [x,y,z]

    def start_subscriber(self):
        rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)

    def target(self):
        rate = rospy.Rate(1)
        rospy.sleep(1)
        while not rospy.is_shutdown():
            self.planning_scene.removeCollisionObject("demo_cube") 
            self.planning_scene.removeCollisionObject("obstacle")
            self.planning_scene.addCube("demo_cube", 0.06,self.model_pose[0],self.model_pose[1],self.model_pose[2])
            self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0],self.obstacle_pose[1],self.obstacle_pose[2])

            print self.model_pose
            group = moveit_commander.MoveGroupCommander("manipulator")
            #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 = pose.position.x
            #pose_target.position.y = pose.position.y
            #pose_target.position.z = pose.position.z
            pose_target.position.x = self.model_pose[0]
            pose_target.position.y = self.model_pose[1]
            pose_target.position.z = self.model_pose[2]+0.05

            group.set_pose_target(pose_target)
            group.go()

            rospy.sleep(1)

if __name__ == '__main__':
    try:
        r = Reaching()
        r.start_subscriber()
        r.target()
        
    except rospy.ROSInterruptException:
        pass

上記にあるように
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
でシミュレーション上の箱の座標を受け取ってその座標に
target() メソッドの中身でリーチングの動作を自動的に行っています。
もちろん現実の物理世界では何らかの方法で対象物の座標は取得しなければなりませんが、シミュレーションなので簡単にMoveIt!の練習をするために対象物の座標を既知としているだけです。

実行するには以下のようにします。

$ roslaunch ur_my_experiment ur10_moveit.launch

これでgazeboとrvizを起動し

$ rosrun ur_my_experiment reaching.py

これでリーチ動作のスクリプトを起動します。するとこんな感じにURが対象物の真上に自動的に立直します。 f:id:primitivem:20170929222206p:plain f:id:primitivem:20170929222218p:plain

ur_my_experimentのlaunchディレクトリ内の sim.worldファイルがこのスクリプトを動かすためのplaygroundになっていて、 そこにリーチする対象物と障害物の2つがworldにあらかじめ配置されています。
この2つの物体をアームが届く範囲で位置を変えると、アームの先端が障害物を避け対象物の真上にリーチするように動きます。 右がRVizの画面で緑色で障害物と対象物が示されておりこの座標だけを与えるとMoveIt!が自動的に障害物を避け対象物の真上に移動するようなモーションプランニングを 自動で行ってくれます。便利な世の中になった!
そしてそのモーションプランが左の画面に示されているGazeboのシミュレーション上で実行されます。もし実機があれば現実にロボットが動くのが左側の画面の内容ということになります。

深度カメラ単体でのテスト

現実の世界では何もしないで対象物の座標を知ることはできないので、何らかの方法で対象物の座標を取得しなければなりません。 そこでまず深度カメラで対象物を認識してそのXY平面(カメラの投影面をXYと定義)での座標を求め、つぎにそのXY座標点での深度を計測してZ座標を求めるという方法で 対処物の座標を取得してみます。もちろんシミュレーション上で。

このpythonスクリプトでそれを行っています。
cv_test.py

import rospy
import cv2
from std_msgs.msg import String
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import random,math
import numpy as np
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray

class Image_Converter:
    def __init__(self):
        self.image_pub = rospy.Publisher("image_topic_2",Image)
        self.bridge = CvBridge()
        
        #HughCircle using RGB image
        self.image_sub = rospy.Subscriber("/depth_camera_rgb/image_raw",Image,self.callback1)
        #HughCircle using depth image
        self.image_sub = rospy.Subscriber("/depth_camera/depth_registered/image_raw",Image,self.callback2)

        self.coordinate = [0.0,0.0]

        self.marker_pub = rospy.Publisher("visualization_marker_array", MarkerArray)

    def callback1(self,data):

        position = [0.0,0.0]

        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)
        
        cimage = cv2.cvtColor(cv_image,cv2.COLOR_BGR2GRAY)
        circles = cv2.HoughCircles(cimage,cv2.cv.CV_HOUGH_GRADIENT,1,20, param1=50,param2=20,minRadius=0,maxRadius=100)
        if circles != None:
            a,b,c = circles.shape
            print "num of circles: ",b
            for i in range(b):
                cv2.circle(cv_image, (circles[0][i][0], circles[0][i][1]), circles[0][i][2], (0, 0, 255), 1)
                #cv2.circle(cv_image, (circles[0][i][0], circles[0][i][1]), 2, (0, 255, 0), 1)
            position[0] = circles[0][0][0]
            position[1] = circles[0][0][1]
            self.coordinate = [position[0],position[1]]
        else:
            print "none"

        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

    def callback2(self,data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
        except CvBridgeError as e:
            print(e)

        depth_array = np.array(cv_image, dtype=np.float32)
        #print np.max(depth_array)

        print self.coordinate
        x = int(self.coordinate[0])
        y = int(self.coordinate[1])
        print x,y
        print depth_array[y][x]

        Z = depth_array[y][x]
        X = Z*(x-320)/554
        Y = Z*(y-240)/554
        print X,Y,Z
        
        markerArray = MarkerArray()
        marker = Marker()
        marker.header.frame_id = "/depth_camera_frame"
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale.x = 0.02
        marker.scale.y = 0.02
        marker.scale.z = 0.02
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = X
        marker.pose.position.y = Y
        marker.pose.position.z = Z

        markerArray.markers.append(marker)
        self.marker_pub.publish(markerArray)

def main():
    ic = Image_Converter()
    rospy.init_node('image_converter', anonymous=True)
    
    rate = rospy.Rate(1) # 10hz
    while not rospy.is_shutdown():
        v = rospy.get_time()
        rospy.loginfo(v)
        
        rate.sleep()

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

def callback1(self,data): 内の処理で球体を二次元画像処理(ハフ変換)として認識しまずXY平面での座標を求めます、
次に def callback2(self,data): 内での処理でその中心座標に当たる部分までの深度を計算しています。
深度画像の任意のピクセルに対応する3次元座標は今回の場合だと以下のようにして求められます。

Z = depth_array[y][x]
X = Z*(x-320)/554
Y = Z*(y-240)/554

この透視投影変換におけるカメラ固有のパラメータはrostopicコマンド

$ rostopic echo /camera_info

を実行することで情報が得られ、この中のP行列の値から取得しました。 http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html このROSのデータ型リファレンスに各パラメータの説明があります。そしてこの変換式によってカメラキャリブレーションを行うことができました。 こうして取得した対象物の3次元座標を元に
markerArray.markers.append(marker)
self.marker_pub.publish(markerArray)
この部分等で、対象物の球体のてっぺんにマカーで赤色の印をつけています。

では実行してみましょう。only_camera.launchには深度カメラのモデルを記述したdepth_camera.urdf をスポーンして水平に向けて球体を写すという実験環境が記述されています。

$ roslaunch ur_my_experiment only_camera.launch

これでgazeboとrvizを起動すると f:id:primitivem:20170929222439p:plain このように深度カメラと対象物となる球体が登場します。左側がシミュレーションで右側がRVizです。
次にこれでカメラの処理を起動させます。

$ rosrun ur_my_experiment cv_test.py

すると以下のように実行されます。
f:id:primitivem:20170929222512p:plain f:id:primitivem:20170929222526p:plain f:id:primitivem:20170929222540p:plain 左下にカメラからの画像が現れ対象物の球体の部分に赤い丸の線が表示されており、これがCVによるハフ変換で認識した球体を表しています。 そして右側のRVizの画面で球体の先端に赤い点がマーキングされているのがわかります。 球体を深度カメラに近づけたり離したり、視野の端にやったりしてもちゃんと球体のてっぺんにマーカーがつくのがわかります。 もちろん2次元画像の認識にとても単純なハフ変換のみを使用しているので、この例では球体以外の対象物を認識することはできません。 これはとても簡単な深度カメラの練習です。

深度カメラとCVを使った対象物へのリーチング

では最後に、深度カメラから取得した座標情報をもとにURロボットの先端をその対象物にリーチするという実験を行ってみましょう。 やっていることは上のリーチング動作と深度カメラ認識の2つを組み合わせただけです。

深度カメラつきのURのplaygroundが sim_for_camera.world です。 ur_and_camera.urdf.xacro が ur本体のxacro(本家のパッケージ)と depth_camera.urdf.xacro(先ほど作った深度カメラ) をインクルードして一つのモデルとしてgazebo及びrviz上にスポーンされている。

実行してみましょう。

$ roslaunch ur_my_experiment ur10_and_camera.launch

これでgazeboとrvizを起動し

$ rosrun ur_my_experiment cv_test.py

これで画像処理ノードを起動し

$ rosrun ur_my_experiment percept_reaching.py

そしてこれでURのリーチ動作のノードを起動します。 この percept_reaching.py は 先ほどのreaching.pyとほぼ同じコードだが、cv_test.pyで認識した対象物の座標情報を受けとりそれを"object"という名前のtfとして発行することでURのbase_linkからの 相対座標に変換してMoveIt!でリーチングしています。わざわざtfを使って座標変換しなくても他にいろいろやり方はあると思うのですが、手っ取り早く動かしたかったのでこのようにしました。
実行結果は以下のようになります。 f:id:primitivem:20170929222702p:plain 初期条件で球体の対象物とURが現れカメラが真上から真下に向いて撮影しているのがわかります。 f:id:primitivem:20170929222751p:plain f:id:primitivem:20170929222711p:plain 深度カメラが球体を認識して赤い点のマーカーを示し f:id:primitivem:20170929222720p:plain その情報をもとにMoveIt!で対象物までURのエンドエフェクター部分がリーチしました。 もろもろ簡易的な仕組みのため動作が限定的で安定しないが、深度カメラから認識した対象物の3次元座標を取得し、 そこにエンドエフェクターをリーチさせる動作をざっくり理解できると思います。
また画像処理の部分とその結果のデータを受け取りリーチングを行う部分を別々のノードとして記述できるところもよいです。

以上、既存のロボットを使ったトップダウン制御の導入的勉強でした。