Pick and Place examples using MoveIt?

[Patrick Goebel]

Hello,

Are there any MoveIt Pick and Place examples lying around for either
Python or C++? Maybe I just missed them on the Wiki but so far I
haven't found any.

Thanks!
patrick

[Ioan Sucan]

Hello Patrik,

There is a 'pick' demo committed just a few hours ago to moveit_commander, under the demos/ folder, for Python.

For C++, in moveit_pr2, there is a pick and place tutorial, which is mostly a demo right now.

Note this code does not do grasping nor does it do perception for you.

Ioan

[Patrick Goebel]

Awesome--thanks!

--patrick

[Michael Ferguson]

Ioan,

Has the moveit_pr2 pick_and_place tutorial been updated for the new interpretation of place goals? I just tried it again, and the demo does pick, but fails to place (it worked with moveit 0.3.30 and earlier for me, which corresponds to the change in place location interpretation)

-Fergs

[Patrick Goebel]

So I gave the Python demo script a try and it runs through until the line:

    robot.right_arm.pick("part")

and then times out with the following error:

Traceback (most recent call last):
  File "/home/patrick/Dropbox/Robotics/ros/rbx2-git/rbx2_arm_navigation/nodes/test_pick.py", line 46, in <module>
    robot.right_arm.pick("part")
  File "/opt/ros/groovy/lib/python2.7/dist-packages/moveit_commander/robot.py", line 133, in __getattr__
    return self.get_group(name)
  File "/opt/ros/groovy/lib/python2.7/dist-packages/moveit_commander/robot.py", line 127, in get_group
    self._groups[name] = MoveGroupCommander(name)
  File "/opt/ros/groovy/lib/python2.7/dist-packages/moveit_commander/move_group.py", line 50, in __init__
    self._g = _moveit_move_group_interface.MoveGroup(name, "robot_description")
RuntimeError: Unable to connect to action server within allotted time

I have my full set of MoveIt launch files running and I can interact with the robot's right_arm group using moveit_commander_cmdline.py from the command line.

--patrick

[Ioan Sucan]

This looks like an error in communicating with the move_group node. Are you running the script on the same machine as the move_group node? If not, perhaps you need to set ROS_IP / ROS_HOSTNAME. I think Acorn has a script for that.

[Ioan Sucan]

Hey Fergs,

Yes, I have. I just checked again and it seems to work fine.

One note though: this does demo not run with demo.launch since the actual gripping needs to be evaluated before triggering the discrete change in the world that makes the object attached.

What I do to test is:

roslaunch pr2_gazebo pr2_empty_world.lauch   // repeat this until no crash

roslaunch pr2_moveit_config move_group.launch

roslaunch pr2_moveit_config moveit_rviz.launch

run  ./devel/lib/pr2_moveit_tutorials/pick_place_tutorial

Ioan

[Patrick Goebel]

Hi Ioan,

All nodes and scripts are running on the same machine and ROS_HOSTNAME, ROS_IP and ROS_MASTER_URI are all set correctly.

Here is my list of topics:

/attached_collision_object
/clicked_point
/cmd_vel
/collision_map
/collision_object
/diagnostics
/feedback
/head_pan_joint/command
/head_tilt_joint/command
/initialpose
/joint_states
/move_base_simple/goal
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/pick_place/parameter_descriptions
/move_group/pick_place/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/odom
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/right_arm_controller/command
/right_arm_controller/follow_joint_trajectory/cancel
/right_arm_controller/follow_joint_trajectory/feedback
/right_arm_controller/follow_joint_trajectory/goal
/right_arm_controller/follow_joint_trajectory/result
/right_arm_controller/follow_joint_trajectory/status
/right_arm_elbow_flex_joint/command
/right_arm_forearm_flex_joint/command
/right_arm_shoulder_lift_joint/command
/right_arm_shoulder_pan_joint/command
/right_arm_shoulder_roll_joint/command
/right_arm_wrist_flex_joint/command
/right_gripper_controller/command
/right_gripper_controller/follow_joint_trajectory/cancel
/right_gripper_controller/follow_joint_trajectory/feedback
/right_gripper_controller/follow_joint_trajectory/goal
/right_gripper_controller/follow_joint_trajectory/result
/right_gripper_controller/follow_joint_trajectory/status
/right_gripper_finger_joint/command
/rosout
/rosout_agg
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/rviz_pi_robot_z935_31311_1289237047/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_pi_robot_z935_31311_1289237047/motionplanning_planning_scene_monitor/parameter_updates
/tf
/trajectory_execution_event

