The goals of this lab are

  • Bring together manipulation and perception for complete robotic task execution.
  • Understand physics-based simulators by using Gazebo
  • Explore object segmentation, object recognition and collision environment management
You will need to create the following scripts:
  1. Python/Bash/C++ script that creates a Gazebo world with a table, waste basket and something graspable on the table (like a coffee cup). The wastebasket can be on the table or floor.
  2. Python/C++ demo script that finds the object on the table using its laser range finder, picks it up, and places it in the waste basket.


  1. A flow diagram showing all the components required to recognize an object and pick it up.
  2. Documentation (explanation / screenshots) of your experiment.  Comment on why the demo simulation sometimes fails, including various environmental differences such as when the PR2 is oriented in various directions.


  • 2 weeks
  • Deliverables are due March 12th.
Running Willow Garage’s tabletop manipulation demo for the PR2:
You will need a suitably powerful computer to run this simulation and manipulation demo. It is likely that the VirtualBox Ubuntu will not be sufficient to run all of this. If you have problems we recommend you install Ubuntu/ROS native on your computer using Wubi for Windows, figure out how to dual-boot a Mac with Ubuntu, or use the class’ ClamArm computer.


For all of the following instructions, the overall step is given, the relevant command-line command is given, and a link to is provided to learn more about it. You are encouraged to read the relevant links.


  1. Launch the simulated PR2 in Gazebo
    • roslaunch pr2_gazebo pr2_empty_world.launch
  2. Add a table and coffee cup to Gazebo.
    • roslaunch gazebo_worlds table.launch
    • roslaunch gazebo_worlds coffee_cup.launch
  3. Edit the table and coffee cup urdf files so that they are located correctly in front of the PR2
    • For example:
    • rosed gazebo_worlds table.urdf.xacro
    • rosservice call gazebo/delete_model '{model_name: table_model}'
  4. Move the PR2 to be closer to the table, raise torso, and move arms out the laser range finder’s way via keyboard teleoperation
    • roslaunch pr2_teleop_general pr2_teleop_general_keyboard.launch
  5. Launch the manipulation pipeline (this includes OMPL, inverse kinematic solver, environmental server)
    • export ROBOT=sim
    • roslaunch pr2_tabletop_manipulation_launch pr2_tabletop_manipulation.launch
  6. View what the robot is seeing in its simulated world by launching Rviz (at the same time). Add all available markers to rviz. Save the rviz config for future reference.
    • rosrun rviz rviz
  7. Start the pick and place demo and do both the continuous and step-by-step operation.
    • rosrun pr2_pick_and_place_demos
  8. You should see the PR2 move the coffee cup from one location to another, assuming the cup is sufficiently centered on the table and in front of the robot.

