Feature/implement grasp detector in grab

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"})

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"),

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
 

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.

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)

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',

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.75, max_torque: float = 0.15) -> None:
         """
         State for detecting whether the robot is holding something using the gripper position.
 

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,25 @@ 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 = False) -> 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
         """
-        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 +356,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)