Sawyer:プログラムでロボットを動かす方法

MoveIt!を使ったロボットアームを制御するサンプルプログラムです。

#!/usr/bin/env python
# -*- coding: utf-8 -*-    #日本語のコメントを入れるためのおまじない

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

from std_msgs.msg import String

import intera_dataflow
from intera_io import IODeviceInterface
from intera_core_msgs.msg import IONodeConfiguration

import rospkg

from gazebo_msgs.srv import (
      SpawnModel,
      DeleteModel,
)

from geometry_msgs.msg import (
      PoseStamped,
      Pose,
      Point,
      Quaternion,
)


rospy.init_node('arm_demo',anonymous=True)    # ノードの初期化。ノード名はarm_demo。
gripper_io = IODeviceInterface("end_effector","right_gripper")

# グリッパを開く
def open_gripper():
  gripper_io.set_signal_value("position_m", 0.15)
  rospy.sleep(2)

# グリッパを閉じる
def close_gripper():
  gripper_io.set_signal_value("position_m", 0.0)
  rospy.sleep(2)

#  Gazebo3Dモデルのロード
def load_gazebo_models(table_pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)), table_reference_frame="world"):
  block_reference_frame="world"
  block_pose = (Pose(position=Point(x=0.7, y=0.0, z=0.7725)), \
                Pose(position=Point(x=0.7, y=0.1, z=0.7725)), \
                Pose(position=Point(x=0.7, y=0.2, z=0.7725)), \
                Pose(position=Point(x=0.7, y=0.3, z=0.7725)), \
                Pose(position=Point(x=0.7, y=0.4, z=0.7725)), \
                Pose(position=Point(x=0.8, y=0.0, z=0.7725)), \
                Pose(position=Point(x=0.8, y=0.1, z=0.7725)), \
                Pose(position=Point(x=0.8, y=0.2, z=0.7725)), \
                Pose(position=Point(x=0.8, y=0.3, z=0.7725)), \
                Pose(position=Point(x=0.8, y=0.4, z=0.7725)))


# 3Dモデルのパスを取得
  model_path = rospkg.RosPack().get_path('sawyer_sim_examples')+"/models/"
  
# テーブルSDFファイルのロード
  table_xml = ''
  with open (model_path + "cafe_table/model.sdf", "r") as table_file:
    table_xml=table_file.read().replace('\n', '')
 
 # ブロックURDFファイルのロード
  block_xml = ''
  with open (model_path + "block/model.urdf", "r") as block_file:
    block_xml=block_file.read().replace('\n', '')
 
 # テーブルの生成
  rospy.wait_for_service('/gazebo/spawn_sdf_model')
  try:
    spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
    resp_sdf  = spawn_sdf("cafe_table", table_xml, "/",
                             table_pose, table_reference_frame)
 except rospy.ServiceException, e:
    rospy.logerr("Spawn URDF service call failed: {0}".format(e))

#  Gazebo 3Dモデルの削除
def delete_gazebo_models():
  try:
    delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
    resp_delete = delete_model("cafe_table")

    for i in range(10):
      block_name = "block" + str(i)
      resp_delete = delete_model(block_name)

  except rospy.ServiceException, e:
    print("Delete Model service call failed: {0}".format(e))

# 目標位置の設定
def set_target_position(pose_target, x, y, z, qx, qy, qz, qw):
  pose_target.position.x = x
  pose_target.position.y = y
  pose_target.position.z = z
  pose_target.orientation.x = qx
  pose_target.orientation.y = qy
  pose_target.orientation.z = qz
  pose_target.orientation.w = qw


