From 6642e7532afb792dee2e8085c74a5bd5fc525d96 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 13 Sep 2022 20:03:39 +0200 Subject: [PATCH 01/23] Added grasp detection after grab and retry grab --- .../robot_smach_states/manipulation/grab.py | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) 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..97a2adee7b 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -17,6 +17,8 @@ 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 robot_smach_states.human_interaction import Say class PrepareEdGrasp(smach.State): @@ -349,9 +351,28 @@ 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': 'RETRY_GRAB', + 'failed': 'done', + 'cannot determine': 'done'}) + + smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item), + transitions={'succeeded': 'RETRY_GRASP_DETECTOR', + 'failed': 'RETRY_GRASP_DETECTOR'}) + + smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm), + transitions={'true': 'done', + 'false': 'SAY_FAILED', + 'failed': 'done', + 'cannot determine': 'done'}) + + smach.StateMachine.add('SAY_FAILED', Say(robot, "I failed grabbing the object"), + transitions={'spoken': 'failed'}) + smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm), transitions={'done': 'failed'}) From 802f7665a80f64ece66dc14be6dd96c026eeb937 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 13 Sep 2022 20:33:19 +0200 Subject: [PATCH 02/23] Fixed typo in state machine --- .../src/robot_smach_states/manipulation/grab.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 97a2adee7b..97281182b6 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -324,7 +324,8 @@ def execute(self, userdata=None): class Grab(smach.StateMachine): def __init__(self, robot, item, arm): """ - 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 @@ -358,7 +359,7 @@ def __init__(self, robot, item, arm): transitions={'true': 'done', 'false': 'RETRY_GRAB', 'failed': 'done', - 'cannot determine': 'done'}) + 'cannot determine': 'RETRY_GRAB'}) smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item), transitions={'succeeded': 'RETRY_GRASP_DETECTOR', @@ -368,7 +369,7 @@ def __init__(self, robot, item, arm): transitions={'true': 'done', 'false': 'SAY_FAILED', 'failed': 'done', - 'cannot determine': 'done'}) + 'cannot determine': 'SAY_FAILED'}) smach.StateMachine.add('SAY_FAILED', Say(robot, "I failed grabbing the object"), transitions={'spoken': 'failed'}) From 0d35c55ca5e3a19e60afe6e44cdfc49516e4b750 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 13 Sep 2022 20:49:23 +0200 Subject: [PATCH 03/23] Fixed underscore --- .../src/robot_smach_states/manipulation/grab.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 97281182b6..4eda8bd694 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -359,7 +359,7 @@ def __init__(self, robot, item, arm): transitions={'true': 'done', 'false': 'RETRY_GRAB', 'failed': 'done', - 'cannot determine': 'RETRY_GRAB'}) + 'cannot_determine': 'RETRY_GRAB'}) smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item), transitions={'succeeded': 'RETRY_GRASP_DETECTOR', @@ -369,7 +369,7 @@ def __init__(self, robot, item, arm): transitions={'true': 'done', 'false': 'SAY_FAILED', 'failed': 'done', - 'cannot determine': 'SAY_FAILED'}) + 'cannot_determine': 'SAY_FAILED'}) smach.StateMachine.add('SAY_FAILED', Say(robot, "I failed grabbing the object"), transitions={'spoken': 'failed'}) From 2be6ad180789ae0971d478e603b07160776866e8 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 13 Sep 2022 21:15:15 +0200 Subject: [PATCH 04/23] Add position detector --- robot_skills/src/robot_skills/arm/arms.py | 6 ++++++ 1 file changed, 6 insertions(+) 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. From 6d53e0f4bcd5ae1680053932349c5e12dd4387ea Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 13 Sep 2022 21:35:10 +0200 Subject: [PATCH 05/23] change min position after testing --- .../robot_smach_states/manipulation/active_grasp_detector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..462b951970 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: + minimum_position: float = -0.80, max_torque: float = 0.15) -> None: """ State for detecting whether the robot is holding something using the gripper position. From 1461fa47ffca7dd3cd4d733cb485de2dff7ba2a0 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Wed, 14 Sep 2022 09:57:56 +0200 Subject: [PATCH 06/23] Improved import --- robot_smach_states/src/robot_smach_states/manipulation/grab.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4eda8bd694..e433612e45 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -18,7 +18,7 @@ from ..util.designators.arm import ArmDesignator from ..util.designators.core import Designator from ..manipulation.active_grasp_detector import ActiveGraspDetector -from robot_smach_states.human_interaction import Say +from ..human_interaction import Say class PrepareEdGrasp(smach.State): From f79701cfca6dcf4f38f1ffb57995bbe99c41afa7 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 27 Sep 2022 21:13:55 +0200 Subject: [PATCH 07/23] Added outcome object_not_grasped of Grab --- challenge_manipulation/src/manipulation.py | 3 ++- .../src/challenge_storing_groceries/manipulate_machine.py | 3 ++- robot_smach_states/src/robot_smach_states/clear.py | 3 ++- .../src/robot_smach_states/manipulation/grab.py | 6 +++--- 4 files changed, 9 insertions(+), 6 deletions(-) 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..68820c0d99 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py +++ b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py @@ -116,7 +116,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): 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/grab.py b/robot_smach_states/src/robot_smach_states/manipulation/grab.py index e433612e45..a374f9b476 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -331,7 +331,7 @@ def __init__(self, robot, item, arm): :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 """ - 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) @@ -357,7 +357,7 @@ def __init__(self, robot, item, arm): smach.StateMachine.add('GRASP_DETECTOR', ActiveGraspDetector(robot, arm), transitions={'true': 'done', - 'false': 'RETRY_GRAB', + 'false': 'object_not_grasped', 'failed': 'done', 'cannot_determine': 'RETRY_GRAB'}) @@ -367,7 +367,7 @@ def __init__(self, robot, item, arm): smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm), transitions={'true': 'done', - 'false': 'SAY_FAILED', + 'false': 'object_not_grasped', 'failed': 'done', 'cannot_determine': 'SAY_FAILED'}) From 78798dfe0dd454fd07f1f82ba660c9d9f7f1ab26 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Tue, 27 Sep 2022 21:23:07 +0200 Subject: [PATCH 08/23] Added outcome to manipulate_machine --- .../src/challenge_storing_groceries/manipulate_machine.py | 1 + 1 file changed, 1 insertion(+) 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 68820c0d99..72685f2309 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py +++ b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py @@ -119,6 +119,7 @@ def lock(userdata=None): 'failed': 'UNLOCK_ITEM_FAIL', 'object_not_grasped': 'UNLOCK_ITEM_FAIL'}) + @smach.cb_interface(outcomes=["unlocked"]) def lock(userdata=None): """ 'Locks' a locking designator """ From 97150dddc144639868b6052ebc219b439a78444c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rodrigo=20Mart=C3=ADn=20N=C3=BA=C3=B1ez?= <83213317+P-ict0@users.noreply.github.com> Date: Sat, 1 Oct 2022 10:03:26 +0200 Subject: [PATCH 09/23] Go to failure if retry fails Co-authored-by: PetervDooren <19806646+PetervDooren@users.noreply.github.com> --- robot_smach_states/src/robot_smach_states/manipulation/grab.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a374f9b476..66f62f7013 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -363,7 +363,7 @@ def __init__(self, robot, item, arm): smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item), transitions={'succeeded': 'RETRY_GRASP_DETECTOR', - 'failed': 'RETRY_GRASP_DETECTOR'}) + 'failed': 'RESET_FAILURE'}) smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm), transitions={'true': 'done', From e86de40e6e7d31b7e7e4193b44ef20acadea181e Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Sat, 1 Oct 2022 10:23:30 +0200 Subject: [PATCH 10/23] Added type hinting --- .../src/robot_smach_states/manipulation/grab.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 66f62f7013..68052ac81e 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -25,8 +25,7 @@ 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 @@ -82,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 @@ -100,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): @@ -292,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 @@ -322,7 +322,7 @@ def execute(self, userdata=None): class Grab(smach.StateMachine): - def __init__(self, robot, item, arm): + def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator): """ 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 From c466380743295a066aa53c752f5bcddef0858b42 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Sat, 1 Oct 2022 10:43:55 +0200 Subject: [PATCH 11/23] Completed type hinting --- robot_smach_states/src/robot_smach_states/manipulation/grab.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 68052ac81e..55b2e80073 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -257,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, From 66232e5afd2a7d8abe3ccabf0d023f38212f257d Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Sat, 1 Oct 2022 10:45:04 +0200 Subject: [PATCH 12/23] Added retry parameter and announcements from robot --- .../robot_smach_states/manipulation/grab.py | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) 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 55b2e80073..ad45f7ad53 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -322,7 +322,7 @@ def execute(self, userdata=None): class Grab(smach.StateMachine): - def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator): + def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, retry: bool = False) -> None: """ 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 @@ -330,6 +330,7 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator): :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 """ smach.StateMachine.__init__(self, outcomes=['done', 'failed', 'object_not_grasped']) @@ -337,6 +338,9 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator): 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', @@ -357,9 +361,15 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator): smach.StateMachine.add('GRASP_DETECTOR', ActiveGraspDetector(robot, arm), transitions={'true': 'done', - 'false': 'object_not_grasped', + 'false': 'SAY_GRASP_NOT_SUCCEEDED', 'failed': 'done', - 'cannot_determine': 'RETRY_GRAB'}) + '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_GRAB'}) smach.StateMachine.add('RETRY_GRAB', PickUp(robot, arm, item), transitions={'succeeded': 'RETRY_GRASP_DETECTOR', @@ -367,14 +377,14 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator): smach.StateMachine.add('RETRY_GRASP_DETECTOR', ActiveGraspDetector(robot, arm), transitions={'true': 'done', - 'false': 'object_not_grasped', + 'false': 'SAY_RETRY_NOT_SUCCEEDED', 'failed': 'done', - 'cannot_determine': 'SAY_FAILED'}) + 'cannot_determine': 'SAY_RETRY_NOT_SUCCEEDED'}) - smach.StateMachine.add('SAY_FAILED', Say(robot, "I failed grabbing the object"), - transitions={'spoken': 'failed'}) + smach.StateMachine.add('SAY_RETRY_NOT_SUCCEEDED', Say(robot, "I failed grasping the object"), + transitions={'spoken': 'RESET_FAILURE'}) - smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm), + smach.StateMachine.add('RESET_FAILURE', ResetOnFailure(robot, arm), transitions={'done': 'failed'}) check_arm_requirements(self, robot) From 8c17a90dfe5557dbfff5bb16d90842cfcca932a1 Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Sat, 1 Oct 2022 10:56:21 +0200 Subject: [PATCH 13/23] Added outcome to cleanup --- challenge_cleanup/src/challenge_cleanup/self_cleanup.py | 4 +++- .../src/challenge_storing_groceries/manipulate_machine.py | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) 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_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py index 72685f2309..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 @@ -119,7 +120,6 @@ def lock(userdata=None): 'failed': 'UNLOCK_ITEM_FAIL', 'object_not_grasped': 'UNLOCK_ITEM_FAIL'}) - @smach.cb_interface(outcomes=["unlocked"]) def lock(userdata=None): """ 'Locks' a locking designator """ @@ -140,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 From b2777008ad8be6e4c93643d8a2eacf550ffc7f7e Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Sat, 1 Oct 2022 11:37:45 +0200 Subject: [PATCH 14/23] add gripperpositiondetector to mockbot --- robot_skills/src/robot_skills/mockbot.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index 5dbabaa116..027326f421 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,12 @@ 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.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) From 7d916029c8ea72f6c86e053d0e873e57003e516f Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Sat, 1 Oct 2022 13:28:26 +0200 Subject: [PATCH 15/23] Changed value after testing --- .../robot_smach_states/manipulation/active_grasp_detector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 462b951970..6a5be85d51 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.80, max_torque: float = 0.15) -> None: + minimum_position: float = -0.75, max_torque: float = 0.15) -> None: """ State for detecting whether the robot is holding something using the gripper position. From a763de0aefa0e6f88245338e4d67c7835f26eb1b Mon Sep 17 00:00:00 2001 From: P-ict0 Date: Sat, 1 Oct 2022 13:31:30 +0200 Subject: [PATCH 16/23] Fixed retry --- .../src/robot_smach_states/manipulation/grab.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 ad45f7ad53..a4954cc727 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -369,7 +369,16 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, retry: bo transitions={'spoken': grasp_failed_next_state}) smach.StateMachine.add('SAY_RETRY', Say(robot, "I will retry to grab it"), - transitions={'spoken': 'RETRY_GRAB'}) + 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', From 5bacddff6923d67164f5aec8450df3387a6368a5 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Sat, 1 Oct 2022 16:31:24 +0200 Subject: [PATCH 17/23] add outcome for the object not grasped --- .../src/robot_smach_states/manipulation/grab.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 a4954cc727..c82dfd2d9c 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -390,10 +390,13 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, retry: bo 'failed': 'done', 'cannot_determine': 'SAY_RETRY_NOT_SUCCEEDED'}) - smach.StateMachine.add('SAY_RETRY_NOT_SUCCEEDED', Say(robot, "I failed grasping the object"), - transitions={'spoken': 'RESET_FAILURE'}) + 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) From f31614e88faffda4099e18660748a122652cdb69 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Tue, 11 Oct 2022 21:08:30 +0200 Subject: [PATCH 18/23] Add value -0.75 to grasp --- hero_skills/src/hero_skills/hero.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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)) From dca7493bae28aa5d53df8da60f4ded926fc37078 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Sun, 16 Oct 2022 15:53:46 +0200 Subject: [PATCH 19/23] Add attribute and old docstring minimum_position to gripper_position_detector --- .../src/robot_skills/arm/gripper_position_detector.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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: """ From b8935189b69211b46a5739f87dbf3a1b459f34bb Mon Sep 17 00:00:00 2001 From: Gustavo Date: Sun, 16 Oct 2022 15:56:58 +0200 Subject: [PATCH 20/23] Add attribute minimum_position to GripperPositionDetector in mockbot.py --- robot_skills/src/robot_skills/mockbot.py | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index 027326f421..9ca5086000 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -180,6 +180,7 @@ def __init__(self, robot_name, tf_buffer, *args, **kwargs): 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) From 7631ff9df5f454514a75c2e42148d50f57bf1513 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Sun, 16 Oct 2022 16:09:18 +0200 Subject: [PATCH 21/23] Add minimum_position from the gripper_position_detector to compare in active_grasp_detector --- .../manipulation/active_grasp_detector.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 6a5be85d51..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.75, 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' From c6d2d7736bdeb27e752daa94aa4b5121d05a3f50 Mon Sep 17 00:00:00 2001 From: Rodrigo Date: Sat, 29 Apr 2023 20:57:24 +0100 Subject: [PATCH 22/23] (grab.py) Added docs --- robot_smach_states/src/robot_smach_states/manipulation/grab.py | 3 +++ 1 file changed, 3 insertions(+) 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 c82dfd2d9c..dd9782c449 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -331,6 +331,9 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, retry: bo :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', 'object_not_grasped']) From a6b42fea481ae6b2e2830d099c5f6954bc35e61d Mon Sep 17 00:00:00 2001 From: Rodrigo Date: Sat, 29 Apr 2023 21:26:42 +0100 Subject: [PATCH 23/23] (temp) change retry to true --- robot_smach_states/src/robot_smach_states/manipulation/grab.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 dd9782c449..81ff722a2d 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -322,7 +322,7 @@ def execute(self, userdata=None): class Grab(smach.StateMachine): - def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, retry: bool = False) -> None: + 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. Performs grasp detection and retries if it's not holding anything