Report the nearest arm point to each keep-out

parol6/PAROL6_ROBOT.py

@@ -112,6 +112,7 @@ def _init_collision_checker(
         if srdf_path and os.path.exists(srdf_path):
             c.load_srdf(srdf_path)
         collision = c
+        _refresh_clearance_geometry()
         logger.info(
             "Collision checker loaded: %d pairs, %d geometry objects",
             c.num_collision_pairs,
@@ -147,6 +148,31 @@ def _init_collision_checker(
 _installation_shapes: list = []
 _installation_geom_names: list[str] = []
 
+_clearance_obstacle_names: list[str] = []
+_clearance_obstacle_indices: dict[str, int] = {}
+_clearance_display_names: dict[str, str] = {}
+
+
+def _refresh_clearance_geometry() -> None:
+    """Cache query metadata at scene edits, outside repeated pose queries."""
+    _clearance_obstacle_names.clear()
+    _clearance_obstacle_indices.clear()
+    _clearance_display_names.clear()
+    if collision is None:
+        return
+    _clearance_display_names.update(collision.geometry_link_names)
+    for prefix, shapes in (
+        ("shape:", _program_shapes),
+        ("install:", _installation_shapes),
+    ):
+        for shape in shapes:
+            if not shape.collision or shape.attachment is not None:
+                continue
+            name = prefix + shape.name
+            if collision.has_geometry(name):
+                _clearance_obstacle_indices[name] = len(_clearance_obstacle_names)
+                _clearance_obstacle_names.append(name)
+
 
 def _refresh_collision_tool_geometry(
     tool_key: str,
@@ -212,9 +238,11 @@ def _refresh_collision_tool_geometry(
             for name in _active_tool_geom_names:
                 collision.remove_geometry_by_name(name)
             _active_tool_geom_names.clear()
+            _refresh_clearance_geometry()
             raise
 
     _active_tool_geom_key = key
+    _refresh_clearance_geometry()
 
 
 def apply_tool(
@@ -335,6 +363,7 @@ def apply_shapes(shapes: "Iterable[Any]") -> None:
         _replace_program_geometry(previous)
         raise
     _program_shapes = shapes
+    _refresh_clearance_geometry()
 
 
 def _replace_program_geometry(shapes: list) -> None:
@@ -399,6 +428,7 @@ def apply_installation_shapes(shapes: "Iterable[Any]") -> None:
             name, s.kind, s.params(), _pose_to_matrix(s.pose), margin=s.margin
         )
         _installation_geom_names.append(name)
+    _refresh_clearance_geometry()
 
 
 def installation_shapes() -> "list[Any]":

parol6/robot.py

@@ -18,18 +18,21 @@
 import time
 from dataclasses import dataclass
 from importlib.resources import files as pkg_files
+from operator import attrgetter
 from pathlib import Path
 from typing import Any, Literal
 
 import msgspec
 import numpy as np
 from numpy.typing import NDArray
+from pinokin import CollisionChecker
 from pinokin import Robot as PinokinRobot
 from pinokin import se3_from_rpy, so3_rpy
 from collections.abc import Callable
 from waldoctl import (
     CartesianKinodynamicLimits,
     ChannelDescriptor,
+    Clearance,
     DryRunClient,
     ElectricGripperTool,
     HomePosition,
@@ -526,6 +529,12 @@ class Parol6IKResult:
 # ===========================================================================
 
 
+_NearestPoints = tuple[
+    str, str, float, tuple[float, float, float], tuple[float, float, float]
+]
+_clearance_distance = attrgetter("distance")
+
+
 class Robot(_RobotABC):
     """Unified PAROL6 robot - inherits from waldoctl.Robot ABC.
 
@@ -584,6 +593,7 @@ def __init__(
         self._T_buf = np.asfortranarray(np.zeros((4, 4), dtype=np.float64))
         self._rpy_buf = np.zeros(3, dtype=np.float64)
         self._T_target_buf = np.zeros((4, 4), dtype=np.float64)
+        self._clearance_best: list[_NearestPoints | None] = []
 
     # -- Identity -----------------------------------------------------------
 
@@ -872,6 +882,60 @@ def min_distance(self, q_rad: NDArray[np.float64]) -> float:
         self._load_q_buf(q_rad)
         return c.min_distance(self._q_buf)
 
+    def clearances(self, q_rad: NDArray[np.float64]) -> list[Clearance]:
+        """Nearest arm/tool witnesses per keep-out, in world metres, closest first."""
+        c: CollisionChecker | None = self._collision_checker
+        if c is None:
+            return []
+        import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+        names = PAROL6_ROBOT._clearance_obstacle_names
+        if not names:
+            return []
+        indices = PAROL6_ROBOT._clearance_obstacle_indices
+        reports = PAROL6_ROBOT._clearance_display_names
+        if len(self._clearance_best) != len(names):
+            self._clearance_best = [None] * len(names)
+        best = self._clearance_best
+        self._load_q_buf(q_rad)
+        pairs: list[_NearestPoints] = c.nearest_points(self._q_buf, names)
+        for pair in pairs:
+            first, second = pair[0], pair[1]
+            index = indices.get(first)
+            if index is not None:
+                if second in indices:
+                    continue
+            else:
+                index = indices.get(second)
+                if index is None:
+                    continue
+            previous = best[index]
+            if previous is None or pair[2] < previous[2]:
+                best[index] = pair
+
+        result: list[Clearance] = []
+        for index, obstacle in enumerate(names):
+            nearest = best[index]
+            best[index] = None
+            if nearest is None:
+                continue
+            first, second, distance, first_point, second_point = nearest
+            if first == obstacle:
+                part, part_point, obstacle_point = second, second_point, first_point
+            else:
+                part, part_point, obstacle_point = first, first_point, second_point
+            result.append(
+                Clearance(
+                    reports.get(part, part),
+                    obstacle,
+                    distance,
+                    part_point,
+                    obstacle_point,
+                )
+            )
+        result.sort(key=_clearance_distance)
+        return result
+
     def apply_shapes(self, shapes) -> None:
         """Apply keep-out shapes to this process's checker (preview/editing viz).
 

parol6/server/transports/udp_transport.py

@@ -26,15 +26,17 @@ class UDPTransport:
     """
 
     def __init__(
-        self, ip: str = "127.0.0.1", port: int = 5001, buffer_size: int = 1024
+        self, ip: str = "127.0.0.1", port: int = 5001, buffer_size: int = 65535
     ):
         """
         Initialize the UDP transport.
 
         Args:
             ip: IP address to bind to
             port: Port number to listen on
-            buffer_size: Size of the receive buffer in bytes
+            buffer_size: Size of the receive buffer in bytes. A datagram
+                longer than this is truncated, so the default holds the
+                largest UDP datagram (a SET_SHAPES of a whole workcell).
         """
         self.ip = ip
         self.port = port

pyproject.toml

@@ -10,31 +10,31 @@ description = "Python library for controlling PAROL6 robot arms"
 requires-python = ">=3.11"
 dependencies = [
     # pinokin: Pinocchio-based FK/IK bindings
-    # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.8
+    # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.9
 
     # macOS ARM64 (Apple Silicon)
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'",
 
     # Windows AMD64
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'",
 
     # Linux x86_64
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'",
 
     # Linux aarch64 (ARM64)
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'",
-    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.8/pinokin-0.1.8-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'",
+    "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.9/pinokin-0.1.9-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'",
 
     "pyserial>=3.4",
     "scipy>=1.11.4",
@@ -45,7 +45,7 @@ dependencies = [
     "psutil>=5.9",
     "msgspec>=0.18",
     "ormsgpack>=1.4.0",
-    "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.15.0",
+    "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.16.0",
 ]
 
 [tool.setuptools.packages.find]

tests/integration/test_shapes_e2e.py

@@ -229,6 +229,19 @@ def test_set_shapes_ack_readback_rejection_and_timeout(
         dead = RobotClient(host=ports.server_ip, port=free_udp_port(), timeout=0.3)
         assert dead.set_shapes([box]) == 0
         assert dead.shapes() is None
+
+        # A whole workcell is one datagram several KiB long; it is applied,
+        # not truncated into a decode error.
+        cell = [
+            Box(
+                name=f"bin{i}", x=0.05, y=0.05, z=0.05, pose=(0.9, 0.06 * i, 0, 0, 0, 0)
+            )
+            for i in range(40)
+        ]
+        assert client.set_shapes(cell) == 1
+        world = client.shapes()
+        assert world is not None
+        assert world.program == tuple(cell)
     finally:
         assert client.set_shapes([]) == 1
         world = client.shapes()

tests/unit/test_collision_integration.py

@@ -55,6 +55,71 @@ def test_robot_collision_methods_clear_at_home():
     assert r.check_trajectory(q_path) == -1
 
 
+def test_robot_clearances_report_nearest_world_points_per_keepout(monkeypatch):
+    from waldoctl import Box
+
+    r = Robot()
+    q = np.zeros(6)
+    previous_program = PAROL6_ROBOT.program_shapes()
+    previous_installation = PAROL6_ROBOT.installation_shapes()
+    try:
+        r.apply_shapes([])
+        PAROL6_ROBOT.apply_installation_shapes([])
+        assert r.clearances(q) == []
+        gap = min(0.001, r.min_distance(q) / 2)
+        base = trimesh.load_mesh(
+            str(Path(PAROL6_ROBOT._mesh_dir) / "meshes" / "base_link_simplified.stl")
+        )
+        floor_top = float(base.bounds[0, 2]) - gap
+        PAROL6_ROBOT.apply_installation_shapes(
+            [
+                Box(
+                    name="floor",
+                    x=2,
+                    y=2,
+                    z=0.1,
+                    pose=(0, 0, floor_top - 0.05, 0, 0, 0),
+                )
+            ]
+        )
+        r.apply_shapes(
+            [
+                Box(
+                    name="floor",
+                    x=2,
+                    y=2,
+                    z=0.1,
+                    pose=(0, 0, floor_top - 1.05, 0, 0, 0),
+                ),
+                Box(name="marker", x=2, y=2, z=2, collision=False),
+            ]
+        )
+        clearances = r.clearances(q)
+        assert [c.obstacle for c in clearances] == ["install:floor", "shape:floor"]
+        nearest = clearances[0]
+        assert nearest.part == "base_link"
+        assert nearest.distance == pytest.approx(gap, abs=1e-7)
+        assert nearest.distance == pytest.approx(r.min_distance(q), abs=1e-7)
+        assert nearest.part_point[2] > nearest.obstacle_point[2]
+        assert nearest.obstacle_point[2] == pytest.approx(floor_top, abs=1e-7)
+        for clearance in clearances:
+            assert np.linalg.norm(
+                np.subtract(clearance.part_point, clearance.obstacle_point)
+            ) == pytest.approx(abs(clearance.distance), abs=1e-7)
+        assert clearances[1].distance == pytest.approx(gap + 1, abs=1e-7)
+
+        r.apply_shapes([])
+        assert [c.obstacle for c in r.clearances(q)] == ["install:floor"]
+        with monkeypatch.context() as patch:
+            patch.setattr(PAROL6_ROBOT, "collision", None)
+            assert r.clearances(q) == []
+        PAROL6_ROBOT.apply_installation_shapes([])
+        assert r.clearances(q) == []
+    finally:
+        PAROL6_ROBOT.apply_installation_shapes(previous_installation)
+        r.apply_shapes(previous_program)
+
+
 def test_guard_joint_path_clear_returns_none():
     positions = np.zeros((10, 6))
     # No exception means no collision detected.