refactor(api): improve labware movement code

api/src/opentrons/motion_planning/__init__.py

@@ -5,6 +5,7 @@
     DEFAULT_IN_LABWARE_ARC_Z_MARGIN,
     MINIMUM_Z_MARGIN,
     get_waypoints,
+    get_gripper_labware_movement_waypoints,
 )
 
 from .types import Waypoint, MoveType
@@ -25,4 +26,5 @@
     "DestinationOutOfBoundsError",
     "ArcOutOfBoundsError",
     "get_waypoints",
+    "get_gripper_labware_movement_waypoints",
 ]

api/src/opentrons/motion_planning/types.py

@@ -29,3 +29,12 @@ class MoveType(Enum):
     GENERAL_ARC = auto_enum_value()
     IN_LABWARE_ARC = auto_enum_value()
     DIRECT = auto_enum_value()
+
+
+@dataclass(frozen=True)
+@final
+class GripperMovementWaypointsWithJawStatus:
+    """Gripper motion waypoint with expected jaw status while moving to the waypoint."""
+
+    position: Point
+    jawOpen: bool

api/src/opentrons/motion_planning/waypoints.py

@@ -5,8 +5,9 @@
 from opentrons.types import Point
 from opentrons.hardware_control.types import CriticalPoint
 
-from .types import Waypoint, MoveType
+from .types import Waypoint, MoveType, GripperMovementWaypointsWithJawStatus
 from .errors import DestinationOutOfBoundsError, ArcOutOfBoundsError
+from ..protocol_engine.types import LabwareMovementOffsetData
 
 DEFAULT_GENERAL_ARC_Z_MARGIN: Final[float] = 10.0
 DEFAULT_IN_LABWARE_ARC_Z_MARGIN: Final[float] = 5.0
