ROS入門 (57) - Python MoveItインタフェース

「Python MoveItインタフェース」についてまとめました。

・Noetic

前回

1. Python MoveItインターフェース

「Python MoveItインターフェース」を利用するには、次の3つのクラスを提供します。

◎ RobotCommander
プランニンググループ（関節のグループ）へのインターフェースを提供します。

robot = moveit_commander.RobotCommander()

◎ MoveGroupCommander
運動モデルやロボットの現在の関節状態などの情報を提供します。

move_group = moveit_commander.MoveGroupCommander("panda_arm")

◎ PlanningSceneInterface
ロボットの周囲の世界に対する内部理解を取得、設定、および更新するためのインターフェイスを提供します。

scene = moveit_commander.PlanningSceneInterface()

「DisplayTrajectory」をパブリッシュすることで、rvizでモーションプラン(軌道)を表示することができます。

display_trajectory_publisher = rospy.Publisher(
    "/move_group/display_planned_path",
    moveit_msgs.msg.DisplayTrajectory,
    queue_size=20,
)

2. ロボット情報の取得

ロボット情報を取得するコードは次のとおりです。

# ロボットの参照フレーム名の取得
planning_frame = move_group.get_planning_frame()
print("============ Planning frame: %s" % planning_frame)

# ロボットのエンドエフェクタ名の取得
eef_link = move_group.get_end_effector_link()
print("============ End effector link: %s" % eef_link)

# ロボットのグループ名群の取得
group_names = robot.get_group_names()
print("============ Available Planning Groups:", robot.get_group_names())

# ロボットの現在の状態の取得
print("============ Printing robot state")
print(robot.get_current_state())
print("")

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

関節角度によるモーションプランニングを行うには、以下のメソッドを使います。

・move_group.go(jointState, wait=True) : モーションプランの計画と実行。
・move_group.set_joint_value_target() : グループに関節角度の目標を指定。

# 関節ゴール
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -tau / 8
joint_goal[2] = 0
joint_goal[3] = -tau / 4
joint_goal[4] = 0
joint_goal[5] = tau / 6
joint_goal[6] = 0

# 関節ゴールによるモーションプランニングの実行
move_group.go(joint_goal, wait=True)

# 動きが残っていないことを保証
move_group.stop()

4. エンドエフェクタの姿勢によるプランニング

エンドエフェクタの姿勢によるモーションプランニングを行うには、以下のメソッドを使います。

・move_group.go(pose, wait=True) : モーションプランの計画と実行。
・move_group.set_pose_target() : グループに関節角度の目標を指定。
・move_group.set_orientation_target() : グループに関節角度の目標を指定。
・move_group.set_rpy_target() : グループに関節角度の目標を指定。
・move_group.set_position_target() : グループに関節角度の目標を指定。

# 姿勢ゴール
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.4
pose_goal.position.y = 0.1
pose_goal.position.z = 0.4

# 姿勢ゴールの指定
move_group.set_pose_target(pose_goal)

# 姿勢ゴールによるモーションプランニングの実行
plan = move_group.go(wait=True)

# 動きが残っていないことを保証
move_group.stop()

# 姿勢によるプランニングの後にターゲットをクリアするのが望ましい(関節のクリアはない)
move_group.clear_pose_targets()

5. デカルトパス

エンドエフェクタが通過するウェイポイントのリストを指定することにより、デカルトパスを直接プランニングできます。 Pythonシェルでインタラクティブに実行する場合は、scale = 1.0を指定します。

# ウェイポイント群
waypoints = []

# ウェイポイント1の追加
wpose = move_group.get_current_pose().pose
wpose.position.z -= scale * 0.1 
wpose.position.y += scale * 0.2 
waypoints.append(copy.deepcopy(wpose))

# ウェイポイント２の追加
wpose.position.x += scale * 0.1  
waypoints.append(copy.deepcopy(wpose))

# ウェイポイント３の追加
wpose.position.y -= scale * 0.1 
waypoints.append(copy.deepcopy(wpose))

# ウェイポイントの計算
(plan, fraction) = move_group.compute_cartesian_path(
    waypoints, 0.01, 0.0  # waypoints, eef_step, jump_threshold
)
return plan, fraction

6. モーションプランの表示

「DisplayTrajectoryメッセージ」をパブリッシュすることで、rvizでモーションプラン(軌道)を表示できます。group.plan()利用時には、自動的に実行されます。

「DisplayTrajectoryメッセージ」のフィールドは、次のとおりです。

・trajectory_start : 現在の関節状態。
・trajectory : モーションプラン(軌道)

# DisplayTrajectoryメッセージの準備
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)

# パブリッシュ
display_trajectory_publisher.publish(display_trajectory)

7. モーションプランの実行

ロボットで計画したモーションプランを実行したい場合は、execute()を使います。

move_group.execute(plan, wait=True)

ロボットの現在の関節状態は、RobotTrajectoryの最初のウェイポイントの許容範囲内にある必要があります。そうでない場合、execute()は失敗します。

8. プランニングシーンへのオブジェクトの追加

プランニングシーンにボックスを追加するコードは、次のとおりです。

box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_hand"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.z = 0.11  # panda_handフレームの上
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.075, 0.075, 0.075))

9. 衝突判定の更新

衝突判定を持つオブジェクトの更新メッセージを公開する前にPythonノードが停止するとメッセージが失われるため、ボックスが表示されない可能性があります。get_attached_objects()とget_known_object_names()で更新が反映されるまで待つと良いでしょう。

start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
    # ボックスがアタッチされたオブジェクトにあるかどうかをテスト
    attached_objects = scene.get_attached_objects([box_name])
    is_attached = len(attached_objects.keys()) > 0

    # ボックスがシーン内にあるかどうかをテスト
    # ボックスを取り付けると、known_objectsからボックスが削除されることに注意
    is_known = box_name in scene.get_known_object_names()

    # 期待される状態にあるかどうかをテスト
    if (box_is_attached == is_attached) and (box_is_known == is_known):
        return True

    # プロセッサで他のスレッドに時間を与えるためにスリープ
    rospy.sleep(0.1)
    seconds = rospy.get_time()

# 戻らずにwhileループを終了すると、タイムアウトになる
return False

10. ロボットへのオブジェクトのアタッチ

pandaの手首にボックスをアタッチします。 オブジェクトを操作するには、プランニングシーンが接触を衝突として報告することなく、ロボットがオブジェクトに触れることができる必要があります。「touch_links」配列にリンク名を追加することで、これらのリンクとボックスの間の衝突を無視するようにプランニングシーンに指示します。pandaの場合、「grasping_group = 'hand'」を設定します。 別のロボットを使用している場合は、この値をエンドエフェクタグループ名に変更する必要があります。

grasping_group = "hand"
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)

11. ロボットからのオブジェクトのデタッチ

プランニングシーンからオブジェクトをデタッチすることができます。

scene.remove_attached_object(eef_link, name=box_name)

12. プランニングシーンからのオブジェクトの削除

プランニングシーンからオブジェクトを削除することができます。削除する前に、オブジェクトをデタッチしておく必要があります。

scene.remove_world_object(box_name)

関連

参考

Move Group Python Interface — moveit_tutorials Noetic documentation

moveit/move_group.py at master · ros-planning/moveit

C++用ユーザライブラリ（MoveGroupInterface）の仕様 — MoveIt! Documentation

次回

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門