[Patrick Goebel]

Hi Ioan,

I am back to trying the 'pick' demo from the moveit_commander repository but now under MoveIt 0.4.3 (Debian install).  When running the script, I get the following error which I did not get with early versions of MoveIt:

Traceback (most recent call last):

  File "/home/patrick/Dropbox/Robotics/ros/rbx2-git/rbx2_arm_navigation/nodes/test_pick_and_place.py", line 43, in <module>
    robot.right_arm.pick("part")
  File "/opt/ros/groovy/lib/python2.7/dist-packages/moveit_commander/robot.py", line 175, in __getattr__
    return self.get_group(name)
  File "/opt/ros/groovy/lib/python2.7/dist-packages/moveit_commander/robot.py", line 150, in get_group
    if not self.has_group():
TypeError: has_group() takes exactly 2 arguments (1 given)

On 05/27/2013 03:04 PM, Ioan Sucan wrote:

[Tiago Pimentel]

Hey Patrick,

Not sure if this will work, but try to pass "name" to:
    if not self.has_group():
so it's like this
    if not self.has_group(name):

Just trying to guess the answer, so don't be amazed if this does not work.

Have a great day,

[Ioan Sucan]

Patrick,

That is an unfortunate typo. Tago's fix should work.

Ioan

[Ioan Sucan]

And as you can see, I have a thing for typos. I'm sorry Tiago!

[Patrick Goebel]

Thanks Ioan and Tiago,

Looks like Ioan fixed the typo in the source version for moveit_commander so I am using that now and the error went away.

Now the issue is that the three scene objects (oole, table, part) are not showing up in RViz.  I'm subscribing to the /planning_scene topic and my fixed frame is set to /odom.  The objects *did* appear in an earlier version (0.4.1?) but no longer.  I've tried adding the objects multiple times with a rospy.sleep(3) in between but still no objects show up in RViz.

--patrick

[Ioan Sucan]

Hello Patrick,

The rviz plugin is not actually listening for such objects -- only for planning scene diffs. What you need to do is publish to move_group (on /planning_scene) and in the rviz plugin, look at /move_group/monitored_planning_scene; that should display the objects.

You can run rqt_reconfigure to check that scene publishing is indeed enabled in move_group;

Ioan

[Patrick Goebel]

D'oh!  I knew that once and then forgot it.  Seems all I had to do was set the arg "publish_monitored_planning_scene" to "true" instead of its default value of "false" in move_group.launch and now the objects show up on the "/move_group/monitored_planning_scene" topic.

Thanks,
patrick

[Omar Chavez Garcia]

Hello Ioan,

May you tell me where can I find the moveit's pick_place demo (C++)? I have searched in my hydro installation but all I have found are the binaries. I checked the documentation, and there is no much information regarding this pick_place demo either. I tested it on gazebo to see what it does, so far it seems to move the arm from one place to another. There is no grasping (did you refer to this example when you mentioned there is no grasping?). 

I've also checked your demo in github, but I can't make it build. I'm getting many errors regarding the moveit::planning_interface::MoveGroup methods. There are incompatibilities or not found methods for this class, e.g., no matching function for pick, PlaceLocation is not a member, .... I modified the CMakeLists and the package.xml to include other needed dependencies. Is it maybe my moveit/ros version? I'm suing hydro and the binaries of moveit included in the repository.

I will appreciate your help regarding this tutorial.

Thanks in advance

Omar