MoveItの起動プランをロボットに送る前に確認する方法 Visualize MoveIt! planned trajectory before sending to a robot

著者:Kei Okada

MoveItの起動プランをロボットに送る前に確認する方法 Visualize MoveIt! planned trajectory before sending to a robot

今回もまたMoveIt関連のお話です(MoveIt関連の以前の投稿は([1]，[2],[3])．

実機につなげて実験している方から，moveit_commanderを利用していて計画した軌道を，ロボットに送る前にrvizで確認したい，という要望を時々尋ねられることがあります．実はこれ，ものすごく簡単にできるんです．

http://wiki.ros.org/rtmros_nextage/Tutorials/Programming_Hiro_NEXTAGE_OPEN_MOVEITで紹介しているnextage_moveit_sample.pyのプログラムで，

rarm.go()

の代わりに

import moveit_msgs.msg
plan = rarm.plan()
print("visualize plan")
display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher = rospy.Publisher(
    '/move_group/display_planned_path',
   moveit_msgs.msg.DisplayTrajectory, queue_size=10)
   display_trajectory_publisher.publish(display_trajectory)
raw_input("wait for display...")

として，rvizのDisplayタブから

のようにmoveit_ros_visualization->Trajectory を追加し，
Loop AnimationとShow Trailにチェックすると以下のようにアニメーション表示することができます．

これで，安心して

rarm.execute(plan)

を実行できますね！


Today, we’ll introduce another tips for using MoveIt! (Out previous blog about MoveIt can be found at [1]，[2],[3])．

We have been asked several times about a way to visualize a planned trajectory path using moveit_commander before sending to a real robot, specially those who using the system while connecting to the real robot.
And, of course, MoveIt! can do this very easily!

Looking into the sample program nextage_moveit_sample.py at
http://wiki.ros.org/rtmros_nextage/Tutorials/Programming_Hiro_NEXTAGE_OPEN_MOVEIT wiki page. First replace

rarm.go()

with

import moveit_msgs.msg
plan = rarm.plan()
print("visualize plan")
display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher = rospy.Publisher(
    '/move_group/display_planned_path',
    moveit_msgs.msg.DisplayTrajectory, queue_size=10)
    display_trajectory_publisher.publish(display_trajectory)
raw_input("wait for display...")

The,
Add moveit_ros_visualization->Trajectory in the Display tab in rviz as

and add check to the Loop Animation and Show Trail displays animation as follows.

Now, you can run

rarm.execute(plan)

command without any worries.