feat(protocol engine): add helpers for intra-frustum calculations (#1…

…6189)

api/src/opentrons/protocol_engine/errors/__init__.py

@@ -69,6 +69,7 @@
     InvalidAxisForRobotType,
     NotSupportedOnRobotType,
     CommandNotAllowedError,
+    InvalidLiquidHeightFound,
 )
 
 from .error_occurrence import ErrorOccurrence, ProtocolCommandFailedError
@@ -146,4 +147,5 @@
     # error occurrence models
     "ErrorOccurrence",
     "CommandNotAllowedError",
+    "InvalidLiquidHeightFound",
 ]

api/src/opentrons/protocol_engine/errors/exceptions.py

@@ -1002,6 +1002,19 @@ def __init__(
         super().__init__(ErrorCodes.GENERAL_ERROR, message, details, wrapping)
 
 
+class InvalidLiquidHeightFound(ProtocolEngineError):
+    """Raised when attempting to estimate liquid height based on volume fails."""
+
+    def __init__(
+        self,
+        message: Optional[str] = None,
+        details: Optional[Dict[str, Any]] = None,
+        wrapping: Optional[Sequence[EnumeratedError]] = None,
+    ) -> None:
+        """Build an InvalidLiquidHeightFound error."""
+        super().__init__(ErrorCodes.GENERAL_ERROR, message, details, wrapping)
+
+
 class EStopActivatedError(ProtocolEngineError):
     """Represents an E-stop event."""
 

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

@@ -0,0 +1,176 @@
+"""Helper functions for liquid-level related calculations inside a given frustum."""
+from typing import List, Tuple
+from numpy import pi, iscomplex, roots, real
+
+from ..errors.exceptions import InvalidLiquidHeightFound
+
+
+def reject_unacceptable_heights(
+    potential_heights: List[float], max_height: float
+) -> float:
+    """Reject any solutions to a polynomial equation that cannot be the height of a frustum."""
+    valid_heights = []
+    for root in potential_heights:
+        # reject any heights that are negative or greater than the max height
+        if not iscomplex(root):
+            # take only the real component of the root and round to 4 decimal places
+            rounded_root = round(real(root), 4)
+            if (rounded_root <= max_height) and (rounded_root >= 0):
+                valid_heights.append(rounded_root)
+    if len(valid_heights) != 1:
+        raise InvalidLiquidHeightFound(
+            message="Unable to estimate valid liquid height from volume."
+        )
+    return valid_heights[0]
+
+
+def rectangular_frustum_polynomial_roots(
+    bottom_length: float,
+    bottom_width: float,
+    top_length: float,
+    top_width: float,
+    total_frustum_height: float,
+) -> Tuple[float, float, float]:
+    """Polynomial representation of the volume of a rectangular frustum."""
+    # roots of the polynomial with shape ax^3 + bx^2 + cx
+    a = (
+        (top_length - bottom_length)
+        * (top_width - bottom_width)
+        / (3 * total_frustum_height**2)
+    )
+    b = (
+        (bottom_length * (top_width - bottom_width))
+        + (bottom_width * (top_length - bottom_length))
+    ) / (2 * total_frustum_height)
+    c = bottom_length * bottom_width
+    return a, b, c
+
+
+def circular_frustum_polynomial_roots(
+    bottom_radius: float,
+    top_radius: float,
+    total_frustum_height: float,
+) -> Tuple[float, float, float]:
+    """Polynomial representation of the volume of a circular frustum."""
+    # roots of the polynomial with shape ax^3 + bx^2 + cx
+    a = pi * ((top_radius - bottom_radius) ** 2) / (3 * total_frustum_height**2)
+    b = pi * bottom_radius * (top_radius - bottom_radius) / total_frustum_height
+    c = pi * bottom_radius**2
+    return a, b, c
+
+
+def volume_from_height_circular(
+    target_height: float,
+    total_frustum_height: float,
+    bottom_radius: float,
+    top_radius: float,
+) -> float:
+    """Find the volume given a height within a circular frustum."""
+    a, b, c = circular_frustum_polynomial_roots(
+        bottom_radius=bottom_radius,
+        top_radius=top_radius,
+        total_frustum_height=total_frustum_height,
+    )
+    volume = a * (target_height**3) + b * (target_height**2) + c * target_height
+    return volume
+
+
+def volume_from_height_rectangular(
+    target_height: float,
+    total_frustum_height: float,
+    bottom_length: float,
+    bottom_width: float,
+    top_length: float,
+    top_width: float,
+) -> float:
+    """Find the volume given a height within a rectangular frustum."""
+    a, b, c = rectangular_frustum_polynomial_roots(
+        bottom_length=bottom_length,
+        bottom_width=bottom_width,
+        top_length=top_length,
+        top_width=top_width,
+        total_frustum_height=total_frustum_height,
+    )
+    volume = a * (target_height**3) + b * (target_height**2) + c * target_height
+    return volume
+
+
+def volume_from_height_spherical(
+    target_height: float,
+    radius_of_curvature: float,
+) -> float:
+    """Find the volume given a height within a spherical frustum."""
+    volume = (
+        (1 / 3) * pi * (target_height**2) * (3 * radius_of_curvature - target_height)
+    )
+    return volume
+
+
+def height_from_volume_circular(
+    volume: float,
+    total_frustum_height: float,
+    bottom_radius: float,
+    top_radius: float,
+) -> float:
+    """Find the height given a volume within a circular frustum."""
+    a, b, c = circular_frustum_polynomial_roots(
+        bottom_radius=bottom_radius,
+        top_radius=top_radius,
+        total_frustum_height=total_frustum_height,
+    )
+    d = volume * -1
+    x_intercept_roots = (a, b, c, d)
+
+    height_from_volume_roots = roots(x_intercept_roots)
+    height = reject_unacceptable_heights(
+        potential_heights=list(height_from_volume_roots),
+        max_height=total_frustum_height,
+    )
+    return height
+
+
+def height_from_volume_rectangular(
+    volume: float,
+    total_frustum_height: float,
+    bottom_length: float,
+    bottom_width: float,
+    top_length: float,
+    top_width: float,
+) -> float:
+    """Find the height given a volume within a rectangular frustum."""
+    a, b, c = rectangular_frustum_polynomial_roots(
+        bottom_length=bottom_length,
+        bottom_width=bottom_width,
+        top_length=top_length,
+        top_width=top_width,
+        total_frustum_height=total_frustum_height,
+    )
+    d = volume * -1
+    x_intercept_roots = (a, b, c, d)
+
+    height_from_volume_roots = roots(x_intercept_roots)
+    height = reject_unacceptable_heights(
+        potential_heights=list(height_from_volume_roots),
+        max_height=total_frustum_height,
+    )
+    return height
+
+
+def height_from_volume_spherical(
+    volume: float,
+    radius_of_curvature: float,
+    total_frustum_height: float,
+) -> float:
+    """Find the height given a volume within a spherical frustum."""
+    a = -1 * pi / 3
+    b = pi * radius_of_curvature
+    c = 0.0
+    d = volume * -1
+    x_intercept_roots = (a, b, c, d)
+
+    height_from_volume_roots = roots(x_intercept_roots)
+    height = reject_unacceptable_heights(
+        potential_heights=list(height_from_volume_roots),
+        max_height=total_frustum_height,
+    )
+    return height