diff --git a/challenge_cleanup/src/challenge_cleanup/self_cleanup.py b/challenge_cleanup/src/challenge_cleanup/self_cleanup.py index c349a476ae..74e3bef637 100644 --- a/challenge_cleanup/src/challenge_cleanup/self_cleanup.py +++ b/challenge_cleanup/src/challenge_cleanup/self_cleanup.py @@ -275,7 +275,9 @@ def __init__(self, robot, selected_entity_designator, room_des): smach.StateMachine.add("GRAB", Grab(robot, selected_entity_designator, arm_designator), - transitions={"done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET"}) + transitions={"done": "SAY_GRAB_SUCCESS", + "failed": "ARM_RESET", + "object_not_grasped": "ARM_RESET"}) smach.StateMachine.add("ARM_RESET", ArmToJointConfig(robot, arm_designator, "reset"), transitions={"succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED"}) diff --git a/challenge_manipulation/src/manipulation.py b/challenge_manipulation/src/manipulation.py index b2bdd45c25..7a25be6e3e 100755 --- a/challenge_manipulation/src/manipulation.py +++ b/challenge_manipulation/src/manipulation.py @@ -516,7 +516,8 @@ def lock(userdata=None): smach.StateMachine.add( "GRAB_ITEM", Grab(robot, self.current_item, self.empty_arm_designator), transitions={ 'done' :'STORE_ITEM', - 'failed' :'SAY_GRAB_FAILED'}) + 'failed' :'SAY_GRAB_FAILED', + 'object_not_grasped': 'SAY_GRAB_FAILED'}) smach.StateMachine.add( "SAY_GRAB_FAILED", states.Say(robot, ["I couldn't grab this thing"], mood="sad"), diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py index b469e4a42f..53a86f634b 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py +++ b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py @@ -21,6 +21,7 @@ class DefaultGrabDesignator(ds.Designator): """ Designator to pick the closest item on top of the table to grab. This is used for testing """ + def __init__(self, robot, surface_designator, area_description): """ Constructor @@ -116,7 +117,8 @@ def lock(userdata=None): smach.StateMachine.add("GRAB_ITEM", states.Grab(robot, self.grab_designator, self.empty_arm_designator), transitions={'done': 'UNLOCK_ITEM_SUCCEED', - 'failed': 'UNLOCK_ITEM_FAIL'}) + 'failed': 'UNLOCK_ITEM_FAIL', + 'object_not_grasped': 'UNLOCK_ITEM_FAIL'}) @smach.cb_interface(outcomes=["unlocked"]) def lock(userdata=None): @@ -138,6 +140,7 @@ def lock(userdata=None): class PlaceSingleItem(smach.State): """ Tries to place an object. A 'place' statemachine is constructed dynamically since this makes it easier to build a statemachine (have we succeeded in grasping the objects?)""" + def __init__(self, robot, place_designator): """ Constructor diff --git a/hero_skills/src/hero_skills/hero.py b/hero_skills/src/hero_skills/hero.py index 79258e97b6..ff3f0883c7 100644 --- a/hero_skills/src/hero_skills/hero.py +++ b/hero_skills/src/hero_skills/hero.py @@ -41,7 +41,7 @@ def __init__(self, connection_timeout=robot.DEFAULT_CONNECTION_TIMEOUT): hero_arm.add_part('gripper_position_detector', gripper_position_detector.GripperPositionDetector(self.robot_name, self.tf_buffer, - "/" + self.robot_name + "/joint_states")) + "/" + self.robot_name + "/joint_states", -0.75)) self.add_arm_part('arm_center', hero_arm) self.add_body_part('head', head.Head(self.robot_name, self.tf_buffer)) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 1fc81c63df..fce4472fd2 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -155,6 +155,12 @@ def handover_detector(self): self._test_die(hasattr(self._arm, 'handover_detector'), "This arm does not have a handover_detector") return self._arm.handover_detector + @property + def gripper_position_detector(self): + self._test_die(hasattr(self._arm, 'gripper_position_detector'), + "This arm does not have a gripper_position_detector") + return self._arm.gripper_position_detector + def has_gripper_type(self, gripper_type=None): """ Query whether the arm has the provided specific type of gripper. diff --git a/robot_skills/src/robot_skills/arm/gripper_position_detector.py b/robot_skills/src/robot_skills/arm/gripper_position_detector.py index 74a44097c2..a6e5e4cef3 100644 --- a/robot_skills/src/robot_skills/arm/gripper_position_detector.py +++ b/robot_skills/src/robot_skills/arm/gripper_position_detector.py @@ -7,7 +7,7 @@ class GripperPositionDetector(RobotPart): - def __init__(self, robot_name: str, tf_buffer: str, joint_topic: str) -> None: + def __init__(self, robot_name: str, tf_buffer: str, joint_topic: str, minimum_position: float) -> None: """ Class for getting the position of the hand motor joint (how much the gripper is open or closed) Values go from 1 (open) to -1 (closed) @@ -15,6 +15,7 @@ def __init__(self, robot_name: str, tf_buffer: str, joint_topic: str) -> None: :param robot_name: Name of the robot :param tf_buffer: tf2_ros.Buffer for use in RobotPart :param joint_topic: Topic to use for measurement + :param minimum_position: Minimum position to assume that the gripper is holding something """ super(GripperPositionDetector, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) @@ -24,6 +25,7 @@ def __init__(self, robot_name: str, tf_buffer: str, joint_topic: str) -> None: self.current_position = None self.store_position = False # Flag to store the position self.start_time = None + self.minimum_position = minimum_position def _joint_callback(self, msg: JointState) -> None: """ diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index 5dbabaa116..9ca5086000 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -153,6 +153,7 @@ def __init__(self, robot_name, tf_buffer, get_joint_states, name): # add parts self.gripper = Gripper(robot_name, tf_buffer) self.handover_detector = HandoverDetector(robot_name, tf_buffer) + self.gripper_position_detector = GripperPositionDetector(robot_name, tf_buffer) def collect_gripper_types(self, gripper_type): return gripper_type @@ -176,6 +177,13 @@ def __init__(self, robot_name, tf_buffer, *args, **kwargs): self.handover_to_robot = AlteredMagicMock() +class GripperPositionDetector(MockedRobotPart): + def __init__(self, robot_name, tf_buffer, *args, **kwargs): + super(GripperPositionDetector, self).__init__(robot_name, tf_buffer) + self.minimum_position = -0.75 + self.detect = AlteredMagicMock(return_value=0.0) + + class Base(MockedRobotPart): def __init__(self, robot_name, tf_buffer, *args, **kwargs): super(Base, self).__init__(robot_name, tf_buffer) diff --git a/robot_smach_states/src/robot_smach_states/clear.py b/robot_smach_states/src/robot_smach_states/clear.py index b1d2d13db1..7af3591b45 100644 --- a/robot_smach_states/src/robot_smach_states/clear.py +++ b/robot_smach_states/src/robot_smach_states/clear.py @@ -111,7 +111,8 @@ def __init__(self, robot, source_location, source_navArea, target_location, targ smach.StateMachine.add('GRAB', Grab(robot, selected_entity_designator, arm_des), transitions={'done': 'INSPECT_TARGET', - 'failed': 'failed'} + 'failed': 'failed', + 'object_not_grasped': 'failed'} ) smach.StateMachine.add('INSPECT_TARGET', diff --git a/robot_smach_states/src/robot_smach_states/manipulation/active_grasp_detector.py b/robot_smach_states/src/robot_smach_states/manipulation/active_grasp_detector.py index b6027f3957..13b69dbcca 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/active_grasp_detector.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/active_grasp_detector.py @@ -14,7 +14,7 @@ class ActiveGraspDetector(smach.State): REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], } def __init__(self, robot: Robot, arm_designator: ArmDesignator, threshold_difference: float = 0.075, - minimum_position: float = -0.82, max_torque: float = 0.15) -> None: + max_torque: float = 0.15) -> None: """ State for detecting whether the robot is holding something using the gripper position. @@ -26,7 +26,6 @@ def __init__(self, robot: Robot, arm_designator: ArmDesignator, threshold_differ :param robot: Robot to execute the state with :param arm_designator: designator that resolves to arm to check :param threshold_difference: Difference between base and final position - :param minimum_position: Minimum position to assume that the gripper is holding something :param max_torque: Max torque of the gripper to perform the test with """ @@ -36,7 +35,6 @@ def __init__(self, robot: Robot, arm_designator: ArmDesignator, threshold_differ self.arm_designator = arm_designator self.threshold_difference = threshold_difference - self.minimum_position = minimum_position self.max_torque = max_torque def execute(self, userdata=None) -> str: @@ -51,7 +49,7 @@ def execute(self, userdata=None) -> str: if first_position is None: rospy.logerr("Cannot retrieve first position") return 'failed' - elif first_position < self.minimum_position: + elif first_position < arm.gripper_position_detector.minimum_position: rospy.logdebug("First position is {}".format(first_position)) return 'cannot_determine' diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grab.py b/robot_smach_states/src/robot_smach_states/manipulation/grab.py index a864b5e427..81ff722a2d 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -17,14 +17,15 @@ from ..manipulation.grasp_point_determination import GraspPointDeterminant from ..util.designators.arm import ArmDesignator from ..util.designators.core import Designator +from ..manipulation.active_grasp_detector import ActiveGraspDetector +from ..human_interaction import Say class PrepareEdGrasp(smach.State): REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], "required_trajectories": ["prepare_grasp"], } - def __init__(self, robot, arm, grab_entity): - # type: (Robot, ArmDesignator, Designator) -> None + def __init__(self, robot: Robot, arm: ArmDesignator, grab_entity: Designator) -> None: """ Set the arm in the appropriate position before actually grabbing @@ -80,7 +81,8 @@ class PickUp(smach.State): REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], "required_goals": ["carrying_pose"], } - def __init__(self, robot, arm, grab_entity, check_occupancy=False): + def __init__(self, robot: Robot, arm: ArmDesignator, grab_entity: Designator, + check_occupancy: bool = False) -> None: """ Pick up an item given an arm and an entity to be picked up @@ -98,7 +100,7 @@ def __init__(self, robot, arm, grab_entity, check_occupancy=False): self._gpd = GraspPointDeterminant(robot) self._check_occupancy = check_occupancy - assert self.robot.get_arm(**self.REQUIRED_ARM_PROPERTIES) is not None,\ + assert self.robot.get_arm(**self.REQUIRED_ARM_PROPERTIES) is not None, \ "None of the available arms meets all this class's requirements: {}".format(self.REQUIRED_ARM_PROPERTIES) def execute(self, userdata=None): @@ -255,7 +257,7 @@ def execute(self, userdata=None): return result - def associate(self, original_entity): + def associate(self, original_entity: Entity) -> Entity: """ Tries to associate the original entity with one of the entities in the world model. This is useful if after an update, the original entity is no longer present in the world model. If no good map can be found, @@ -290,7 +292,7 @@ class ResetOnFailure(smach.State): REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], } - def __init__(self, robot, arm): + def __init__(self, robot: Robot, arm: ArmDesignator): """ Constructor @@ -320,20 +322,28 @@ def execute(self, userdata=None): class Grab(smach.StateMachine): - def __init__(self, robot, item, arm): + def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, retry: bool = True) -> None: """ - Let the given robot move to an entity and grab that entity using some arm + Let the given robot move to an entity and grab that entity using some arm. Performs grasp detection and retries + if it's not holding anything :param robot: Robot to use :param item: Designator that resolves to the item to grab. E.g. EntityByIdDesignator :param arm: Designator that resolves to the arm to use for grabbing. Eg. UnoccupiedArmDesignator + :param retry: On True the robot will retry the grab if it fails to grasp the object + Note: Don't use the retry option if you want to grab a thin item (E.g. the edge of a bowl) + because it will always trigger "cannot_determine" and the robot will try to grab it again. + ToDo: Make this more robust """ - smach.StateMachine.__init__(self, outcomes=['done', 'failed']) + smach.StateMachine.__init__(self, outcomes=['done', 'failed', 'object_not_grasped']) # Check types or designator resolve types check_type(item, Entity) check_type(arm, PublicArm) + # Check retry + grasp_failed_next_state = 'SAY_RETRY' if retry else 'object_not_grasped' + with self: smach.StateMachine.add('RESOLVE_ARM', ResolveArm(arm, self), transitions={'succeeded': 'NAVIGATE_TO_GRAB', @@ -349,10 +359,47 @@ def __init__(self, robot, item, arm): 'failed': 'RESET_FAILURE'}) smach.StateMachine.add('GRAB', PickUp(robot, arm, item), - transitions={'succeeded': 'done', + transitions={'succeeded': 'GRASP_DETECTOR', + 'failed': 'RESET_FAILURE'}) + + smach.StateMachine.add('GRASP_DETECTOR', ActiveGraspDetector(robot, arm), + transitions={'true': 'done', + 'false': 'SAY_GRASP_NOT_SUCCEEDED', + 'failed': 'done', + 'cannot_determine': 'SAY_GRASP_NOT_SUCCEEDED'}) + + smach.StateMachine.add('SAY_GRASP_NOT_SUCCEEDED', Say(robot, "I failed grasping the object"), + transitions={'spoken': grasp_failed_next_state}) + + smach.StateMachine.add('SAY_RETRY', Say(robot, "I will retry to grab it"), + transitions={'spoken': 'RETRY_NAVIGATE_TO_GRAB'}) + + smach.StateMachine.add('RETRY_NAVIGATE_TO_GRAB', NavigateToGrasp(robot, arm, item), + transitions={'unreachable': 'RESET_FAILURE', + 'goal_not_defined': 'RESET_FAILURE', + 'arrived': 'RETRY_PREPARE_GRASP'}) + + smach.StateMachine.add('RETRY_PREPARE_GRASP', PrepareEdGrasp(robot, arm, item), + transitions={'succeeded': 'RETRY_GRAB', + 'failed': 'RESET_FAILURE'}) + + smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item), + transitions={'succeeded': 'RETRY_GRASP_DETECTOR', 'failed': 'RESET_FAILURE'}) - smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm), + smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm), + transitions={'true': 'done', + 'false': 'SAY_RETRY_NOT_SUCCEEDED', + 'failed': 'done', + 'cannot_determine': 'SAY_RETRY_NOT_SUCCEEDED'}) + + smach.StateMachine.add('SAY_RETRY_NOT_SUCCEEDED', Say(robot, "I failed grasping the object again"), + transitions={'spoken': 'RESET_NOT_GRASPED'}) + + smach.StateMachine.add('RESET_FAILURE', ResetOnFailure(robot, arm), transitions={'done': 'failed'}) + smach.StateMachine.add('RESET_NOT_GRASPED', ResetOnFailure(robot, arm), + transitions={'done': 'object_not_grasped'}) + check_arm_requirements(self, robot)