Panels → main: plugin tools + planner/status fixes

parol6/client/dry_run_client.py

@@ -173,6 +173,14 @@ def __init__(
         # TCP offset on the module-level robot singleton.
         PAROL6_ROBOT.apply_tool("NONE")
 
+        # Spawn-mode subprocess: the tool registry is freshly imported with only
+        # native tools, so plugin tools must be registered here too or
+        # select_tool() of a plugin tool fails in apply_tool (mirrors the planner
+        # worker).
+        from parol6.tools import register_plugin_tools
+
+        register_plugin_tools()
+
         self._state = ControllerState()
         init_deg = np.asarray(
             initial_joints_deg if initial_joints_deg is not None else HOME_ANGLES_DEG,

parol6/robot.py

@@ -41,9 +41,10 @@
     PositionLimits,
     Robot as _RobotABC,
     ToolSpec,
-    ToolsSpec,
+    ToolsCollection,
     ToolStatus,
     ToolType,
+    resolve_variant_tcp,
 )
 
 from parol6.client.async_client import AsyncRobotClient
@@ -56,6 +57,8 @@
     ElectricGripperConfig,
     PneumaticGripperConfig,
     get_registry,
+    plugin_tool_keys,
+    register_plugin_tools,
 )
 from parol6.utils.ik import check_limits, solve_ik
 
@@ -385,38 +388,6 @@ def channel_descriptors(self) -> tuple[ChannelDescriptor, ...]:
         )
 
 
-class _ToolsCollection(ToolsSpec):
-    """Concrete ToolsSpec for PAROL6."""
-
-    def __init__(self, tools: tuple[ToolSpec, ...]) -> None:
-        self._tools = tools
-        self._by_key = {t.key: t for t in tools}
-
-    @property
-    def available(self) -> tuple[ToolSpec, ...]:
-        return self._tools
-
-    @property
-    def default(self) -> ToolSpec:
-        return self._by_key.get("NONE", self._tools[0])
-
-    def __getitem__(self, key: str) -> ToolSpec:
-        return self._by_key[key]
-
-    def __contains__(self, item: object) -> bool:
-        # ToolType is a StrEnum, so test it before the plain-str branch —
-        # otherwise a category like ToolType.GRIPPER would misroute to the
-        # by-key lookup (keyed by tool key, not category) and return False.
-        if isinstance(item, ToolType):
-            return any(t.tool_type == item for t in self._tools)
-        if isinstance(item, str):
-            return item in self._by_key
-        return False
-
-    def by_type(self, tool_type: str | ToolType) -> tuple[ToolSpec, ...]:
-        return tuple(t for t in self._tools if t.tool_type == tool_type)
-
-
 # ===========================================================================
 # Helper builders
 # ===========================================================================
