Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

A Simple Pick And Place Example Using The Pick And Place Manager

This tutorial uses the pick_and_place_manager, which provides high-level functionality for scripting simple pick-and-place tasks in Python. If you are not concerned with customizing all the nitty-gritty details of doing basic pick-and-place tasks, you may find the helper functions in the pick_and_place_manager useful.

If you wish to understand/modify what's going on at a lower level, or wish to see examples in C++, see the following tutorial, which directly uses the grasp and place services offered by the manipulation pipeline: Writing a Simple Pick and Place Application and also the example code described here: Pick and Place Keyboard Interface

Robot Setup

Bring up the robot and position it at the edge of a table, facing the table. When manipulating objects on tables, the workspace of the arms is generally increased by raising the robot torso all the way up. Place an object on the table, within reach of the robot.

Starting the Manipulation Pipeline

Start the manipulation pipeline (see the relevant tutorial here)

The Code

The following code is also available in pr2_pick_and_place_demos/test/simple_pick_and_place_example.py (this code is for Electric; look at the code in pr2_pick_and_place_demos for earlier versions):

   1 import roslib
   2 roslib.load_manifest('pr2_pick_and_place_demos')
   3 import rospy
   4 from pr2_pick_and_place_demos.pick_and_place_manager import *
   5 from object_manipulator.convert_functions import *
   6 
   7 class SimplePickAndPlaceExample():
   8 
   9     def __init__(self):
  10 
  11         rospy.loginfo("initializing pick and place manager")
  12         self.papm = PickAndPlaceManager()
  13         rospy.loginfo("finished initializing pick and place manager")
  14 
  15 
  16     #pick up the nearest object to PointStamped target_point with whicharm 
  17     #(0=right, 1=left)
  18     def pick_up_object_near_point(self, target_point, whicharm):
  19         
  20         rospy.loginfo("moving the arms to the side")
  21         self.papm.move_arm_to_side(0)  #right arm
  22         self.papm.move_arm_to_side(1)  #left arm
  23 
  24         rospy.loginfo("pointing the head at the target point")
  25         self.papm.point_head(get_xyz(target_point.point),
  26                              target_point.header.frame_id)
  27 
  28         rospy.loginfo("detecting the table and objects")
  29         self.papm.call_tabletop_detection(update_table = 1, 
  30                              update_place_rectangle = 1, 
  31                              clear_attached_objects = 1)     
  32 
  33         rospy.loginfo("picking up the nearest object to the target point")
  34         success = self.papm.pick_up_object_near_point(target_point, 
  35                                                       whicharm)
  36         
  37         if success:
  38             rospy.loginfo("pick-up was successful!  Moving arm to side")
  39             self.papm.move_arm_to_side(whicharm)
  40         else:
  41             rospy.loginfo("pick-up failed.")
  42 
  43         return success
  44 
  45     
  46     #place the object held in whicharm (0=right, 1=left) down in the 
  47     #place rectangle defined by place_rect_dims (x,y) 
  48     #and place_rect_center (PoseStamped)
  49     def place_object(self, whicharm, place_rect_dims, place_rect_center):
  50 
  51         self.papm.set_place_area(place_rect_center, place_rect_dims)
  52         
  53         rospy.loginfo("putting down the object in the %s gripper"\
  54                       %self.papm.arm_dict[whicharm])
  55         success = self.papm.put_down_object(whicharm, 
  56                       max_place_tries = 25,
  57                       use_place_override = 1)
  58 
  59         if success:
  60             rospy.loginfo("place returned success")
  61         else:
  62             rospy.loginfo("place returned failure")
  63 
  64         return success
  65 
  66 
  67 if __name__ == "__main__":
  68     rospy.init_node('simple_pick_and_place_example')
  69     sppe = SimplePickAndPlaceExample()
  70 
  71     #adjust for your table 
  72     table_height = .72                                          
  73 
  74     #.5 m in front of robot, centered
  75     target_point_xyz = [.5, 0, table_height-.05]                
  76     target_point = create_point_stamped(target_point_xyz, 'base_link')
  77     success = sppe.pick_up_object_near_point(target_point, 0)   #right arm
  78 
  79     if success:
  80 
  81         #square of size 30 cm by 30 cm
  82         place_rect_dims = [.3, .3]                              
  83 
  84         #.5 m in front of robot, to the right
  85         center_xyz = [.5, -.15, table_height-.05]               
  86 
  87         #aligned with axes of frame_id
  88         center_quat = [0,0,0,1]                                 
  89         place_rect_center = create_pose_stamped(center_xyz+center_quat,
  90                                                     'base_link')
  91 
  92         sppe.place_object(0, place_rect_dims, place_rect_center)

In this example, we ask the pick and place manager to just pick up the nearest object to a specified point on a table in front of the robot, and to drop it somewhere in a 30 cm by 30 cm rectangular region on the right side of the table in front of the robot.

The robot starts by moving its arms to the side, so that the arms do not get in the way of doing a proper detection of the table and objects. It will first try to move them in a collision-free way, although if the arms are in the way when taking the initial collision map, this may not be possible. Be cautioned that if it cannot move freely, it will eventually execute an open-loop trajectory to move the arm out of the way, which may cause it to hit obstacles.

The robot will then do a detection of the table and obstacles, and take a new collision map. You can see the table, objects, and collision map in rviz by adding the following topics:

In CTurtle/Diamondback:

• /collision_model_markers/environment_server_right_arm (of type Builtin/Markers) - these markers will show the results of the tabletop object detection added to the collision environment. Note the table added as a thin box. Database-recognized objects get added as triangle meshes, while unrecognized objects get added to the collision environment as bounding boxes.

• /collision_rebroadcast_/environment_server_right_arm (of type Mapping/CollisionMap) - these markers will show the collision map, which is the part of the environment seen by the tilting laser but for which we have no addition semantic information.

In Electric:

• /planning_scene_markers (of type Builtin/Markers) - these markers show the planning scene last used to do collision-free arm motion planning. The table is added as a convex hull. Database-recognized objects get added as triangle meshes, while unrecognized objects get added to the collision environment as bounding boxes. Note that this is only showing the planning scene as of the last planning request; for more up-to-date views of the collision map, you may also want:

• /occupied_cells (of type Builtin/Markers) - these markers show the current state of the octomap (collision map) generated by the collider node, using laser rangefinder data.

When attempting to place the object in the place rectangle, it searches over a number of possible rotations at each point in a 5x5 grid, stretched to the desired place rectangle dimensions (which in this case is 30 cm by 30 cm). If all of the possible grid locations are taken (usually because the table is too crowded), it will perform a last-resort place of the object by starting high and trying to push other objects out of the way.

You can see the place rectangle drawn in rviz as a set of tiny boxes (one for each place location) by adding the Builtin/Markers topic /grasp_markers. Other useful topics for visualizing the grasp pipeline can be found here: Useful rviz topics for the grasp pipeline

The pick and place manager has many other useful functions for scripting pick and place tasks; you can try out much of the functionality through the keyboard interface obtained by running pick_and_place_manager.py (in pr2_pick_and_place_demos/src/pr2_pick_and_place_demos) directly, or through the interface offered by The Pick and Place Autonomous Demo.