Move Group Python Interface

../../_images/move_group_python_interface.png

One of the simplest MoveIt! user interfaces is through the Python-based Move Group Interface. These wrappers provide functionality for most operations that the average user will likely need, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot.

Watch this quick YouTube video demo to see the power of the Move Group Python interface!

Getting Started

If you haven’t already done so, make sure you’ve completed the steps in Getting Started.

Start RViz and MoveGroup node

Open two shells. Run this command in the first shell and wait for everything to finish loading:

roslaunch panda_moveit_config demo.launch

Now run the Python code directly in the other shell using rosrun. Note in some instances you may need to make the python script executable:

rosrun moveit_tutorials move_group_python_interface_tutorial.py

Expected Output

In RViz, we should be able to see the following:

Press <enter> in the shell terminal where you ran the rosrun command in between each step
  1. The robot plans and moves its arm to the joint goal.
  2. The robot plans a path to a pose goal.
  3. The robot plans a Cartesian path.
  4. The robot displays the Cartesian path plan again.
  5. The robot executes the Cartesian path plan.
  6. A box appears at the location of the Panda end effector.
  7. The box changes colors to indicate that it is now attached.
  8. The robot plans and executes a Cartesian path with the box attached.
  9. The box changes colors again to indicate that it is now detached.
  10. The box disappears.

The Entire Code

Note: the entire code can be seen here in the tutorials GitHub repository.

To use the Python MoveIt! interfaces, we will import the moveit_commander namespace. This namespace provides us with a MoveGroupCommander class, a PlanningSceneInterface class, and a RobotCommander class. (More on these below)

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
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

First initialize moveit_commander and a rospy node:

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
                anonymous=True)

Instantiate a RobotCommander object. This object is the outer-level interface to the robot:

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 Panda arm so we set group_name = panda_arm. If you are using a different robot, you should change this value to the name of your robot arm planning group. This interface can be used to plan and execute motions on the Panda:

group_name = "panda_arm"
group = moveit_commander.MoveGroupCommander(group_name)

We create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize:

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

Getting Basic Information

# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame

# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link

# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", 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 Joint Goal

The Panda’s zero configuration is at a singularity so the first thing we want to do is move it to a slightly better configuration.

# We can get the joint values from the group and adjust some of the values:
joint_goal = group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = pi/3
joint_goal[6] = 0

# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
group.go(joint_goal, wait=True)

# Calling ``stop()`` ensures that there is no residual movement
group.stop()

Planning to a Pose Goal

We can plan a motion for this group to a desired pose for the end-effector:

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
group.set_pose_target(pose_goal)

Now, we call the planner to compute the plan and execute it.

plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()

Cartesian Paths

You can plan a Cartesian path directly by specifying a list of waypoints for the end-effector to go through:

waypoints = []

wpose = group.get_current_pose().pose
wpose.position.z -= scale * 0.1  # First move up (z)
wpose.position.y += scale * 0.2  # and sideways (y)
waypoints.append(copy.deepcopy(wpose))

wpose.position.x += scale * 0.1  # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))

wpose.position.y -= scale * 0.1  # Third move sideways (y)
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 disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold

# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction

Displaying a Trajectory

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):

A DisplayTrajectory msg has two primary fields, trajectory_start and trajectory. We populate the trajectory_start with our current robot state to copy over any AttachedCollisionObjects and add our plan to the trajectory.

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory);

Executing a Plan

Use execute if you would like the robot to follow the plan that has already been computed:

group.execute(plan, wait=True)

Note: The robot’s current joint state must be within some tolerance of the first waypoint in the RobotTrajectory or execute() will fail

Adding Objects to the Planning Scene

First, we will create a box in the planning scene at the location of the left finger:

box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

Ensuring Collision Updates Are Receieved

If the Python node dies before publishing a collision object update message, the message could get lost and the box will not appear. To ensure that the updates are made, we wait until we see the changes reflected in the get_known_object_names() and get_known_object_names() lists. For the purpose of this tutorial, we call this function after adding, removing, attaching or detaching an object in the planning scene. We then wait until the updates have been made or timeout seconds have passed

start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
  # Test if the box is in attached objects
  attached_objects = scene.get_attached_objects([box_name])
  is_attached = len(attached_objects.keys()) > 0

  # Test if the box is in the scene.
  # Note that attaching the box will remove it from known_objects
  is_known = box_name in scene.get_known_object_names()

  # Test if we are in the expected state
  if (box_is_attached == is_attached) and (box_is_known == is_known):
    return True

  # Sleep so that we give other threads time on the processor
  rospy.sleep(0.1)
  seconds = rospy.get_time()

# If we exited the while loop without returning then we timed out
return False

Attaching Objects to the Robot

Next, we will attach the box to the Panda wrist. Manipulating objects requires the robot be able to touch them without the planning scene reporting the contact as a collision. By adding link names to the touch_links array, we are telling the planning scene to ignore collisions between those links and the box. For the Panda robot, we set grasping_group = 'hand'. If you are using a different robot, you should change this value to the name of your end effector group name.

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

Detaching Objects from the Robot

We can also detach and remove the object from the planning scene:

scene.remove_attached_object(eef_link, name=box_name)

Removing Objects from the Planning Scene

We can remove the box from the world.

scene.remove_world_object(box_name)

Note: The object must be detached before we can remove it from the world

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.

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page