12 Responses to Advanced Robotics Lab #3: Tabletop Manipulation

  1. Charles Chen says:

    The following error can occur when running the part 7 command (rosrun pr2_pick_and_place_demos

      File “/opt/ros/electric/stacks/pr2_object_manipulation/manipulation/pr2_gripper_reactive_approach/src/pr2_gripper_reactive_approach/”, line 1435, in return_cartesian_pose
        (trans, rot) = self.tf_listener.lookupTransform(frame, self.whicharm+’_wrist_roll_link’, rospy.Time(0))

    tf.ExtrapolationException: Lookup would require extrapolation into the past.  Requested time 17.529000000 but the earliest data is at time 17.649000000, when looking up transform from frame [/r_wrist_roll_link] to frame [/base_link]

    I ran into this even though I am running a native Ubuntu install.  This can be fixed by adding a wait to the start of the call to return_cartesian_pose:

    roscd pr2_object_manipulation
    cd manipulation
    cd pr2_gripper_reactive_approach
    cd src
    cd pr2_gripper_reactive_approach
    sudo gedit

    Around line 1430, there is the line:
    (trans, rot) = self.tf_listener.lookupTransform(frame, self.whicharm+’_wrist_roll_link’, rospy.Time(0))

    Above this add:

    Save, and make the package again
    rosmake pr2_object_manipulation

    Since there’s a sleep every time that method is called, the simulation will be a little slower.  You can adjust the wait duration to be just larger than the delay that you see on your machine. 0.5 works for me most of the time.

    • Dave says:

      I have seen this issue before on my laptop, it simply means the computer is lagging and not able to publish messages fast enough before the messages become expired. When a joint location message is read, the program sees that the message is too out of date and throws an error message. Try closing any other programs on your computer that might be taking CPU cycles. Or your fix of adding a sleep timer in there seems to work too.

    • Dylan Canfield says:

      You should probably avoid editing this file in the path of ROS. By copying this stack to your ROS Workspace and editing it there, you have two advantages: 1) The original state of the stack is preserved if you mess something up. 2) Updates are guaranteed to not clobber your changes.

      As long as you have your ROS workspace set before the stacks install location, your custom stack will be loaded in place of the default. That is to say you should have the ROS_PACKAGE_PATH environment variable set to something like:

      Then you just have to copy the pr2_object_manipulation folder into your ROS workspace:
      cp -rav /opt/ros/electric/stacks/pr2_object_manipulation ~/ros_workspace

      I did the same thing for the simulator_gazebo folder when editing the coffe cup and table positions.

  2. Ben says:

    I can’t seem to get the table and coffeecup to show up in Rviz (they show up in Gazebo just fine, and the pr2 shows up Rviz).  Any ideas what I might be doing wrong (or failing to do)? I’m pretty much following the directions step by step, and not seeing any obvious error messages…  

    • Charles Chen says:

      When you run the pick and place demo and try to do table top detection (option t),  does it work? If not, then you are running into the same thing I am.  I’m looking into why the detection is not working for me, but so far haven’t come up with any answers.

    • Dave says:

      I think you might be expecting the wrong thing to happen. The coffee cup and table never visually appear in Rviz, but instead their existence within the point cloud should be visible within the /occupied_cells topic. So once you add that marker you should see (blue?) blobs of point cloud data. Gazebo is the replacement for a physical world/robot, whereas Rviz simply visualizes the world as the robot sees it.

    • Charles Chen says:

      To get point clouds to show up in rviz, add a topic of type Point Cloud2. When it has come up in the list of displays on the left, change the topic to octomap_point_cloud.  If running in virtual machine, give it a minute or so, since it will be somewhat slow to change over.

      If you get error messages when running the pick and place demo that say that the point cloud has 0 points, then you need to change the point cloud source topic.

      On line 57 of pr2_tabletop_manipulation.launch in pr2_tabletop_manipulation_launch/launch,

      Change the value property to “octomap_point_cloud”

  3. Anshul says:

    I have saved a useful RViz config file (AdvacedRoboricsLab3.vcg) with all the important markers added to it on the Andrews Hall computer. It is saved on the Desktop and can be loaded into RViz  from File -> Open Config.

    – Anshul

  4. Charles Chen says:

    So I’ve been trying to get the pick and place demo working on my machine, and I am still having issues getting the PR2 to detect and pick up an object.  Whenever it detects an object, it seems to immediately remove that object from the collision space and its list of detected objects.  I get messages like the following:
    [ INFO] [1331430469.128883109, 707.295000000]: Added 1 object to namespace table in collision space
    [ INFO] [1331430469.129540248, 707.295000000]: Added 1 object to namespace graspable_object_67 in collision space
    [ INFO] [1331430469.130271769, 707.295000000]: Removed object ‘graspable_object_67’ from collision space

    I’ve tried moving the object around the table, using different objects, and having the robot get closer to the table, to see if it would make a difference, and so far it does not seem to help.  It just doesn’t seem to be able to tell that there is an object on the table at the time that it attempts to pick up an object.

    [INFO] [WallTime: 1331434270.667024] [1076.316000] calling tabletop detection
    [INFO] [WallTime: 1331434270.929747] [1076.351000] tabletop detection reports success
    [INFO] [WallTime: 1331434270.936520] [1076.351000] table front edge x: 0.315
    [INFO] [WallTime: 1331434270.937125] [1076.351000] table height: 0.505
    [INFO] [WallTime: 1331434270.937739] [1076.351000] detection finished, finding bounding boxes for clusters and sorting objects
    [INFO] [WallTime: 1331434270.938709] [1076.351000] Object detection complete
    [INFO] [WallTime: 1331434270.939515] [1076.351000] No objects detected.
    [INFO] [WallTime: 1331434270.940265] [1076.351000] no objects in view!
    [INFO] [WallTime: 1331434270.940951] [1076.351000] switching sides!  pick-up side is now l, put-down is r
    [INFO] [WallTime: 1331434271.006929] [1076.355000] starting to move objects from l side to r side
    [INFO] [WallTime: 1331434271.007938] [1076.355000] object_tries:0

    Has anyone run into something similar, and did you come up with a fix for it?

    • Dylan Canfield says:

      I am getting past this point, what are the positions of your table and cup? 
      I get the following:

       [INFO] [WallTime: 1331498159.207956] [39.200000] tabletop detection reports success
      [INFO] [WallTime: 1331498159.356553] [39.237000] detection finished, finding bounding boxes for clusters and sorting objects
      [INFO] [WallTime: 1331498159.517564] [39.265000] object 0, red: point cluster with 2380 points and col name graspable_object_5
      [INFO] [WallTime: 1331498159.518497] [39.266000] object 1, green: point cluster with 370 points and col name graspable_object_6
      [INFO] [WallTime: 1331498159.519139] [39.266000] object 2, blue: point cluster with 3250 points and col name graspable_object_4
      [INFO] [WallTime: 1331498159.520655] [39.266000] object dist from back edge is -0.204
      [INFO] [WallTime: 1331498159.521248] [39.266000] counting right: ignoring object not on the right side, y pos: 0.052
      [INFO] [WallTime: 1331498159.521604] [39.266000] object dist from back edge is -0.124
      [INFO] [WallTime: 1331498159.522603] [39.266000] dist_from_point:0.074
      [INFO] [WallTime: 1331498159.523377] [39.266000] attempting to grasp an object, whicharm = 0

      Can you try with the positions I used for the table and cup?
      Here is the position I chose for the table:    x: 0.6    y: 0    z: 0Here is the position I chose for the cup:    x: .5    y: -0.05    z: .57

      Let me know if that gets you to the same point, because I am still having trouble actually grasping the cup:

      INFO] [WallTime: 1331498167.715485] [41.193000] high point grasp qualities: 0.882 0.848 0.843 0.812 0.812 0.807 0.784 0.773 0.747 0.746 0.729 0.722 0.705 0.679 0.667 0.664 0.659 0.624 0.614 0.599 0.592 0.572 0.533 0.528 0.514 0.497 0.463 0.457 0.449 0.432 0.338 0.273 0.243 0.224 0.191 0.186 0.185 0.175 0.168 0.164 0.158 0.146 0.143 0.142 0.138 0.129 0.121 0.116 0.111 0.104 0.086 0.082 0.082 0.069 0.065 0.064 0.062 0.058 0.055 0.054 0.049 0.044 0.043 0.039 0.035[INFO] [WallTime: 1331498167.716136] [41.193000] total number of grasps found:125[ INFO] [1331498168.258693111, 41.330000000]: waitForService: Service [/r_interpolated_ik_motion_plan_set_params] has not been advertised, waiting…[ INFO] [1331498172.604469142, 42.346000000]: Waiting for service right_arm/interpolated_ik_set_params remapped to /r_interpolated_ik_motion_plan_set_params[ INFO] [1331498172.605735533, 42.346000000]: waitForService: Service [/r_interpolated_ik_motion_plan_set_params] has not been advertised, waiting…[ INFO] [1331498177.062074437, 43.354000000]: Waiting for service right_arm/interpolated_ik_set_params remapped to /r_interpolated_ik_motion_plan_set_params[ INFO] [1331498177.063371304, 43.354000000]: waitForService: Service [/r_interpolated_ik_motion_plan_set_params] has not been advertised, waiting…[ INFO] [1331498181.386792154, 44.367000000]: Waiting for service right_arm/interpolated_ik_set_params remapped to /r_interpolated_ik_motion_plan_set_params[ INFO] [1331498181.388203604, 44.367000000]: waitForService: Service [/r_interpolated_ik_motion_plan_set_params] has not been advertised, waiting…[ INFO] [1331498185.609745395, 45.371000000]: Waiting for service right_arm/interpolated_ik_set_params remapped to /r_interpolated_ik_motion_plan_set_params[ INFO] [1331498185.610827547, 45.371000000]: waitForService: Service [/r_interpolated_ik_motion_plan_set_params] has not been advertised, waiting…[ERROR] [1331498190.127724295, 46.388000000]: Grasp error; exception: grasp execution:mechanism:service or action not found:right_arm/interpolated_ik_set_params remapped to /r_interpolated_ik_motion_plan_set_params

      • Charles Chen says:

        Is your table really at x: 0.6? When I have the cup and table in those positions, the cup ends up on the floor. 

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Set your Twitter account name in your settings to use the TwitterBar Section.