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
 

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

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/arm/gripper_position_detector.py

@@ -7,14 +7,15 @@
 
 
 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)
 
         :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:
         """

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)

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:
+                 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'
 

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)