ROS入門 (37) - MoveItのPythonインターフェイスの利用

npaka

2021年12月9日 22:36

「MoveIt」のPythonインターフェイスの利用方法をまとめました。

前回

1. 開発環境の準備

「ROS入門 (35) - MoveItのセットアップ」と同様です。

2. rvizとMoveItの起動

ロボットアームの動作確認のため、rvizとMoveItを起動します。

$ roslaunch panda_moveit_config demo.launch

3. ロボット情報の出力

ロボット情報を出力を行う手順は、次のとおりです。

(1) 「~/catkin_ws/src/moveit_tutorials/src/robot_info.py」を作成し、以下のように編集し、実行権限を付加(chmod u+x <ファイル名>)。

・robot_info.py

#!/usr/bin/env python
# coding: UTF-8
import sys
import moveit_commander
import rospy

# メイン
def main():
    # MoveitCommanderの初期化
    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの初期化
    rospy.init_node('robot_info')

    # RobotCommanderのインスタンス化
    robot = moveit_commander.RobotCommander()

    # ロボット情報
    print "==Robot Info=="
    print "[ group_names ]", robot.get_group_names()
    print "[ current_state ] ", robot.get_current_state()
    print ""

    # MoveGroupCommanderのインスタンス化
    move_group = moveit_commander.MoveGroupCommander("panda_arm")

    # グループ情報
    print "==Group Info=="
    print "[ name ] ", move_group.get_name()
    print "[ planning_frame ] ", move_group.get_planning_frame()
    print "[ interface_description ] ", move_group.get_interface_description()
    print ""

    # ジョイント情報
    print "==Joint Info=="
    print "[ active_joints ] ", move_group.get_active_joints()
    print "[ joints ] ", move_group.get_joints()
    print "[ current_joint_values ] ", move_group.get_current_joint_values()
    print ""

    # エンドエフェクタ情報
    print "==EndEffector Info=="
    print "[ has_end_effector_link ] ", move_group.has_end_effector_link()
    print "[ end_effector_link ] ", move_group.get_end_effector_link()
    print "[ current_pose ] ", move_group.get_current_pose()
    print "[ current_rpy ] ", move_group.get_current_rpy()
    print ""


if __name__ == "__main__":
    main()

「MoveitCommander」には、以下の2つのインタフェースがあります。

「RobotCommander」の主な情報取得メソッドは、次のとおりです。

「MoveGroupCommander」の主な情報取得メソッドは、次のとおりです。

(2) 実行。

$ rosrun moveit_tutorials robot_info.py

[ INFO] [1640325462.543879400]: Loading robot model 'panda'...
==Robot Info==
[ group_names ] ['hand', 'panda_arm', 'panda_arm_hand']
[ WARN] [1640325462.860356000]: Unable to update multi-DOF joint 'virtual_joint': Failure to lookup transform between 'world' and 'panda_link0' with TF exception: "world" passed to lookupTransform argument target_frame does not exist. 
[ current_state ]  joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "world"
  name: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6,
  panda_joint7, panda_finger_joint1, panda_finger_joint2]
  position: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785, 0.035, 0.035]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "world"
  joint_names: [virtual_joint]
  transforms: 
    - 
      translation: 
        x: 0.0
        y: 0.0
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False

