feat(api): use instrument max achievable height in plan_moves (#5193)

* feat(api): use instrument max achievable height in plan_moves

closes #5156

api/src/opentrons/config/robot_configs.py

@@ -41,6 +41,7 @@
 Y_CURRENT_LOW = 0.3
 Y_CURRENT_HIGH = 1.25
 
+Z_RETRACT_DISTANCE = 2
 
 HIGH_CURRENT: Dict[str, float] = {
     'X': X_CURRENT_HIGH,
@@ -177,7 +178,8 @@
         'mount_offset',
         'log_level',
         'tip_probe',
-        'default_pipette_configs'
+        'default_pipette_configs',
+        'z_retract_distance'
     ]
 )
 
@@ -290,7 +292,9 @@ def build_config(deck_cal: List[List[float]],
         tip_probe=_build_tip_probe(
             _tip_probe_settings_with_migration(robot_settings)),
         default_pipette_configs=robot_settings.get(
-            'default_pipette_configs', DEFAULT_PIPETTE_CONFIGS)
+            'default_pipette_configs', DEFAULT_PIPETTE_CONFIGS),
+        z_retract_distance=robot_settings.get(
+            'z_retract_distance', Z_RETRACT_DISTANCE),
     )
     return cfg
 

api/src/opentrons/hardware_control/api.py

