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
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 # do not use tactile-based grasping or tactile-based lift goal.use_reactive_execution = 0 goal.use_reactive_lift = 0 # send the goal try: grasper_grasp_action_client.send_goal(goal) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %e" (grasper_grasp_name, e))
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 # do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False pickup_goal.use_reactive_execution = False pickup_client.send_goal_and_wait(pickup_goal, rospy.Duration(120.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 # do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False pickup_goal.use_reactive_execution = False pickup_client.send_goal_and_wait(pickup_goal, rospy.Duration(120.0))