diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index b32ec71..2ff61b1 100644 --- a/parol6/client/dry_run_client.py +++ b/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, diff --git a/parol6/robot.py b/parol6/robot.py index e0ebf36..6e3b9e3 100644 --- a/parol6/robot.py +++ b/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]: diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 05aae5d..2d42365 100644 --- a/parol6/server/controller.py +++ b/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) diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 57f2ea2..6dd921f 100644 --- a/parol6/server/motion_planner.py +++ b/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.""" diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 5adbda2..0264f0a 100644 --- a/parol6/server/status_cache.py +++ b/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) diff --git a/parol6/tools.py b/parol6/tools.py index c2f890d..42f0b2f 100644 --- a/parol6/tools.py +++ b/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 diff --git a/pyproject.toml b/pyproject.toml index 1a71886..71ff823 100644 --- a/pyproject.toml +++ b/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", diff --git a/tests/integration/conftest.py b/tests/integration/conftest.py index ef9003c..345c240 100644 --- a/tests/integration/conftest.py +++ b/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 diff --git a/tests/integration/test_tool_operations.py b/tests/integration/test_tool_operations.py index f9b9f9e..fee8462 100644 --- a/tests/integration/test_tool_operations.py +++ b/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}" diff --git a/tests/unit/test_tcp_offset.py b/tests/unit/test_tcp_offset.py index f3fc638..239f29f 100644 --- a/tests/unit/test_tcp_offset.py +++ b/tests/unit/test_tcp_offset.py @@ -74,6 +74,43 @@ def test_dry_run_select_tool_resets_tcp_offset(): assert client.tcp_offset() == [0.0, 0.0, 0.0] +# ── Variant TCP resolution ─────────────────────────────────────────────── + + +def test_get_tool_transform_variant_honors_rpy(): + """Variant tcp_origin/tcp_rpy override the tool transform field-independently + (matching the client-side ToolSpec semantics).""" + from waldoctl import ToolVariant + + from parol6.tools import _TOOL_REGISTRY, ToolConfig, get_tool_transform + + cfg = ToolConfig( + name="Test", + description="", + transform=np.eye(4), + variants=( + ToolVariant( + key="angled", + display_name="Angled", + tcp_origin=(0.012, 0.0, 0.09), + tcp_rpy=(0.0, 0.26, 0.0), + ), + ToolVariant(key="rpy_only", display_name="RPY", tcp_rpy=(0.0, 0.26, 0.0)), + ), + ) + _TOOL_REGISTRY["_TEST_VARIANT_TOOL"] = cfg + try: + T = get_tool_transform("_TEST_VARIANT_TOOL", variant_key="angled") + np.testing.assert_allclose(T[:3, 3], (0.012, 0.0, 0.09), atol=1e-12) + assert np.isclose(T[0, 0], np.cos(0.26), atol=1e-9) + + T = get_tool_transform("_TEST_VARIANT_TOOL", variant_key="rpy_only") + np.testing.assert_allclose(T[:3, 3], (0.0, 0.0, 0.0), atol=1e-12) + assert np.isclose(T[0, 0], np.cos(0.26), atol=1e-9) + finally: + del _TOOL_REGISTRY["_TEST_VARIANT_TOOL"] + + # ── Planner routing (regression: SetTcpOffsetCmd must reach the planner) ──