@@ -1556,3 +1556,22 @@ async def update_instrument_offset(self, mount,
         await self.update_config(instrument_offset=inst_offs)
         pip.update_instrument_offset(new_offset)
         robot_configs.save_robot_settings(self._config)
+
+    def get_instrument_max_height(
+            self,
+            mount: top_types.Mount,
+            critical_point: CriticalPoint = None) -> float:
+        """Return max achievable height of the attached instrument
+        based on the current critical point
+        """
+        pip = self._attached_instruments[mount]
+        assert pip
+        cp = self._critical_point_for(mount, critical_point)
+
+        max_height = pip.config.home_position - \
+            self._config.z_retract_distance + cp.z
+
+        _, _, transformed_z = linal.apply_reverse(
+            self._config.gantry_calibration,
+            (0, 0, max_height))
+        return transformed_z

api/src/opentrons/protocol_api/geometry.py

@@ -8,14 +8,18 @@
 from .labware import (Labware, Well,
                       quirks_from_any_parent)
 from .definitions import DeckItem
-from .module_geometry import ThermocyclerGeometry, ModuleType
+from .module_geometry import ThermocyclerGeometry, ModuleGeometry, ModuleType
 from opentrons.hardware_control.types import CriticalPoint
 from opentrons.system.shared_data import load_shared_data
 
 
 MODULE_LOG = logging.getLogger(__name__)
 
 
+class LabwareHeightError(Exception):
+    pass
+
+
 def max_many(*args):
     return functools.reduce(max, args[1:], args[0])
 
@@ -34,10 +38,12 @@ def plan_moves(
         from_loc: types.Location,
         to_loc: types.Location,
         deck: 'Deck',
+        instr_max_height: float,
         well_z_margin: float = 5.0,
         lw_z_margin: float = 10.0,
         force_direct: bool = False,
-        minimum_z_height: float = None)\
+        minimum_lw_z_margin: float = 1.0,
+        minimum_z_height: float = None,)\
         -> List[Tuple[types.Point,
                       Optional[CriticalPoint]]]:
     """ Plan moves between one :py:class:`.Location` and another.
@@ -91,20 +97,42 @@ def plan_moves(
     if to_lw and to_lw == from_lw:
         # If we know the labwares we’re moving from and to, we can calculate
         # a safe z based on their heights
-        # TODO: Remove these awful Well.top() calls when we eliminate the back
-        #       compat wrapper
         if to_well:
-            to_safety = Well.top(to_well).point.z + well_z_margin
+            to_safety = to_well.top().point.z + well_z_margin
         else:
             to_safety = to_lw.highest_z + well_z_margin
         if from_well:
-            from_safety = Well.top(from_well).point.z + well_z_margin
+            from_safety = from_well.top().point.z + well_z_margin
         else:
             from_safety = from_lw.highest_z + well_z_margin
+        # if we are already at the labware, we know the instr max height would
+        # be tall enough
+        if max(from_safety, to_safety) > instr_max_height:
+            to_safety = instr_max_height
+            from_safety = 0.0  # (ignore since it's in a max())
     else:
         # One of our labwares is invalid so we have to just go above
         # deck.highest_z since we don’t know where we are
         to_safety = deck.highest_z + lw_z_margin
+
+        if to_safety > instr_max_height:
+            if instr_max_height >= (deck.highest_z + minimum_lw_z_margin):
+                to_safety = instr_max_height
+            else:
+                tallest_lw = list(filter(
+                    lambda lw: lw.highest_z == deck.highest_z,
+                    [lw for lw in deck.data.values() if lw]))[0]
+                if isinstance(tallest_lw, ModuleGeometry) and\
+                        tallest_lw.labware:
+                    tallest_lw = tallest_lw.labware
+                raise LabwareHeightError(
+                    f"The {tallest_lw} has a total height of {deck.highest_z}"
+                    " mm, which is too tall for your current pipette "
+                    "configurations. The longest pipette on your robot can "
+                    f"only be raised to {instr_max_height} mm above the deck."
+                    " This may be because the labware is incorrectly defined,"
+                    " incorrectly calibrated, or physically too tall. Please "
+                    "check your labware definitions and calibrations.")
         from_safety = 0.0  # (ignore since it’s in a max())
 
     safe = max_many(

api/src/opentrons/protocol_api/instrument_context.py

@@ -1095,9 +1095,13 @@ def move_to(self, location: types.Location, force_direct: bool = False,
             if isinstance(mod, ThermocyclerContext):
                 mod.flag_unsafe_move(to_loc=location, from_loc=from_loc)
 
+        instr_max_height = \
+            self._hw_manager.hardware.get_instrument_max_height(self._mount)
         moves = geometry.plan_moves(from_loc, location, self._ctx.deck,
+                                    instr_max_height,
                                     force_direct=force_direct,
-                                    minimum_z_height=minimum_z_height)
+                                    minimum_z_height=minimum_z_height
+                                    )
         self._log.debug("move_to: {}->{} via:\n\t{}"
                         .format(from_loc, location, moves))
         try:

api/tests/opentrons/config/test_robots_config.py

@@ -27,6 +27,7 @@
             'multi': [10, 11, 12]
         }
     },
+    'z_retract_distance': 2,
     'tip_length': 999,
     'mount_offset': [-3, -2, -1],
     'serial_speed': 888,

api/tests/opentrons/protocol_api/test_geometry.py

@@ -8,6 +8,7 @@
 
 labware_name = 'corning_96_wellplate_360ul_flat'
 trough_name = 'usascientific_12_reservoir_22ml'
+P300M_GEN2_MAX_HEIGHT = 155.75
 
 
 def test_slot_names():
@@ -95,21 +96,29 @@ def test_direct_movs():
     deck = Deck()
     lw1 = labware.load(labware_name, deck.position_for(1))
 
-    same_place = plan_moves(lw1.wells()[0].top(), lw1.wells()[0].top(), deck)
+    same_place = plan_moves(
+        lw1.wells()[0].top(), lw1.wells()[0].top(), deck,
+        instr_max_height=P300M_GEN2_MAX_HEIGHT)
     assert same_place == [(lw1.wells()[0].top().point, None)]
 
-    same_well = plan_moves(lw1.wells()[0].top(), lw1.wells()[0].bottom(), deck)
+    same_well = plan_moves(
+        lw1.wells()[0].top(), lw1.wells()[0].bottom(), deck,
+        instr_max_height=P300M_GEN2_MAX_HEIGHT)
     assert same_well == [(lw1.wells()[0].bottom().point, None)]
 
 
 def test_basic_arc():
     deck = Deck()
     lw1 = labware.load(labware_name, deck.position_for(1))
     lw2 = labware.load(labware_name, deck.position_for(2))
+    deck[1] = lw1
+    deck[2] = lw2
+
     # same-labware moves should use the smaller safe z
     same_lw = plan_moves(lw1.wells()[0].top(),
                          lw1.wells()[8].bottom(),
                          deck,
+                         P300M_GEN2_MAX_HEIGHT,
                          7.0, 15.0)
     check_arc_basic(same_lw, lw1.wells()[0].top(), lw1.wells()[8].bottom())
     assert same_lw[0][0].z == lw1.wells()[0].top().point.z + 7.0
@@ -119,6 +128,7 @@ def test_basic_arc():
     different_lw = plan_moves(lw1.wells()[0].top(),
                               lw2.wells()[0].bottom(),
                               deck,
+                              P300M_GEN2_MAX_HEIGHT,
                               7.0, 15.0)
     check_arc_basic(different_lw,
                     lw1.wells()[0].top(), lw2.wells()[0].bottom())
@@ -133,13 +143,15 @@ def test_force_direct():
     same_lw = plan_moves(lw1.wells()[0].top(),
                          lw1.wells()[8].bottom(),
                          deck,
+                         P300M_GEN2_MAX_HEIGHT,
                          7.0, 15.0, force_direct=True)
     assert same_lw == [(lw1.wells()[8].bottom().point, None)]
 
     # different-labware moves should move direct
     different_lw = plan_moves(lw1.wells()[0].top(),
                               lw2.wells()[0].bottom(),
                               deck,
+                              P300M_GEN2_MAX_HEIGHT,
                               7.0, 15.0, force_direct=True)
     assert different_lw == [(lw2.wells()[0].bottom().point, None)]
 
@@ -150,25 +162,31 @@ def test_no_labware_loc():
     deck = Deck()
     lw1 = labware.load(labware_name, deck.position_for(1))
     lw2 = labware.load(labware_name, deck.position_for(2))
+    deck[1] = lw1
+    deck[2] = lw2
     # Various flavors of locations without labware should work
     no_lw = lw1.wells()[0].top()._replace(labware=None)
 
-    no_from = plan_moves(no_lw, lw2.wells()[0].bottom(), deck, 7.0, 15.0)
+    no_from = plan_moves(no_lw, lw2.wells()[0].bottom(), deck,
+                         P300M_GEN2_MAX_HEIGHT, 7.0, 15.0)
     check_arc_basic(no_from, no_lw, lw2.wells()[0].bottom())
     assert no_from[0][0].z == deck.highest_z + 15.0
 
-    no_to = plan_moves(lw1.wells()[0].bottom(), no_lw, deck, 7.0, 15.0)
+    no_to = plan_moves(lw1.wells()[0].bottom(), no_lw, deck,
+                       P300M_GEN2_MAX_HEIGHT, 7.0, 15.0)
     check_arc_basic(no_to, lw1.wells()[0].bottom(), no_lw)
     assert no_from[0][0].z == deck.highest_z + 15.0
 
     no_well = lw1.wells()[0].top()._replace(labware=lw1)
 
-    no_from_well = plan_moves(no_well, lw1.wells()[1].top(), deck, 7.0, 15.0)
+    no_from_well = plan_moves(no_well, lw1.wells()[1].top(), deck,
+                              P300M_GEN2_MAX_HEIGHT, 7.0, 15.0)
     check_arc_basic(no_from_well, no_well, lw1.wells()[1].top())
     assert no_from_well[0][0].z\
         == labware_def['dimensions']['zDimension'] + 7.0
 
-    no_to_well = plan_moves(lw1.wells()[1].top(), no_well, deck, 7.0, 15.0)
+    no_to_well = plan_moves(lw1.wells()[1].top(), no_well, deck,
+                            P300M_GEN2_MAX_HEIGHT, 7.0, 15.0)
     check_arc_basic(no_to_well, lw1.wells()[1].top(), no_well)
     assert no_to_well[0][0].z\
         == labware_def['dimensions']['zDimension'] + 7.0
@@ -204,20 +222,22 @@ def test_arc_lower_minimum_z_height():
     tall_point = old_top.point._replace(z=tall_z)
     tall_top = old_top._replace(point=tall_point)
     to_tall = plan_moves(
-        lw1.wells()[2].top(), tall_top, deck, 7.0, 15.0, False,
+        lw1.wells()[2].top(), tall_top, deck,
+        P300M_GEN2_MAX_HEIGHT, 7.0, 15.0, False,
         minimum_z_height=minimum_z_height)
     check_arc_basic(to_tall, lw1.wells()[2].top(), tall_top)
     assert to_tall[0][0].z == tall_z
 
     from_tall = plan_moves(
-        tall_top, lw1.wells()[3].top(), deck, 7.0, 15.0,
+        tall_top, lw1.wells()[3].top(), deck,
+        P300M_GEN2_MAX_HEIGHT, 7.0, 15.0,
         minimum_z_height=minimum_z_height)
     check_arc_basic(from_tall, tall_top, lw1.wells()[3].top())
     assert from_tall[0][0].z == tall_z
 
     no_well = tall_top._replace(labware=lw1)
     from_tall_lw = plan_moves(no_well, lw1.wells()[4].bottom(), deck,
-                              7.0, 15.0)
+                              P300M_GEN2_MAX_HEIGHT, 7.0, 15.0)
     check_arc_basic(from_tall_lw, no_well, lw1.wells()[4].bottom())
 
 
@@ -229,7 +249,8 @@ def test_direct_minimum_z_height():
     zmo = 150
     # This would normally be a direct move since it’s inside the same well,
     # but we want to check that we override it into an arc
-    moves = plan_moves(from_loc, to_loc, deck, minimum_z_height=zmo)
+    moves = plan_moves(from_loc, to_loc, deck, P300M_GEN2_MAX_HEIGHT,
+                       minimum_z_height=zmo)
     assert len(moves) == 3
     assert moves[0][0].z == zmo  # equals zmo b/c 150 is max of all safe z's
     check_arc_basic(moves, from_loc, to_loc)
@@ -243,7 +264,7 @@ def test_direct_cp():
     # start in default cp
     from_nothing = plan_moves(Location(Point(50, 50, 50), None),
                               trough.wells()[0].top(),
-                              deck)
+                              deck, P300M_GEN2_MAX_HEIGHT)
     check_arc_basic(from_nothing, Location(Point(50, 50, 50), None),
                     trough.wells()[0].top())
     assert from_nothing[0][1] is None
@@ -254,7 +275,7 @@ def test_direct_cp():
     # arc
     from_centered_arc = plan_moves(trough.wells()[0].top(),
                                    trough.wells()[1].top(),
-                                   deck)
+                                   deck, P300M_GEN2_MAX_HEIGHT)
     check_arc_basic(from_centered_arc,
                     trough.wells()[0].top(), trough.wells()[1].top())
     assert from_centered_arc[0][1] == CriticalPoint.XY_CENTER
@@ -263,11 +284,12 @@ def test_direct_cp():
     # or direct
     from_centered_direct = plan_moves(trough.wells()[0].top(),
                                       trough.wells()[1].bottom(),
-                                      deck)
+                                      deck, P300M_GEN2_MAX_HEIGHT)
     assert from_centered_direct[0][1] == CriticalPoint.XY_CENTER
     # when moving from centered to normal, only the first move should be
     # centered
-    to_normal = plan_moves(trough.wells()[0].top(), lw1.wells()[0].top(), deck)
+    to_normal = plan_moves(trough.wells()[0].top(), lw1.wells()[0].top(), deck,
+                           P300M_GEN2_MAX_HEIGHT)
     check_arc_basic(to_normal, trough.wells()[0].top(), lw1.wells()[0].top())
     assert to_normal[0][1] == CriticalPoint.XY_CENTER
     assert to_normal[1][1] is None
@@ -297,3 +319,37 @@ def test_gen2_module_transforms():
         deck.position_for('3'),
         MAX_SUPPORTED_VERSION)
     assert mmod2.labware_offset == Point(1.425, -0.125, 82.25)
+
+
+def test_instr_max_height():
+    deck = Deck()
+    trough = labware.load(trough_name, deck.position_for(1))
+    trough2 = labware.load(trough_name, deck.position_for(2))
+    deck[1] = trough
+    deck[2] = trough2
+
+    # if the highest deck height is between 1 mm and 10 mm below
+    # the max instrument achievable height, we use the max instrument
+    # height as the safe height
+    instr_max_height = trough.wells()[0].top().point.z + 1
+    moves1 = plan_moves(
+        trough.wells()[0].top(), trough2.wells()[0].top(),
+        deck, round(instr_max_height, 2), 7.0, 15.0)
+    assert moves1[0][0].z == round(instr_max_height, 2)
+
+    # if the highest deck height is > 10 mm below the max instrument
+    # height, we use the lw_z_margin instead
+    instr_max_height = trough.wells()[0].top().point.z + 30
+    moves2 = plan_moves(
+        trough.wells()[0].top(), trough2.wells()[0].top(),
+        deck, round(instr_max_height, 2), 7.0, 15.0)
+    assert moves2[0][0].z ==\
+        round(trough.wells()[0].top().point.z, 2) + 15.0
+
+    # it fails if the highest deck height is less than 1 mm below
+    # the max instr achievable height
+    instr_max_height = trough.wells()[0].top().point.z
+    with pytest.raises(Exception):
+        plan_moves(
+            trough.wells()[0].top(), trough2.wells()[0].top(),
+            deck, round(instr_max_height, 2), 7.0, 15.0)