@@ -462,10 +433,15 @@ def _decompose_transform(
     return origin, rpy
 
 
-def _build_tools() -> _ToolsCollection:
-    """Build typed tool specs from the parol6 tool registry."""
+def _build_tools() -> ToolsCollection:
+    """Build typed tool specs from the parol6 tool registry. Plugin-registered
+    keys are excluded — they reach ``robot.tools`` through waldoctl composition
+    as their own ToolSpec classes, not as registry-derived natives."""
+    plugin_keys = plugin_tool_keys()
     tools: list[ToolSpec] = []
     for key, cfg in get_registry().items():
+        if key in plugin_keys:
+            continue
         origin, rpy = _decompose_transform(cfg.transform)
         common = dict(
             key=key,
@@ -493,7 +469,7 @@ def _build_tools() -> _ToolsCollection:
         else:
             tools.append(_ToolImpl(**common, tool_type=ToolType.NONE))
 
-    return _ToolsCollection(tuple(tools))
+    return ToolsCollection(tuple(tools), default_key="NONE")
 
 
 def _resolve_urdf_path() -> str:
@@ -555,9 +531,12 @@ def __init__(
         self._timeout = timeout
         self._manager = _ServerManager(normalize_logs=normalize_logs)
 
-        # Build configuration eagerly
+        # Build configuration eagerly. Native tools snapshot first, then plugin
+        # tools join this process's registry so client-side paths (e.g. the
+        # SelectToolCmd wire validation) accept them.
         self._joints = _build_joints()
         self._tools = _build_tools()
+        register_plugin_tools()
         self._urdf_path = _resolve_urdf_path()
         self._mesh_dir = _resolve_mesh_dir()
         self._motion_profiles = tuple(p.value.upper() for p in ProfileType)
@@ -596,7 +575,9 @@ def joints(self) -> JointsSpec:
         return self._joints
 
     @property
-    def tools(self) -> _ToolsCollection:
+    def native_tools(self) -> ToolsCollection:
+        """PAROL6's built-in tools. The waldoctl ``Robot.tools`` property
+        composes these with any plugin tools registered via ``waldoctl.tools``."""
         return self._tools
 
     @property
@@ -689,7 +670,12 @@ def set_active_tool(
         """
         from parol6.tools import get_tool_transform
 
-        T_tool = get_tool_transform(tool_key, variant_key=variant_key)
+        try:
+            T_tool = get_tool_transform(tool_key, variant_key=variant_key)
+        except ValueError:
+            # Plugin tool (waldoctl.tools) not in PAROL6's registry — derive its
+            # TCP from the ToolSpec instead.
+            T_tool = self._plugin_tool_transform(tool_key, variant_key)
 
         if tcp_offset_m is not None and any(v != 0 for v in tcp_offset_m):
             T_offset = np.eye(4)
@@ -703,6 +689,28 @@ def set_active_tool(
         else:
             self._pinokin.clear_tool_transform()
 
+    def _plugin_tool_transform(
+        self, tool_key: str, variant_key: str | None
+    ) -> NDArray[np.float64]:
+        """Flange→TCP transform for a plugin tool, from its ToolSpec's
+        ``tcp_origin``/``tcp_rpy`` (variant overrides win). Identity (with a
+        warning) if the key is unknown to both registries."""
+        try:
+            spec = self.tools[tool_key]
+        except KeyError:
+            logger.warning(
+                "Unknown tool %r; using identity TCP. Available: %s",
+                tool_key,
+                [t.key for t in self.tools.available],
+            )
+            return np.eye(4)
+        origin, rpy = resolve_variant_tcp(
+            spec.tcp_origin, spec.tcp_rpy, spec.variants, variant_key
+        )
+        T = np.zeros((4, 4), dtype=np.float64)
+        se3_from_rpy(origin[0], origin[1], origin[2], rpy[0], rpy[1], rpy[2], T)
+        return T
+
     def fk(
         self, q_rad: NDArray[np.float64], out: NDArray[np.float64]
     ) -> NDArray[np.float64]:

parol6/server/controller.py

@@ -111,6 +111,11 @@ def __init__(self, config: ControllerConfig):
         self.shutdown_event = threading.Event()
         self._initialized = False
 
+        # Register plugin tools (waldoctl.tools) before any SELECT_TOOL.
+        from parol6.tools import register_plugin_tools
+
+        register_plugin_tools()
+
         # Core components
         self.state_manager = StateManager()
         self.udp_transport: UDPTransport | None = None
@@ -887,5 +892,11 @@ def _pin_to_core(self, core: int) -> None:
         try:
             psutil.Process().cpu_affinity([core])
             logger.debug("Pinned process to CPU core %d", core)
-        except (AttributeError, NotImplementedError, psutil.AccessDenied) as e:
+        except (
+            AttributeError,
+            NotImplementedError,
+            psutil.Error,
+            OSError,
+            ValueError,
+        ) as e:
             logger.debug("Could not pin to CPU core: %s", e)

parol6/server/motion_planner.py

@@ -529,9 +529,15 @@ def motion_planner_main(
     """Worker process main loop — compute trajectories and forward inline commands."""
     signal.signal(signal.SIGINT, signal.SIG_IGN)
     from parol6.server import set_pdeathsig
+    from parol6.tools import register_plugin_tools
 
     set_pdeathsig()
 
+    # Spawn-mode subprocess: the registry is freshly imported with only native
+    # tools, so plugin tools must be registered here too or SELECT_TOOL/SyncTool
+    # of a plugin tool fails in apply_tool.
+    register_plugin_tools()
+
     # Keep planning off the control loop's core so it never steals real-time
     # cycles. We were spawned before the controller pinned itself, so we still
     # hold the full affinity here and just drop the loop's core.
@@ -679,12 +685,19 @@ def start(self, avoid_core: int | None = None) -> None:
         # off the CPU the loop will pin (avoiding ~3s of contention) and ensures
         # the controller does not accept a motion command before the worker can
         # process it (which otherwise stalls the first command for ~5s).
-        if self._ready_event.wait(timeout=30.0):
-            logger.debug("Motion planner worker ready")
-        else:
-            logger.warning(
-                "Motion planner worker not ready after 30s; continuing anyway"
-            )
+        # Poll for readiness; fail fast if the worker dies during its heavy spawn
+        # import instead of blocking the full 30s on a dead process.
+        for _ in range(300):  # 300 * 0.1s = 30s
+            if self._ready_event.wait(timeout=0.1):
+                logger.debug("Motion planner worker ready")
+                return
+            if not self._process.is_alive():
+                logger.error(
+                    "Motion planner worker died during startup (exit code %s)",
+                    self._process.exitcode,
+                )
+                return
+        logger.warning("Motion planner worker not ready after 30s; continuing anyway")
 
     def stop(self) -> None:
         """Shut down the planner subprocess gracefully."""

parol6/server/status_cache.py

@@ -15,7 +15,7 @@
 import numpy as np
 from numba import njit
 from pinokin import arrays_equal_6
-from waldoctl import ActionState, ToolStatus
+from waldoctl import ActionState, ToolState, ToolStatus
 
 from parol6.config import speed_steps_to_rad, steps_to_deg, steps_to_rad
 from parol6.protocol.wire import pack_status
@@ -415,6 +415,15 @@ def update_from_state(self, state: ControllerState) -> None:
         # Populate tool status from hardware state via the tool config
         ts = self.tool_status
         ts.key = state.current_tool
+        # Reset value fields to defaults first so a tool whose populate_status is
+        # a no-op (NONE / passive / plugin tools) doesn't inherit the previous
+        # tool's engaged/part_detected/positions/etc. (in-place, zero-alloc).
+        ts.state = ToolState.OFF
+        ts.engaged = False
+        ts.part_detected = False
+        ts.fault_code = 0
+        ts.positions = ()
+        ts.channels = ()
         cfg = get_registry().get(state.current_tool)
         if cfg is not None:
             cfg.populate_status(state, ts)

parol6/tools.py

@@ -15,7 +15,7 @@
 from typing import TYPE_CHECKING, Protocol, runtime_checkable
 
 import numpy as np
-from pinokin import se3_from_trans
+from pinokin import se3_from_rpy, se3_from_trans
 from waldoctl import (
     CameraSpec,
     LinearMotion,
@@ -27,7 +27,7 @@
 )
 
 if TYPE_CHECKING:
-    from waldoctl import ToolStatus
+    from waldoctl import ToolSpec, ToolStatus
 
     from parol6.commands.base import MotionCommand
     from parol6.commands.gripper_commands import (
@@ -385,15 +385,66 @@ def list_tools() -> list[str]:
     return list(_TOOL_REGISTRY.keys())
 
 
+def _tool_config_from_spec(spec: "ToolSpec") -> ToolConfig:
+    """Build a minimal controller :class:`ToolConfig` from a waldoctl ToolSpec
+    (TCP transform only; base no-op status/command suit non-actuated tools)."""
+    transform = np.zeros((4, 4), dtype=np.float64)
+    ox, oy, oz = spec.tcp_origin
+    rx, ry, rz = spec.tcp_rpy
+    se3_from_rpy(ox, oy, oz, rx, ry, rz, transform)
+    return ToolConfig(
+        name=spec.display_name,
+        description=spec.description,
+        transform=transform,
+        meshes=spec.meshes,
+        motions=spec.motions,
+        variants=spec.variants,
+        camera_spec=spec.camera_spec,
+    )
+
+
+_PLUGIN_KEYS: set[str] = set()
+
+
+def plugin_tool_keys() -> frozenset[str]:
+    """Keys added by :func:`register_plugin_tools` (vs. native registrations)."""
+    return frozenset(_PLUGIN_KEYS)
+
+
+def register_plugin_tools() -> int:
+    """Register ``waldoctl.tools`` entry-point tools into the controller registry
+    so ``SELECT_TOOL`` resolves their TCP. Idempotent; no-op when none installed;
+    on collision the first registration wins (natives register at import time).
+    Returns the number added."""
+    from waldoctl.discovery import iter_plugin_tool_specs
+
+    count = 0
+    for spec in iter_plugin_tool_specs():
+        if spec.key != spec.key.upper():
+            logger.warning(
+                "Plugin tool key %r is not uppercase; SELECT_TOOL uppercases "
+                "names on the wire, so this tool cannot be selected",
+                spec.key,
+            )
+        if spec.key in _TOOL_REGISTRY:
+            continue
+        _TOOL_REGISTRY[spec.key] = _tool_config_from_spec(spec)
+        _PLUGIN_KEYS.add(spec.key)
+        count += 1
+    if count:
+        logger.info("Registered %d plugin tool(s) into the controller registry", count)
+    return count
+
+
 def get_tool_transform(
     tool_name: str,
     variant_key: str | None = None,
 ) -> np.ndarray:
     """Get the 4x4 transformation matrix for a tool or variant.
 
-    When *variant_key* is given and the matching variant has a
-    ``tcp_origin``, returns a transform built from the variant's TCP
-    instead of the tool-level transform.
+    When *variant_key* is given and matches, the variant's ``tcp_origin`` /
+    ``tcp_rpy`` override the tool-level transform field-independently
+    (matching the client-side ToolSpec semantics).
 
     Raises ValueError if *tool_name* is not registered.
     """
@@ -402,8 +453,15 @@ def get_tool_transform(
         raise ValueError(f"Unknown tool '{tool_name}'. Available: {list_tools()}")
     if variant_key:
         for v in cfg.variants:
-            if v.key == variant_key and v.tcp_origin is not None:
-                return _make_tcp_transform(*v.tcp_origin)
+            if v.key == variant_key:
+                out = cfg.transform.copy()
+                if v.tcp_rpy is not None:
+                    rot = np.zeros((4, 4), dtype=np.float64)
+                    se3_from_rpy(0.0, 0.0, 0.0, *v.tcp_rpy, rot)
+                    out[:3, :3] = rot[:3, :3]
+                if v.tcp_origin is not None:
+                    out[:3, 3] = v.tcp_origin
+                return out
         logger.warning("Variant '%s' not found for tool '%s'", variant_key, tool_name)
     return cfg.transform
 

pyproject.toml

@@ -41,7 +41,7 @@ dependencies = [
     "ruckig>=0.12.2",
     "toppra>=0.6.3",
     "interpolatepy>=2.0.0",
-    "numpy>=2.0",
+    "numpy>=2.0,<2.5",  # numba (0.6x) requires numpy<2.5
     "numba>=0.59",
     "psutil>=5.9",
     "msgspec>=0.18",

tests/integration/conftest.py

@@ -16,5 +16,6 @@ def clean_state(server_proc, client):
     client.select_profile("LINEAR")
     idx = client.home()
     assert idx >= 0, "Home command failed to send"
-    assert client.wait_command(idx, timeout=5.0), "Home did not complete"
+    # Generous timeout: the first home cold-JITs the numba motion pipeline.
+    assert client.wait_command(idx, timeout=30.0), "Home did not complete"
     return client

tests/integration/test_tool_operations.py

@@ -217,9 +217,10 @@ def test_registry_completeness(self):
         robot = Robot()
         tools = robot.tools
 
-        # 5 tools registered
+        # Native count stays 5; robot.tools may compose plugin tools on top.
+        native_keys = [t.key for t in robot.native_tools.available]
+        assert len(native_keys) == 5
         keys = [t.key for t in tools.available]
-        assert len(keys) == 5
         for expected in ("NONE", "PNEUMATIC", "SSG-48", "MSG", "VACUUM"):
             assert expected in keys, f"{expected} not in {keys}"