def execute(self, graspable_object, graspable_object_name, collision_support_surface_name, pickup_direction=None, arm_name="right_arm", desired_approach_distance = 0.05, min_approach_distance = 0.02, desired_lift_distance = 0.25, min_lift_distance = 0.2): """ Pickup the object. """ pickup_goal = PickupGoal() pickup_goal.target = graspable_object pickup_goal.collision_object_name = graspable_object_name pickup_goal.collision_support_surface_name = collision_support_surface_name pickup_goal.arm_name = arm_name pickup_goal.desired_approach_distance = desired_approach_distance pickup_goal.min_approach_distance = min_approach_distance if pickup_direction != None: pickup_goal.lift.direction = pickup_direction else: #follow z direction pickup_direction = Vector3Stamped() pickup_direction.header.stamp = rospy.get_rostime() pickup_direction.header.frame_id = "/base_link"; pickup_direction.vector.x = 0; pickup_direction.vector.y = 0; pickup_direction.vector.z = 1; pickup_goal.lift.direction = pickup_direction; #request a vertical lift of 15cm after grasping the object pickup_goal.lift.desired_distance = desired_lift_distance pickup_goal.lift.min_distance = min_lift_distance #do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = True; pickup_goal.use_reactive_execution = True; self.pickup_client.send_goal(pickup_goal) #timeout after 1sec #TODO: change this when using the robot self.pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(90.0)) rospy.loginfo("Got Pickup results") pickup_result = self.pickup_client.get_result() if self.pickup_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("The pickup action has failed: " + str(pickup_result.manipulation_result.value) ) return -1 return pickup_result
def pickup_object(self, collision_map_processing_msg, which_arm, index=0, desired_approach_distance=0.2, min_approach_distance=0.05, lift_desired_distance=0.1, lift_min_distance=0.05, wait=rospy.Duration()): ''' Picks up a previously detected object @param collision_map_processing_msg: The message resulting from calling a TabletopCollisionMapProcessing service. @param index: The index of the object to grasp, if more than one @param which_arm: left_arm or right_arm @param desired_approach_distance: how far the pre-grasp should ideally be away from the grasp @param min_approach_distance: how much distance between pre-grasp and grasp must actually be feasible for the grasp not to be rejected @param lift_desired_distance: the desired lift distance @param lift_min_distance: the min distance that must be considered feasible before the grasp is even attempted @param wait: how long to wait for the action to be successful ''' pickup_goal = PickupGoal() pickup_goal.target = collision_map_processing_msg.graspable_objects[ index] pickup_goal.collision_object_name = collision_map_processing_msg.collision_object_names[ index] pickup_goal.collision_support_surface_name = collision_map_processing_msg.collision_support_surface_name pickup_goal.arm_name = which_arm pickup_goal.lift.direction.header.frame_id = "base_link" pickup_goal.lift.direction.vector.x = 0 pickup_goal.lift.direction.vector.y = 0 pickup_goal.lift.direction.vector.z = 1 pickup_goal.lift.desired_distance = lift_desired_distance pickup_goal.lift.min_distance = lift_min_distance # do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False pickup_goal.use_reactive_execution = False self.pickup_client.send_goal_and_wait(pickup_goal, wait)
def pickup_object(self, collision_map_processing_msg, which_arm, index = 0, desired_approach_distance = 0.2, min_approach_distance = 0.05, lift_desired_distance = 0.1, lift_min_distance = 0.05, wait = rospy.Duration()): ''' Picks up a previously detected object @param collision_map_processing_msg: The message resulting from calling a TabletopCollisionMapProcessing service. @param index: The index of the object to grasp, if more than one @param which_arm: left_arm or right_arm @param desired_approach_distance: how far the pre-grasp should ideally be away from the grasp @param min_approach_distance: how much distance between pre-grasp and grasp must actually be feasible for the grasp not to be rejected @param lift_desired_distance: the desired lift distance @param lift_min_distance: the min distance that must be considered feasible before the grasp is even attempted @param wait: how long to wait for the action to be successful ''' pickup_goal = PickupGoal() pickup_goal.target = collision_map_processing_msg.graspable_objects[index] pickup_goal.collision_object_name = collision_map_processing_msg.collision_object_names[index] pickup_goal.collision_support_surface_name = collision_map_processing_msg.collision_support_surface_name pickup_goal.arm_name = which_arm pickup_goal.lift.direction.header.frame_id = "base_link" pickup_goal.lift.direction.vector.x = 0 pickup_goal.lift.direction.vector.y = 0 pickup_goal.lift.direction.vector.z = 1 pickup_goal.lift.desired_distance = lift_desired_distance pickup_goal.lift.min_distance = lift_min_distance # do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False pickup_goal.use_reactive_execution = False self.pickup_client.send_goal_and_wait(pickup_goal, wait)
def pickup_object( self, graspable, graspable_name, table_name, which_arm, execute=True, lift_desired_distance=0.1, lift_min_distance=0.05, allow_support_collision=True, ignore_collisions=False, desired_grasps=None, wait=rospy.Duration(), ): """Picks up a previously detected object. Parameters: graspable: an object_manipulation_msgs/GraspableObject msg instance. This usually comes from a Detector.call_collision_map_processing call. graspable_name: the name of the object to graps. It is provided by Detector.call_collision_map_processing. table_name: the name of the table. Again provided by Detector.call_collision_map_processing. which_arm: left_arm or right_arm exectute: True|False, whether to execute the grasp or just check for feasibility lift_desired_distance: the desired lift distance lift_min_distance: the min distance that must be considered feasible before the grasp is even attempted allow_support_collision: whether collisions between the gripper and the support surface should be acceptable during move from pre-grasp to grasp and during lift. Collisions when moving to the pre-grasp location are still not allowed even if this is set to true. ignore_collisions: set this to true if you want to ignore all collisions throughout the pickup and also move directly to the pre-grasp using Cartesian controllers desired_grasps: a list of custom grasps. If not set, pickup will automatically generate grasps. wait: how long to wait for the action to be successful Return: a object_manipulation_msgs.PickupResult msg """ rospy.loginfo("Calling the pickup action") pickup_goal = PickupGoal() pickup_goal.target = graspable pickup_goal.collision_object_name = graspable_name pickup_goal.collision_support_surface_name = table_name pickup_goal.only_perform_feasibility_test = not execute pickup_goal.arm_name = which_arm pickup_goal.lift.direction.header.frame_id = "base_link" pickup_goal.lift.direction.vector.x = 0 pickup_goal.lift.direction.vector.y = 0 pickup_goal.lift.direction.vector.z = 1 pickup_goal.lift.desired_distance = lift_desired_distance pickup_goal.lift.min_distance = lift_min_distance # do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False pickup_goal.use_reactive_execution = False pickup_goal.allow_gripper_support_collision = allow_support_collision pickup_goal.ignore_collisions = ignore_collisions if desired_grasps: pickup_goal.desired_grasps = desired_grasps self.pickup_client.send_goal_and_wait(pickup_goal, wait) res = self.pickup_client.get_result() assert isinstance(res, PickupResult) manipulation_result = res.manipulation_result if manipulation_result.value != manipulation_result.SUCCESS: rospy.logerr("Error during pickup") return None self.last_pickup_result = res return res
def pick_up(self, whicharm="l", NUM_TRIES=5, near_pose=None): self.pickup_client.cancel_goal() for i in range(NUM_TRIES): rospy.loginfo("Attempt %d to grasp " % i ) # call the tabletop detection rospy.loginfo("Calling tabletop detector") detection_call = TabletopDetectionRequest() # we want recognized database objects returned # set this to false if you are using the pipeline without the database detection_call.return_clusters = True #we want the individual object point clouds returned as well detection_call.return_models = True detection_call.num_models = 1 detection_call_response = self.object_detection_srv(detection_call).detection if not detection_call_response: rospy.loginfo("Tabletop detection service failed") success=False; break if detection_call_response.result != detection_call_response.SUCCESS: rospy.loginfo("Tabletop detection returned error code %d", detection_call_response.result) success=False; break if len(detection_call_response.clusters)==0\ and len(detection_call_response.models)==0 : rospy.loginfo("The tabletop detector detected the table, " "but found no objects") success=False; break # call collision map processing rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() # pass the result of the tabletop detection processing_call.detection_result = detection_call_response print detection_call_response.models import pdb;pdb.set_trace() # ask for the existing map and collision models to be reset processing_call.reset_collision_models = True processing_call.reset_attached_models = True #ask for the results to be returned in base link frame processing_call.desired_frame = "base_link" processing_call_response = self.collision_processing_srv(processing_call) if not processing_call_response: rospy.loginfo("Collision map processing service failed") success=False; break #the collision map processor returns instances of graspable objects if len(processing_call_response.graspable_objects) == 0: rospy.loginfo("Collision map processing returned no graspable objects") success=False; break graspable_objects = processing_call_response.graspable_objects # sort graspable objects import pdb; # sort graspable objects objects_with_offsets = [None]*len(graspable_objects) #near_point = np.array(list(near_pose[:3]) + [1.0]) for i, obj in enumerate(graspable_objects): pdb.set_trace() points = point_cloud_to_mat(obj.cluster) #centroid =np.reshape(np.mean(points, axis=1), 4) - near_point #offset = np.linalg.norm(centroid - near_point) #objects_with_offsets[i] = (offset,obj) #ordering = sorted(objects_with_offsets, key=lambda item:float((item[0]))) #graspable_objects = [items[1] for items in ordering] #print [items[0] for items in ordering] for i, grasp_object in enumerate(graspable_objects): # call object pickup rospy.loginfo("Calling the pickup action"); pickup_goal = PickupGoal() # pass one of the graspable objects returned # by the collision map processor #pickup_goal.target = processing_call_response.graspable_objects[0] pickup_goal.target = grasp_object # pass the name that the object has in the collision environment # this name was also returned by the collision map processor pickup_goal.collision_object_name = \ processing_call_response.collision_object_names[i] # pass the collision name of the table, also returned by the collision # map processor pickup_goal.collision_support_surface_name = \ processing_call_response.collision_support_surface_name # pick up the object with the left arm if whicharm=="l": pickup_goal.arm_name = "left_arm" else: pickup_goal.arm_name = "right_arm" # we will be lifting the object along the "vertical" direction # which is along the z axis in the base_link frame direction = Vector3Stamped() direction.header.stamp = rospy.Time.now() direction.header.frame_id = "base_link" direction.vector.x = 0 direction.vector.y = 0 direction.vector.z = 1 pickup_goal.lift.direction = direction #request a vertical lift of 10cm after grasping the object pickup_goal.lift.desired_distance = 0.1; pickup_goal.lift.min_distance = 0.05; #do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False; pickup_goal.use_reactive_execution = False; #pickup_goal.allow_gripper_support_collision = True #pickup_goal.ignore_collisions = True #send the goal rospy.loginfo("Waiting for the pickup action...") self.pickup_client.send_goal_and_wait(pickup_goal,rospy.Duration(15)) pickup_result =self.pickup_client.get_result() assert isinstance(pickup_result, PickupResult) success = pickup_result.manipulation_result.value ==\ pickup_result.manipulation_result.SUCCESS if success: rospy.loginfo("Success! Grasped Object.") return success if not success: rospy.loginfo("failed to grasp object") return success
def pickup(self, graspable_object, graspable_object_name, object_name): """ Try to pick up the given object. Sends a message (PickupGoal from actionlib_msgs) to the manipularior action server on /object_manipulator/object_manipulator_pickup/goal """ info_tmp = "Picking up " + object_name rospy.loginfo(info_tmp) pickup_goal = PickupGoal() pickup_goal.target = graspable_object pickup_goal.collision_object_name = graspable_object_name pickup_goal.collision_support_surface_name = self.gui.collision_support_surface_name #pickup_goal.additional_link_padding pickup_goal.ignore_collisions = True pickup_goal.arm_name = "right_arm" #pickup_goal.desired_approach_distance = 0.08 This does not exist anymore in the message #pickup_goal.min_approach_distance = 0.02 This does not exist anymore in the message direction = Vector3Stamped() direction.header.stamp = rospy.get_rostime() direction.header.frame_id = "/base_link" direction.vector.x = 0 direction.vector.y = 0 direction.vector.z = 1 pickup_goal.lift.direction = direction #request a vertical lift of 15cm after grasping the object pickup_goal.lift.desired_distance = 0.1 pickup_goal.lift.min_distance = 0.07 #do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = True pickup_goal.use_reactive_execution = True self.pickup_result = self.pickupservice.pick(pickup_goal) #pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction) #pickup_client.wait_for_server() #rospy.loginfo("Pickup server ready") #pickup_client.send_goal(pickup_goal) #TODO: change this when using the robot #pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0)) loginfo("Got Pickup results") #self.pickup_result = pickup_client.get_result() #print "Pickup result: "+str(self.pickup_result) ''' if pickup_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("The pickup action has failed: " + str(self.pickup_result.manipulation_result.value) ) QMessageBox.warning(self, "Warning", "Pickup action failed: "+str(self.pickup_result.manipulation_result.value)) for tested_grasp,tested_grasp_result in zip(self.pickup_result.attempted_grasps,self.pickup_result.attempted_grasp_results): if tested_grasp_result.result_code==7: self.grasp_display_publisher.publish(tested_grasp) return -1 ''' if self.pickup_result.manipulation_result.value == ManipulationResult.SUCCESS: loginfo("Pick succeeded, now lifting") self.pickupservice.lift(0.07) else: loginfo("Pick failed") return 1 return 0
# print out detected objects for (ind, object) in enumerate(col_res.graspable_objects): if object.type == GraspableObject.DATABASE_MODEL: rospy.loginfo("object %d: recognized object with id %d" % (ind, object.model_pose.model_id)) else: rospy.loginfo("object %d: point cluster with %d points" % (ind, len(object.cluster.points))) ''' object pickup ''' rospy.loginfo("grasping object...") # call object pickup goal = PickupGoal() # pass one of the graspable objects returned by the collision map processor goal.target = col_res.graspable_objects[0] # pass the name that the object has in the collision environment, this name was also returned by the collision map processor goal.collision_object_name = col_res.collision_object_names[0] # pass the collision name of the table, also returned by the collision map processor goal.collision_support_surface_name = col_res.collision_support_surface_name # pick up the object with the left arm goal.arm_name = "left_arm" # specify the desired distance between pre-grasp and final grasp goal.desired_approach_distance = .1 goal.min_approach_distance = .05 # we will be lifting the object along the vertical direction which is along the z axis in the base_link frame goal.lift = GripperTranslation() goal.lift.direction = create_vector3_stamped([0., 0., 1.], 'base_link') # request a vertical lift of 10cm after grasping the object goal.lift.desired_distance = .1 goal.lift.min_distance = .05
rospy.loginfo(dist) if(dist < min_dist): min_dist = dist min_index = index index += 1 pickup_client = SimpleActionClient("/object_manipulator/object_manipulator_pickup", PickupAction) pickup_client.wait_for_server() rospy.loginfo("Calling the pickup action") pickup_goal = PickupGoal() pickup_goal.target = col_res.graspable_objects[min_index]; #pass the name that the object has in the collision environment #this name was also returned by the collision map processor pickup_goal.collision_object_name = col_res.collision_object_names[min_index] #pass the collision name of the table, also returned by the collision #map processor pickup_goal.collision_support_surface_name = col_res.collision_support_surface_name #pick up the object with the left arm pickup_goal.arm_name = "left_arm" #we will be lifting the object along the "vertical" direction #which is along the z axis in the base_link frame direction = Vector3Stamped()
rospy.loginfo(dist) if (dist < min_dist): min_dist = dist min_index = index index += 1 pickup_client = SimpleActionClient( "/object_manipulator/object_manipulator_pickup", PickupAction) pickup_client.wait_for_server() rospy.loginfo("Calling the pickup action") pickup_goal = PickupGoal() pickup_goal.target = col_res.graspable_objects[min_index] #pass the name that the object has in the collision environment #this name was also returned by the collision map processor pickup_goal.collision_object_name = col_res.collision_object_names[ min_index] #pass the collision name of the table, also returned by the collision #map processor pickup_goal.collision_support_surface_name = col_res.collision_support_surface_name #pick up the object with the left arm pickup_goal.arm_name = "left_arm" #we will be lifting the object along the "vertical" direction #which is along the z axis in the base_link frame
sys.exit() rospy.loginfo("Calling Pickup") max_len = 0 index = 0 for i, c in enumerate(detection_reply.detection.clusters): if len(c.points) > max_len: max_len = len(c.points) index = i rospy.loginfo( "Grasping object %d with %d points" % (index, len(detection_reply.detection.clusters[index].points))) pickup_goal = PickupGoal() pickup_goal.target = processing_reply.graspable_objects[index] pickup_goal.collision_object_name = processing_reply.collision_object_names[ index] pickup_goal.collision_support_surface_name = processing_reply.collision_support_surface_name pickup_goal.arm_name = "left_arm" pickup_goal.desired_approach_distance = 0.2 pickup_goal.min_approach_distance = 0.05 pickup_goal.lift.direction.header.frame_id = "base_link" pickup_goal.lift.direction.vector.x = 0 pickup_goal.lift.direction.vector.y = 0 pickup_goal.lift.direction.vector.z = 1 # request a vertical lift of 10cm after grasping the object pickup_goal.lift.desired_distance = 0.1
sys.exit() sys.exit() rospy.loginfo("Calling Pickup") max_len = 0 index = 0 for i,c in enumerate(detection_reply.detection.clusters): if len(c.points) > max_len: max_len = len(c.points) index = i rospy.loginfo("Grasping object %d with %d points"%(index, len(detection_reply.detection.clusters[index].points))) pickup_goal = PickupGoal() pickup_goal.target = processing_reply.graspable_objects[index] pickup_goal.collision_object_name = processing_reply.collision_object_names[index] pickup_goal.collision_support_surface_name = processing_reply.collision_support_surface_name pickup_goal.arm_name = "left_arm" pickup_goal.desired_approach_distance = 0.2 pickup_goal.min_approach_distance = 0.05 pickup_goal.lift.direction.header.frame_id = "base_link" pickup_goal.lift.direction.vector.x = 0 pickup_goal.lift.direction.vector.y = 0 pickup_goal.lift.direction.vector.z = 1 # request a vertical lift of 10cm after grasping the object pickup_goal.lift.desired_distance = 0.1 pickup_goal.lift.min_distance = 0.05