[ INFO] [1640325463.960647000]: Ready to take commands for planning group panda_arm.
==Group Info==
[ name ]  panda_arm
[ planning_frame ]  world
[ interface_description ]  name: "OMPL"
planner_ids: [hand, 'hand[BFMTkConfigDefault]', 'hand[BKPIECEkConfigDefault]', 'hand[BiESTkConfigDefault]',
  'hand[BiTRRTkConfigDefault]', 'hand[ESTkConfigDefault]', 'hand[FMTkConfigDefault]',
  'hand[KPIECEkConfigDefault]', 'hand[LBKPIECEkConfigDefault]', 'hand[LBTRRTkConfigDefault]',
  'hand[LazyPRMkConfigDefault]', 'hand[LazyPRMstarkConfigDefault]', 'hand[PDSTkConfigDefault]',
  'hand[PRMkConfigDefault]', 'hand[PRMstarkConfigDefault]', 'hand[ProjESTkConfigDefault]',
  'hand[RRTConnectkConfigDefault]', 'hand[RRTkConfigDefault]', 'hand[RRTstarkConfigDefault]',
  'hand[SBLkConfigDefault]', 'hand[SPARSkConfigDefault]', 'hand[SPARStwokConfigDefault]',
  'hand[STRIDEkConfigDefault]', 'hand[TRRTkConfigDefault]', 'hand[TrajOptDefault]',
  panda_arm, 'panda_arm[BFMTkConfigDefault]', 'panda_arm[BKPIECEkConfigDefault]',
  'panda_arm[BiESTkConfigDefault]', 'panda_arm[BiTRRTkConfigDefault]', 'panda_arm[ESTkConfigDefault]',
  'panda_arm[FMTkConfigDefault]', 'panda_arm[KPIECEkConfigDefault]', 'panda_arm[LBKPIECEkConfigDefault]',
  'panda_arm[LBTRRTkConfigDefault]', 'panda_arm[LazyPRMkConfigDefault]', 'panda_arm[LazyPRMstarkConfigDefault]',
  'panda_arm[PDSTkConfigDefault]', 'panda_arm[PRMkConfigDefault]', 'panda_arm[PRMstarkConfigDefault]',
  'panda_arm[ProjESTkConfigDefault]', 'panda_arm[RRTConnectkConfigDefault]', 'panda_arm[RRTkConfigDefault]',
  'panda_arm[RRTstarkConfigDefault]', 'panda_arm[SBLkConfigDefault]', 'panda_arm[SPARSkConfigDefault]',
  'panda_arm[SPARStwokConfigDefault]', 'panda_arm[STRIDEkConfigDefault]', 'panda_arm[TRRTkConfigDefault]',
  'panda_arm[TrajOptDefault]', panda_arm_hand, 'panda_arm_hand[BFMTkConfigDefault]',
  'panda_arm_hand[BKPIECEkConfigDefault]', 'panda_arm_hand[BiESTkConfigDefault]',
  'panda_arm_hand[BiTRRTkConfigDefault]', 'panda_arm_hand[ESTkConfigDefault]', 'panda_arm_hand[FMTkConfigDefault]',
  'panda_arm_hand[KPIECEkConfigDefault]', 'panda_arm_hand[LBKPIECEkConfigDefault]',
  'panda_arm_hand[LBTRRTkConfigDefault]', 'panda_arm_hand[LazyPRMkConfigDefault]',
  'panda_arm_hand[LazyPRMstarkConfigDefault]', 'panda_arm_hand[PDSTkConfigDefault]',
  'panda_arm_hand[PRMkConfigDefault]', 'panda_arm_hand[PRMstarkConfigDefault]', 'panda_arm_hand[ProjESTkConfigDefault]',
  'panda_arm_hand[RRTConnectkConfigDefault]', 'panda_arm_hand[RRTkConfigDefault]',
  'panda_arm_hand[RRTstarkConfigDefault]', 'panda_arm_hand[SBLkConfigDefault]', 'panda_arm_hand[SPARSkConfigDefault]',
  'panda_arm_hand[SPARStwokConfigDefault]', 'panda_arm_hand[STRIDEkConfigDefault]',
  'panda_arm_hand[TRRTkConfigDefault]', 'panda_arm_hand[TrajOptDefault]']

==Joint Info==
[ active_joints ]  ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7']
[ joints ]  ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_joint8']
[ current_joint_values ]  [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]

==EndEffector Info==
[ has_end_effector_link ]  True
[ end_effector_link ]  panda_link8
[ current_pose ]  header: 
  seq: 0
  stamp: 
    secs: 1640325464
    nsecs: 125233888
  frame_id: "world"
pose: 
  position: 
    x: 0.307019570052
    y: -5.22132961561e-12
    z: 0.590269558277
  orientation: 
    x: 0.923955699469
    y: -0.382499497279
    z: 1.3249325839e-12
    w: 3.20041176635e-12
[ current_rpy ]  [3.1415926535848926, -4.896669808048392e-12, -0.785000000006922]

4. 関節の角度によるモーションプランニング

