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)
TabletopCollisionMapProcessing) req = TabletopCollisionMapProcessingRequest() req.detection_result = tdr req.reset_collision_models = True req.reset_attached_models = True req.reset_static_map = True req.take_static_collision_map = True req.desired_frame = 'base_link' coll_map_res = update_collision_map(req) rospy.loginfo('found %d clusters, grabbing first' % len(coll_map_res.graspable_objects)) #obj_pcluster = res.clusters[0] req = PickupGoal() req.arm_name = 'left_arm' req.target = coll_map_res.graspable_objects[0] #req.target.type = GraspableObject.POINT_CLUSTER #req.target.cluster = obj_pcluster req.desired_approach_distance = 0.05 req.min_approach_distance = 0.03 req.lift.direction.header.frame_id = 'base_link' req.lift.direction.vector.x = 0 req.lift.direction.vector.y = 0 req.lift.direction.vector.z = 1 req.lift.desired_distance = 0.03 req.lift.min_distance = 0.02 req.collision_object_name = coll_map_res.collision_object_names[0] req.collision_support_surface_name = coll_map_res.collision_support_surface_name req.use_reactive_execution = False
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
sys.exit() # 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
dist = sqrt(pow(x_dist, 2) + pow(y_dist, 2)) 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"