| Note: This tutorial assumes you have read through the DARRT page and followed the directions for downloading. |
 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. |
Simple Pick and Place
Description: Use DARRT to plan a simple pick and place and the sushi code to execute that pick and place.Tutorial Level: INTERMEDIATE
Next Tutorial: Using Other Primitives
Contents
Pick and Place Goal
The DARRTGoal message is long and complicated but most of the parameters are optional. For a simple pick and place, only two fields matter: the pickup_goal and the place_goal. The pickup goal tells the robot which object to pick up and the place goal tells it where to put it back down. Make sure all of the mandatory parts of the pick and place goals are filled in. For a general overview of the object_manipulation_msgs/PickupGoal and object_manipulation_msgs/PlaceGoal messages see the object_manipulation_msgs stack.
Recall that the default goal plans for PICK, PLACE, and WARP which is all we shall discuss in this tutorial. For an example of sending more complicated goals, see the next tutorial.
Stationary Pick and Place
We will first go through a simple example that likely does not move the robot base.
The Code
The following code code uses the sushi code to find an object on the table in front of the robot, pick it up, and move it over 20cm. This code can also be found in darrt_actions/scripts/stationary_pickplace_demo.py.
1 #!/usr/bin/env/python
2 import roslib; roslib.load_manifest('darrt_actions')
3
4 from darrt_msgs.msg import DARRTAction, DARRTGoal
5 from darrt_actions.darrt_trajectories import get_trajectory_markers, Executor
6
7 from pr2_tasks.tableware_detection import TablewareDetection
8 from pr2_tasks.pickplace_definitions import PlaceGoal
9 from pr2_python.planning_scene_interface import get_planning_scene_interface
10
11 from actionlib import SimpleActionClient
12 from visualization_msgs.msg import MarkerArray
13 import rospy
14 import copy
15
16 def main():
17 #Planning scene interface
18 psi = get_planning_scene_interface()
19
20 #Find an object to pick up
21 rospy.loginfo('Doing detection')
22 detector = TablewareDetection()
23 det = detector.detect_objects(add_objects_as_mesh=False)
24 if not det.pickup_goals:
25 rospy.loginfo('Nothing to pick up!')
26 return
27
28 #Set planning scene to reflect the changes we have just made
29 psi.reset()
30
31 #DARRT action client
32 client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
33 rospy.loginfo('Waiting for DARRT action')
34 client.wait_for_server()
35 rospy.loginfo('Found DARRT action')
36
37 #DARRTGoal
38 goal = DARRTGoal()
39
40 #pickup goal (from the tabletop detection)
41 goal.pickup_goal = det.pickup_goals[0]
42 goal.pickup_goal.arm_name = 'right_arm'
43
44 #place goal (move left 20 cm)
45 place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
46 place_pose_stamped.pose.position.y += 0.2
47 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name,
48 [place_pose_stamped],
49 collision_support_surface_name =
50 det.table_name,
51 collision_object_name =
52 goal.pickup_goal.collision_object_name)
53
54 #Send the goal to the action
55 rospy.loginfo('Sending goal')
56 client.send_goal_and_wait(goal)
57 rospy.loginfo('Returned')
58
59
60
61 #Check the result
62 result = client.get_result()
63 if result.error_code != result.SUCCESS:
64 rospy.logerr('Planning failed. Error code: '+str(result.error_code))
65 return
66 rospy.loginfo('Planning succeeded!')
67
68 #at this point the planning is done.
69 #now we execute and visualize the plan
70
71 #visualize trajectory
72 pub = rospy.Publisher('darrt_trajectory', MarkerArray)
73 marray = get_trajectory_markers(result.primitive_trajectories)
74 for i in range(10):
75 pub.publish(marray)
76 rospy.sleep(0.05)
77
78
79 #Executor is a Python class for executing DARRT plans
80 executor = Executor()
81
82 #execute trajectory
83 rospy.loginfo('Press enter to execute')
84 raw_input()
85 executor.execute_trajectories(result)
86
87 rospy.init_node('pickplace_test_node')
88 main()
Examining the Code
Now, let's break the code down.
5 from darrt_actions.darrt_trajectories import get_trajectory_markers, Executor
The darrt_trajectories.py file in darrt_actions/src/darrt_actions contains the function for visualizing trajectories and the class for executing them.
This block of code calls tableware detection from the sushi block of code. See the sushi tutorial for more information. Note that we do not add objects as mesh because the meshes are too large for fast collision checking.
We pick up the first thing we see. We also tell DARRT to use the right arm. The pickup goal returned by tabletop detection is already in the form DARRT expects.
44 #place goal (move left 20 cm)
45 place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
46 place_pose_stamped.pose.position.y += 0.2
47 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name,
48 [place_pose_stamped],
49 collision_support_surface_name =
50 det.table_name,
51 collision_object_name =
52 goal.pickup_goal.collision_object_name)
We move the object left 20cm relative to the robot. The object_pose_stamped in the pickup goal is in a robot frame (not a world frame).
The trajectory is shown using the get_trajectory_markers function from darrt_trajectories.py. The color indicates the time along the trajectory (red->purple), not the primitive.
We use the Executor class from darrt_trajectories.py to execute trajectories.
Running The Code
Add a visualization_msgs/MarkerArray to RViz with the topic /darrt_trajectory before you run the code. If the planning succeeds you should see a rainbow trajectory. Press enter when prompted to execute it.
Because the robot can remain stationary, it is likely it will since the number of base poses around a table that are not in collision are small. However, it is possible this plan will return a warp (for a true stationary pick and place you would only want to pass the PICK and PLACE primitives without WARP) and the base may move. Base movement is done using the SBPL 3D Nav planner from the sushi code.
Using Warp
The above code may not move the robot's base because the place point is specifically chosen near the pick point. For an example of moving the robot's base, choose a place point far away from the object you are picking.
Example Code
The following example works in the simulation (Willow) set of maps:
1 #!/usr/bin/env/python
2 import roslib; roslib.load_manifest('darrt_actions')
3
4 from darrt_msgs.msg import DARRTAction, DARRTGoal
5 from darrt_actions.darrt_trajectories import get_trajectory_markers, Executor
6
7 from pr2_python.world_interface import WorldInterface
8 from pr2_tasks.tableware_detection import TablewareDetection
9 from pr2_tasks.pickplace_definitions import PlaceGoal
10 from pr2_python.planning_scene_interface import get_planning_scene_interface
11
12 from actionlib import SimpleActionClient
13 from visualization_msgs.msg import MarkerArray
14 from geometry_msgs.msg import PoseStamped
15 import rospy
16
17 def main():
18 #Planning scene interface
19 psi = get_planning_scene_interface()
20
21 #Find an object to pick up
22 rospy.loginfo('Doing detection')
23 detector = TablewareDetection()
24 det = detector.detect_objects(add_objects_as_mesh=False)
25 if not det.pickup_goals:
26 rospy.loginfo('Nothing to pick up!')
27 return
28
29 #Set the planning scene to reflect what we have just done
30 psi.reset()
31
32 #DARRT action client
33 client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
34 rospy.loginfo('Waiting for DARRT action')
35 client.wait_for_server()
36 rospy.loginfo('Found DARRT action')
37
38 #DARRTGoal
39 goal = DARRTGoal()
40
41 #pickup goal (from the tabletop detection)
42 goal.pickup_goal = det.pickup_goals[0]
43 goal.pickup_goal.arm_name = 'right_arm'
44
45 #world interface to tell us whether we're using the
46 #map or odom_combined frame
47 wi = WorldInterface()
48
49 #place goal (move far away)
50 place_pose_stamped = PoseStamped()
51 place_pose_stamped.header.frame_id = wi.world_frame
52 place_pose_stamped.pose.position.x = 0
53 place_pose_stamped.pose.position.y = 0
54 place_pose_stamped.pose.position.z = 0.85
55 place_pose_stamped.pose.orientation.w = 1.0
56 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name,
57 [place_pose_stamped],
58 collision_support_surface_name =
59 det.table_name,
60 collision_object_name =
61 goal.pickup_goal.collision_object_name)
62
63 #Send the goal to the action
64 rospy.loginfo('Sending goal')
65 client.send_goal_and_wait(goal)
66 rospy.loginfo('Returned')
67
68 #Check the result
69 result = client.get_result()
70 if result.error_code != result.SUCCESS:
71 rospy.logerr('Planning failed. Error code: '+str(result.error_code))
72 return
73 rospy.loginfo('Planning succeeded!')
74
75 #at this point the planning is done.
76 #now we execute and visualize the plan
77
78 #visualize trajectory
79 pub = rospy.Publisher('darrt_trajectory', MarkerArray)
80 marray = get_trajectory_markers(result.primitive_trajectories)
81 for i in range(10):
82 pub.publish(marray)
83 rospy.sleep(0.05)
84
85
86 #Executor is a Python class for executing DARRT plans
87 executor = Executor()
88
89 #execute trajectory
90 rospy.loginfo('Press enter to execute')
91 raw_input()
92 executor.execute_trajectories(result)
93
94 rospy.init_node('pickplace_test_node')
95 main()
If you are not using the simulation maps, don't forget to change the place goal for your own environment:
World Frame
This example also illustrates the idea of the "world_frame". If the robot is localizing to a map, we want to treat the "/map" frame as the frame that is stationary with respect to stationary objects (like tables). If there is no map, we must use the "/odom_combined" frame. The world interface determines which frame is the world frame by checking the parent frame of the multi DOF joint in a call to /environment_server/get_robot_state. I find just instantiating a world interface and asking it for the world frame to be less work:
Running
First launch DARRT on the robot. For warp to work, you must have a map for 3D Nav to use:
roslaunch darrt darrt.launch use_map:=true
Before running this code add two visualization_msgs/MarkerArray topics in RViz: /darrt_trajectory and /visualization_msgs. The latter topic will show the plans generated by 3D Nav.
Run the code, pressing enter when prompted. After the pick, the robot will plan to move its arm to its side and then create and execute a 3D Nav plan to where it can place. This is the warp - the planning happens outside of the DARRT plan. Once it finishes the warp, it will execute the rigid transfer planned by DARRT.
Beyond Pick and Place
Firstly, this code will only work well when almost all grasps work for both the pick and the place. If you have constrained places, you should pass a non-zero object_sampling_fraction to the goal.
For an overview of using other primitives, see the next tutorial.