関節の角度によるモーションプランニングの手順は、次のとおりです。

(1) 「~/catkin_ws/src/moveit_tutorials/src/joint_planner.py」を作成し、以下のように編集し、実行権限を付加(chmod u+x <ファイル名>)。

・joint_planner.py

#!/usr/bin/env python
# coding: UTF-8
import sys
from math import pi

import moveit_commander
import rospy


def main():
    # MoveitCommanderの初期化
    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの生成
    rospy.init_node("joint_planner")

    # MoveGroupCommanderの準備
    move_group = moveit_commander.MoveGroupCommander("panda_arm")

    # 関節の角度でゴール状態を指定
    joint_goal = [0, -20 * pi / 180, 0, -30 * pi / 180, 0, 160 * pi / 180, 0]
    move_group.set_joint_value_target(joint_goal)

    # モーションプランの計画と実行
    move_group.go(wait=True)

    # 後処理
    move_group.stop()


if __name__ == "__main__":
    main()

関節の角度によるモーションプランニングで使う主なメソッドは、次のとおりです。

(2) 実行。

$ rosrun moveit_tutorials joint_planner.py

5. エンドエフェクタの姿勢によるモーションプランニング

エンドエフェクタの姿勢によるモーションプランニングの手順は、次のとおりです。

(1) 「~/catkin_ws/src/moveit_tutorials/src/pose_planner.py」を作成し、以下のように編集し、実行権限を付加(chmod u+x <ファイル名>)。

・pose_planner.py

#!/usr/bin/env python
# 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")

    # MoveGroupCommanderの準備
    move_group = moveit_commander.MoveGroupCommander("panda_arm")

    # エンドポイントの姿勢でゴール状態を指定
    pose_goal = geometry_msgs.msg.Pose()
    pose_goal.position = Vector3(0.7, 0.0, 0.7)
    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()

エンドエフェクタの姿勢によるモーションプランニングで使う主なメソッドは、次のとおりです。

(2) 実行。

$ rosrun moveit_tutorials pose_planner.py

6. モーションプランが見つからない場合の対処

モーションプランが見つからないと、以下のメッセージが表示されます。

ABORTED: No motion plan found. No execution attempted.

主な原因と対処方法は、次のとおりです。

◎ ゴールに到達可能だが、ゴール近くの関節設定を見つけることができなかった。
(1) 到達可能なゴールであるかを確認。
(2) ゴールの許容誤差をあまくする。
(3) 異なるIKまたはパラメータを使用。

◎ ゴールまでのパスを見つけることができなかった。
(1) 移動可能なパスが存在するかを確認。
(2) 別のプランナーを試す。
(3) プランニング時間を増やす。

この対応で使う主なメソッドは、次のとおりです。

◎ 近似解の指定
・move_group.set_joint_value_target(arg1, arg2, arg3) : arg2以降にTrueで近似解を許可。

◎ ゴール状態の許容誤差の指定
・move_gorup.set_goal_tolerance(value) : 関節の角度と、エンドエフェクタの位置と向きの許容誤差の指定。
・move_group.set_goal_joint_tolerance(value) : 関節の角度の許容誤差の指定。
・move_group.set_goal_position_tolerance(value) : エンドエフェクタの位置の許容誤差の指定。
・move_group.set_goal_orientation_tolerance(value) : エンドエフェクタの向きの許容誤差の指定。

◎ プランナーIDの指定
・move_group.set_planner_id(planner_id) : プランナーIDの指定。(RRTConnectkConfigDefaultなど)
・move_gruop.get_planner_id() : プランナーIDの取得。

◎ プランニング時間の指定
・move_group.set_planning_time(sec) : プランニング時間の指定。
・move_group.get_planning_time() : プランニング時間の取得。

◎ ROSパラメータの指定
Unity Robotics Hubのサンプルで使われている設定になります。

<param name="/move_group/trajectory_execution/execution_duration_monitoring" value="false" />
<param name="/move_group/trajectory_execution/allowed_start_tolerance" value="0.0" />
<param name="/move_group/planning_pipelines/ompl/start_state_max_bounds_error" value="0.3" />

7. 参考

次回

この記事が気に入ったらサポートをしてみませんか？