def place(self, arm, obj_name, pose): ''' places the given object with the given arm in the give pose. @type arm: String @param arm: 'left_arm' or 'right_arm'. arm to place @type obj_name: String @param obj_name: name of the object being placed from the collision map @type pose: PoseStamped @param pose: pose to place the object @rtype: boolean @returns whether the place was successful ''' try: if arm == "right_arm": place_goal = PlaceGoal('right_arm', [pose], collision_support_surface_name='all', collision_object_name=obj_name) else: place_goal = PlaceGoal('left_arm', [pose], collision_support_surface_name='table', collision_object_name=obj_name) print "ABOUT TO PLACE" doodie = self.pickplace.place(place_goal) print "doodie=" + str(doodie) except ManipulationError: rospy.loginfo("POSE FAILED") return False return True
def pickplace_main(): rospy.loginfo('Waiting for action') rospy.loginfo('Doing detection') detector = TablewareDetection() det = detector.detect_objects(add_objects_as_mesh=False) if not det.pickup_goals: rospy.loginfo('Nothing to pick up!') return wi = WorldInterface() psi = get_planning_scene_interface() add_map_tables(wi) psi.reset() client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) client.wait_for_server() goal = DARRTGoal() goal.pickup_goal = det.pickup_goals[0] goal.pickup_goal.lift_distance = 0.3 goal.pickup_goal.arm_name = 'right_arm' place_pose_stamped = PoseStamped() place_pose_stamped.header.frame_id = wi.world_frame place_pose_stamped.pose.position.x = -1.3 place_pose_stamped.pose.position.y = 0.7 if 'graspable' in goal.pickup_goal.collision_object_name: place_pose_stamped.pose.position.z = 0.8 else: place_pose_stamped.pose.position.z = 0.75 place_pose_stamped.pose.orientation.w = 1.0 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name = 'serving_table', collision_object_name = goal.pickup_goal.collision_object_name) goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT] goal.object_sampling_fraction = 0.3 goal.retreat_distance = 0.3 goal.debug_level = 2 goal.do_pause = False goal.execute = False goal.planning_time = 30 goal.tries = 10 goal.goal_bias = 0.2 average_time = 0.0 rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0])) rospy.loginfo('Restart after '+str(goal.planning_time)) for i in range(NTRIALS): rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') result = client.get_result() if result.error_code != result.SUCCESS: rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+ ' successful trials with an average time of '+str(average_time/float(i))) return average_time += result.planning_time rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time)) rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def main(): client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) pub = rospy.Publisher('darrt_trajectory', MarkerArray) rospy.loginfo('Waiting for action') rospy.loginfo('Doing detection') detector = TablewareDetection() det = detector.detect_objects(add_objects_as_mesh=False) if not det.pickup_goals: rospy.loginfo('Nothing to pick up!') return client.wait_for_server() goal = DARRTGoal() goal.pickup_goal = det.pickup_goals[0] goal.pickup_goal.arm_name = 'right_arm' place_pose_stamped = PoseStamped() place_pose_stamped.header.frame_id = 'map' place_pose_stamped.pose.position.x -= 1.3 place_pose_stamped.pose.position.y -= 0.3 place_pose_stamped.pose.position.z = 0.98 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name = det.table_name, collision_object_name = goal.pickup_goal.collision_object_name) goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT] goal.min_grasp_distance_from_surface = 0.17 goal.object_sampling_fraction = 0.9 goal.retreat_distance = 0.3 goal.debug_level = 2 goal.do_pause = False goal.planning_time = 6000 goal.tries = 1 goal.goal_bias = 0.2 rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') result = client.get_result() if result.error_code == result.SUCCESS: #pickle everything!! great excitement filename = 'pickplace_and_objects.pck' rospy.loginfo('Pickling to '+filename) #the last bit allows us to recreate the planning pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb')) marray = MarkerArray() if result.error_code == result.SUCCESS: for t in result.primitive_trajectories: marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers for (i, m) in enumerate(marray.markers): m.id = i (m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1) while not rospy.is_shutdown(): pub.publish(marray) rospy.sleep(0.1)
def place(pickplace, obj_name, pose): # rospy.loginfo("pose"+str(pose)) try: place_goal = PlaceGoal('right_arm', [pose], collision_support_surface_name='table', collision_object_name=obj_name) #return False pickplace.place(place_goal) except ManipulationError: rospy.loginfo("POSE FAILED") return False return True
def main(): pub = rospy.Publisher('/fail_location', Marker) pickplace = PickPlace(search_place=[]) place_pose = PoseStamped() place_pose.header.frame_id = '/torso_lift_link' place_pose.pose.position.x = 0.7 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = -0.28 place_pose.pose.orientation.x = 0.707 place_pose.pose.orientation.w = 0.707 obj = sys.argv[1] try: place_goal = PlaceGoal('right_arm', [place_pose], \ collision_support_surface_name='table', \ collision_object_name=obj) pickplace.place(place_goal) except ManipulationError: pass # my_world = WorldInterface() # fail_loc = my_world.collision_object(obj) # rospy.loginfo("FAIL LOC="+str(fail_loc)) marker = Marker() marker.header.frame_id = '/torso_lift_link' marker.ns = '' marker.id = 0 marker.type = Marker.SPHERE marker.action = marker.ADD marker.header.frame_id = place_pose.header.frame_id marker.pose = place_pose.pose marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 0.8 rospy.loginfo("marker msg=" + str(marker)) pub.publish(marker) rospy.sleep(15.0)
def plate_main(): pub = rospy.Publisher('darrt_trajectory', MarkerArray) rospy.loginfo('Waiting for action') rospy.loginfo('Doing detection') wi = WorldInterface() psi = get_planning_scene_interface() detector = TablewareDetection() plate = wi.collision_object('plate') good_detection = False goal = DARRTGoal() if plate: rospy.loginfo('Use last detection?') good_detection = (raw_input() == 'y') ops = PoseStamped() ops.header = copy.deepcopy(plate.header) ops.pose = copy.deepcopy(plate.poses[0]) goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id), object_pose_stamped=ops, collision_object_name='plate', collision_support_surface_name='serving_table') while not good_detection: det = detector.detect_objects(add_table_to_map=False, add_objects_as_mesh=False, table_name='serving_table') goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi) psi.reset() if not goal.pickup_goal: rospy.loginfo('Nothing to pick up!') rospy.loginfo('Good detection?') good_detection = (raw_input() == 'y') if not goal.pickup_goal: rospy.loginfo('Nothing to pick up!') return add_map_tables(wi) psi.reset() client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) client.wait_for_server() goal.pickup_goal.arm_name = 'right_arm' goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped, goal.pickup_goal.target.reference_frame_id) goal.pickup_goal.lift.desired_distance = 0.3 place_pose_stamped = PoseStamped() place_pose_stamped.header.frame_id = wi.world_frame # place_pose_stamped.pose.position.x = 1.3 # place_pose_stamped.pose.position.y = -0.3 # place_pose_stamped.pose.position.z = 1.01 place_pose_stamped.pose.position.x = 0.0 place_pose_stamped.pose.position.y = 0.0 place_pose_stamped.pose.position.z = 0.76 place_pose_stamped.pose.orientation.w = 1.0 # place_pose_stamped.pose.orientation.x = 0.1 # place_pose_stamped.pose.orientation.y = 0.1 # place_pose_stamped.pose.orientation.w = np.sqrt(0.98) #place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped) #place_pose_stamped.pose.position.x -= 0.2 #place_pose_stamped.pose.position.y -= 0.2 #place_pose_stamped.pose.position.z += 0.2 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name = 'dirty_table', collision_object_name = goal.pickup_goal.collision_object_name) goal.place_goal.approach.direction.header.frame_id = wi.world_frame goal.place_goal.approach.direction.vector.x = np.sqrt(0.18) goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18) goal.place_goal.approach.direction.vector.z = -0.8 goal.place_goal.approach.desired_distance = 0.2 goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT] goal.min_grasp_distance_from_surface = 0.19 goal.object_sampling_fraction = 0.7 goal.retreat_distance = 0.3 goal.debug_level = 2 goal.do_pause = False goal.execute = False goal.planning_time = 6000 goal.tries = 1 goal.goal_bias = 0.2 rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') result = client.get_result() marray = MarkerArray() if result.error_code == result.SUCCESS: #pickle everything!! great excitement filename = 'trajectory_and_objects.pck' rospy.loginfo('Pickling to '+filename) #the last bit allows us to recreate the planning pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb')) rospy.loginfo('Successful write') for t in result.primitive_trajectories: marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers for (i, m) in enumerate(marray.markers): m.id = i (m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1) while not rospy.is_shutdown(): pub.publish(marray) rospy.sleep(0.1)
def main(): #Find an object to pick up rospy.loginfo('Doing detection') detector = TablewareDetection() det = detector.detect_objects(add_objects_as_mesh=False) if not det.pickup_goals: rospy.loginfo('Nothing to pick up!') return #DARRT action client client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) rospy.loginfo('Waiting for DARRT action') client.wait_for_server() rospy.loginfo('Found DARRT action') #DARRTGoal goal = DARRTGoal() #pickup goal (from the tabletop detection) goal.pickup_goal = det.pickup_goals[0] goal.pickup_goal.arm_name = 'right_arm' #place goal (move left 20 cm) place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped) place_pose_stamped.pose.position.y += 0.2 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name = det.table_name, collision_object_name = goal.pickup_goal.collision_object_name) #Send the goal to the action rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') #Check the result result = client.get_result() if result.error_code != result.SUCCESS: rospy.logerr('Planning failed. Error code: '+str(result.error_code)) return rospy.loginfo('Planning succeeded!') #at this point the planning is done. #now we execute and visualize the plan #visualize trajectory pub = rospy.Publisher('darrt_trajectory', MarkerArray) marray = get_trajectory_markers(result.primitive_trajectories) for i in range(10): pub.publish(marray) rospy.sleep(0.05) #Executor is a Python class for executing DARRT plans executor = Executor() #execute trajectory rospy.loginfo('Press enter to execute') raw_input() executor.execute_trajectories(result)
def main(): #Find an object to pick up psi = get_planning_scene_interface() psi.reset() rospy.loginfo('Doing detection') detector = TablewareDetection() det = detector.detect_objects(add_objects_as_mesh=False) if not det.pickup_goals: rospy.loginfo('Nothing to pick up!') return psi.reset() #DARRT action client client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) rospy.loginfo('Waiting for DARRT action') client.wait_for_server() rospy.loginfo('Found DARRT action') #DARRTGoal goal = DARRTGoal() #pickup goal (from the tabletop detection) goal.pickup_goal = det.pickup_goals[0] goal.pickup_goal.arm_name = 'right_arm' #world interface to tell us whether we're using the #map or odom_combined frame wi = WorldInterface() #place goal (move far away) place_pose_stamped = PoseStamped() place_pose_stamped.header.frame_id = wi.world_frame place_pose_stamped.pose.position.x = -2 place_pose_stamped.pose.position.y = 0 place_pose_stamped.pose.position.z = 0.85 place_pose_stamped.pose.orientation.w = 1.0 goal.place_goal = PlaceGoal( goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name=det.table_name, collision_object_name=goal.pickup_goal.collision_object_name) goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT] goal.planning_time = 30 goal.tries = 3 goal.debug_level = 1 goal.object_sampling_fraction = 0.5 #Send the goal to the action rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') #Check the result result = client.get_result() if result.error_code != result.SUCCESS: rospy.logerr('Planning failed. Error code: ' + str(result.error_code)) return rospy.loginfo('Planning succeeded!') #at this point the planning is done. #now we execute and visualize the plan #visualize trajectory pub = rospy.Publisher('darrt_trajectory', MarkerArray) marray = get_trajectory_markers(result.primitive_trajectories) for i in range(10): pub.publish(marray) rospy.sleep(0.05) #Executor is a Python class for executing DARRT plans executor = Executor() #execute trajectory rospy.loginfo('Press enter to execute') raw_input() executor.execute_trajectories(result) rospy.loginfo('Successfully executed!')