diff --git a/parol6/robot.py b/parol6/robot.py index e0ebf36..26c87e8 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..fbe6f50 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 diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 57f2ea2..46786e8 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. 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/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..c3527de 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) ──