diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 8bba775..6992620 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -27,15 +27,15 @@ jobs: # ruckig 0.17.3 sdist doesn't build under scikit-build-core 1.0; # pre-install the fixed commit until a release lands (pantor/ruckig#262). pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff" - pip install -e ".[dev]" - # Override the pinned waldoctl with the matching feature branch if one - # exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag - # in pyproject can't clobber it. Deps are kept (no --no-deps): the - # refactored waldoctl imports nicegui, which parol6 doesn't otherwise - # install. Skipped on main so main CI exercises the released pin. + # Resolve the shared contract branch before the package: the new + # release tag is created only after its companion PR merges. BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}" if [ "$BRANCH" != "main" ] && git ls-remote --heads https://github.com/Jepson2k/waldoctl.git "$BRANCH" 2>/dev/null | grep -q .; then - pip install --force-reinstall "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}" + sed -i.bak "s#waldoctl.git@v[0-9.]*#waldoctl.git@${BRANCH}#" pyproject.toml + fi + pip install -e ".[dev]" + if [ -f pyproject.toml.bak ]; then + mv pyproject.toml.bak pyproject.toml fi - name: Run pre-commit uses: pre-commit/action@v3.0.1 @@ -112,15 +112,15 @@ jobs: # ruckig 0.17.3 sdist doesn't build under scikit-build-core 1.0; # pre-install the fixed commit until a release lands (pantor/ruckig#262). pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff" - pip install -e ".[dev]" pytest-timeout - # Override the pinned waldoctl with the matching feature branch if one - # exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag - # in pyproject can't clobber it. Deps are kept (no --no-deps): the - # refactored waldoctl imports nicegui, which parol6 doesn't otherwise - # install. Skipped on main so main CI exercises the released pin. + # Resolve the shared contract branch before the package: the new + # release tag is created only after its companion PR merges. BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}" if [ "$BRANCH" != "main" ] && git ls-remote --heads https://github.com/Jepson2k/waldoctl.git "$BRANCH" 2>/dev/null | grep -q .; then - pip install --force-reinstall "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}" + sed -i.bak "s#waldoctl.git@v[0-9.]*#waldoctl.git@${BRANCH}#" pyproject.toml + fi + pip install -e ".[dev]" pytest-timeout + if [ -f pyproject.toml.bak ]; then + mv pyproject.toml.bak pyproject.toml fi # Override the pinned pinokin v0.1.6 wheel with the matching-branch diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index 5032297..3c8714c 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -14,6 +14,7 @@ CmdType.WRITE_IO, CmdType.SET_TCP_OFFSET, CmdType.SET_SHAPES, + CmdType.SET_STATUS_RATE, } # Query command types (use request/response, not ACK) @@ -36,6 +37,7 @@ CmdType.IS_SIMULATOR, CmdType.TCP_OFFSET, CmdType.SHAPES, + CmdType.STATUS_RATE, } # Streaming commands are fire-and-forget (no ACK needed) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 43c8953..13886f4 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -17,7 +17,13 @@ from waldoctl import RobotClient as _RobotClientABC, Shape, ShapeWorld, ToolStatus from msgspec.structs import asdict from waldoctl.shapes import shape_from_wire -from waldoctl.status import ActionState, ActivityResult, LoopStatsResult, ToolResult +from waldoctl.status import ( + ActionState, + ActivityResult, + LoopStatsResult, + StatusRate, + ToolResult, +) from waldoctl.tools import ToolSpec from .. import config as cfg @@ -65,6 +71,9 @@ ReachableCmd, ResetCmd, ResetLoopStatsCmd, + SetStatusRateCmd, + StatusRateCmd, + StatusRateResultStruct, ResetStateCmd, Response, StopCmd, @@ -243,6 +252,10 @@ class AsyncRobotClient(_RobotClientABC): Query commands: request/response with timeout and simple retry """ + @property + def skill_capabilities(self) -> frozenset[str]: + return super().skill_capabilities | {"backend.parol6"} + def __init__( self, host: str = "127.0.0.1", @@ -931,6 +944,33 @@ async def reset_loop_stats(self) -> int: """ return await self._send(ResetLoopStatsCmd()) + async def set_status_rate(self, hz: float) -> int: + """Set the rate the controller broadcasts status at. + + Category: Configuration + + Example: + rbt.set_status_rate(100) + """ + return await self._send(SetStatusRateCmd(hz=float(hz))) + + async def status_rate(self) -> StatusRate | None: + """Current broadcast rate and the control rate it divides. + + Category: Query + + Example: + rate = rbt.status_rate() + """ + resp = await self._request(StatusRateCmd()) + if not isinstance(resp, StatusRateResultStruct): + return None + return StatusRate( + hz=resp.hz, + control_hz=resp.control_hz, + servable=tuple(float(v) for v in resp.servable), + ) + async def select_tool(self, tool_name: str, variant_key: str = "") -> int: """Set the active end-effector tool on the controller. diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 8b04a49..780bec6 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -38,6 +38,7 @@ import re as _re import parol6.protocol.wire as _wire +from waldoctl.commands import CommandKind, command_table from ..protocol.wire import ( HomeCmd, SelectToolCmd, @@ -77,6 +78,9 @@ def _pascal_to_snake(name: str) -> str: _UPPER_FIELDS: frozenset[str] = frozenset({"tool_name", "tool_key", "profile"}) +_COMMANDS = command_table() +_AXIS_INDEX: dict[str, int] = {"X": 0, "Y": 1, "Z": 2, "RX": 3, "RY": 4, "RZ": 5} + def build_cmd(name: str, *args: Any, **kwargs: Any) -> Any: """Build a command struct by method name.""" @@ -520,6 +524,10 @@ def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None: # ---- Explicit methods for state reads ---- + @property + def skill_capabilities(self) -> frozenset[str]: + return frozenset({"motion.joint", "motion.linear", "backend.parol6"}) + def angles(self) -> list[float]: steps_to_rad(self._state.Position_in, self._q_rad_buf) return np.degrees(self._q_rad_buf).tolist() @@ -572,6 +580,52 @@ def servo_j( return self._dispatch(build_cmd("servo_j_pose", pose, **kwargs)) return self._dispatch(build_cmd("servo_j", angles or [], **kwargs)) + def jog_j( + self, + joint: int = -1, + speed: float = 0.0, + duration: float = 0.1, + *, + joints: list[int] | None = None, + speeds: list[float] | None = None, + accel: float = 1.0, + ) -> DryRunResult | None: + """The live client's signature, so a script's jog previews as written.""" + speed_arr = [0.0] * 6 + if joints is not None and speeds is not None: + for j, s in zip(joints, speeds): + speed_arr[j] = s + elif joint >= 0: + speed_arr[joint] = speed + else: + raise ValueError("jog_j requires either joint= or joints=/speeds=") + return self._dispatch( + _wire.JogJCmd(speeds=speed_arr, duration=duration, accel=accel) + ) + + def jog_l( + self, + frame: str, + axis: str | None = None, + speed: float = 0.0, + duration: float = 0.1, + *, + axes: list[str] | None = None, + speeds_list: list[float] | None = None, + accel: float = 1.0, + ) -> DryRunResult | None: + vel = [0.0] * 6 + if axes is not None and speeds_list is not None: + for a, s in zip(axes, speeds_list): + vel[_AXIS_INDEX[a]] = s + elif axis is not None: + vel[_AXIS_INDEX[axis]] = speed + else: + raise ValueError("jog_l requires either axis= or axes=/speeds_list=") + return self._dispatch( + _wire.JogLCmd(frame=frame, velocities=vel, duration=duration, accel=accel) + ) + def delay(self, seconds: float = 0.0) -> None: pass @@ -586,8 +640,19 @@ def __getattr__(self, name: str) -> Any: if name not in _CMD_STRUCTS: raise AttributeError(f"'{type(self).__name__}' has no attribute '{name}'") - def method(*args: Any, **kwargs: Any) -> DryRunResult | None: - cmd = build_cmd(name, *args, **kwargs) - return self._dispatch(cmd) + spec = _COMMANDS.get(name) + applies = spec is not None and spec.kind in ( + CommandKind.SYSTEM, + CommandKind.CONTROL, + ) + + def method(*args: Any, **kwargs: Any) -> DryRunResult | int | None: + result = self._dispatch(build_cmd(name, *args, **kwargs)) + if not applies: + return result + # A system or control command answers as the live client does: + # 1 when it applied, negative when the planner refused it. Its + # planner result carries no path a program could wait on. + return -1 if result is not None and result.error is not None else 1 return method diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 803edd4..3c72873 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -14,7 +14,12 @@ from waldoctl.sync_tools import SyncTool from waldoctl import PingResult, ToolStatus -from waldoctl.status import ActivityResult, LoopStatsResult, ToolResult +from waldoctl.status import ( + ActivityResult, + LoopStatsResult, + StatusRate, + ToolResult, +) from waldoctl.types import Axis, Frame from ..protocol.wire import ( @@ -145,6 +150,16 @@ def _bind_default_tools(self) -> None: # ---------- tool access ---------- + def run_skill( + self, invoke: Callable[[AsyncRobotClient], Coroutine[Any, Any, T]] + ) -> T: + """Execute a Python skill using this connection and its existing loop.""" + return _run(invoke(self._inner)) + + @property + def skill_capabilities(self) -> frozenset[str]: + return self._inner.skill_capabilities + @property def tool(self) -> SyncTool: """Active bound tool. Raises if no tool has been set.""" @@ -319,6 +334,14 @@ def reset_loop_stats(self) -> int: """Reset control-loop min/max metrics and overrun count.""" return _run(self._inner.reset_loop_stats()) + def set_status_rate(self, hz: float) -> int: + """Set the rate the controller broadcasts status at.""" + return _run(self._inner.set_status_rate(hz)) + + def status_rate(self) -> StatusRate | None: + """Current broadcast rate and the control rate it divides.""" + return _run(self._inner.status_rate()) + def tools(self) -> ToolResult | None: """Current tool and available tools. diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 7918ee6..a2a69e0 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -22,6 +22,8 @@ JointSpeedsCmd, LoopStatsCmd, LoopStatsResultStruct, + StatusRateCmd, + StatusRateResultStruct, PingCmd, PingResultStruct, PoseCmd, @@ -182,6 +184,28 @@ def compute(self, state: "ControllerState") -> bytes: ) +@register_command(CmdType.STATUS_RATE) +class StatusRateCommand(QueryCommand[StatusRateCmd]): + """Return the broadcast rate and the control rate it divides.""" + + PARAMS_TYPE = StatusRateCmd + QUERY_TYPE = QueryType.STATUS_RATE + + __slots__ = () + + def compute(self, state: "ControllerState") -> bytes: + return pack_response( + StatusRateResultStruct( + hz=state.status_rate_hz, + # The configured rate, not 1/INTERVAL_S: inverting the + # interval adds float noise to a value `achievable()` and the + # divisor arithmetic treat as exact (1/(1/49) is 49.000000001). + control_hz=float(cfg.CONTROL_RATE_HZ), + servable=cfg.servable_status_rates(), + ) + ) + + @register_command(CmdType.PING) class PingCommand(QueryCommand[PingCmd]): """Respond to ping requests.""" diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 070d029..5180eb2 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -11,13 +11,17 @@ MotionCommand, SystemCommand, ) +from parol6.config import CONTROL_RATE_HZ, servable_status_rates from parol6.protocol.wire import ( CheckpointCmd, CmdType, DelayCmd, ResetLoopStatsCmd, ResetStateCmd, + SetStatusRateCmd, ) +from parol6.utils.error_catalog import make_error +from parol6.utils.error_codes import ErrorCode from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState @@ -89,6 +93,44 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: return ExecutionStatusCode.COMPLETED +@register_command(CmdType.SET_STATUS_RATE) +class SetStatusRateCommand(SystemCommand[SetStatusRateCmd]): + """Change the status broadcast rate for this session. + + Status is emitted every Nth control tick, so a rate that does not divide + the control rate evenly cannot be served. It is refused rather than + rounded to a neighbour: a capture taken at a rate nobody asked for is + wrong in a way nothing reports. + """ + + PARAMS_TYPE = SetStatusRateCmd + + __slots__ = () + + def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: + hz = float(self.p.hz) + control = int(CONTROL_RATE_HZ) + # Ordered so the modulo only ever sees a finite, in-range, integral + # divisor: int(0.5) is 0 and int(nan) raises, and either would leave + # as a generic tick failure instead of the refusal that names the + # rates this controller can serve. + if not (1.0 <= hz <= control) or not hz.is_integer() or control % int(hz) != 0: + allowed = ", ".join(f"{hz:g}" for hz in servable_status_rates()) + self.fail( + make_error( + ErrorCode.SYS_STATUS_RATE_INVALID, + requested=hz, + control=control, + allowed=allowed, + ) + ) + return ExecutionStatusCode.FAILED + state.status_rate_hz = hz + logger.info("Status broadcast rate set to %g Hz", hz) + self.finish() + return ExecutionStatusCode.COMPLETED + + @register_command(CmdType.CHECKPOINT) class CheckpointCommand(MotionCommand[CheckpointCmd]): """Queue marker that sets state.last_checkpoint on execution. diff --git a/parol6/config.py b/parol6/config.py index 0f2b657..b738120 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -88,13 +88,36 @@ def _trace(self, msg, *args, **kwargs): STATUS_RATE_HZ: float = float(os.getenv("PAROL6_STATUS_RATE_HZ", "50")) STATUS_STALE_S: float = float(os.getenv("PAROL6_STATUS_STALE_S", "0.5")) + +def status_broadcast_interval(hz: float) -> int: + """Control ticks between status broadcasts at *hz*. + + Derived on demand rather than fixed at import: the rate is a session + knob (SET_STATUS_RATE), and a constant computed from the boot rate would + be a second answer to the question of how often status goes out. + """ + return max(1, int(CONTROL_RATE_HZ) // int(hz)) + + +def servable_status_rates() -> tuple[float, ...]: + """Broadcast rates this controller accepts, highest first. + + Status goes out every Nth control tick, so the servable rates are the + divisors of the control rate. One answer, used by the query that reports + the set and by the refusal that names it. + """ + control = int(CONTROL_RATE_HZ) + return tuple( + float(control // n) for n in range(1, control + 1) if control % n == 0 + ) + + # Validate STATUS_RATE_HZ divides evenly into CONTROL_RATE_HZ for polling if int(CONTROL_RATE_HZ) % int(STATUS_RATE_HZ) != 0: raise ValueError( f"STATUS_RATE_HZ ({STATUS_RATE_HZ}) must divide evenly into " f"CONTROL_RATE_HZ ({CONTROL_RATE_HZ})" ) -STATUS_BROADCAST_INTERVAL: int = int(CONTROL_RATE_HZ) // int(STATUS_RATE_HZ) # Max ticks to hold MOVE at trajectory endpoint waiting for Position_in to converge. # At 100Hz control rate, 20 ticks = 200ms. If the robot hasn't reached the target diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index bcab3d3..5e5b7a0 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -9,7 +9,7 @@ Wire format uses msgpack arrays with integer type codes: - OK: MsgType.OK (just the integer) - ERROR: [MsgType.ERROR, message] -- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed, enabled, homing_step, joints_homed, loop_health] +- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed, enabled, homing_step, joints_homed, loop_health, drive_faults] - RESPONSE: [MsgType.RESPONSE, query_type, value] - COMMAND: [CmdType.XXX, ...params] """ @@ -92,6 +92,7 @@ class QueryType(IntEnum): IS_SIMULATOR = auto() TCP_OFFSET = auto() SHAPES = auto() + STATUS_RATE = auto() class CmdType(IntEnum): @@ -159,6 +160,10 @@ class CmdType(IntEnum): SET_SHAPES = auto() # Collision-world readback query (installation + program layers) SHAPES = auto() + # Status broadcast rate: set it for a session, read it back with the + # control rate it divides. + SET_STATUS_RATE = auto() + STATUS_RATE = auto() # ============================================================================= @@ -868,6 +873,30 @@ class ActivityCmd( pass +class StatusRateCmd( + msgspec.Struct, + tag=int(CmdType.STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """STATUS_RATE: [CmdType.STATUS_RATE] — read the broadcast rate.""" + + pass + + +class SetStatusRateCmd( + msgspec.Struct, + tag=int(CmdType.SET_STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """SET_STATUS_RATE: [CmdType.SET_STATUS_RATE, hz]""" + + hz: float + + class LoopStatsCmd( msgspec.Struct, tag=int(CmdType.LOOP_STATS), @@ -1004,6 +1033,22 @@ class StatusResultStruct( tool_status: list +class StatusRateResultStruct( + msgspec.Struct, + tag=int(QueryType.STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """Broadcast rate, the control rate it divides, and the rates the + controller accepts -- its own answer, so a caller can pick one that will + be accepted instead of discovering the constraint by rejection.""" + + hz: float + control_hz: float + servable: tuple[float, ...] = () + + class LoopStatsResultStruct( msgspec.Struct, tag=int(QueryType.LOOP_STATS), @@ -1211,6 +1256,7 @@ class ShapesResultStruct( Response = ( StatusResultStruct | LoopStatsResultStruct + | StatusRateResultStruct | ToolResultStruct | CurrentActionResultStruct | PingResultStruct @@ -1371,6 +1417,7 @@ def pack_status( joints_homed: Sequence[int] = _NO_JOINTS_HOMED, p99_period_s: float = 0.0, overruns: int = 0, + drive_faults: Sequence[Sequence[str]] = (), ) -> bytes: """Pack a status broadcast message. @@ -1419,6 +1466,7 @@ def pack_status( homing_step, joints_homed, (p99_period_s, overruns), + drive_faults, ), option=ormsgpack.OPT_SERIALIZE_NUMPY, ) @@ -1478,9 +1526,10 @@ class StatusBuffer: warnings: list[tuple] = field(default_factory=list) link_health: dict = field(default_factory=dict) # The drives report per-joint error FLAGS over the serial link, not - # analog temperature or current registers, so there is nothing to put - # here: empty is what tells a consumer "no such sensor" rather than - # "all zero". Flags surface as faults, not as a trend. + # analog temperature or current registers, so this carries "faults" + # (one label tuple per joint) and no temperature/current series: empty + # lists are what tell a consumer "no such sensor" rather than "all + # zero", and a list of empty tuples is an all-clear. drive_health: dict = field(default_factory=dict) # The control loop's own health: p99_period_s and overruns. Empty from # producers that predate the field, which is how a consumer tells @@ -1603,7 +1652,7 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: tool_status_tuple, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed, enabled, homing_step, joints_homed, - loop_health] + loop_health, drive_faults] Args: data: Raw msgpack bytes @@ -1684,6 +1733,16 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: "p99_period_s": float(lh[0]), "overruns": int(lh[1]), } + if len(msg) > 29: + # One label tuple per joint, empty when that drive is healthy. + # Absent entirely from producers that predate the field, which is + # what tells a consumer this backend reports no drive faults at + # all rather than reporting all-clear. Replaced wholesale only on + # change, so a snapshot's shallow dict copy keeps the labels that + # were current when it was taken. + faults = [tuple(f) for f in msg[29]] + if buf.drive_health.get("faults") != faults: + buf.drive_health["faults"] = faults return True except Exception as e: @@ -1957,6 +2016,8 @@ def unpack_rx_frame_into( "QueueCmd", "ActivityCmd", "LoopStatsCmd", + "StatusRateCmd", + "SetStatusRateCmd", "ProfileCmd", "Command", # Mixin @@ -1964,6 +2025,7 @@ def unpack_rx_frame_into( # Response structs "StatusResultStruct", "LoopStatsResultStruct", + "StatusRateResultStruct", "ToolResultStruct", "CurrentActionResultStruct", "PingResultStruct", diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 0f6252c..25d06b0 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -71,9 +71,8 @@ MCAST_PORT, MCAST_IF, MCAST_TTL, - STATUS_RATE_HZ, STATUS_STALE_S, - STATUS_BROADCAST_INTERVAL, + status_broadcast_interval, ) import psutil @@ -193,7 +192,7 @@ def _initialize_components(self) -> None: try: logger.debug( - f"StatusBroadcaster config: group={MCAST_GROUP} port={MCAST_PORT} ttl={MCAST_TTL} iface={MCAST_IF} rate_hz={STATUS_RATE_HZ} stale_s={STATUS_STALE_S}" + f"StatusBroadcaster config: group={MCAST_GROUP} port={MCAST_PORT} ttl={MCAST_TTL} iface={MCAST_IF} stale_s={STATUS_STALE_S}" ) self._status_broadcaster = StatusBroadcaster( state_mgr=self.state_manager, @@ -201,7 +200,6 @@ def _initialize_components(self) -> None: port=MCAST_PORT, ttl=MCAST_TTL, iface_ip=MCAST_IF, - rate_hz=STATUS_RATE_HZ, stale_s=STATUS_STALE_S, ) logger.debug("StatusBroadcaster initialized") @@ -538,7 +536,12 @@ def _main_control_loop(self): self._timer.start() pt = self._phase_timer tick_count = 0 - broadcast_interval = STATUS_BROADCAST_INTERVAL + # Re-derived from the state rather than captured once: SET_STATUS_RATE + # moves the rate mid-session, and a snapshot taken here would keep + # broadcasting at whatever the rate was at boot. The sentinel rate + # never matches, so the first tick derives the real interval. + broadcast_rate_hz = 0.0 + broadcast_interval = 1 while self.running: try: @@ -558,6 +561,10 @@ def _main_control_loop(self): with pt.phase("execute"): self._execute_commands(state) + if state.status_rate_hz != broadcast_rate_hz: + broadcast_rate_hz = state.status_rate_hz + broadcast_interval = status_broadcast_interval(broadcast_rate_hz) + if tick_count % broadcast_interval == 0: with pt.phase("status"): if self._status_broadcaster: diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 080e2a2..29d686b 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -343,6 +343,12 @@ def _world_guard( def cancel(self, state: ControllerState) -> None: """Clear buffer, drain stale segments, and stop playback.""" + if self._active is not None: + # Planned trajectories live here rather than in CommandExecutor. + # Cancelling its command cannot clear this player's activity. + state.action_current = "" + state.action_params = "" + state.action_state = ActionState.IDLE self._active = None self._step = 0 self._inline_cmd = None diff --git a/parol6/server/state.py b/parol6/server/state.py index 3a0f8c8..2bc35ac 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -8,6 +8,7 @@ import numpy as np import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6 import config as _cfg from pinokin import arrays_equal_6 from parol6.config import CONTROL_RATE_HZ, steps_to_rad from parol6.motion import CartesianStreamingExecutor, StreamingExecutor @@ -245,6 +246,12 @@ class ControllerState: # firmware to clear the homed bits, 3 waiting for every joint); meaningful # only while action_current is "HomeCommand". homing_step: int = 0 + # Status broadcast rate for this session. Mutable so SET_STATUS_RATE can + # raise it for a capture or a tuning run and drop it back without a + # restart; the control loop re-derives its broadcast interval when this + # moves. Constrained to divisors of CONTROL_RATE_HZ, since status is + # emitted every Nth tick. + status_rate_hz: float = _cfg.STATUS_RATE_HZ action_state: ActionState = ActionState.IDLE # IDLE, EXECUTING, ERROR action_next: str = "" queue_nonstreamable: list[str] = field(default_factory=list) diff --git a/parol6/server/status_broadcast.py b/parol6/server/status_broadcast.py index f967ec6..9754ba6 100644 --- a/parol6/server/status_broadcast.py +++ b/parol6/server/status_broadcast.py @@ -29,8 +29,11 @@ class StatusBroadcaster: - cfg.STATUS_UNICAST_HOST (default "127.0.0.1") General: - - cfg.STATUS_RATE_HZ (default 50) - cfg.STATUS_STALE_S (default 0.2) -> skip broadcast if cache is stale + + How often tick() is called is the control loop's business: the rate is a + session knob living in ControllerState, and a copy here would be a second + answer to it. """ def __init__( @@ -40,7 +43,6 @@ def __init__( port: int = cfg.MCAST_PORT, ttl: int = cfg.MCAST_TTL, iface_ip: str = cfg.MCAST_IF, - rate_hz: float = cfg.STATUS_RATE_HZ, stale_s: float = cfg.STATUS_STALE_S, ) -> None: self._state_mgr = state_mgr diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 9cb270b..4babe2a 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -39,6 +39,16 @@ from parol6.tools import get_tool_transform from parol6 import config as _cfg +# Drive-fault labels indexed by (overtemperature | following-error << 1). +# Built once: the bits are read every control tick, and the repo's hot path +# does not allocate. +_DRIVE_FAULT_LABELS: tuple[tuple[str, ...], ...] = ( + (), + ("overtemperature",), + ("following_error",), + ("overtemperature", "following_error"), +) + logger = logging.getLogger(__name__) @@ -198,7 +208,20 @@ def __init__(self) -> None: self._tcp_pos_buf: np.ndarray = np.zeros(3, dtype=np.float64) self._tcp_pos_initialized: bool = False - self._status_rate_hz: float = _cfg.STATUS_RATE_HZ + # Broadcast period the last TCP sample was taken at. The rate is a + # session knob, and the gap being differentiated was governed by the + # period in force when the earlier sample was taken, not by the one + # that has just replaced it. + self._tcp_sample_period_s: float = ( + _cfg.INTERVAL_S * _cfg.status_broadcast_interval(_cfg.STATUS_RATE_HZ) + ) + + # Per-joint drive faults, one bit per condition. One entry per joint + # always — an all-clear list of empty tuples is how a consumer tells + # "this backend reports faults, none active" from "this backend has + # no fault reporting", which is an empty list. + self._drive_fault_bits = np.zeros(6, dtype=np.uint8) + self._drive_faults: list[tuple[str, ...]] = [() for _ in range(6)] # IK enablement results (pre-allocated for zero-alloc reads) self._joint_en = np.ones(12, dtype=np.uint8) @@ -440,7 +463,7 @@ def update_from_state(self, state: ControllerState) -> None: self._tcp_pos_buf[1] = self.pose[7] self._tcp_pos_buf[2] = self.pose[11] if self._tcp_pos_initialized: - dt = 1.0 / self._status_rate_hz + dt = self._tcp_sample_period_s dx = self._tcp_pos_buf[0] - self._prev_tcp_pos[0] dy = self._tcp_pos_buf[1] - self._prev_tcp_pos[1] dz = self._tcp_pos_buf[2] - self._prev_tcp_pos[2] @@ -448,6 +471,9 @@ def update_from_state(self, state: ControllerState) -> None: else: self._tcp_pos_initialized = True self._prev_tcp_pos[:] = self._tcp_pos_buf + self._tcp_sample_period_s = ( + _cfg.INTERVAL_S * _cfg.status_broadcast_interval(state.status_rate_hz) + ) else: # Robot not moving — reset TCP speed to zero self.tcp_speed = 0.0 @@ -529,6 +555,16 @@ def update_from_state(self, state: ControllerState) -> None: if enabled_changed: self._enabled = state.enabled + faults_changed = False + for i in range(6): + bits = (1 if state.Temperature_error_in[i] else 0) | ( + 2 if state.Position_error_in[i] else 0 + ) + if self._drive_fault_bits[i] != bits: + self._drive_fault_bits[i] = bits + self._drive_faults[i] = _DRIVE_FAULT_LABELS[bits] + faults_changed = True + # Only a live HomeCommand owns homing_step; any cancel path that drops # the command clears action_current, so derive "idle" from that. step = state.homing_step if state.action_current == "HomeCommand" else 0 @@ -580,6 +616,7 @@ def update_from_state(self, state: ControllerState) -> None: or collision_changed or depth_changed or loop_changed + or faults_changed ): self._binary_dirty = True @@ -618,6 +655,7 @@ def to_binary(self) -> bytes: joints_homed=self._joints_homed, p99_period_s=self._p99_period_s, overruns=self._overruns, + drive_faults=self._drive_faults, ) self._binary_dirty = False return self._binary_cache diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 396d3cd..4a75378 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -121,6 +121,13 @@ class _ErrorTemplate: remedy="Check parameter ranges and types.", ), # -- System / safety -- + ErrorCode.SYS_STATUS_RATE_INVALID: _ErrorTemplate( + title="Status rate not achievable", + cause="Status is broadcast every Nth control tick, so {requested} Hz " + "does not divide the {control} Hz control loop evenly.", + effect="Broadcast rate unchanged.", + remedy="Pick a rate that divides {control} Hz: {allowed}.", + ), ErrorCode.SYS_CONTROLLER_DISABLED: _ErrorTemplate( title="Controller disabled", cause="Motion command sent while controller is disabled. {detail}", @@ -157,15 +164,21 @@ class _ErrorTemplate: def make_error( code: ErrorCode, command_index: int = -1, **params: object ) -> RobotError: - """Create a RobotError from the catalog, formatting placeholders in title/cause.""" + """Create a RobotError from the catalog, formatting its placeholders. + + Every field is formatted, not just the ones that usually carry a + placeholder: a remedy is where a refusal says what would have worked, so + a template that puts its parameter there must not reach the client with + the placeholder still in it. + """ tmpl = _CATALOG[code] return RobotError( command_index=command_index, code=int(code), title=tmpl.title.format_map(params) if params else tmpl.title, cause=tmpl.cause.format_map(params) if params else tmpl.cause, - effect=tmpl.effect, - remedy=tmpl.remedy, + effect=tmpl.effect.format_map(params) if params else tmpl.effect, + remedy=tmpl.remedy.format_map(params) if params else tmpl.remedy, ) diff --git a/parol6/utils/error_codes.py b/parol6/utils/error_codes.py index bf31cd4..c75a241 100644 --- a/parol6/utils/error_codes.py +++ b/parol6/utils/error_codes.py @@ -40,3 +40,4 @@ class ErrorCode(IntEnum): SYS_PORT_SAVE_FAILED = 52 SYS_PROFILE_INVALID = 53 SYS_SELF_COLLISION = 54 + SYS_STATUS_RATE_INVALID = 55 diff --git a/pyproject.toml b/pyproject.toml index 692601c..dcc440e 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.12.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.14.0", ] [tool.setuptools.packages.find] diff --git a/tests/integration/test_drive_faults.py b/tests/integration/test_drive_faults.py new file mode 100644 index 0000000..71b0ef1 --- /dev/null +++ b/tests/integration/test_drive_faults.py @@ -0,0 +1,77 @@ +"""Per-joint drive faults ride the status broadcast. + +The drivers report fault bits over the serial link rather than analog +temperature or current registers, so this backend's drive health is faults +and nothing else — and an all-clear has to be distinguishable from a +backend that reports no faults at all, or a display cannot tell "healthy" +from "not instrumented". + +Driven through the real cache and the real codec: a ControllerState carrying +the bits the firmware would set, encoded by the cache the broadcaster uses, +decoded by the buffer the client fills. +""" + +import pytest + +from parol6.protocol.wire import StatusBuffer, decode_status_bin_into +from parol6.server.state import ControllerState +from parol6.server.status_cache import StatusCache + + +def _decode(cache: StatusCache) -> StatusBuffer: + buf = StatusBuffer() + assert decode_status_bin_into(cache.to_binary(), buf), "STATUS failed to decode" + return buf + + +@pytest.mark.integration +def test_faults_reach_the_client_against_the_joint_that_tripped(): + """A tripped drive names its condition, on its own joint, and a healthy + bus still reports one entry per joint so all-clear is visible as such.""" + cache = StatusCache() + try: + state = ControllerState() + + cache.update_from_state(state) + healthy = _decode(cache).drive_health["faults"] + assert healthy == [(), (), (), (), (), ()], ( + "a healthy bus must still report one entry per joint, so that " + f"all-clear is distinguishable from no reporting: {healthy}" + ) + + state.Temperature_error_in[2] = 1 + state.Position_error_in[4] = 1 + cache.update_from_state(state) + faults = _decode(cache).drive_health["faults"] + assert faults[2] == ("overtemperature",), faults + assert faults[4] == ("following_error",), faults + assert all(faults[i] == () for i in (0, 1, 3, 5)), ( + f"a fault on one drive must not read as a fault on another: {faults}" + ) + + # Both conditions on one drive, and the earlier fault clearing. + state.Temperature_error_in[2] = 0 + state.Temperature_error_in[4] = 1 + cache.update_from_state(state) + faults = _decode(cache).drive_health["faults"] + assert faults[2] == (), "a cleared fault must stop being reported" + assert set(faults[4]) == {"overtemperature", "following_error"}, faults + finally: + cache.close() + + +@pytest.mark.integration +def test_a_bus_with_no_analog_registers_reports_no_readings(): + """Faults are this backend's only drive health. Empty temperature and + current lists are what tell a consumer there is no such sensor, rather + than a row of zeros that reads as a cold, idle drive.""" + cache = StatusCache() + try: + cache.update_from_state(ControllerState()) + health = _decode(cache).drive_health + assert health.get("faults"), "faults are reported" + assert not health.get("temperatures_c"), health + assert not health.get("currents_ma"), health + assert health.get("bus_voltage_v") is None, health + finally: + cache.close() diff --git a/tests/integration/test_skills.py b/tests/integration/test_skills.py new file mode 100644 index 0000000..4870254 --- /dev/null +++ b/tests/integration/test_skills.py @@ -0,0 +1,82 @@ +"""The generic skill contract over the real fake-serial controller.""" + +import asyncio + +import pytest +from waldoctl.skills import observe_skills, skill + +from parol6 import AsyncRobotClient +from parol6.protocol.wire import ActionState + + +@skill( + id="test.nudge", + version="1.0.0", + requires=frozenset({"motion.joint", "backend.parol6"}), +) +async def nudge(rbt: AsyncRobotClient, *, degrees: float) -> list[float]: + target = await rbt.angles() + assert target is not None + target[0] += degrees + index = await rbt.move_j(target, speed=0.5) + assert index >= 0 and await rbt.wait_command(index, timeout=20.0) + observed = await rbt.angles() + assert observed is not None + return observed + + +def test_sync_skill_uses_the_supplied_connection_and_preserves_results( + client, server_proc +): + before = client.angles() + events = [] + with observe_skills(events.append): + after = nudge(client, degrees=-5.0) + assert after[0] == pytest.approx(before[0] - 5.0, abs=0.5) + assert [event.phase for event in events] == ["started", "completed"] + + +def test_async_cancel_stops_the_controller_and_prevents_the_next_move( + client, server_proc, ports +): + async def scenario(): + async with AsyncRobotClient( + host=ports.server_ip, port=ports.server_port + ) as rbt: + start = await rbt.angles() + assert start is not None + moving = asyncio.Event() + + @skill(id="test.cancel", version="1.0.0") + async def sequence(rbt: AsyncRobotClient) -> None: + target = list(start) + target[0] -= 20 + index = await rbt.move_j(target, duration=5.0) + assert index >= 0 + moving.set() + try: + await rbt.wait_command(index, timeout=15.0) + except asyncio.CancelledError: + # Catching cancellation must not allow another command. + await rbt.move_j(start, speed=0.5) + pytest.fail("a cancelled invocation issued another command") + + events = [] + with observe_skills(events.append): + task = asyncio.create_task(sequence.async_call(rbt)) + await asyncio.wait_for(moving.wait(), timeout=10.0) + assert await rbt.wait_status( + lambda s: s.angles[0] < start[0] - 1.0, timeout=10.0 + ) + task.cancel() + with pytest.raises(asyncio.CancelledError): + await task + assert events[-1].phase == "cancelled" + assert events[-1].stop_confirmed is True + assert await rbt.wait_status( + lambda s: s.action_state == ActionState.IDLE and s.queued_segments == 0, + timeout=3.0, + ), await rbt.activity() + assert await nudge.async_call(rbt, degrees=-2.0) is not None + + asyncio.run(scenario()) diff --git a/tests/integration/test_status_broadcast_autofailover.py b/tests/integration/test_status_broadcast_autofailover.py index 4e333af..fa603d9 100644 --- a/tests/integration/test_status_broadcast_autofailover.py +++ b/tests/integration/test_status_broadcast_autofailover.py @@ -49,7 +49,6 @@ def _force_unicast_setup(self: StatusBroadcaster) -> None: group=group, port=port, iface_ip=iface, - rate_hz=20.0, stale_s=1.0, ) @@ -155,7 +154,7 @@ async def test_multicast_send_errors_should_trigger_fallback_but_currently_do_no state_mgr = StateManager() broadcaster = StatusBroadcaster( - state_mgr=state_mgr, port=port, iface_ip="127.0.0.1", rate_hz=20.0, stale_s=2.0 + state_mgr=state_mgr, port=port, iface_ip="127.0.0.1", stale_s=2.0 ) # StatusBroadcaster is now a polling class - call tick() manually diff --git a/tests/integration/test_status_rate.py b/tests/integration/test_status_rate.py new file mode 100644 index 0000000..dac25f3 --- /dev/null +++ b/tests/integration/test_status_rate.py @@ -0,0 +1,180 @@ +"""The status broadcast rate is a session knob, not a boot constant. + +Raising it is how a capture or a tuning run gets resolution the default +50 Hz cannot give. Status is emitted every Nth control tick, so the rates a +controller can serve are the divisors of its control rate — reported as the +control rate itself, so a caller computes the set rather than probing for +it by rejection. +""" + +import asyncio +import time + +import pytest + +from parol6 import AsyncRobotClient +from parol6.server.state import ControllerState +from parol6.server.status_cache import StatusCache +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import MotionError + + +async def _observed_hz(client: AsyncRobotClient, frames: int = 40) -> float: + """Measure arrival rate over *frames* distinct broadcasts.""" + seen = 0 + start = 0.0 + async for _ in client.stream_status(): + if seen == 0: + start = time.perf_counter() + seen += 1 + if seen > frames: + break + return frames / max(time.perf_counter() - start, 1e-9) + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_the_rate_reports_the_loop_it_divides(server_proc, ports): + """``control_hz`` is what makes the constraint computable by a caller: + every rate it implies must actually be accepted.""" + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + + rate = await client.status_rate() + assert rate is not None + assert rate.control_hz > 0.0 + assert rate.hz > 0.0 + assert rate.control_hz % rate.hz == 0.0, ( + f"the controller is broadcasting at {rate.hz} Hz, which does not " + f"divide its own {rate.control_hz} Hz loop" + ) + + # Everything achievable() derives from control_hz must be accepted; + # that is the whole contract of reporting the loop rate instead of a + # list, so it is checked rather than assumed. + for candidate in rate.achievable(): + assert await client.set_status_rate(candidate) > 0, ( + f"{candidate} Hz divides {rate.control_hz} Hz but was refused" + ) + assert await client.set_status_rate(rate.hz) > 0 + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_raising_the_rate_delivers_more_frames(server_proc, ports): + """The point of the knob is resolution, so the change has to show up in + the arrival rate rather than only in the readback.""" + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + original = await client.status_rate() + assert original is not None + + low = original.control_hz / 10 + high = original.control_hz / 2 + try: + assert await client.set_status_rate(low) > 0 + await asyncio.sleep(0.3) + slow = await _observed_hz(client) + + assert await client.set_status_rate(high) > 0 + back = await client.status_rate() + assert back is not None and back.hz == high + await asyncio.sleep(0.3) + fast = await _observed_hz(client) + finally: + await client.set_status_rate(original.hz) + + assert fast > slow * 2, ( + f"asked for {high} Hz after {low} Hz but saw {fast:.1f} vs {slow:.1f}" + ) + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports): + """Refused, never rounded to a neighbour: a capture taken at a rate nobody + asked for is wrong in a way nothing reports. The refusal has to reach the + caller carrying the rates that would have worked, since that is the whole + of what an operator needs — including for the rates whose arithmetic the + check itself cannot survive: 0.5 Hz floors to a zero divisor, and NaN + cannot be made an int at all, so a validator that divides before it + screens turns a refusal into a crash. + """ + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + before = await client.status_rate() + assert before is not None + assert before.servable, ( + "the controller knows its divisor set -- it formats it into the " + "refusal -- so the query has to report it rather than leaving the " + "client to re-derive one backend's rule" + ) + assert before.achievable() == before.servable + assert before.hz in before.servable + assert before.control_hz == max(before.servable) + achievable = before.achievable() + + for bogus in (0.0, -50.0, 0.5, 62.5, float("nan"), float("inf")): + assert bogus not in achievable + with pytest.raises(MotionError) as caught: + await client.set_status_rate(bogus) + + refusal = caught.value.robot_error + assert refusal.code == ErrorCode.SYS_STATUS_RATE_INVALID, ( + f"{bogus} Hz came back as {refusal.title!r} rather than as an " + f"unservable rate: {refusal.cause}" + ) + unnamed = [hz for hz in achievable if f"{hz:g}" not in refusal.remedy] + assert not unnamed, ( + f"refusing {bogus} Hz has to say what would work instead, but " + f"{unnamed} are missing from {refusal.remedy!r}" + ) + + after = await client.status_rate() + assert after is not None and after.hz == before.hz, ( + "a refused rate must leave the broadcast alone" + ) + + +@pytest.mark.integration +def test_the_speed_derivative_follows_the_rate_it_was_sampled_at(): + """TCP speed is a difference over the broadcast period, so the period the + cache divides by has to be the one the controller is actually broadcasting + at — and the sample that straddles a rate change spans the period it was + taken at, not the one that has just replaced it. Get either wrong and a + steady arm appears to change speed the moment somebody changes the rate. + + Only J1 moves, so equal step increments are equal chords of one circle + about the base axis: the displacement is the same every sample, and any + change in the reported speed is the period alone. + """ + cache = StatusCache() + try: + state = ControllerState() + + def advance() -> float: + state.Position_in[0] += 200 + cache.update_from_state(state) + return cache.tcp_speed + + advance() # first difference has nothing to difference against + started = advance() + assert started > 0.0, "a moving arm has to report a speed" + + # Halve whatever rate this environment configured, rather than + # assuming the 50 Hz default: the cache reads its period from the + # state, so a shell with PAROL6_STATUS_RATE_HZ set would otherwise + # fail the ratio for reasons that have nothing to do with the code. + state.status_rate_hz = state.status_rate_hz / 2 + straddling = advance() + settled = advance() + + assert straddling == pytest.approx(started, rel=1e-3), ( + "the sample taken before the rate changed spans the old period" + ) + assert settled == pytest.approx(started / 2, rel=1e-3), ( + "half the broadcast rate is twice the period, so the same " + f"movement per frame is half the speed: {settled} vs {started}" + ) + finally: + cache.close() diff --git a/tests/integration/test_unhomed_motion_gate.py b/tests/integration/test_unhomed_motion_gate.py index d177f0c..2c5f739 100644 --- a/tests/integration/test_unhomed_motion_gate.py +++ b/tests/integration/test_unhomed_motion_gate.py @@ -54,8 +54,10 @@ def test_home_calibrate_rereferences_homed_robot(client: RobotClient, server_pro assert client.wait_status(lambda s: not s.homed, timeout=5.0) # Progress is published while the firmware seeks the end stops... assert client.wait_status( - lambda s: bool(s.homing.get("active")) - and any(state.name == "SEEKING" for state, _ in s.homing["joints"]), + lambda s: ( + bool(s.homing.get("active")) + and any(state.name == "SEEKING" for state, _ in s.homing["joints"]) + ), timeout=5.0, ) assert client.wait_command(idx, timeout=30.0) diff --git a/tests/test_examples.py b/tests/test_examples.py index b4a5d77..b36fd11 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -27,11 +27,13 @@ @pytest.mark.examples @pytest.mark.timeout(300) @pytest.mark.parametrize("script", EXAMPLES) -def test_example_runs(script): +def test_example_runs(script, ports): """Run each example as a subprocess and check it exits cleanly.""" result = subprocess.run( [sys.executable, str(EXAMPLES_DIR / script)], - env=ENV, + # Windows can reserve the default status port even with no listener. + # The subprocess and its controller share the OS-probed test port. + env={**ENV, "PAROL6_MCAST_PORT": str(ports.mcast_port)}, capture_output=True, text=True, timeout=240, diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py index 763f26b..5f86cd8 100644 --- a/tests/unit/test_collision_enablement.py +++ b/tests/unit/test_collision_enablement.py @@ -94,8 +94,9 @@ def test_gate_greys_direction_entering_new_pair_while_inside(): checker = _FakeChecker( lambda q: -0.01 + q[0], # A dominates; J1+ improves, J1- deepens lambda q: True, - pairs=lambda q: [("L6", "shape:A")] - + ([("L4", "shape:B")] if q[1] > 0.001 else []), + pairs=lambda q: ( + [("L6", "shape:A")] + ([("L4", "shape:B")] if q[1] > 0.001 else []) + ), ) gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6)) assert joint_en[0] == 1 # J1+ pure escape -> enabled @@ -122,8 +123,9 @@ def test_collision_blocked_new_pair_blocks_even_when_global_min_improves(): inside = _FakeChecker( lambda q: -0.05 + q[0], # A dominates the global min; +x improves it lambda q: True, - pairs=lambda q: [("L6", "shape:A")] - + ([("L5", "shape:B")] if q[0] > 0.05 else []), + pairs=lambda q: ( + [("L6", "shape:A")] + ([("L5", "shape:B")] if q[0] > 0.05 else []) + ), ) # Pure escape (no new contact) stays allowed… assert collision_blocked(inside, np.zeros(6), np.full(6, 0.01)) is False diff --git a/tests/unit/test_dry_run_script_compat.py b/tests/unit/test_dry_run_script_compat.py index b6e89e8..44a6c8f 100644 --- a/tests/unit/test_dry_run_script_compat.py +++ b/tests/unit/test_dry_run_script_compat.py @@ -11,8 +11,9 @@ import numpy as np import pytest +from waldoctl import CommandKind, command_table -from parol6.client.dry_run_client import DryRunRobotClient +from parol6.client.dry_run_client import _CMD_STRUCTS, DryRunRobotClient HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] POSE_A = [0.0, 280.0, 200.0, 90.0, 0.0, 90.0] @@ -176,3 +177,57 @@ def test_snap_carries_the_pending_blend_chain(self): assert len(result.joint_trajectory_rad) > 1 assert np.allclose(np.degrees(result.end_joints_rad), HOME, atol=0.5) assert client.flush() == [] + + +def test_jogs_take_the_live_clients_arguments(client): + """`rbt.jog_j(0, 0.5, 1.0)` and `rbt.jog_l("WRF", "X", 0.5, 1.0)` are the + forms the docs show; the preview must plan them, not the wire struct's + field order.""" + before = np.asarray(client.angles()) + result = client.jog_j(0, 0.5, 1.0) + assert result is not None and result.error is None + after = np.degrees(result.end_joints_rad) + assert after[0] > before[0] + 1.0 + assert np.allclose(after[1:], before[1:], atol=1e-6) + + client = DryRunRobotClient() + x_before = client.pose()[0] + result = client.jog_l("WRF", "X", 0.5, 1.0) + assert result is not None and result.error is None + assert result.tcp_poses[-1][0] * 1000.0 > x_before + 1.0 + assert client.pose()[0] > x_before + 1.0 + with pytest.raises(ValueError, match="joint="): + client.jog_j(speed=0.5) + + +_STATE_ARGS = { + "reset": (), + "reset_state": (), + "set_status_rate": (50,), + "simulator": (True,), + "teleport": (HOME,), + "set_shapes": ([],), + "select_profile": ("RUCKIG",), + "select_tool": ("NONE",), + "set_tcp_offset": (0.0, 0.0, 0.0), + "connect_hardware": ("/dev/null",), + "stop": (), + "estop": (), +} + + +@pytest.mark.parametrize( + "name", + sorted( + n + for n, s in command_table().items() + if s.kind in (CommandKind.SYSTEM, CommandKind.CONTROL) and n in _CMD_STRUCTS + ), +) +def test_state_commands_answer_with_the_live_clients_int_codes(client, name): + """`if rbt.stop() < 0:` must read the same in preview as on the arm: a + system or control command returns 1/0/negative, never a planner result.""" + assert name in _STATE_ARGS, f"add sample arguments for {name}" + result = getattr(client, name)(*_STATE_ARGS[name]) + assert isinstance(result, int) and not isinstance(result, bool) + assert result == 1