@@ -118,3 +119,45 @@ def get_waypoints(
     waypoints.append(dest_waypoint)
 
     return waypoints
+
+
+def get_gripper_labware_movement_waypoints(
+    from_labware_center: Point,
+    to_labware_center: Point,
+    gripper_home_z: float,
+    offset_data: LabwareMovementOffsetData,
+) -> List[GripperMovementWaypointsWithJawStatus]:
+    """Get waypoints for moving labware using a gripper."""
+    pick_up_offset = offset_data.pick_up_offset
+    drop_offset = offset_data.drop_offset
+
+    pick_up_location = from_labware_center + Point(
+        pick_up_offset.x, pick_up_offset.y, pick_up_offset.z
+    )
+    drop_location = to_labware_center + Point(
+        drop_offset.x, drop_offset.y, drop_offset.z
+    )
+
+    waypoints_with_jaw_status = [
+        GripperMovementWaypointsWithJawStatus(
+            position=Point(pick_up_location.x, pick_up_location.y, gripper_home_z),
+            jawOpen=False,
+        ),
+        GripperMovementWaypointsWithJawStatus(position=pick_up_location, jawOpen=True),
+        # Gripper grips the labware here
+        GripperMovementWaypointsWithJawStatus(
+            position=Point(pick_up_location.x, pick_up_location.y, gripper_home_z),
+            jawOpen=False,
+        ),
+        GripperMovementWaypointsWithJawStatus(
+            position=Point(drop_location.x, drop_location.y, gripper_home_z),
+            jawOpen=False,
+        ),
+        GripperMovementWaypointsWithJawStatus(position=drop_location, jawOpen=False),
+        # Gripper ungrips here
+        GripperMovementWaypointsWithJawStatus(
+            position=Point(drop_location.x, drop_location.y, gripper_home_z),
+            jawOpen=True,
+        ),
+    ]
+    return waypoints_with_jaw_status

api/src/opentrons/protocol_engine/commands/move_labware.py

@@ -128,16 +128,17 @@ async def execute(self, params: MoveLabwareParams) -> MoveLabwareResult:
                 )
 
             validated_current_loc = (
-                self._labware_movement.ensure_valid_gripper_location(
+                self._state_view.geometry.ensure_valid_gripper_location(
                     current_labware.location
                 )
             )
-            validated_new_loc = self._labware_movement.ensure_valid_gripper_location(
+            validated_new_loc = self._state_view.geometry.ensure_valid_gripper_location(
                 available_new_location,
             )
             user_offset_data = LabwareMovementOffsetData(
-                pickUpOffset=params.pickUpOffset,
-                dropOffset=params.dropOffset,
+                pick_up_offset=params.pickUpOffset
+                or LabwareOffsetVector(x=0, y=0, z=0),
+                drop_offset=params.dropOffset or LabwareOffsetVector(x=0, y=0, z=0),
             )
             # Skips gripper moves when using virtual gripper
             await self._labware_movement.move_labware_with_gripper(

api/src/opentrons/protocol_engine/execution/labware_movement.py

@@ -1,18 +1,18 @@
 """Labware movement command handling."""
 from __future__ import annotations
 
-from typing import Optional, Union, List, TYPE_CHECKING
+from typing import Optional, Union, TYPE_CHECKING
 from opentrons_shared_data.gripper.constants import (
     LABWARE_GRIP_FORCE,
     IDLE_STATE_GRIP_FORCE,
 )
 
-from opentrons.types import Point
 from opentrons.hardware_control import HardwareControlAPI
 from opentrons.hardware_control.types import OT3Mount, Axis
+from opentrons.motion_planning import get_gripper_labware_movement_waypoints
+
 from opentrons.protocol_engine.state import StateStore
 from opentrons.protocol_engine.resources.ot3_validation import ensure_ot3_hardware
-from opentrons.protocol_engine.types import ModuleModel
 
 from .thermocycler_movement_flagger import ThermocyclerMovementFlagger
 from .heater_shaker_movement_flagger import HeaterShakerMovementFlagger
@@ -30,15 +30,12 @@
     ModuleLocation,
     OnLabwareLocation,
     LabwareLocation,
-    LabwareOffsetVector,
     LabwareMovementOffsetData,
 )
 
 if TYPE_CHECKING:
     from opentrons.protocol_engine.execution import EquipmentHandler, MovementHandler
 
-_ADDITIONAL_TC2_PICKUP_OFFSET = 3.5
-
 
 # TODO (spp, 2022-10-20): name this GripperMovementHandler if it doesn't handle
 #  any non-gripper implementations
@@ -91,7 +88,7 @@ async def move_labware_with_gripper(
         new_location: Union[DeckSlotLocation, ModuleLocation, OnLabwareLocation],
         user_offset_data: LabwareMovementOffsetData,
     ) -> None:
-        """Move a loaded labware from one location to another."""
+        """Move a loaded labware from one location to another using gripper."""
         use_virtual_gripper = self._state_store.config.use_virtual_gripper
         if use_virtual_gripper:
             return
@@ -105,16 +102,6 @@ async def move_labware_with_gripper(
                 "No gripper found for performing labware movements."
             )
 
-        is_tc2_pickup = False
-
-        if isinstance(current_location, ModuleLocation):
-            module_id = current_location.moduleId
-            if (
-                self._state_store.modules.get_connected_model(module_id)
-                == ModuleModel.THERMOCYCLER_MODULE_V2
-            ):
-                is_tc2_pickup = True
-
         gripper_mount = OT3Mount.GRIPPER
 
         # Retract all mounts
@@ -124,116 +111,39 @@ async def move_labware_with_gripper(
         async with self._thermocycler_plate_lifter.lift_plate_for_labware_movement(
             labware_location=current_location
         ):
-            labware_pickup_offset = self.get_final_labware_movement_offset_vector(
-                additional_offset_vector=user_offset_data.pickUpOffset,
-                is_pickup_from_tc2=is_tc2_pickup,
-            )
-
-            waypoints_to_labware = self._get_gripper_movement_waypoints(
-                labware_id=labware_id,
-                location=current_location,
-                current_position=await ot3api.gantry_position(mount=gripper_mount),
-                gripper_home_z=gripper_homed_position.z,
-                labware_offset_vector=labware_pickup_offset,
+            final_offsets = (
+                self._state_store.geometry.get_final_labware_movement_offset_vectors(
+                    from_location=current_location,
+                    to_location=new_location,
+                    additional_offset_vector=user_offset_data,
+                )
             )
-
-            for waypoint in waypoints_to_labware[:-1]:
-                await ot3api.move_to(mount=gripper_mount, abs_position=waypoint)
-
-            # TODO: We do this to have the gripper move to location with
-            #  closed grip and open right before picking up the labware to
-            #  avoid collisions as much as possible.
-            #  See https://opentrons.atlassian.net/browse/RLAB-214
-            await ot3api.home_gripper_jaw()
-            await ot3api.move_to(
-                mount=gripper_mount, abs_position=waypoints_to_labware[-1]
+            from_labware_center = self._state_store.geometry.get_labware_center(
+                labware_id=labware_id, location=current_location
             )
-            await ot3api.grip(force_newtons=LABWARE_GRIP_FORCE)
-            labware_drop_offset = self.get_final_labware_movement_offset_vector(
-                additional_offset_vector=user_offset_data.dropOffset
+            to_labware_center = self._state_store.geometry.get_labware_center(
+                labware_id=labware_id, location=new_location
             )
-
-            # TODO: see https://opentrons.atlassian.net/browse/RLAB-215
-            await ot3api.home(axes=[Axis.Z_G])
-
-            waypoints_to_new_location = self._get_gripper_movement_waypoints(
-                labware_id=labware_id,
-                location=new_location,
-                current_position=waypoints_to_labware[-1],
+            movement_waypoints = get_gripper_labware_movement_waypoints(
+                from_labware_center=from_labware_center,
+                to_labware_center=to_labware_center,
                 gripper_home_z=gripper_homed_position.z,
-                labware_offset_vector=labware_drop_offset,
+                offset_data=final_offsets,
             )
 
-            for waypoint in waypoints_to_new_location:
-                await ot3api.move_to(mount=gripper_mount, abs_position=waypoint)
-
-            await ot3api.ungrip()
-            # TODO: see https://opentrons.atlassian.net/browse/RLAB-215
-            await ot3api.home(axes=[Axis.Z_G])
+            for waypoint_data in movement_waypoints:
+                if waypoint_data.jawOpen:
+                    await ot3api.ungrip()
+                else:
+                    await ot3api.grip(force_newtons=LABWARE_GRIP_FORCE)
+                await ot3api.move_to(
+                    mount=gripper_mount, abs_position=waypoint_data.position
+                )
 
-            # Keep the gripper in gripped position so it avoids colliding with
+            # Keep the gripper in idly gripped position to avoid colliding with
             # things like the thermocycler latches
             await ot3api.grip(force_newtons=IDLE_STATE_GRIP_FORCE)
 
-    # TODO (spp, 2022-10-19): Move this to motion planning and
-    #  test waypoints generation in isolation.
-    def _get_gripper_movement_waypoints(
-        self,
-        labware_id: str,
-        location: Union[DeckSlotLocation, ModuleLocation, OnLabwareLocation],
-        current_position: Point,
-        gripper_home_z: float,
-        labware_offset_vector: LabwareOffsetVector,
-    ) -> List[Point]:
-        """Get waypoints for gripper to move to a specified location."""
-        labware_center = self._state_store.geometry.get_labware_center(
-            labware_id=labware_id, location=location
-        )
-        waypoints: List[Point] = [
-            Point(current_position.x, current_position.y, gripper_home_z),
-            Point(
-                labware_center.x + labware_offset_vector.x,
-                labware_center.y + labware_offset_vector.y,
-                gripper_home_z,
-            ),
-            Point(
-                labware_center.x + labware_offset_vector.x,
-                labware_center.y + labware_offset_vector.y,
-                labware_center.z + labware_offset_vector.z,
-            ),
-        ]
-        return waypoints
-
-    # TODO (spp, 2022-12-14): https://opentrons.atlassian.net/browse/RLAB-237
-    @staticmethod
-    def get_final_labware_movement_offset_vector(
-        additional_offset_vector: Optional[LabwareOffsetVector],
-        is_pickup_from_tc2: bool = False,
-    ) -> LabwareOffsetVector:
-        """Calculate the final labware offset vector to use in labware movement."""
-        user_offset_vector = additional_offset_vector or LabwareOffsetVector(
-            x=0, y=0, z=0
-        )
-        if is_pickup_from_tc2:
-            # TODO (fps, 2022-05-30): Remove this once RLAB-295 is merged
-            user_offset_vector.z += _ADDITIONAL_TC2_PICKUP_OFFSET
-
-        return user_offset_vector
-
-    # TODO (spp, 2022-10-20): move to labware view
-    @staticmethod
-    def ensure_valid_gripper_location(
-        location: LabwareLocation,
-    ) -> Union[DeckSlotLocation, ModuleLocation, OnLabwareLocation]:
-        """Ensure valid on-deck location for gripper, otherwise raise error."""
-        if not isinstance(
-            location, (DeckSlotLocation, ModuleLocation, OnLabwareLocation)
-        ):
-            raise LabwareMovementNotAllowedError(
-                "Off-deck labware movements are not supported using the gripper."
-            )
-        return location
-
     async def ensure_movement_not_obstructed_by_module(
         self, labware_id: str, new_location: LabwareLocation
     ) -> None:

api/src/opentrons/protocol_engine/execution/movement.py

@@ -78,10 +78,6 @@ async def move_to_well(
             self._state_store.modules.get_heater_shaker_movement_restrictors()
         )
 
-        # TODO (spp, 2022-12-14): remove once we understand why sometimes moveLabware
-        #  fails saying that h/s latch is closed even when it is not.
-        log.info(f"H/S movement restrictors: {hs_movement_restrictors}")
-
         dest_slot_int = self._state_store.geometry.get_ancestor_slot_name(
             labware_id
         ).as_int()

api/src/opentrons/protocol_engine/state/geometry.py

@@ -6,6 +6,7 @@
 from opentrons_shared_data.labware.constants import WELL_NAME_PATTERN
 
 from .. import errors
+from ..errors import LabwareMovementNotAllowedError
 from ..types import (
     OFF_DECK_LOCATION,
     LoadedLabware,
@@ -24,6 +25,8 @@
     DeckType,
     CurrentWell,
     TipGeometry,
+    LabwareMovementOffsetData,
+    ModuleModel,
 )
 from .config import Config
 from .labware import LabwareView
@@ -35,6 +38,7 @@
 
 
 SLOT_WIDTH = 128
+_ADDITIONAL_TC2_PICKUP_OFFSET = 3.5
 
 
 class _TipDropSection(enum.Enum):
@@ -616,3 +620,41 @@ def _get_drop_tip_well_x_offset(
                 if x_well_offset < 0:
                     x_well_offset = 0
         return x_well_offset
+
+    def get_final_labware_movement_offset_vectors(
+        self,
+        from_location: LabwareLocation,
+        to_location: LabwareLocation,
+        additional_offset_vector: LabwareMovementOffsetData,
+    ) -> LabwareMovementOffsetData:
+        """Calculate the final labware offset vector to use in labware movement."""
+        # TODO (fps, 2022-05-30): Update this once RLAB-295 is merged
+        #  Get location-based offsets from deck/module/adapter definitions,
+        #  then add additional offsets
+        pick_up_offset = additional_offset_vector.pick_up_offset
+        drop_offset = additional_offset_vector.drop_offset
+
+        if isinstance(from_location, ModuleLocation):
+            module_id = from_location.moduleId
+            if (
+                self._modules.get_connected_model(module_id)
+                == ModuleModel.THERMOCYCLER_MODULE_V2
+            ):
+                pick_up_offset.z += _ADDITIONAL_TC2_PICKUP_OFFSET
+
+        return LabwareMovementOffsetData(
+            pick_up_offset=pick_up_offset, drop_offset=drop_offset
+        )
+
+    @staticmethod
+    def ensure_valid_gripper_location(
+        location: LabwareLocation,
+    ) -> Union[DeckSlotLocation, ModuleLocation, OnLabwareLocation]:
+        """Ensure valid on-deck location for gripper, otherwise raise error."""
+        if not isinstance(
+            location, (DeckSlotLocation, ModuleLocation, OnLabwareLocation)
+        ):
+            raise LabwareMovementNotAllowedError(
+                "Off-deck labware movements are not supported using the gripper."
+            )
+        return location