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

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

  57     #when planning for push, we want to sometimes sample the object location
  58     goal.object_sampling_fraction = 0.5

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.

Wiki: darrt/Tutorials/Using Other Primitives (last edited 2012-08-12 01:12:50 by JennyBarry)