def reset_robot(self, tabletop_collision_map_processing_result=None): rospy.loginfo('resetting robot') ordered_collision_operations = OrderedCollisionOperations() if tabletop_collision_map_processing_result: closest_index = self.info[0][0] collision_object_name = tabletop_collision_map_processing_result.collision_object_names[ closest_index] collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = ARM_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [ collision_operation ] collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append( collision_operation) collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append( collision_operation) else: self.reset_collision_map() current_state = find_current_arm_state() rospy.loginfo('arm is currently in %s state' % current_state) if current_state not in ['READY', 'TUCK']: # check if the gripper is empty grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: rospy.loginfo( 'arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK'])) self.open_gripper() if current_state != 'READY' and not self.ready_arm( ordered_collision_operations): return False self.gentle_close_gripper() return True
def reset_robot(self, tabletop_collision_map_processing_result=None): rospy.loginfo('resetting robot') ordered_collision_operations = OrderedCollisionOperations() if tabletop_collision_map_processing_result: closest_index = self.info[0][0] collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index] collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = ARM_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [collision_operation] collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append(collision_operation) collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append(collision_operation) else: self.reset_collision_map() current_state = find_current_arm_state() rospy.loginfo('arm is currently in %s state' % current_state) if current_state not in ['READY', 'TUCK']: # check if the gripper is empty grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: rospy.loginfo('arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK'])) self.open_gripper() if current_state != 'READY' and not self.ready_arm(ordered_collision_operations): return False self.gentle_close_gripper() return True
def build_collision_operations(self, arm_name, penetration_depth, enable=False): msg = OrderedCollisionOperations() if arm_name == "right_arm": link_names = ["r_gripper_palm_joint", "r_gripper_l_finger_joint", "r_gripper_l_finger_tip_joint", "r_gripper_led_joint", "r_gripper_motor_accelerometer_joint", "r_gripper_motor_slider_joint", "r_gripper_motor_screw_joint", "r_gripper_r_finger_joint", "r_gripper_r_finger_tip_joint", "r_gripper_joint", "r_gripper_tool_joint", ] else: link_names = ["l_gripper_palm_joint", "l_gripper_l_finger_joint", "l_gripper_l_finger_tip_joint", "l_gripper_led_joint", "l_gripper_motor_accelerometer_joint", "l_gripper_motor_slider_joint", "l_gripper_motor_screw_joint", "l_gripper_r_finger_joint", "l_gripper_r_finger_tip_joint", "l_gripper_joint", "l_gripper_tool_joint", ] for link in link_names: collision = CollisionOperation() if enable: collision.operation = CollisionOperation.ENABLE else: collision.operation = CollisionOperation.DISABLE collision.object1 = link collision.object2 = "collision_map" collision.penetration_distance = penetration_depth msg.collision_operations.append(collision) return msg
def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message('/joint_states', JointState) req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = 'base_link' approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] push_distance = 0.40 grasppos = [ pose.position.x, pose.position.y - push_distance, pose.position.z ] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4 } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [ res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len) ] vels = [ res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len) ] times = [ res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len) ] error_codes = [ error_code_dict[error_code.val] for error_code in res.trajectory_error_codes ] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[ 1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
def put_down_at(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() bbox_center = self.find_cluster_bounding_box_center( goal.graspable_object.cluster) if not bbox_center: self.action_server.set_aborted() goal.target_location.z = bbox_center.z x_dist = goal.target_location.x - bbox_center.x y_dist = goal.target_location.y - bbox_center.y target = goal.graspable_object target.cluster.points = [ Point32(p.x + x_dist, p.y + y_dist, p.z) for p in target.cluster.points ] collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations.collision_operations = [ collision_operation, collision_operation2, collision_operation3 ] # set gripper padding to 0 gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS]) # move into pre-grasp pose if not move_arm_joint_goal( self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record put down sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded( PutDownAtResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted put down failed' % ACTION_NAME) self.action_server.set_aborted()
def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2, collision_operation3 ] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded( PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded(PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message("/joint_states", JointState) req = GetPositionFKRequest() req.header.frame_id = "base_link" req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = "base_link" approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] push_distance = 0.40 grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations, ) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4, } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)] vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)] times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)] error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
def put_down_at(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() bbox_center = self.find_cluster_bounding_box_center(goal.graspable_object.cluster) if not bbox_center: self.action_server.set_aborted() goal.target_location.z = bbox_center.z x_dist = goal.target_location.x - bbox_center.x y_dist = goal.target_location.y - bbox_center.y target = goal.graspable_object target.cluster.points = [Point32(p.x+x_dist, p.y+y_dist, p.z) for p in target.cluster.points] collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations.collision_operations = [collision_operation, collision_operation2, collision_operation3] # set gripper padding to 0 gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) # move into pre-grasp pose if not move_arm_joint_goal(self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record put down sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded(PutDownAtResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted put down failed' % ACTION_NAME) self.action_server.set_aborted()