From 33cf7cd96739cebf8676a5a973d54fd615f0ca18 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 11 Oct 2026 21:20:53 -0400 Subject: [PATCH 1/4] Receive commands up to the largest UDP datagram The controller read commands into a 1024-byte buffer, and the kernel truncates a longer datagram to fit. A SET_SHAPES carrying about a dozen keep-outs is longer than that, so a whole workcell failed with "Decode error: Input data was truncated". The buffer now holds 65535 bytes, allocated once as before. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q4aXBLJyxHmGVUxK4KJqgn --- parol6/server/transports/udp_transport.py | 6 ++++-- tests/integration/test_shapes_e2e.py | 13 +++++++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/parol6/server/transports/udp_transport.py b/parol6/server/transports/udp_transport.py index e36752b..6917b48 100644 --- a/parol6/server/transports/udp_transport.py +++ b/parol6/server/transports/udp_transport.py @@ -26,7 +26,7 @@ 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. @@ -34,7 +34,9 @@ def __init__( 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 diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py index 10997c5..797f19e 100644 --- a/tests/integration/test_shapes_e2e.py +++ b/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() From 78f4b0d413b069735bac69eab21a79bbe71896f5 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 13 Oct 2026 09:48:40 -0400 Subject: [PATCH 2/4] Report the nearest arm point to each keep-out Robot.clearances(q) returns, for every keep-out of either layer, the arm or tool geometry closest to it: both witness points in world metres and the signed distance, nearest first, named as colliding_pairs names them. It asks pinokin's nearest_points for the keep-out pairs only, and the keep-out list is cached when shapes or the tool change, not per call. Needs pinokin's CollisionChecker.nearest_points (pinokin 6f88065). Implemented by Codex from Claude's spec. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q4aXBLJyxHmGVUxK4KJqgn --- parol6/PAROL6_ROBOT.py | 30 +++++++++++ parol6/robot.py | 64 +++++++++++++++++++++++ tests/unit/test_collision_integration.py | 65 ++++++++++++++++++++++++ 3 files changed, 159 insertions(+) diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py index cb7ff32..995cee2 100644 --- a/parol6/PAROL6_ROBOT.py +++ b/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]": diff --git a/parol6/robot.py b/parol6/robot.py index 5f3aa09..c4f23c9 100644 --- a/parol6/robot.py +++ b/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). diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py index 1e6b153..f51e0d3 100644 --- a/tests/unit/test_collision_integration.py +++ b/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. From 9f1e49becaed9ac3a3980b4c7f1317361ddafb33 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 14 Oct 2026 09:29:16 -0400 Subject: [PATCH 3/4] Require pinokin 0.1.9 for nearest-point queries Robot.clearances calls CollisionChecker.nearest_points, which first ships in pinokin 0.1.9. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q4aXBLJyxHmGVUxK4KJqgn --- pyproject.toml | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index c0d2d0d..241000a 100644 --- a/pyproject.toml +++ b/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", From 75b8b55dddad01cb5d9c39b2d06b6acc732f04b2 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sat, 24 Oct 2026 14:22:47 -0400 Subject: [PATCH 4/4] Require waldoctl 0.16.0 for Robot.clearances Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q4aXBLJyxHmGVUxK4KJqgn --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 241000a..2be4afa 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -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]