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.

Setup

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 "============ End effector: %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.

group.clear_pose_targets()

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[0] = 1.0
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()

print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)

Cartesian Paths

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[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].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.

moveit_commander.roscpp_shutdown()

The entire code

The entire code can be seen here in the MoveIt! Github project.

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

Expected Output

In Rviz, we should be able to see the following (there will be a delay of 5-10 seconds between each step):

  1. The robot moves its left arm to the pose goal in front of it (plan1)
  2. The robot again moves its left arm to the same goal (plan1 again)
  3. The robot moves its left arm to the joint goal to the side,
  4. 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