fix(api): add checking for imposible dynamic moves (#20054)

ryanthecoder · web-flow · commit 658fbd992c2c · 2025-11-07T15:41:04.000-05:00
# Overview
This PR updates the OT3 Controller's motion planning to adjust the
commanded speed for a multi axis move if one of the plunger's are in the
moving axis. this ensures that the pipettes' plunger will move at the
speed commanded.

Right now we only move the plunger and other axis during dynamic
pipetting so this won't affect anything else in the system.

After doing it's best to adjust this speed, if the move is still
impossible an error will be logged into the api logs.
diff --git a/api/src/opentrons/hardware_control/backends/ot3controller.py b/api/src/opentrons/hardware_control/backends/ot3controller.py
@@ -662,7 +662,12 @@ def _build_move_node_axis_runner(
     ) -> Tuple[Optional[MoveGroupRunner], bool]:
         if not target:
             return None, False
-        move_target = MoveTarget.build(position=target, max_speed=speed)
+        # Create a target that doesn't incorporate the plunger into a joint axis with the gantry
+        plunger_axes = [Axis.P_L, Axis.P_R]
+        move_target = self._move_manager.devectorize_axes(
+            origin, target, speed, plunger_axes
+        )
+
         try:
             _, movelist = self._move_manager.plan_motion(
                 origin=origin, target_list=[move_target]
@@ -693,6 +698,20 @@ def _build_move_node_axis_runner(
                 move_group, ordered_nodes, (delay_nodes, delay_time)
             )
 
+        (
+            plunger_slowed,
+            error_str,
+        ) = self._move_manager.ensure_pipette_flow_rate_unchanged(
+            [node_to_axis(node) for node in ordered_nodes],
+            origin,
+            target,
+            speed,
+            move_group,
+            [(ax, axis_to_node(ax)) for ax in plunger_axes],
+        )
+        if plunger_slowed:
+            log.error(error_str)
+
         return (
             MoveGroupRunner(
                 move_groups=[move_group],
diff --git a/api/tests/opentrons/hardware_control/backends/test_ot3_controller.py b/api/tests/opentrons/hardware_control/backends/test_ot3_controller.py
@@ -1483,6 +1483,148 @@ async def test_controller_move(
         assert gear_position == gear_position
 
 
+@pytest.mark.parametrize(
+    argnames=["origin_pos", "target_pos", "expected_pos", "gear_position"],
+    argvalues=[
+        [
+            {
+                Axis.X: 0,
+                Axis.Y: 0,
+                Axis.Z_L: 0,
+                Axis.Z_R: 0,
+                Axis.P_L: 0,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            {
+                Axis.X: 10,
+                Axis.Y: 10,
+                Axis.Z_L: 50,
+                Axis.P_L: 70,
+            },
+            {
+                Axis.X: 10,
+                Axis.Y: 10,
+                Axis.Z_L: 50,
+                Axis.Z_R: 0,
+                Axis.P_L: 70,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            None,
+        ],
+        [
+            {
+                Axis.X: 0,
+                Axis.Y: 0,
+                Axis.Z_L: 0,
+                Axis.Z_R: 0,
+                Axis.P_L: 0,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            {
+                Axis.X: 20,
+                Axis.Y: 20,
+                Axis.Z_L: 10,
+                Axis.P_L: 48,
+            },
+            {
+                Axis.X: 20,
+                Axis.Y: 20,
+                Axis.Z_L: 10,
+                Axis.Z_R: 0,
+                Axis.P_L: 48,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            None,
+        ],
+        [
+            {
+                Axis.X: 0,
+                Axis.Y: 0,
+                Axis.Z_L: 0,
+                Axis.Z_R: 0,
+                Axis.P_L: 0,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            {
+                Axis.P_L: 70,
+            },
+            {
+                Axis.X: 0,
+                Axis.Y: 0,
+                Axis.Z_L: 0,
+                Axis.Z_R: 0,
+                Axis.P_L: 70,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            None,
+        ],
+        [
+            {
+                Axis.X: 0,
+                Axis.Y: 0,
+                Axis.Z_L: 0,
+                Axis.Z_R: 0,
+                Axis.P_L: 0,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            {
+                Axis.P_L: 30,  # Too short to hit top speed
+            },
+            {
+                Axis.X: 0,
+                Axis.Y: 0,
+                Axis.Z_L: 0,
+                Axis.Z_R: 0,
+                Axis.P_L: 30,
+                Axis.P_R: 0,
+                Axis.Z_G: 0,
+                Axis.G: 0,
+            },
+            None,
+        ],
+    ],
+)
+async def test_controller_move_dynamic(
+    controller: OT3Controller,
+    mock_present_devices: mock.AsyncMock,
+    origin_pos: Dict[Axis, float],
+    target_pos: Dict[Axis, float],
+    expected_pos: Dict[Axis, float],
+    gear_position: Optional[float],
+) -> None:
+    from copy import deepcopy
+
+    controller.update_constraints_for_gantry_load(GantryLoad.LOW_THROUGHPUT)
+
+    run_target_pos = deepcopy(target_pos)
+    config = {"run.side_effect": move_group_run_side_effect(controller, run_target_pos)}
+    with mock.patch(  # type: ignore [call-overload]
+        "opentrons.hardware_control.backends.ot3controller.MoveGroupRunner",
+        spec=MoveGroupRunner,
+        **config
+    ):
+        await controller.move(origin_pos, target_pos, 70)
+        position = await controller.update_position()
+        gear_position = controller.gear_motor_position
+
+        assert position == expected_pos
+        assert gear_position == gear_position
+
+
 @pytest.mark.parametrize(
     argnames=["axes", "pipette_has_sensor"],
     argvalues=[[[Axis.P_L, Axis.P_R], True], [[Axis.P_L, Axis.P_R], False]],
diff --git a/hardware/opentrons_hardware/hardware_control/motion_planning/move_manager.py b/hardware/opentrons_hardware/hardware_control/motion_planning/move_manager.py
@@ -1,6 +1,6 @@
 """Move manager."""
 import logging
-from typing import List, Tuple, Generic
+from typing import List, Tuple, Generic, Dict
 from opentrons_hardware.hardware_control.motion_planning import move_utils
 from opentrons_hardware.hardware_control.motion_planning.types import (
     Coordinates,
@@ -10,6 +10,10 @@
     AxisKey,
     CoordinateValue,
 )
+from opentrons_hardware.hardware_control.motion import MoveGroup
+from opentrons_hardware.firmware_bindings.constants import NodeId
+from numpy import isclose
+import math
 
 log = logging.getLogger(__name__)
 
@@ -57,6 +61,77 @@ def _add_dummy_start_end_to_moves(
         end_move = Move.build_dummy(move_list[0].unit_vector.keys())
         return [start_move] + move_list + [end_move]
 
+    def devectorize_axes(
+        self,
+        origin: Coordinates[AxisKey, CoordinateValue],
+        target: Coordinates[AxisKey, CoordinateValue],
+        speed: float,
+        axes: List[AxisKey],
+    ) -> MoveTarget[AxisKey]:
+        """This helper method is used when a plunger is moving in conjunction with other axis.
+
+        It adjusts the speed by multiplying it by the inverse of the plunger's unit vector.
+        If only the plunger is moving, this will just result in the speed being multiplied by 1
+        and if there is more axes moving the 'speed' argument will be increased in such a way that
+        the plunger will have the speed specified by the speed argument once the target is created.
+        """
+        move_target = MoveTarget.build(position=target, max_speed=speed)  # type: ignore[type-var]
+        move_info = self._get_initial_moves_from_targets(origin, [move_target])[1]
+        target_axes = move_info.unit_vector.keys()
+        for axis in axes:
+            if axis in target_axes:
+                move_target = MoveTarget.build(  # type: ignore[type-var]
+                    position=target,
+                    max_speed=float(speed * (1 / move_info.unit_vector[axis].item())),
+                )
+        return move_target
+
+    def ensure_pipette_flow_rate_unchanged(
+        self,
+        moving_axes: List[AxisKey],
+        origin: Dict[AxisKey, float],
+        target: Dict[AxisKey, float],
+        speed: float,
+        move_group: MoveGroup,
+        plunger_axes: List[Tuple[AxisKey, NodeId]],
+    ) -> Tuple[bool, str]:
+        """This examines the move group created to see if the plunger's speed is being reduced."""
+        for axis, node in plunger_axes:
+            if axis in moving_axes:
+                pip_constraints = self.get_constraints()[axis]
+                max_speed = pip_constraints.max_speed
+                check_speed = speed
+                if speed > max_speed:
+                    # if we've commanded an arbitrarily high speed drop it to the max speed.
+                    check_speed = max_speed.item()
+                pip_distance = abs(target[axis] - origin[axis])
+                # check to see if we can actually achieve this speed.
+                acceleration_time = (
+                    pip_constraints.max_speed - pip_constraints.max_speed_discont
+                ) / pip_constraints.max_acceleration
+                acceleration_distance = (
+                    pip_constraints.max_speed_discont * acceleration_time
+                    + 0.5 * pip_constraints.max_acceleration * (acceleration_time**2)
+                )
+                if 2 * acceleration_distance > pip_distance:
+                    # distance commanded is too short to achieve commanded speed drop the check speed
+                    # to as fast as it can do in that distance
+                    # V_max = sqrt(2 * Accel * d_accel_phase + discontinuity^2)
+                    check_speed = math.sqrt(
+                        2 * pip_constraints.max_acceleration * pip_distance / 2
+                        + pip_constraints.max_speed_discont**2
+                    )
+                pipette_speed = 0.0
+                # Iterate through the move group and find the top speed.
+                for step in move_group:
+                    pipette_speed = max(pipette_speed, step[node].velocity_mm_sec.item())  # type: ignore [union-attr]
+                if not isclose(pipette_speed, check_speed):
+                    return (
+                        True,
+                        f"Slowing down the plunger flow rate, commanded speed {check_speed} actual speed {pipette_speed}",
+                    )
+        return (False, "")
+
     def plan_motion(
         self,
         origin: Coordinates[AxisKey, CoordinateValue],
diff --git a/hardware/tests/opentrons_hardware/hardware_control/test_motion_plan.py b/hardware/tests/opentrons_hardware/hardware_control/test_motion_plan.py
@@ -267,3 +267,58 @@ def test_pipette_high_speed_motion() -> None:
             top_set_axis_speed = unit_vector[set_axis_kind] * block.final_speed
             if top_set_axis_speed != 0:
                 assert abs(top_set_axis_speed) == dummy_em_pipette_max_speed
+
+
+@given(
+    x_constraint=generate_axis_constraint(),
+    y_constraint=generate_axis_constraint(),
+    z_constraint=generate_axis_constraint(),
+    a_constraint=generate_axis_constraint(),
+    b_constraint=generate_axis_constraint(),
+    c_constraint=generate_axis_constraint(),
+)
+async def test_plunger_devectorize(
+    x_constraint: AxisConstraints,
+    y_constraint: AxisConstraints,
+    z_constraint: AxisConstraints,
+    a_constraint: AxisConstraints,
+    b_constraint: AxisConstraints,
+    c_constraint: AxisConstraints,
+) -> None:
+    """Test to make sure the devectorize function can isolate an axis from the move target speed limit."""
+    constraints: SystemConstraints[str] = {
+        "X": x_constraint,
+        "Y": y_constraint,
+        "Z": z_constraint,
+        "A": a_constraint,
+        "B": b_constraint,
+        "C": c_constraint,
+    }
+    manager = move_manager.MoveManager(constraints=constraints)
+    origin = {"X": 0, "Y": 0, "Z": 0, "A": 0}
+    target = {"X": 10, "Y": 10, "Z": 10, "A": 20}
+    speed = 10
+    devectorized = manager.devectorize_axes(origin, target, speed, ["A"])
+
+    converged, blend_log = manager.plan_motion(
+        origin=origin,
+        target_list=[devectorized],
+        iteration_limit=20,
+    )
+    uv = blend_log[0][0].unit_vector
+
+    assert (uv["A"]) * devectorized.max_speed == speed
+
+    # make sure that if the axis isn't included it doesn't effect other axes.
+    target = {"X": 10, "Y": 10, "Z": 10}
+    speed = 10
+    devectorized = manager.devectorize_axes(origin, target, speed, ["A"])
+
+    converged, blend_log = manager.plan_motion(
+        origin=origin,
+        target_list=[devectorized],
+        iteration_limit=20,
+    )
+    uv = blend_log[0][0].unit_vector
+
+    assert devectorized.max_speed == speed
