| Note: This tutorial assumes that you have completed the previous tutorials: Simple Pick and Place. |
 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. |
Using Other Primitives
Description: Using DARRT to plan for goals other than pick and place.Tutorial Level: INTERMEDIATE
Contents
A Push Example
By default, DARRT plans for PICK, PLACE, and WARP. To change this, we change the set of primitives we send to the goal. The following set of code tells DARRT to only plan for pushing.
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 the planning scene to reflect the changes we 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 right 10 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 #tell DARRT to only plan for PUSH
55 goal.primitives = [goal.PUSH]
56
57 #when planning for push, we want to sometimes sample the object location
58 goal.object_sampling_fraction = 0.5
59
60 #Send the goal to the action
61 rospy.loginfo('Sending goal')
62 client.send_goal_and_wait(goal)
63 rospy.loginfo('Returned')
64
65 #Check the result
66 result = client.get_result()
67 if result.error_code != result.SUCCESS:
68 rospy.logerr('Planning failed. Error code: '+str(result.error_code))
69 return
70 rospy.loginfo('Planning succeeded!')
71
72 #at this point the planning is done.
73 #now we execute and visualize the plan
74
75 #visualize trajectory
76 pub = rospy.Publisher('darrt_trajectory', MarkerArray)
77 marray = get_trajectory_markers(result.primitive_trajectories)
78 for i in range(10):
79 pub.publish(marray)
80 rospy.sleep(0.05)
81
82
83 #Executor is a Python class for executing DARRT plans
84 executor = Executor()
85
86 #execute trajectory
87 rospy.loginfo('Press enter to execute')
88 raw_input()
89 executor.execute_trajectories(result)
90
91 rospy.init_node('pickplace_test_node')
92 main()
This code is almost identical to the the code from the last tutorial except for the line in which we assign the goal primitives:
55 goal.primitives = [goal.PUSH]
and where we increase the object sampling fraction
Simple Pick and Place often works without sampling the object's position anywhere except the goal because the challenge is in moving the robot to the object. However, this will never allow the robot to, for example, push an object around an obstacle. Therefore, with push and in general, it is a good idea to set a object sampling fraction greater than 0.
Now when you run the code, DARRT should plan and execute a pushing path.
