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 execute(self):
rospy.loginfo('resetting robot')
self.action_index = 0
self.reset_collision_map()
current_state = find_current_arm_state()
if current_state not in ['READY', 'TUCK']:
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied: self.open_gripper()
if current_state != 'READY' and not self.ready_arm(): return False
self.gentle_close_gripper()
return True
def execute(self):
rospy.loginfo('resetting robot')
self.action_index = 0
self.reset_collision_map()
current_state = find_current_arm_state()
if current_state not in ['READY', 'TUCK']:
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied: self.open_gripper()
if current_state != 'READY' and not self.ready_arm(): 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
