Moveitへの司令(python)

ツールで手先の位置を指定するだけでは使えないので，プログラムから目標位置へ移動指示を行う方法を説明する．

このページはPythonを使うことを前提とする．C++のページは別のページで説明．

まず，moveit_setup_assistantでmoveit_configを作っているものとする．

自作した６自由度のマニピュレータのページで，sixdofarm_moveit_configを作っているので，それを使ってpythonから目標位置へのコマンドを送ることを考える．

公式のMoveIt!のチュートリアルにある'Move Group Python Interface'のページのプログラムどおりです．

公式のサンプルはこちら．move_group_python_interface_tutorial.pyをダウンロードしてちょっと修正して実行してもOK.

group_name = "panda_arm" を"sixdofarm"に変更すれば良い．

#!/usr/bin/env python3

# coding: UTF-8

import sys

import geometry_msgs.msg

import moveit_commander

import rospy

import tf

from geometry_msgs.msg import Quaternion, Vector3

def main():

    # MoveitCommanderの初期化

    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの生成

    rospy.init_node("pose_planner")

    # MoveGroupの設定

    move_group = moveit_commander.MoveGroupCommander("single") #Moveit!SetupAssistantで「single」でグループを作ったので"single"と記載

    # ゴール位置の指定

    pose_goal = geometry_msgs.msg.Pose()

    pose_goal.position = Vector3(0.1, -0.1, 0.3) #手先の届く範囲に設定しておかないとタイムアウトエラーとなるので手先の届く範囲に指定

    q = tf.transformations.quaternion_from_euler(0, 0, 0)

    pose_goal.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])

    move_group.set_pose_target(pose_goal)

    # モーションプランの計画と実行

    move_group.go(wait=True)

    # 後処理

    move_group.stop()

    move_group.clear_pose_targets()

if __name__ == "__main__":

    main()

最初にジョイントで目的の姿勢を決めてから手先のポーズを指定するプログラムの例は下記

#!/usr/bin/env python3

# coding: UTF-8

import sys

import geometry_msgs.msg

import moveit_commander

import rospy

import tf

from geometry_msgs.msg import Quaternion, Vector3

from math import pi

from tf import TransformListener

def main(): 

    # MoveitCommanderの初期化

    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの生成

    rospy.init_node("planner")

    listener = TransformListener()

    # MoveGroupCommanderの準備

    move_group = moveit_commander.MoveGroupCommander("single")

    # 関節の角度でゴール状態を指定

    # joint_goal = [0.0, 30.0 * pi / 180.0, 30.0 * pi / 180.0, 0.0, 30.0 * pi / 180.0, 0.0]

    joint_goal = [0.0, -45.0 * pi / 180.0, 0, 45.0 * pi / 180.0, 90.0 * pi / 180.0,0]

    move_group.set_joint_value_target(joint_goal)

    # モーションプランの計画と実行

    move_group.go(wait=True)

    listener.waitForTransform('/base', '/link7', rospy.Time(), rospy.Duration(4.0))

    (trans, rot) = listener.lookupTransform('/base', '/link7', rospy.Time(0))

    print("Translation: ", trans)

    print("Rotation: ", rot)

    # エンドポイントの姿勢でゴール状態を指定

    pose_goal = geometry_msgs.msg.Pose()

    pose_goal.position = Vector3(trans[0], trans[1], trans[2]-0.05)

    # q = tf.transformations.quaternion_from_euler(0, 0, 0)

    # pose_goal.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])

    pose_goal.orientation = Quaternion(x=rot[0],y=rot[1],z=rot[2],w=rot[3])

    move_group.set_pose_target(pose_goal)

    # モーションプランの計画と実行

    move_group.go(wait=True)

    # 2回目の移動

    # 現在位置と姿勢を取得

    listener.waitForTransform('/base', '/link7', rospy.Time(), rospy.Duration(4.0))

    (trans, rot) = listener.lookupTransform('/base', '/link7', rospy.Time(0))

    pose_goal = geometry_msgs.msg.Pose()

    pose_goal.position = Vector3(trans[0], trans[1]+0.05, trans[2])

    pose_goal.orientation = Quaternion(x=rot[0],y=rot[1],z=rot[2],w=rot[3])

    move_group.set_pose_target(pose_goal)

    move_group.go(wait=True)

    # 3回目の移動 2回目で取得した位置を基準に移動

    pose_goal = geometry_msgs.msg.Pose()

    pose_goal.position = Vector3(trans[0], trans[1]-0.05, trans[2])

    pose_goal.orientation = Quaternion(x=rot[0],y=rot[1],z=rot[2],w=rot[3])

    move_group.set_pose_target(pose_goal)

    move_group.go(wait=True)

    # 後処理

    move_group.stop()

    move_group.clear_pose_targets()

if __name__ == "__main__":

    main()