専門実験2023ロボット制御Ⅰ:第1週 実習3 プログラムでロボットを動かす方法2

今週の実習ではロボットアームとグリッパの開閉をプログラムで実現する方法を学びます。このデモプログラムを応用すると産業用ロボットで最もよく使われるピック&プレースを実現できます。

参 考   

なお、このプログラムは東京オープンソースロボティクス協会の次のウェブサイトを参考にしました.

このデモプログラムでは,グリッパの設定と開閉のコードに関する部分を赤字で示しています。グリッパの開閉については以下のサイトを参考にしました。

        詳細についてはMoveItのAPIドキュメントをご覧ください.

デモプログラム:arm_demo2.py

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

import sys, math, copy
import rospy, tf, geometry_msgs.msg

from moveit_commander import MoveGroupCommander, RobotCommander
from geometry_msgs.msg import Pose, PoseStamped

# グリッパを開く
def open_gripper():
   print(“Opening Gripper…”)
   gripper_joint_angle[0] = -0.01
   gripper_group.set_joint_value_target(gripper_joint_angle)
   plan2 = gripper_group.go()
   gripper_group.stop()
   gripper_group.clear_pose_targets()
   rospy.sleep(1)

# グリッパを閉じる
def close_gripper():
   print(“Closing Gripper…”)
   gripper_joint_angle[0] = 0.0
   gripper_group.set_joint_value_target(gripper_joint_angle)
   plan2 = gripper_group.go()
   gripper_group.stop()
   gripper_group.clear_pose_targets()
   rospy.sleep(1)

if __name__ == ‘__main__’:   # このプログラムをモジュール化するためのおまじない
   node_name = “arm_demo2” # ノード名
   rospy.init_node(node_name, anonymous=True ) # ノードの初期化

   ## MoveGroupCommanderオブジェクトのインスタンス生成。これがジョイントへのインタフェースになる。
   ## このインタフェースは運動計画と動作実行に使われる。
   group = MoveGroupCommander(“arm”)
   gripper_group = MoveGroupCommander(“gripper”) # 追加
   
   group.set_planning_time(600.0) # 動作計画に使う時間[s]の設定
   
   # 初期姿勢の取得
   pose_init = group.get_current_pose()  # エンドエフェクタの位置(x, y, z)を取得
   rospy.loginfo( “Get Initial Pose\n{}”.format( pose_init ) )
   rpy_init  = group.get_current_rpy()   # エンドエフェクタの姿勢(roll, pitch, yaw)を取得
   rospy.loginfo( “Get Initial RPY:{}”.format( rpy_init ) )
   
   # グリッパの初期値を取得
   gripper_joint_angle = gripper_group.get_current_joint_values()
   print(“Get Current Gripper angle:\n{}\n”.format(gripper_joint_angle))
   
   # グリッパを開く
   open_gripper()   
   
   # 姿勢1
   rospy.loginfo( “Starting Pose 1”)
   pose_target_1 =  [ 0.12, 0.0, 0.1, 0.0, math.pi/2.0, 0.0 ] # [ x, y, z, r, p, y ]
   group.set_pose_target( pose_target_1 ) # エンドエフェクタの姿勢設定
   group.go()   
   rospy.sleep(5.0) # 5秒のスリープ
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format( pose_current ) )
   
   # グリッパを閉じる
   close_gripper()
   
   # 姿勢2
   rospy.loginfo( “Starting Pose 2”)
   group.set_pose_target( [ 0.2, 0.0, 0.2, 0.0, 0.0, 0.0 ] )
   group.go()   
   rospy.sleep(5.0)
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format( pose_current ) )
   
   # 姿勢3
   rospy.loginfo( “Starting Pose 3”)   
   pose_target_3 = Pose()
   pose_target_3.position.x =  0.10
   pose_target_3.position.y =  0.10
   pose_target_3.position.z =  0.10
   pose_target_3.orientation.x = -0.2706  # 姿勢(クオータニオン)
   pose_target_3.orientation.y =  0.6533  # 姿勢(クオータニオン)
   pose_target_3.orientation.z =  0.2706  # 姿勢(クオータニオン)
   pose_target_3.orientation.w =  0.6533  # 姿勢(クオータニオン)

   #  逆運動学で近似解を計算するためのエンドエフェクタの姿勢設定.2番目の引数がTrueだと近似解.
   # 2番目の引数がFalseまたは,無い場合は厳密解となる.
   group.set_joint_value_target(pose_target_3, True)
   group.go()   
   rospy.sleep(5.0)
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format( pose_current ) )
   
   # Pose 3 Z:-0.05[m]
   rospy.loginfo( “Starting Pose 3 Z:-0.05[m]”)
   pose_target_3.position.z -= 0.05   
   group.set_joint_value_target(pose_target_3, True )
   group.go()   
   rospy.sleep(5.0)
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format( pose_current ) )
   
   # 姿勢4
   rospy.loginfo( “Starting Pose 4”)   
   pose_target_4 = Pose()
   pose_target_4.position.x =  0.10
   pose_target_4.position.y = -0.10
   pose_target_4.position.z =  0.05
   pose_target_4.orientation.x =  0.2706
   pose_target_4.orientation.y =  0.6533
   pose_target_4.orientation.z = -0.2706
   pose_target_4.orientation.w =  0.6533   
   group.set_joint_value_target( pose_target_4, True )
   group.go()   
   rospy.sleep(5.0)
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format(pose_current ) )
   
   # 姿勢4 Z:+0.05[m]
   rospy.loginfo( “Starting Pose 4 Z:+0.05[m]”)
   pose_target_4.position.z += 0.05   
   group.set_joint_value_target( pose_target_4, True )
   group.go()   
   rospy.sleep(5.0)
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format( pose_current ) )
   
   # 初期姿勢に戻る
   rospy.loginfo( “Back to Initial Pose”)
   group.set_joint_value_target( pose_init, True )
   group.go()
   
   rospy.sleep(5.0)
   pose_current = group.get_current_pose()
   rospy.loginfo( “Get Current Pose:\n{}\n”.format( pose_current ) )

実行方法

  • 1番目のUbuntu端末を開き、次のコマンドでRviz(ROSの視覚化ツール)やGazeboを起動する。

    • $ source ~/.bashrc

    • $ sudo chmod 777 /dev/ttyUSB0

    • $ roslaunch open_manipulator_controllers joint_trajectory_controller.launch sim:=false

  • 2番目の端末を開き、次のコマンドでロボットアームを動かす。

    • $ cd ~/catkin_ws/src/open_manipulator/open_manipulator_controller/scripts

    • $ python3 arm_demo2.py

コメント

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