def pick_and_place(self, objects, object_poses): self.is_picking = True grasps = None object_name = "" random.shuffle(objects) for object in objects: object_name = object.type.key rospy.loginfo("Getting grasp for object " + object_name) graspResponse = self.graspService(object_name) if graspResponse.success: rospy.loginfo("grasps were found for object " + object_name) grasps = graspResponse.grasps break if grasps is None: rospy.logerr( "Failed to find any grasps for the objects identified") self.is_picking = False return rospy.loginfo("Finding a valid place pose") place_poses = self.getValidPlacePoses() rospy.loginfo("Attempting to pick up object " + object_name) MoveHelper.move_to_neutral("left", True) pickSuccess = False try: pickSuccess = self.pick(object_poses[object_name], object_name) except Exception as e: traceback.print_exc() #if isinstance(e, TypeError): # pickSuccess = True #else: raise e finally: self.is_placing = pickSuccess self.is_picking = False if not pickSuccess: rospy.logerr("Object pick up failed") return place_result = False try: for place_pose in place_poses: rospy.loginfo("Attempting to place object") if self.place(object_name, object_poses[object_name], place_pose): break except Exception as e: traceback.print_exc() raise e finally: self.is_placing = False
def pick_and_place(self, object_id, object_pose, destination_point): object_name = object_id print("Attempting to pick up object " + object_name) self.is_picking = True rospy.loginfo("Finding a valid place pose") place_poses = self.getValidPlacePoses(destination_point) rospy.loginfo("Attempting to pick up object " + object_name) MoveHelper.move_to_neutral("left", True) pickSuccess = False try: pickSuccess = self.pick(object_pose, object_name) rospy.loginfo("Pick success: " + str(pickSuccess)) except Exception as e: traceback.print_exc() raise e finally: self.is_placing = pickSuccess self.is_picking = False if not pickSuccess: rospy.logerr("Object pick up failed") return place_result = False try: for place_pose in place_poses: rospy.loginfo("Attempting to place object") if self.place(object_name, object_pose, place_pose): break except Exception as e: traceback.print_exc() raise e finally: self.is_placing = False
def go(self, args): #moveit_commander.roscpp_initialize(args) #left_arm = baxter_interface.limb.Limb("left") #left_arm.move_to_neutral() object_name = "spoon" MoveHelper.move_to_neutral("left", True) self.scene.remove_world_object(object_name) rospy.sleep(5.0) pose = self.addObject(object_name) place_poses = self.getValidPlacePoses() pickSuccess = False try: pickSuccess = self.pick(pose, object_name) except Exception as e: traceback.print_exc() if isinstance(e, TypeError): pickSuccess = True else: raise e if not pickSuccess: rospy.logerr("Object pick up failed") return place_result = False try: for place_pose in place_poses: rospy.loginfo("Attempting to place object") rospy.loginfo(str(place_pose)) if self.place(object_name, place_pose): break except Exception as e: traceback.print_exc() raise e
def go(self, args): moveit_commander.roscpp_initialize(args) MoveHelper.move_to_neutral("left", True) rospy.sleep(5.0) rospy.spin()