def main():
  print("*** Start Demo ***")

  ## moveit_commanderとrospyの初期化.
  moveit_commander.roscpp_initialize(sys.argv)

  ## RobotCommanderオブジェクトのインスタンス生成。これがロボットのインタフェースになる。
  robot = moveit_commander.RobotCommander()

  ## PlanningSceneInterfaceオブジェクトのインスタンス生成。これがロボット周囲環境へのインタフェースになる。
  scene = moveit_commander.PlanningSceneInterface()

  ## MoveGroupCommanderオブジェクトのインスタンス生成。これがジョイントへのインタフェースになる。
  ## このインタフェースは運動計画と動作実行に使われる。
  group = moveit_commander.MoveGroupCommander("right_arm")


  ## DisplayTrajectoryパブリッシャーの生成。RVIZ用の軌道視覚化に使われる。
  ## trajectories for RVIZ to visualize.
  display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                      moveit_msgs.msg.DisplayTrajectory)

  ## 基本情報の取得
  ## ロボットの参照フレーム(基準座標系)を取得する
  print("*** ロボットの参照フレーム: %s" % group.get_planning_frame())

  ## ハンドの参照フレーム(基準座標系)を取得する
  print("*** ハンドの参照フレーム: %s" % group.get_end_effector_link())

  ## ロボットの全グループを表示する。
  print("*** ロボットグループ:")
  print(robot.get_group_names())

  ## ロボットの状態表示
  print("*** ロボットの状態表示")
  print(robot.get_current_state())

  ## 目標姿勢への運動計画
  print("*** Plan 1の生成 ***")
  # position x, y,z; orientation x,y,z,w
  pose_target = geometry_msgs.msg.Pose()
  set_target_position(pose_target,0.5, 0.0, -0.1, 0.0, 1.0, 0.0, 0.0)
  #set_target_position(pose_target,0.7, -0.05, 1.2, 0.0, 1.0, 0.0, 0.0)
  group.set_pose_target(pose_target)


  ## 計画の実行
  plan1 = group.plan()
  print("*** RVIZがPlan 1を表示するまで待機 ***")
  rospy.sleep(5)

  ## RVIZでの計画された軌道表示
  print("*** Plan1 表示 ***")
  display_trajectory = moveit_msgs.msg.DisplayTrajectory()

  display_trajectory.trajectory_start = robot.get_current_state()
  display_trajectory.trajectory.append(plan1)
  display_trajectory_publisher.publish(display_trajectory);

  print("*** Plan1が表示されるのを再度待機 ***")
  rospy.sleep(5)

  # 実際のロボットを使う場合は下行のコメントを外す
  group.go(wait=True)

  # 目標姿勢の消去
  group.clear_pose_targets()

  #  テーブルとブロックのロード
  load_gazebo_models()

  # 終了時にテーブルとブロックを削除。終了時に削除したくない場合は下行の先頭に#をつけてコメント化する。
  rospy.on_shutdown(delete_gazebo_models)

  print("*** Plan 2 ***")
  pose_target = geometry_msgs.msg.Pose()
  set_target_position(pose_target,0.5, 0.0, 0.3, 0.0, 1.0, 0.0, 0.0)
  group.set_pose_target(pose_target)

  print("Close gripper")
  close_gripper()
  plan2 = group.plan()
  rospy.sleep(5)
  group.go(wait=True)
  group.clear_pose_targets()

  print("*** Plan 3 ***")
  pose_target = geometry_msgs.msg.Pose()
  set_target_position(pose_target,0.5, 0.0, -0.1, 0.0, 1.0, 0.0, 0.0)
  group.set_pose_target(pose_target)

  print("Gripper open")
  open_gripper()
  plan3 = group.plan()
  rospy.sleep(5)
  group.go(wait=True)
  group.clear_pose_targets()
  rospy.sleep(5)

  print("Close gripper")
  close_gripper()
  rospy.sleep(5)

  ## collision object messageの定義
  collision_object = moveit_msgs.msg.CollisionObject()


  ## moveit_commanderの終了
  moveit_commander.roscpp_shutdown()

  print("End")


if __name__=='__main__':
  try:
    main()
  except rospy.ROSInterruptException:
    pass

実 行

  • 1番目の端末を開き、次のコマンドでgazeboを起動する。
    • $ cd ~/ros_ws
    • $ ./intera.sh sim
    • $ roslaunch sawyer_gazebo sawyer_world.launch electric_gripper:=true
  • 2番目の端末を開く。必要があればキーボードの操作によりアームを好きな姿勢に移動する。次に、ロボットを稼働状態にしコントローラを起動する。
    • アームの移動(必要があれば)
    • $ cd ~/ros_ws
    • $ ./intera.sh sim
    • $ rosrun intera_examples joint_position_keyboard.py
    • 移動できたらCtrlキーを押しながら、Cキーを押してキー操作のアプリを落とす。
    • 同じ端末で、次のコマンドによりロボットを稼働状態にする。
      • $ rosrun intera_interface enable_robot.py -e
    • 次のコマンドでコントローラを起動する。
      • $ rosrun intera_interface joint_trajectory_action_server.py
  • 3番目の端末を開き、次のコマンドでrviz(ROSの視覚化ツール)を起動する。
    • $ cd ~/ros_ws
    • $ ./intera.sh sim
    • $ roslaunch sawyer_moveit_config sawyer_moveit.launch electric_gripper:=true
  • 4番目の端末を開き、次のコマンドでロボットアームを動かす。
    • $ cd ~/ros_ws
    • $ ./intera.sh sim
    • $ cd ~/ros_ws/src/sawyer_simulator/sawyer_sim_examples/scripts
    • $ python arm_demo.py

演習

  • デモプログラムの実行
  • 自作プログラムの作成と実行方法
    • 作成方法
      • デモプログラムarm_demo.pyを以下のコマンドを使いgeditなどのエディタで開く。別名(例えば arm_my_demo.py)をつけて保存する。保存するディレクトリはarm_demo.pyと同じところにする。
        • $ cd ~/ros_ws
        • $ ./intera.sh sim
        • $ cd ~/ros_ws/src/sawyer_simulator/sawyer_sim_examples/scripts
        • $ gedit arm_demo.py
      • arm_my_demo.pyをエディタで開きファイルを編集・保存する。
    • 実行方法
      • 端末が開いていたら全閉じる。
      • 上の実行方法の1から3番目の端末での作業を実施する。
      • 新しい端末を開き、次のコマンドを実行する。一番最後のarm_my_demo.pyは適宜変更する。
        • $ cd ~/ros_ws
        • $ ./intera.sh sim
        • $ cd ~/ros_ws/src/sawyer_simulator/sawyer_sim_examples/scripts
        • $ python arm_my_demo.py
  • ハンズオン
    • デモプログラムを参考にテーブル上のブロックを掴んで、積み上げるプログラムを作りなさい。
    • ブロックの位置をずらしてもよい。
    • ヒント:手先の位置姿勢を設定して実行しても”no motion plan”などと表示されてアームが動ない場合があります。それは可動域や干渉あるいは特異点などで、その位置姿勢に到達できないのです。その場合は、ロボットの初期姿勢を変更するか目標の位置と姿勢を変更するしかありません。

終わり

コメント

Folding@home Kanazawa (ID 257261)

みんなのためにおうちで新型コロナウイルスを解析しよう!

タイトルとURLをコピーしました