v1: PreciseFlexArmBackend: classify the arm's reach (standard / extended / unknown) from controller link lengths

pylabrobot/brooks/kinematics.py

@@ -16,26 +16,53 @@
 """
 
 from dataclasses import dataclass
-from math import atan2, cos, hypot, pi, radians, degrees, sin
-from typing import TYPE_CHECKING
+from math import atan2, cos, degrees, hypot, pi, radians, sin
+from typing import TYPE_CHECKING, Literal, Tuple
 
 from pylabrobot.capabilities.arms.standard import JointPose
 
 if TYPE_CHECKING:
   from pylabrobot.brooks.precise_flex import PreciseFlexCartesianPose
 
 
+# Known PF400 link-length configs (l1 = shoulder->elbow, l2 = elbow->wrist), in mm, per the 615287
+# System Dimensions - the single source of truth for the standard vs extended arm.
+ARM_LINKS_STANDARD = (225.0, 210.0)
+ARM_LINKS_EXTENDED = (302.0, 289.0)
+_LINK_MATCH_TOLERANCE = 5.0  # mm; per-link calibration spread allowed when matching a read
+
+
 @dataclass
 class PF400Params:
   """Calibrated link lengths; sub-mm FK residual on a held-out probe set."""
 
-  l1: float = 302.0  # shoulder -> elbow [mm]
-  l2: float = 289.0  # elbow -> wrist [mm]
+  l1: float = ARM_LINKS_EXTENDED[0]  # shoulder -> elbow [mm]
+  l2: float = ARM_LINKS_EXTENDED[1]  # elbow -> wrist [mm]
   gripper_length: float = 162.0  # wrist -> TCP [mm]
   gripper_z_offset: float = 0.0
   eps: float = 1e-6
 
 
+def _classify_pf400_reach(links: Tuple[float, float]) -> Literal["standard", "extended", "unknown"]:
+  """Classify (l1, l2) link lengths as the standard or extended PF400 arm, or "unknown".
+
+  "unknown" means the lengths match neither known config - a sign the arm's device-stored link
+  lengths may have been changed.
+
+  Args:
+    links: (l1, l2) link lengths in mm (inner shoulder -> elbow, outer elbow -> wrist).
+  Returns:
+    "standard", "extended", or "unknown".
+  """
+  l1, l2 = links
+  tol = _LINK_MATCH_TOLERANCE
+  if abs(l1 - ARM_LINKS_STANDARD[0]) <= tol and abs(l2 - ARM_LINKS_STANDARD[1]) <= tol:
+    return "standard"
+  if abs(l1 - ARM_LINKS_EXTENDED[0]) <= tol and abs(l2 - ARM_LINKS_EXTENDED[1]) <= tol:
+    return "extended"
+  return "unknown"
+
+
 class IKError(ValueError):
   """Target pose is unreachable."""
 

pylabrobot/brooks/precise_flex.py

@@ -7,16 +7,16 @@
 from enum import IntEnum
 from typing import Dict, List, Literal, Optional
 
-from pylabrobot.brooks.error_codes import ERROR_CODES
-from pylabrobot.brooks.data_ids import DataID
+from pylabrobot.brooks import kinematics
 from pylabrobot.brooks.confirmed_firmware_versions import (
   SUPPORTED_ROBOT_TYPES,
   is_confirmed,
   is_supported_model,
   suggest_entry,
 )
+from pylabrobot.brooks.data_ids import DataID
+from pylabrobot.brooks.error_codes import ERROR_CODES
 from pylabrobot.brooks.tcs_modules import missing_required_modules
-from pylabrobot.brooks import kinematics
 from pylabrobot.capabilities.arms.backend import (
   CanFreedrive,
   HasJoints,
@@ -76,7 +76,8 @@ class PreciseFlexConfiguration:
   via ``request_parameter`` and the ``version`` command). The kinematics/flags
   tier is supplied at construction or derived: link lengths are not on the arm,
   ``has_rail`` comes from the joint set, ``is_dual_gripper`` from the axis_mask
-  ``&H80`` bit, and ``is_vision_gripper``/``reach_class`` from the model name.
+  ``&H80`` bit, ``is_vision_gripper`` from the model name, and ``reach_class`` from the
+  controller-read link lengths.
   """
 
   # --- identity / version (DataIDs 100-110, 2002, 116; version command) ---
@@ -108,7 +109,9 @@ class PreciseFlexConfiguration:
   has_rail: bool = False
   is_dual_gripper: bool = False
   is_vision_gripper: bool = False
-  reach_class: Literal["standard", "extended"] = "standard"
+  # "unknown" if the controller-read link lengths match neither known arm; defaults to "extended"
+  # to match the default PF400Params (the extended/XR link lengths)
+  reach_class: Literal["standard", "extended", "unknown"] = "extended"
 
   @property
   def gripper_width_range(self) -> tuple:
@@ -1461,10 +1464,17 @@ async def _request_configuration(self) -> "PreciseFlexConfiguration":
     else:
       kinematic_params = self._kinematics_params
       kinematics_source = "provided"
-    # Classify by reach: the standard 400 has l1+l2 ~= 435 mm, the extended ~= 591.
-    reach_class: Literal["standard", "extended"] = (
-      "extended" if (kinematic_params.l1 + kinematic_params.l2) >= 513 else "standard"
-    )
+    reach_class = kinematics._classify_pf400_reach((kinematic_params.l1, kinematic_params.l2))
+    if reach_class == "unknown":
+      logger.warning(
+        "[PreciseFlex %s] link lengths l1=%.1f l2=%.1f match neither the standard %s nor "
+        "extended %s PF400 arm; the arm's device-stored link lengths may have been changed",
+        self.driver.io._host,
+        kinematic_params.l1,
+        kinematic_params.l2,
+        kinematics.ARM_LINKS_STANDARD,
+        kinematics.ARM_LINKS_EXTENDED,
+      )
 
     return PreciseFlexConfiguration(
       manufacturer=await self.request_manufacturer(),

pylabrobot/brooks/precise_flex_tests.py

@@ -2,6 +2,7 @@
 from typing import Tuple
 from unittest.mock import AsyncMock, MagicMock
 
+from pylabrobot.brooks import kinematics
 from pylabrobot.brooks.precise_flex import Axis, PreciseFlex400Backend
 
 
@@ -20,6 +21,16 @@ def _make_backend(
   return backend, driver
 
 
+class TestClassifyPF400Reach(unittest.TestCase):
+  """Link lengths are classified as standard, extended, or unknown reach."""
+
+  def test_classify_pf400_reach(self):
+    self.assertEqual(kinematics._classify_pf400_reach((225, 210)), "standard")
+    self.assertEqual(kinematics._classify_pf400_reach((302, 289)), "extended")
+    self.assertEqual(kinematics._classify_pf400_reach((303, 288)), "extended")  # within tolerance
+    self.assertEqual(kinematics._classify_pf400_reach((500, 500)), "unknown")
+
+
 class TestPreciseFlex400Gripper(unittest.IsolatedAsyncioTestCase):
   def setUp(self):
     # closed_gripper_position=500 ⇒ min_gripper_width(60mm) maps to 500 units.