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 b6027f395..5806f09bc 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.75, max_torque: float = 0.1) -> None: """ State for detecting whether the robot is holding something using the gripper position. 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 90b2a5434..812ce6747 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -14,7 +14,10 @@ from ..utility import check_arm_requirements, ResolveArm from ..util.designators import check_type from ..navigation.navigate_to_grasp import NavigateToGrasp + +from ..manipulation.active_grasp_detector import ActiveGraspDetector from ..manipulation.grasp_point_determination import GraspPointDeterminant +from ..human_interaction.human_interaction import Say from ..util.designators.arm import ArmDesignator from ..util.designators.core import Designator @@ -339,9 +342,20 @@ def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator, room: Des 'failed': 'RESET_FAILURE'}) smach.StateMachine.add('GRAB', PickUp(robot, arm, item), - transitions={'succeeded': 'done', + transitions={'succeeded': 'CHECK_PICK_SUCCESSFUL', 'failed': 'RESET_FAILURE'}) + smach.StateMachine.add("CHECK_PICK_SUCCESSFUL", + ActiveGraspDetector(robot, arm), + transitions={'true': "done", + 'false': "SAY_MISSED_GRASP", + 'failed': "RESET_FAILURE", + 'cannot_determine': "SAY_MISSED_GRASP"} + ) + + smach.StateMachine.add("SAY_MISSED_GRASP", Say(robot, "Oops, it seems I missed it."), + transitions={"spoken": "RESET_FAILURE"}) + smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm), transitions={'done': 'failed'})