From 4c26c6aa4ad2d52200bad9b93d367703c01bf8f2 Mon Sep 17 00:00:00 2001 From: Caila Marashaj <98041399+caila-marashaj@users.noreply.github.com> Date: Fri, 8 Dec 2023 14:04:49 -0500 Subject: [PATCH] feat(api): raise an error when the gripper pickup fails (#14130) --- .../instruments/ot3/gripper.py | 26 ++- .../protocol_engine/errors/__init__.py | 1 + .../execution/labware_movement.py | 12 +- .../hardware_control/test_gripper.py | 49 ++++- .../test_labware_movement_handler.py | 180 ++++++++++++++---- shared-data/errors/definitions/1/errors.json | 4 + .../gripper/definitions/1/gripperV1.1.json | 3 +- .../gripper/definitions/1/gripperV1.2.json | 3 +- .../gripper/definitions/1/gripperV1.json | 3 +- shared-data/gripper/schemas/1.json | 5 + shared-data/js/types.ts | 1 + .../opentrons_shared_data/errors/codes.py | 1 + .../errors/exceptions.py | 17 ++ .../gripper/gripper_definition.py | 1 + 14 files changed, 265 insertions(+), 41 deletions(-) diff --git a/api/src/opentrons/hardware_control/instruments/ot3/gripper.py b/api/src/opentrons/hardware_control/instruments/ot3/gripper.py index 3eb3c863522..90667167216 100644 --- a/api/src/opentrons/hardware_control/instruments/ot3/gripper.py +++ b/api/src/opentrons/hardware_control/instruments/ot3/gripper.py @@ -22,7 +22,10 @@ ) from ..instrument_abc import AbstractInstrument from opentrons.hardware_control.dev_types import AttachedGripper, GripperDict -from opentrons_shared_data.errors.exceptions import CommandPreconditionViolated +from opentrons_shared_data.errors.exceptions import ( + CommandPreconditionViolated, + FailedGripperPickupError, +) from opentrons_shared_data.gripper import ( GripperDefinition, @@ -101,6 +104,10 @@ def remove_probe(self) -> None: assert self.attached_probe self._attached_probe = None + @property + def max_allowed_grip_error(self) -> float: + return self._geometry.max_allowed_grip_error + @property def jaw_width(self) -> float: jaw_max = self.geometry.jaw_width["max"] @@ -196,6 +203,23 @@ def check_calibration_pin_location_is_accurate(self) -> None: }, ) + def check_labware_pickup(self, labware_width: float) -> None: + """Ensure that a gripper pickup succeeded.""" + # check if the gripper is at an acceptable position after attempting to + # pick up labware + expected_gripper_position = labware_width + current_gripper_position = self.jaw_width + if ( + abs(current_gripper_position - expected_gripper_position) + > self.max_allowed_grip_error + ): + raise FailedGripperPickupError( + details={ + "expected jaw width": expected_gripper_position, + "actual jaw width": current_gripper_position, + }, + ) + def critical_point(self, cp_override: Optional[CriticalPoint] = None) -> Point: """ The vector from the gripper mount to the critical point, which is selectable diff --git a/api/src/opentrons/protocol_engine/errors/__init__.py b/api/src/opentrons/protocol_engine/errors/__init__.py index d330be61a3e..d3c3bb6d79e 100644 --- a/api/src/opentrons/protocol_engine/errors/__init__.py +++ b/api/src/opentrons/protocol_engine/errors/__init__.py @@ -138,4 +138,5 @@ "NotSupportedOnRobotType", # error occurrence models "ErrorOccurrence", + "FailedGripperPickupError", ] diff --git a/api/src/opentrons/protocol_engine/execution/labware_movement.py b/api/src/opentrons/protocol_engine/execution/labware_movement.py index b65e91d0845..1d3f6194f8f 100644 --- a/api/src/opentrons/protocol_engine/execution/labware_movement.py +++ b/api/src/opentrons/protocol_engine/execution/labware_movement.py @@ -135,7 +135,7 @@ async def move_labware_with_gripper( post_drop_slide_offset=post_drop_slide_offset, ) labware_grip_force = self._state_store.labware.get_grip_force(labware_id) - + gripper_opened = False for waypoint_data in movement_waypoints: if waypoint_data.jaw_open: if waypoint_data.dropping: @@ -146,12 +146,22 @@ async def move_labware_with_gripper( # on the side of a falling tiprack catches the jaw. await ot3api.disengage_axes([Axis.Z_G]) await ot3api.ungrip() + gripper_opened = True if waypoint_data.dropping: # We lost the position estimation after disengaging the axis, so # it is necessary to home it next await ot3api.home_z(OT3Mount.GRIPPER) else: await ot3api.grip(force_newtons=labware_grip_force) + # we only want to check position after the gripper has opened and + # should be holding labware + if gripper_opened: + assert ot3api.hardware_gripper + ot3api.hardware_gripper.check_labware_pickup( + labware_width=self._state_store.labware.get_dimensions( + labware_id + ).y + ) await ot3api.move_to( mount=gripper_mount, abs_position=waypoint_data.position ) diff --git a/api/tests/opentrons/hardware_control/test_gripper.py b/api/tests/opentrons/hardware_control/test_gripper.py index 02d2285bdb0..3f7266399ef 100644 --- a/api/tests/opentrons/hardware_control/test_gripper.py +++ b/api/tests/opentrons/hardware_control/test_gripper.py @@ -1,5 +1,7 @@ -from typing import Optional, Callable, TYPE_CHECKING +from typing import Optional, Callable, TYPE_CHECKING, Any, Generator import pytest +from contextlib import nullcontext +from unittest.mock import MagicMock, patch, PropertyMock from opentrons.types import Point from opentrons.calibration_storage import types as cal_types @@ -7,6 +9,7 @@ from opentrons.hardware_control.types import CriticalPoint from opentrons.config import gripper_config from opentrons_shared_data.gripper import GripperModel +from opentrons_shared_data.errors.exceptions import FailedGripperPickupError if TYPE_CHECKING: from opentrons.hardware_control.instruments.ot3.instrument_calibration import ( @@ -26,6 +29,24 @@ def fake_offset() -> "GripperCalibrationOffset": return load_gripper_calibration_offset("fakeid123") +@pytest.fixture +def mock_jaw_width() -> Generator[MagicMock, None, None]: + with patch( + "opentrons.hardware_control.instruments.ot3.gripper.Gripper.jaw_width", + new_callable=PropertyMock, + ) as jaw_width: + yield jaw_width + + +@pytest.fixture +def mock_max_grip_error() -> Generator[MagicMock, None, None]: + with patch( + "opentrons.hardware_control.instruments.ot3.gripper.Gripper.max_allowed_grip_error", + new_callable=PropertyMock, + ) as max_error: + yield max_error + + @pytest.mark.ot3_only def test_id_get_added_to_dict(fake_offset: "GripperCalibrationOffset") -> None: gripr = gripper.Gripper(fake_gripper_conf, fake_offset, "fakeid123") @@ -67,6 +88,32 @@ def test_load_gripper_cal_offset(fake_offset: "GripperCalibrationOffset") -> Non ) +@pytest.mark.ot3_only +@pytest.mark.parametrize( + argnames=["jaw_width_val", "error_context"], + argvalues=[ + (89, nullcontext()), + (100, pytest.raises(FailedGripperPickupError)), + (50, pytest.raises(FailedGripperPickupError)), + (85, nullcontext()), + ], +) +def test_check_labware_pickup( + mock_jaw_width: Any, + mock_max_grip_error: Any, + jaw_width_val: float, + error_context: Any, +) -> None: + """Test that FailedGripperPickupError is raised correctly.""" + # This should only be triggered when the difference between the + # gripper jaw and labware widths is greater than the max allowed error. + gripr = gripper.Gripper(fake_gripper_conf, fake_offset, "fakeid123") + mock_jaw_width.return_value = jaw_width_val + mock_max_grip_error.return_value = 6 + with error_context: + gripr.check_labware_pickup(85) + + @pytest.mark.ot3_only def test_reload_instrument_cal_ot3(fake_offset: "GripperCalibrationOffset") -> None: old_gripper = gripper.Gripper( diff --git a/api/tests/opentrons/protocol_engine/execution/test_labware_movement_handler.py b/api/tests/opentrons/protocol_engine/execution/test_labware_movement_handler.py index 838039488f0..f1b7797c2d6 100644 --- a/api/tests/opentrons/protocol_engine/execution/test_labware_movement_handler.py +++ b/api/tests/opentrons/protocol_engine/execution/test_labware_movement_handler.py @@ -5,7 +5,7 @@ import pytest from decoy import Decoy, matchers -from typing import TYPE_CHECKING, Union, Optional +from typing import TYPE_CHECKING, Union, Optional, Tuple from opentrons.protocol_engine.execution import EquipmentHandler, MovementHandler from opentrons.hardware_control import HardwareControlAPI @@ -22,6 +22,7 @@ LabwareLocation, NonStackedLocation, LabwareMovementOffsetData, + Dimensions, ) from opentrons.protocol_engine.execution.thermocycler_plate_lifter import ( ThermocyclerPlateLifter, @@ -85,6 +86,22 @@ def heater_shaker_movement_flagger(decoy: Decoy) -> HeaterShakerMovementFlagger: return decoy.mock(cls=HeaterShakerMovementFlagger) +@pytest.fixture +def hardware_gripper_offset_data() -> Tuple[ + LabwareMovementOffsetData, LabwareMovementOffsetData +]: + """Get a set of mocked labware offset data.""" + user_offset_data = LabwareMovementOffsetData( + pickUpOffset=LabwareOffsetVector(x=123, y=234, z=345), + dropOffset=LabwareOffsetVector(x=111, y=222, z=333), + ) + final_offset_data = LabwareMovementOffsetData( + pickUpOffset=LabwareOffsetVector(x=-1, y=-2, z=-3), + dropOffset=LabwareOffsetVector(x=1, y=2, z=3), + ) + return user_offset_data, final_offset_data + + def default_experimental_movement_data() -> LabwareMovementOffsetData: """Experimental movement data with default values.""" return LabwareMovementOffsetData( @@ -93,6 +110,51 @@ def default_experimental_movement_data() -> LabwareMovementOffsetData: ) +async def set_up_decoy_hardware_gripper( + decoy: Decoy, ot3_hardware_api: OT3API, state_store: StateStore +) -> None: + """Shared hardware gripper decoy setup.""" + decoy.when(state_store.config.use_virtual_gripper).then_return(False) + decoy.when(ot3_hardware_api.has_gripper()).then_return(True) + decoy.when(ot3_hardware_api._gripper_handler.is_ready_for_jaw_home()).then_return( + True + ) + assert ot3_hardware_api.hardware_gripper + decoy.when( + await ot3_hardware_api.gantry_position(mount=OT3Mount.GRIPPER) + ).then_return(Point(x=777, y=888, z=999)) + + decoy.when( + await ot3_hardware_api.encoder_current_position_ot3(OT3Mount.GRIPPER) + ).then_return({Axis.G: 4.0}) + + decoy.when( + state_store.labware.get_dimensions(labware_id="my-teleporting-labware") + ).then_return(Dimensions(x=100, y=85, z=0)) + + decoy.when( + ot3_hardware_api.hardware_gripper.geometry.max_allowed_grip_error + ).then_return(6.0) + + decoy.when(ot3_hardware_api.hardware_gripper.jaw_width).then_return(89) + + decoy.when( + state_store.labware.get_grip_force("my-teleporting-labware") + ).then_return(100) + + decoy.when(state_store.labware.get_labware_offset("new-offset-id")).then_return( + LabwareOffset( + id="new-offset-id", + createdAt=datetime(year=2022, month=10, day=20), + definitionUri="my-labware", + location=LabwareOffsetLocation( + slotName=DeckSlotName.SLOT_5 + ), # this location doesn't matter for this test + vector=LabwareOffsetVector(x=0.5, y=0.6, z=0.7), + ) + ) + + @pytest.mark.ot3_only @pytest.fixture def subject( @@ -116,6 +178,83 @@ def subject( ) +@pytest.mark.ot3_only +async def test_check_labware_pickup( + decoy: Decoy, + state_store: StateStore, + thermocycler_plate_lifter: ThermocyclerPlateLifter, + ot3_hardware_api: OT3API, + subject: LabwareMovementHandler, + hardware_gripper_offset_data: Tuple[ + LabwareMovementOffsetData, LabwareMovementOffsetData + ], +) -> None: + """Test that the gripper position check is called at the right time.""" + # This function should only be called when after the gripper opens, + # and then closes again. This is when we expect the labware to be + # in the gripper jaws. + await set_up_decoy_hardware_gripper(decoy, ot3_hardware_api, state_store) + assert ot3_hardware_api.hardware_gripper + + user_offset_data, final_offset_data = hardware_gripper_offset_data + + starting_location = DeckSlotLocation(slotName=DeckSlotName.SLOT_1) + to_location = DeckSlotLocation(slotName=DeckSlotName.SLOT_2) + + mock_tc_context_manager = decoy.mock() + decoy.when( + thermocycler_plate_lifter.lift_plate_for_labware_movement( + labware_location=starting_location + ) + ).then_return(mock_tc_context_manager) + + decoy.when( + state_store.geometry.get_final_labware_movement_offset_vectors( + from_location=starting_location, + to_location=to_location, + additional_offset_vector=user_offset_data, + ) + ).then_return(final_offset_data) + + decoy.when( + state_store.geometry.get_labware_grip_point( + labware_id="my-teleporting-labware", location=starting_location + ) + ).then_return(Point(101, 102, 119.5)) + + decoy.when( + state_store.geometry.get_labware_grip_point( + labware_id="my-teleporting-labware", location=to_location + ) + ).then_return(Point(201, 202, 219.5)) + + decoy.when(ot3_hardware_api.hardware_gripper.check_labware_pickup(85)).then_return() + + await subject.move_labware_with_gripper( + labware_id="my-teleporting-labware", + current_location=starting_location, + new_location=DeckSlotLocation(slotName=DeckSlotName.SLOT_2), + user_offset_data=user_offset_data, + post_drop_slide_offset=Point(x=1, y=1, z=1), + ) + + decoy.verify( + await ot3_hardware_api.home(axes=[Axis.Z_L, Axis.Z_R, Axis.Z_G]), + await mock_tc_context_manager.__aenter__(), + await ot3_hardware_api.grip(force_newtons=100), + await ot3_hardware_api.ungrip(), + await ot3_hardware_api.grip(force_newtons=100), + ot3_hardware_api.hardware_gripper.check_labware_pickup(labware_width=85), + await ot3_hardware_api.grip(force_newtons=100), + ot3_hardware_api.hardware_gripper.check_labware_pickup(labware_width=85), + await ot3_hardware_api.grip(force_newtons=100), + ot3_hardware_api.hardware_gripper.check_labware_pickup(labware_width=85), + await ot3_hardware_api.disengage_axes([Axis.Z_G]), + await ot3_hardware_api.ungrip(), + await ot3_hardware_api.ungrip(), + ) + + # TODO (spp, 2022-10-18): # 1. Should write an acceptance test w/ real labware on ot3 deck. # 2. This test will be split once waypoints generation is moved to motion planning. @@ -154,30 +293,17 @@ async def test_move_labware_with_gripper( from_location: Union[DeckSlotLocation, ModuleLocation, OnLabwareLocation], to_location: Union[DeckSlotLocation, ModuleLocation, OnLabwareLocation], slide_offset: Optional[Point], + hardware_gripper_offset_data: Tuple[ + LabwareMovementOffsetData, LabwareMovementOffsetData + ], ) -> None: """It should perform a labware movement with gripper by delegating to OT3API.""" # TODO (spp, 2023-07-26): this test does NOT stub out movement waypoints in order to # keep this as the semi-smoke test that it previously was. We should add a proper # smoke test for gripper labware movement with actual labware and make this a unit test. + await set_up_decoy_hardware_gripper(decoy, ot3_hardware_api, state_store) - user_offset_data = LabwareMovementOffsetData( - pickUpOffset=LabwareOffsetVector(x=123, y=234, z=345), - dropOffset=LabwareOffsetVector(x=111, y=222, z=333), - ) - final_offset_data = LabwareMovementOffsetData( - pickUpOffset=LabwareOffsetVector(x=-1, y=-2, z=-3), - dropOffset=LabwareOffsetVector(x=1, y=2, z=3), - ) - - decoy.when(state_store.config.use_virtual_gripper).then_return(False) - decoy.when(ot3_hardware_api.has_gripper()).then_return(True) - decoy.when(ot3_hardware_api._gripper_handler.is_ready_for_jaw_home()).then_return( - True - ) - - decoy.when( - await ot3_hardware_api.gantry_position(mount=OT3Mount.GRIPPER) - ).then_return(Point(x=777, y=888, z=999)) + user_offset_data, final_offset_data = hardware_gripper_offset_data decoy.when( state_store.geometry.get_final_labware_movement_offset_vectors( @@ -192,15 +318,11 @@ async def test_move_labware_with_gripper( labware_id="my-teleporting-labware", location=from_location ) ).then_return(Point(101, 102, 119.5)) - decoy.when( state_store.geometry.get_labware_grip_point( labware_id="my-teleporting-labware", location=to_location ) ).then_return(Point(201, 202, 219.5)) - decoy.when( - state_store.labware.get_grip_force("my-teleporting-labware") - ).then_return(100) mock_tc_context_manager = decoy.mock() decoy.when( thermocycler_plate_lifter.lift_plate_for_labware_movement( @@ -208,18 +330,6 @@ async def test_move_labware_with_gripper( ) ).then_return(mock_tc_context_manager) - decoy.when(state_store.labware.get_labware_offset("new-offset-id")).then_return( - LabwareOffset( - id="new-offset-id", - createdAt=datetime(year=2022, month=10, day=20), - definitionUri="my-labware", - location=LabwareOffsetLocation( - slotName=DeckSlotName.SLOT_5 - ), # this location doesn't matter for this test - vector=LabwareOffsetVector(x=0.5, y=0.6, z=0.7), - ) - ) - expected_waypoints = [ Point(100, 100, 999), # move to above slot 1 Point(100, 100, 116.5), # move to labware on slot 1 diff --git a/shared-data/errors/definitions/1/errors.json b/shared-data/errors/definitions/1/errors.json index 1f31ac9af6e..7ed86dcff30 100644 --- a/shared-data/errors/definitions/1/errors.json +++ b/shared-data/errors/definitions/1/errors.json @@ -114,6 +114,10 @@ "detail": "Execution Cancelled", "category": "roboticsControlError" }, + "2015": { + "detail": "Gripper Pickup Failed", + "category": "roboticsControlError" + }, "3000": { "detail": "A robotics interaction error occurred.", "category": "roboticsInteractionError" diff --git a/shared-data/gripper/definitions/1/gripperV1.1.json b/shared-data/gripper/definitions/1/gripperV1.1.json index b5587c9e78a..ccc4b1d918d 100644 --- a/shared-data/gripper/definitions/1/gripperV1.1.json +++ b/shared-data/gripper/definitions/1/gripperV1.1.json @@ -23,6 +23,7 @@ "jawWidth": { "min": 60.0, "max": 92.0 - } + }, + "maxAllowedGripError": 6.0 } } diff --git a/shared-data/gripper/definitions/1/gripperV1.2.json b/shared-data/gripper/definitions/1/gripperV1.2.json index 5cb3c045bc6..d4b33ecd087 100644 --- a/shared-data/gripper/definitions/1/gripperV1.2.json +++ b/shared-data/gripper/definitions/1/gripperV1.2.json @@ -23,6 +23,7 @@ "jawWidth": { "min": 60.0, "max": 92.0 - } + }, + "maxAllowedGripError": 6.0 } } diff --git a/shared-data/gripper/definitions/1/gripperV1.json b/shared-data/gripper/definitions/1/gripperV1.json index 093ed42f745..a066a9de6d7 100644 --- a/shared-data/gripper/definitions/1/gripperV1.json +++ b/shared-data/gripper/definitions/1/gripperV1.json @@ -22,6 +22,7 @@ "jawWidth": { "min": 58.0, "max": 90.0 - } + }, + "maxAllowedGripError": 6.0 } } diff --git a/shared-data/gripper/schemas/1.json b/shared-data/gripper/schemas/1.json index 1ecc38087e9..4ec4d6ee0f8 100644 --- a/shared-data/gripper/schemas/1.json +++ b/shared-data/gripper/schemas/1.json @@ -109,6 +109,11 @@ "additionalProperties": { "type": "number" } + }, + "maxAllowedGripError": { + "title": "MaxAllowedGripError", + "minimum": 0.0, + "type": "number" } }, "required": [ diff --git a/shared-data/js/types.ts b/shared-data/js/types.ts index 15b51591b6d..21ef29f8db4 100644 --- a/shared-data/js/types.ts +++ b/shared-data/js/types.ts @@ -533,6 +533,7 @@ export interface GripperDefinition { pinOneOffsetFromBase: [number, number, number] pinTwoOffsetFromBase: [number, number, number] jawWidth: { min: number; max: number } + maxAllowedGripError: number } } diff --git a/shared-data/python/opentrons_shared_data/errors/codes.py b/shared-data/python/opentrons_shared_data/errors/codes.py index ab79e03b06e..5788b2fca93 100644 --- a/shared-data/python/opentrons_shared_data/errors/codes.py +++ b/shared-data/python/opentrons_shared_data/errors/codes.py @@ -58,6 +58,7 @@ class ErrorCodes(Enum): UNMATCHED_TIP_PRESENCE_STATES = _code_from_dict_entry("2012") POSITION_UNKNOWN = _code_from_dict_entry("2013") EXECUTION_CANCELLED = _code_from_dict_entry("2014") + FAILED_GRIPPER_PICKUP_ERROR = _code_from_dict_entry("2015") ROBOTICS_INTERACTION_ERROR = _code_from_dict_entry("3000") LABWARE_DROPPED = _code_from_dict_entry("3001") LABWARE_NOT_PICKED_UP = _code_from_dict_entry("3002") diff --git a/shared-data/python/opentrons_shared_data/errors/exceptions.py b/shared-data/python/opentrons_shared_data/errors/exceptions.py index 9483b404965..5bf2fc0e67c 100644 --- a/shared-data/python/opentrons_shared_data/errors/exceptions.py +++ b/shared-data/python/opentrons_shared_data/errors/exceptions.py @@ -447,6 +447,23 @@ def __init__( ) +class FailedGripperPickupError(RoboticsControlError): + """Raised when the gripper expects to be holding an object, but the jaw is closed farther than expected.""" + + def __init__( + self, + details: Optional[Dict[str, Any]] = None, + wrapping: Optional[Sequence[EnumeratedError]] = None, + ) -> None: + """Build a FailedGripperPickupError.""" + super().__init__( + ErrorCodes.FAILED_GRIPPER_PICKUP_ERROR, + "Expected to grip labware, but none found.", + details, + wrapping, + ) + + class EdgeNotFoundError(RoboticsControlError): """An error indicating that a calibration square edge was not able to be found.""" diff --git a/shared-data/python/opentrons_shared_data/gripper/gripper_definition.py b/shared-data/python/opentrons_shared_data/gripper/gripper_definition.py index 70ca41f54ce..4c4c30c623b 100644 --- a/shared-data/python/opentrons_shared_data/gripper/gripper_definition.py +++ b/shared-data/python/opentrons_shared_data/gripper/gripper_definition.py @@ -69,6 +69,7 @@ class Geometry(GripperBaseModel): pin_one_offset_from_base: Offset pin_two_offset_from_base: Offset jaw_width: Dict[str, float] + max_allowed_grip_error: _StrictNonNegativeFloat PolynomialTerm = Tuple[_StrictNonNegativeInt, float]