Code Used in this Tutorial Available
Code can be found at moveit_tutorials repository in doc folder. Use kinetic-devel branch.
Move Group Python Interface Tutorial¶
In MoveIt!, the primary user interface is through the RobotCommander class. It provides functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot.
To use the python interface to move_group, import the moveit_commander module. We also import rospy and some messages that we will use.
import sys import copy import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg
First initialize moveit_commander and rospy.
print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
Instantiate a RobotCommander object. This object is an interface to the robot as a whole.
robot = moveit_commander.RobotCommander()
Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the left arm. This interface can be used to plan and execute motions on the left arm.
group = moveit_commander.MoveGroupCommander("left_arm")
We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..." rospy.sleep(10) print "============ Starting tutorial "
Getting Basic Information¶
We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
We can get a list of all the groups in the robot
print "============ Robot Groups:" print robot.get_group_names()
Sometimes for debugging it is useful to print the entire state of the robot.
print "============ Printing robot state" print robot.get_current_state() print "============"
Planning to a Pose goal¶
We can plan a motion for this group to a desired pose for the end-effector
print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target)
Now, we call the planner to compute the plan and visualize it if successful Note that we are just planning, not asking move_group to actually move the robot
plan1 = group.plan() print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5)
You can ask RVIZ to visualize a plan (aka trajectory) for you. But the group.plan() method does this automatically so this is not that useful here (it just displays the same trajectory again).
print "============ Visualizing plan1" display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5)
Moving to a pose goal¶
Moving to a pose goal is similar to the step above except we now use the go() function. Note that the pose goal we had set earlier is still active and so the robot will try to move to that goal. We will not use that function in this tutorial since it is a blocking function and requires a controller to be active and report success on execution of a trajectory.
# Uncomment below line when working with a real robot # group.go(wait=True) # Use execute instead if you would like the robot to follow # the plan that has already been computed # group.execute(plan1)
Planning to a joint-space goal¶
Let’s set a joint space goal and move towards it. First, we will clear the pose target we had just set.
Then, we will get the current set of joint values for the group
group_variable_values = group.get_current_joint_values() print "============ Joint values: ", group_variable_values
Now, let’s modify one of the joints, plan to the new joint space goal and visualize the plan
group_variable_values = 1.0 group.set_joint_value_target(group_variable_values) plan2 = group.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(5)
You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through.
waypoints =  # start with the current pose waypoints.append(group.get_current_pose().pose) # first orient gripper and move forward (+x) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints.position.x + 0.1 wpose.position.y = waypoints.position.y wpose.position.z = waypoints.position.z waypoints.append(copy.deepcopy(wpose)) # second move down wpose.position.z -= 0.10 waypoints.append(copy.deepcopy(wpose)) # third move to the side wpose.position.y += 0.05 waypoints.append(copy.deepcopy(wpose))
We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the eef_step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it.
(plan3, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(5) # Uncomment the line below to execute this plan on a real robot. # group.execute(plan3)
Adding/Removing Objects and Attaching/Detaching Objects¶
First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
When finished shut down moveit_commander.
The launch file¶
The entire launch file is here on github. All the code in this tutorial can be run from the moveit_tutorials package that you have as part of your MoveIt! setup.
Running the code¶
Make sure your python file is executable:
roscd moveit_tutorials/doc/pr2_tutorials/planning/scripts/ chmod +x move_group_python_interface_tutorial.py
Launch the moveit! demo interface:
roslaunch pr2_moveit_config demo.launch
Now run the python code directly using rosrun:
rosrun moveit_tutorials move_group_python_interface_tutorial.py
In Rviz, we should be able to see the following (there will be a delay of 5-10 seconds between each step):
- The robot moves its left arm to the pose goal in front of it (plan1)
- The robot again moves its left arm to the same goal (plan1 again)
- The robot moves its left arm to the joint goal to the side,
- The robot moves its left arm along the desired cartesian path.
Open Source Feedback
See something that needs improvement? Please open a pull request on this GitHub page