diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index d00dc20..5c1f997 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -657,13 +657,18 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg: # --------------- Motion / Control --------------- async def home( - self, wait: bool = False, timeout: float = 60.0, **wait_kwargs: Any + self, + wait: bool = False, + calibrate: bool = False, + timeout: float = 60.0, + **wait_kwargs: Any, ) -> int: """Home the robot to its home position. - Unhomed, this runs the full referencing sequence (each joint seeks - its limit switch, then moves to standby). Already homed, it returns - to standby with a normal planned, collision-checked joint move. + Unhomed, or with ``calibrate=True``, this runs the full referencing + sequence (each joint seeks its limit switch, then moves to standby). + Already homed, it returns to standby with a normal planned, + collision-checked joint move. Returns the command index (≥ 0) on success, -1 on failure. @@ -674,9 +679,10 @@ async def home( Args: wait: If True, block until motion completes + calibrate: Re-run the referencing sequence even when already homed timeout: Maximum time to wait in seconds (only used when wait=True) """ - index = await self._send(HomeCmd()) + index = await self._send(HomeCmd(force=calibrate)) assert isinstance(index, int) if wait and index >= 0: ok = await self.wait_command(index, timeout=timeout) diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 8e3d058..370498d 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -11,7 +11,7 @@ from collections.abc import Callable, Coroutine from typing import Any, TypeVar, overload -from waldoctl.tools import ToolSpec +from waldoctl.sync_tools import SyncTool from waldoctl import PingResult, ToolStatus from waldoctl.status import ActivityResult, ToolResult @@ -132,7 +132,7 @@ def __init__( # `from parol6 import RobotClient; rbt = RobotClient(...)` works without # going through Robot.create_sync_client(). The Robot factory rebinds # these afterwards from the same registry. - self._bound_tools: dict[str, ToolSpec] = {} + self._bound_tools: dict[str, SyncTool] = {} self._bind_default_tools() def _bind_default_tools(self) -> None: @@ -147,7 +147,7 @@ def _bind_default_tools(self) -> None: # ---------- tool access ---------- @property - def tool(self) -> ToolSpec: + def tool(self) -> SyncTool: """Active bound tool. Raises if no tool has been set.""" key = (self._inner._active_tool_key or "").upper() if not key: @@ -175,20 +175,24 @@ def port(self) -> int: # ---------- motion / control ---------- - def home(self, wait: bool = False, timeout: float = 60.0) -> int: + def home( + self, wait: bool = False, calibrate: bool = False, timeout: float = 60.0 + ) -> int: """Home the robot to its home position. - Unhomed, this runs the full referencing sequence (each joint seeks - its limit switch, then moves to standby). Already homed, it returns + Unhomed, or with ``calibrate=True``, this runs the full referencing + sequence (each joint seeks its limit switch, then moves to + standby). Already homed, it returns to standby with a normal planned, collision-checked joint move. Returns the command index (≥ 0) on success, -1 on failure. Args: wait: If True, block until motion completes. + calibrate: Re-run the referencing sequence even when already homed. timeout: Maximum time to wait in seconds (only used when wait=True). """ - return _run(self._inner.home(wait=wait, timeout=timeout)) + return _run(self._inner.home(wait=wait, calibrate=calibrate, timeout=timeout)) def teleport( self, diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 231d7e0..ddce538 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -43,7 +43,7 @@ def _enc_hook(obj: object) -> object: """Custom encoder hook for numpy types.""" if isinstance(obj, np.ndarray): - return obj.tolist() # type: ignore[no-matching-overload, ty:no-matching-overload] + return obj.tolist() # type: ignore[no-matching-overload] if isinstance(obj, (np.integer, np.floating)): return obj.item() raise NotImplementedError(f"Cannot encode {type(obj)}") @@ -500,9 +500,13 @@ def __post_init__(self) -> None: class HomeCmd( msgspec.Struct, tag=int(CmdType.HOME), array_like=True, frozen=True, gc=False ): - """HOME: [CmdType.HOME]""" + """HOME: [CmdType.HOME, force] - pass + ``force`` re-runs the firmware referencing sequence on an already + homed arm instead of the planned return to standby. + """ + + force: bool = False class ResetCmd( diff --git a/parol6/robot.py b/parol6/robot.py index 3795156..a1bac31 100644 --- a/parol6/robot.py +++ b/parol6/robot.py @@ -967,7 +967,7 @@ def create_sync_client(self, **kwargs: Any) -> SyncRobotClient: import copy from parol6.client.sync_client import _run - from waldoctl.sync_tools import make_sync_tool + from waldoctl.sync_tools import SyncTool, make_sync_tool host: str = kwargs.get("host", self._host) port: int = kwargs.get("port", self._port) @@ -980,7 +980,7 @@ def create_sync_client(self, **kwargs: Any) -> SyncRobotClient: bound_spec._get_status = client._inner._tool_status # type: ignore[attr-defined, ty:unresolved-attribute] async_bound[spec.key] = bound_spec client._inner._bound_tools = async_bound - bound: dict[str, ToolSpec] = {} + bound: dict[str, SyncTool] = {} for key, async_tool in async_bound.items(): bound[key] = make_sync_tool(async_tool, _run) client._bound_tools = bound diff --git a/parol6/server/controller.py b/parol6/server/controller.py index ab250d4..738b085 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -10,9 +10,11 @@ import sys import threading import time -from dataclasses import dataclass, replace +from dataclasses import dataclass from typing import Any +import psutil +from waldoctl import ActionState from parol6.ack_policy import AckPolicy from parol6.commands.base import ( @@ -29,9 +31,18 @@ StopCommand, ) from parol6.commands.utility_commands import ResetStateCommand -from parol6.server.command_executor import CommandExecutor, QueueFullError -from parol6.server.motion_planner import MotionPlanner, PlanCommand -from parol6.server.segment_player import SegmentPlayer +from parol6.config import ( + INTERVAL_S, + MAX_POLL_COUNT, + MCAST_GROUP, + MCAST_IF, + MCAST_PORT, + MCAST_TTL, + STATUS_BROADCAST_INTERVAL, + STATUS_RATE_HZ, + STATUS_STALE_S, + TRACE, +) from parol6.protocol.wire import ( CommandCode, ToolActionCmd, @@ -40,18 +51,14 @@ pack_ok_index, unpack_rx_frame_into, ) -from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error -from parol6.utils.error_codes import ErrorCode +from parol6.server.async_logging import AsyncLogHandler +from parol6.server.command_executor import CommandExecutor, QueueFullError from parol6.server.command_registry import ( CommandCategory, create_command, create_command_from_struct, discover_commands, ) -from parol6.server.state import ControllerState, StateManager -from waldoctl import ActionState -from parol6.server.status_broadcast import StatusBroadcaster -from parol6.server.async_logging import AsyncLogHandler from parol6.server.loop_timer import ( EventRateMetrics, GCTracker, @@ -59,24 +66,16 @@ PhaseTimer, format_hz_summary, ) +from parol6.server.motion_planner import MotionPlanner, PlanCommand +from parol6.server.segment_player import SegmentPlayer +from parol6.server.state import ControllerState, StateManager +from parol6.server.status_broadcast import StatusBroadcaster from parol6.server.status_cache import close_cache, get_cache from parol6.server.transport_manager import TransportManager from parol6.server.transports.mock_serial_transport import MockSerialTransport from parol6.server.transports.udp_transport import UDPTransport -from parol6.config import ( - TRACE, - INTERVAL_S, - MAX_POLL_COUNT, - MCAST_GROUP, - MCAST_PORT, - MCAST_IF, - MCAST_TTL, - STATUS_RATE_HZ, - STATUS_STALE_S, - STATUS_BROADCAST_INTERVAL, -) - -import psutil +from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error +from parol6.utils.error_codes import ErrorCode logger = logging.getLogger("parol6.server.controller") @@ -415,7 +414,11 @@ def _tick_tool_cmd(self, state: ControllerState) -> None: raw_error = self._tool_cmd.robot_error or make_error( ErrorCode.MOTN_TICK_FAILED, detail=type(self._tool_cmd).__name__ ) - state.error = replace(raw_error, command_index=self._tool_cmd_index) + # The refusal type is an exception now, not a dataclass, so + # re-attributing it is a rebuild from its own wire fields. + attributed = raw_error.to_wire() + attributed[0] = self._tool_cmd_index + state.error = RobotError.from_wire(attributed) state.action_state = ActionState.ERROR state.completed_command_index = max( state.completed_command_index, self._tool_cmd_index diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 9a3a7fe..7eb0e1f 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -236,7 +236,11 @@ def process(self, params: object, command_index: int = 0) -> list[Segment]: # Fast-path home: an already-referenced robot returns to the standby # pose with a normal planned (collision-checked) joint move instead # of re-running the firmware switch-seek. - if isinstance(params, HomeCmd) and bool(self.state.Homed_in[:6].all()): + if ( + isinstance(params, HomeCmd) + and not params.force + and bool(self.state.Homed_in[:6].all()) + ): params = MoveJCmd(angles=self._home_deg, speed=self._home_return_speed) cmd_class = self._registry.get_command_for_struct(type(params)) diff --git a/parol6/server/transports/serial_transport.py b/parol6/server/transports/serial_transport.py index ace4b03..a26e822 100644 --- a/parol6/server/transports/serial_transport.py +++ b/parol6/server/transports/serial_transport.py @@ -8,7 +8,6 @@ import logging import os import time -from typing import cast import numba import numpy as np @@ -415,9 +414,7 @@ def get_latest_frame_view(self) -> tuple[memoryview | None, int, float]: Return a tuple of (memoryview|None, version:int, timestamp:float). The memoryview points to a stable 52-byte buffer which is updated by the reader. """ - mv = cast( - "memoryview | None", self._frame_mv if self._frame_version > 0 else None - ) + mv = self._frame_mv if self._frame_version > 0 else None return (mv, self._frame_version, self._frame_ts) def _update_hz_tracking(self) -> None: diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index b102a6a..3301824 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -8,45 +8,14 @@ from dataclasses import dataclass -from .error_codes import ErrorCode - - -@dataclass(frozen=True) -class RobotError: - """Structured error with code, title, cause, effect, and remedy.""" - - command_index: int - code: int - title: str - cause: str - effect: str - remedy: str +from waldoctl.errors import RobotError as _RobotError - def to_wire(self) -> list: - """Serialize to a list for ormsgpack packing.""" - return [ - self.command_index, - self.code, - self.title, - self.cause, - self.effect, - self.remedy, - ] - - @staticmethod - def from_wire(data: list) -> RobotError: - """Reconstruct from a wire-format list.""" - return RobotError( - command_index=data[0], - code=data[1], - title=data[2], - cause=data[3], - effect=data[4], - remedy=data[5], - ) +from .error_codes import ErrorCode - def __str__(self) -> str: - return f"[{self.code}] {self.title}: {self.cause}" +# The refusal type is waldoctl's: a frontend represents a refused command +# the same way whichever backend raised it. Same six fields, same wire +# list in both directions, and an exception a client can raise as-is. +RobotError = _RobotError @dataclass(frozen=True) diff --git a/pyproject.toml b/pyproject.toml index 4a08877..7111869 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,7 +46,7 @@ dependencies = [ "psutil>=5.9", "msgspec>=0.18", "ormsgpack>=1.4.0", - "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.7.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.11.2", ] [tool.setuptools.packages.find] diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py index 59269cd..b5e19c0 100644 --- a/tests/unit/test_collision_integration.py +++ b/tests/unit/test_collision_integration.py @@ -250,7 +250,7 @@ def test_decode_gate_rejects_malformed_wire(): ("box", [0.1] * 3, [0, 0, float("nan"), 0, 0, 0], True, None, "b"), ("box", [0.1] * 3, [0, 0, 0, 0, 0], True, None, "b"), # short pose ("box", [0.1] * 3, [0, 0, 0, 0, 0, 0], True, -0.01, "b"), # neg margin - ("plane", [0, 0, 0, 0.5], [0, 0, 0, 0, 0, 0], True, None, "b"), # 0 normal + ("plane", [0, 0, 1, 0.5], [0, 0, 0, 0, 0, 0], True, None, "b"), # kind gone ] for wire in bad_wires: with pytest.raises(ValueError):