refactor(protocol-engine): Implement moveToCoordinates (#10840)

Closes #10757.

api/src/opentrons/motion_planning/waypoints.py

@@ -18,7 +18,7 @@ def get_waypoints(
     dest: Point,
     *,
     max_travel_z: float,
-    min_travel_z: float = 0.0,
+    min_travel_z: float,
     move_type: MoveType = MoveType.GENERAL_ARC,
     xy_waypoints: Sequence[Tuple[float, float]] = (),
     origin_cp: Optional[CriticalPoint] = None,
@@ -41,17 +41,22 @@ def get_waypoints(
 
     :returns: A list of :py:class:`.Waypoint` locations to move through.
     """
-    # NOTE(mc, 2020-10-28): This function is currently experimental. Flipping
-    # `use_experimental_waypoint_planning` to True in
-    # `opentrons.protocols.geometry.plan_moves` causes three test failures at
-    # the time of this writing.
+    # NOTE(mm, 2022-06-22):
+    # This function is used by v6+ JSON protocols and v3+
+    # Python API protocols, but not v2 Python API protocols.
     #
-    # Eventually, it may take over for opentrons.hardware_control.util.plan_arc
+    # Flipping `use_experimental_waypoint_planning` to True to make PAPIv2 use this too
+    # causes three test failures at the time of this writing.
+    # Eventually, those may be resolved and this may take over for
+    # opentrons.hardware_control.util.plan_arc, which PAPIv2 currently uses.
     dest_waypoint = Waypoint(dest, dest_cp)
     waypoints: List[Waypoint] = []
 
     # a direct move can ignore all arc and waypoint planning
     if move_type == MoveType.DIRECT:
+        # TODO(mm, 2022-06-17): This will not raise an out-of-bounds error
+        # even if the destination is far out of bounds. A protocol can run into this by
+        # doing a direct move to bad coordinates. Should we raise in that case?
         return [dest_waypoint]
 
     # ensure destination is not out of bounds

api/src/opentrons/protocol_engine/__init__.py

@@ -24,6 +24,7 @@
     LabwareOffsetCreate,
     LabwareOffsetVector,
     LabwareOffsetLocation,
+    DeckPoint,
     DeckSlotLocation,
     ModuleLocation,
     Dimensions,
@@ -68,6 +69,7 @@
     "LabwareOffsetVector",
     "LabwareOffsetLocation",
     "DeckSlotLocation",
+    "DeckPoint",
     "ModuleLocation",
     "Dimensions",
     "EngineStatus",

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

@@ -1,13 +1,21 @@
 """Move to coordinates command request, result, and implementation models."""
 from __future__ import annotations
+
 from pydantic import BaseModel, Field
-from typing import Optional, Type
+from typing import Optional, Type, TYPE_CHECKING
 from typing_extensions import Literal
 
+from opentrons.hardware_control import HardwareControlAPI
+from opentrons.types import Point
+
 from ..types import DeckPoint
 from .pipetting_common import PipetteIdMixin
 from .command import AbstractCommandImpl, BaseCommand, BaseCommandCreate
 
+if TYPE_CHECKING:
+    from ..execution import MovementHandler
+    from ..state import StateView
+
 
 MoveToCoordinatesCommandType = Literal["moveToCoordinates"]
 
@@ -55,12 +63,70 @@ class MoveToCoordinatesImplementation(
 ):
     """Move to coordinates command implementation."""
 
-    def __init__(self, **kwargs: object) -> None:
-        pass
+    def __init__(
+        self,
+        state_view: StateView,
+        hardware_api: HardwareControlAPI,
+        movement: MovementHandler,
+        **kwargs: object,
+    ) -> None:
+        self._state_view = state_view
+        self._hardware_api = hardware_api
+        self._movement = movement
 
     async def execute(self, params: MoveToCoordinatesParams) -> MoveToCoordinatesResult:
         """Move the requested pipette to the requested coordinates."""
-        raise NotImplementedError()
+        await self._move_to_coordinates(
+            pipette_id=params.pipetteId,
+            deck_coordinates=params.coordinates,
+            direct=params.forceDirect,
+            additional_min_travel_z=params.minimumZHeight,
+        )
+        return MoveToCoordinatesResult()
+
+    async def _move_to_coordinates(
+        self,
+        pipette_id: str,
+        deck_coordinates: DeckPoint,
+        direct: bool,
+        additional_min_travel_z: Optional[float],
+    ) -> None:
+        hw_mount = self._state_view.pipettes.get(
+            pipette_id=pipette_id
+        ).mount.to_hw_mount()
+
+        origin = await self._hardware_api.gantry_position(
+            mount=hw_mount,
+            # critical_point=None to get the current position of whatever tip is
+            # currently attached (if any).
+            critical_point=None,
+        )
+
+        max_travel_z = self._hardware_api.get_instrument_max_height(
+            mount=hw_mount,
+            # critical_point=None to get the maximum z-coordinate
+            # given whatever tip is currently attached (if any).
+            critical_point=None,
+        )
+
+        # calculate the movement's waypoints
+        waypoints = self._state_view.motion.get_movement_waypoints_to_coords(
+            origin=origin,
+            dest=Point(
+                x=deck_coordinates.x, y=deck_coordinates.y, z=deck_coordinates.z
+            ),
+            max_travel_z=max_travel_z,
+            direct=direct,
+            additional_min_travel_z=additional_min_travel_z,
+        )
+
+        # move through the waypoints
+        for waypoint in waypoints:
+            await self._hardware_api.move_to(
+                mount=hw_mount,
+                abs_position=waypoint.position,
+                critical_point=waypoint.critical_point,
+            )
 
 
 class MoveToCoordinates(BaseCommand[MoveToCoordinatesParams, MoveToCoordinatesResult]):

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

@@ -98,7 +98,7 @@ async def move_to_well(
         max_travel_z = self._hardware_api.get_instrument_max_height(mount=hw_mount)
 
         # calculate the movement's waypoints
-        waypoints = self._state_store.motion.get_movement_waypoints(
+        waypoints = self._state_store.motion.get_movement_waypoints_to_well(
             pipette_id=pipette_id,
             labware_id=labware_id,
             well_name=well_name,
@@ -110,11 +110,11 @@ async def move_to_well(
         )
 
         # move through the waypoints
-        for wp in waypoints:
+        for waypoint in waypoints:
             await self._hardware_api.move_to(
                 mount=hw_mount,
-                abs_position=wp.position,
-                critical_point=wp.critical_point,
+                abs_position=waypoint.position,
+                critical_point=waypoint.critical_point,
             )
 
     async def move_relative(

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

@@ -4,12 +4,7 @@
 
 from opentrons.types import MountType, Point, DeckSlotName
 from opentrons.hardware_control.types import CriticalPoint
-from opentrons.motion_planning import (
-    MoveType,
-    Waypoint,
-    MotionPlanningError,
-    get_waypoints,
-)
+from opentrons import motion_planning
 
 from .. import errors
 from ..types import WellLocation
@@ -68,7 +63,7 @@ def get_pipette_location(
             critical_point = CriticalPoint.XY_CENTER
         return PipetteLocationData(mount=mount, critical_point=critical_point)
 
-    def get_movement_waypoints(
+    def get_movement_waypoints_to_well(
         self,
         pipette_id: str,
         labware_id: str,
@@ -78,8 +73,8 @@ def get_movement_waypoints(
         origin_cp: Optional[CriticalPoint],
         max_travel_z: float,
         current_well: Optional[CurrentWell] = None,
-    ) -> List[Waypoint]:
-        """Get the movement waypoints from an origin to a given location."""
+    ) -> List[motion_planning.Waypoint]:
+        """Calculate waypoints to a destination that's specified as a well."""
         location = current_well or self._pipettes.get_current_well()
         center_dest = self._labware.get_has_quirk(
             labware_id,
@@ -100,13 +95,13 @@ def get_movement_waypoints(
             and labware_id == location.labware_id
         ):
             move_type = (
-                MoveType.IN_LABWARE_ARC
+                motion_planning.MoveType.IN_LABWARE_ARC
                 if well_name != location.well_name
-                else MoveType.DIRECT
+                else motion_planning.MoveType.DIRECT
             )
             min_travel_z = self._geometry.get_labware_highest_z(labware_id)
         else:
-            move_type = MoveType.GENERAL_ARC
+            move_type = motion_planning.MoveType.GENERAL_ARC
             min_travel_z = self._geometry.get_all_labware_highest_z()
             if location is not None:
                 if self._module.should_dodge_thermocycler(
@@ -123,8 +118,7 @@ def get_movement_waypoints(
             #  could crash onto the thermocycler if current well is not known.
 
         try:
-            # TODO(mc, 2021-01-08): inject `get_waypoints` via constructor
-            return get_waypoints(
+            return motion_planning.get_waypoints(
                 move_type=move_type,
                 origin=origin,
                 origin_cp=origin_cp,
@@ -134,5 +128,50 @@ def get_movement_waypoints(
                 max_travel_z=max_travel_z,
                 xy_waypoints=extra_waypoints,
             )
-        except MotionPlanningError as error:
+        except motion_planning.MotionPlanningError as error:
+            raise errors.FailedToPlanMoveError(str(error))
+
+    def get_movement_waypoints_to_coords(
+        self,
+        origin: Point,
+        dest: Point,
+        max_travel_z: float,
+        direct: bool,
+        additional_min_travel_z: Optional[float],
+    ) -> List[motion_planning.Waypoint]:
+        """Calculate waypoints to a destination that's specified as deck coordinates.
+
+        Args:
+            origin: The start point of the movement.
+            dest: The end point of the movement.
+            max_travel_z: How high, in deck coordinates, the pipette can go.
+                This should be measured at the bottom of whatever tip is currently
+                attached (if any).
+            direct: If True, move directly. If False, move in an arc.
+            additional_min_travel_z: The minimum height to clear, if moving in an arc.
+                Ignored if `direct` is True. If lower than the default height,
+                the default is used; this can only increase the height, not decrease it.
+        """
+        all_labware_highest_z = self._geometry.get_all_labware_highest_z()
+        if additional_min_travel_z is None:
+            additional_min_travel_z = float("-inf")
+        min_travel_z = max(all_labware_highest_z, additional_min_travel_z)
+
+        move_type = (
+            motion_planning.MoveType.DIRECT
+            if direct
+            else motion_planning.MoveType.GENERAL_ARC
+        )
+
+        try:
+            return motion_planning.get_waypoints(
+                origin=origin,
+                dest=dest,
+                min_travel_z=min_travel_z,
+                max_travel_z=max_travel_z,
+                move_type=move_type,
+                origin_cp=None,
+                dest_cp=None,
+            )
+        except motion_planning.MotionPlanningError as error:
             raise errors.FailedToPlanMoveError(str(error))

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

@@ -14,6 +14,7 @@
     LoadPipetteResult,
     AspirateResult,
     DispenseResult,
+    MoveToCoordinatesResult,
     MoveToWellResult,
     PickUpTipResult,
     DropTipResult,
@@ -87,8 +88,12 @@ def _handle_command(self, command: Command) -> None:
                 labware_id=command.params.labwareId,
                 well_name=command.params.wellName,
             )
+
         # TODO(mc, 2021-11-12): wipe out current_well on movement failures, too
-        elif isinstance(command.result, HomeResult):
+        elif isinstance(command.result, (HomeResult, MoveToCoordinatesResult)):
+            # A command left the pipette in a place that we can't associate
+            # with a logical well location. Set the current well to None
+            # to reflect the fact that it's now unknown.
             self._state.current_well = None
 
         if isinstance(command.result, LoadPipetteResult):

api/src/opentrons/protocol_runner/json_command_translator.py

@@ -115,7 +115,6 @@ def translate(
         exclude_commands = [
             "loadLiquid",
             "moveToSlot",
-            "moveToCoordinates",
         ]
         commands_to_parse = [
             command

api/tests/opentrons/motion_planning/test_waypoints.py

@@ -18,6 +18,7 @@ def test_get_waypoints_direct() -> None:
         origin=Point(1, 2, 3),
         dest=Point(1, 2, 4),
         move_type=MoveType.DIRECT,
+        min_travel_z=0,
         max_travel_z=100,
     )