From 337237f71826c000d3819b0b88faf5c75c79f953 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 4 Sep 2026 21:02:39 +0000 Subject: [PATCH 1/2] Report per-joint drive faults, and make the status rate settable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The drivers report fault bits over the serial link rather than analog temperature or current registers, so drive_health carries faults and nothing else here: one label tuple per joint, empty when that drive is healthy. A list of empty tuples is deliberately not the same as an empty list — the first is an all-clear, the second is a backend with no fault reporting at all, and a display that cannot tell them apart will show a bus it never asked about as healthy. The label view is rebuilt only when a bit moves, so the 100 Hz path allocates nothing while the drives are fine, and the wire entry is replaced only on change so a snapshot's shallow copy keeps the labels current when it was taken. SET_STATUS_RATE makes the broadcast rate a session knob rather than a boot constant, which is what lets a capture or a tuning run get resolution the 50 Hz default cannot. Status is emitted every Nth control tick, so the achievable rates are the divisors of the control rate; the STATUS_RATE query reports that loop rate rather than a list of legal values, letting a caller compute the set itself. A rate that does not divide evenly is refused with a remedy naming the ones that do, never rounded to a neighbour — a capture taken at a rate nobody asked for is wrong in a way nothing reports. The control loop re-derives its broadcast interval when the rate moves instead of capturing it once at startup, and the status cache adopts the new period too, since it differentiates TCP position against it. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01DsMBr94VMTsLJtPJq7h6A5 --- parol6/ack_policy.py | 2 + parol6/client/async_client.py | 34 +++++- parol6/client/sync_client.py | 15 ++- parol6/commands/query_commands.py | 20 ++++ parol6/commands/utility_commands.py | 42 +++++++ parol6/protocol/wire.py | 69 ++++++++++- parol6/server/controller.py | 11 ++ parol6/server/state.py | 7 ++ parol6/server/status_cache.py | 43 +++++++ parol6/utils/error_catalog.py | 7 ++ parol6/utils/error_codes.py | 1 + parol6/utils/errors.py | 8 ++ tests/integration/test_drive_faults.py | 77 ++++++++++++ tests/integration/test_status_rate.py | 111 ++++++++++++++++++ tests/integration/test_unhomed_motion_gate.py | 6 +- tests/unit/test_collision_enablement.py | 10 +- 16 files changed, 450 insertions(+), 13 deletions(-) create mode 100644 tests/integration/test_drive_faults.py create mode 100644 tests/integration/test_status_rate.py 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 ac41cd1..d7b2a5e 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, @@ -931,6 +940,29 @@ 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) + 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/sync_client.py b/parol6/client/sync_client.py index 803edd4..89e84b5 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 ( @@ -319,6 +324,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..0f092a2 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,24 @@ 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, + control_hz=1.0 / max(cfg.INTERVAL_S, 1e-9), + ) + ) + + @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..18dccc7 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -11,13 +11,19 @@ MotionCommand, SystemCommand, ) +from parol6.config import CONTROL_RATE_HZ from parol6.protocol.wire import ( CheckpointCmd, CmdType, DelayCmd, ResetLoopStatsCmd, ResetStateCmd, + SetStatusRateCmd, ) +from parol6.server.status_cache import get_cache +from parol6.utils.error_catalog import make_error +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import ConfigurationError from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState @@ -89,6 +95,42 @@ 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) + if hz <= 0.0 or hz > control or control % int(hz) != 0 or hz != int(hz): + allowed = ", ".join( + str(control // n) for n in range(1, control + 1) if control % n == 0 + ) + raise ConfigurationError( + make_error( + ErrorCode.SYS_STATUS_RATE_INVALID, + requested=hz, + control=control, + allowed=allowed, + ) + ) + state.status_rate_hz = hz + get_cache().set_status_rate(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/protocol/wire.py b/parol6/protocol/wire.py index 8a821f7..672aab9 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() # ============================================================================= @@ -861,6 +866,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), @@ -997,6 +1026,19 @@ class StatusResultStruct( tool_status: list +class StatusRateResultStruct( + msgspec.Struct, + tag=int(QueryType.STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """Broadcast rate, and the control rate it divides.""" + + hz: float + control_hz: float + + class LoopStatsResultStruct( msgspec.Struct, tag=int(QueryType.LOOP_STATS), @@ -1204,6 +1246,7 @@ class ShapesResultStruct( Response = ( StatusResultStruct | LoopStatsResultStruct + | StatusRateResultStruct | ToolResultStruct | CurrentActionResultStruct | PingResultStruct @@ -1364,6 +1407,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. @@ -1412,6 +1456,7 @@ def pack_status( homing_step, joints_homed, (p99_period_s, overruns), + drive_faults, ), option=ormsgpack.OPT_SERIALIZE_NUMPY, ) @@ -1471,9 +1516,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 @@ -1596,7 +1642,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 @@ -1677,6 +1723,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: @@ -1950,6 +2006,8 @@ def unpack_rx_frame_into( "QueueCmd", "ActivityCmd", "LoopStatsCmd", + "StatusRateCmd", + "SetStatusRateCmd", "ProfileCmd", "Command", # Mixin @@ -1957,6 +2015,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..c80ba3f 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -73,6 +73,7 @@ MCAST_TTL, STATUS_RATE_HZ, STATUS_STALE_S, + CONTROL_RATE_HZ, STATUS_BROADCAST_INTERVAL, ) @@ -538,6 +539,10 @@ def _main_control_loop(self): self._timer.start() pt = self._phase_timer tick_count = 0 + # Re-derived rather than captured once: SET_STATUS_RATE moves the rate + # mid-session, and a local snapshot would keep broadcasting at whatever + # the rate was at boot. + broadcast_rate_hz = 0.0 broadcast_interval = STATUS_BROADCAST_INTERVAL while self.running: @@ -558,6 +563,12 @@ 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 = max( + 1, int(CONTROL_RATE_HZ) // int(broadcast_rate_hz) + ) + if tick_count % broadcast_interval == 0: with pt.phase("status"): if self._status_broadcaster: 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_cache.py b/parol6/server/status_cache.py index 9cb270b..d6c666f 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -200,6 +200,16 @@ def __init__(self) -> None: self._status_rate_hz: float = _cfg.STATUS_RATE_HZ + # Per-joint drive faults. The firmware reports two bitfields; the + # label view is rebuilt only when a bit moves, so the 100 Hz path + # allocates nothing while the drives are healthy. 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._temp_fault_bits = np.zeros(6, dtype=np.uint8) + self._poserr_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) self._cart_en_wrf = np.ones(12, dtype=np.uint8) @@ -529,6 +539,25 @@ def update_from_state(self, state: ControllerState) -> None: if enabled_changed: self._enabled = state.enabled + faults_changed = False + for i in range(6): + temp_bit = 1 if state.Temperature_error_in[i] else 0 + pos_bit = 1 if state.Position_error_in[i] else 0 + if self._temp_fault_bits[i] != temp_bit: + self._temp_fault_bits[i] = temp_bit + faults_changed = True + if self._poserr_fault_bits[i] != pos_bit: + self._poserr_fault_bits[i] = pos_bit + faults_changed = True + if faults_changed: + for i in range(6): + labels = [] + if self._temp_fault_bits[i]: + labels.append("overtemperature") + if self._poserr_fault_bits[i]: + labels.append("following_error") + self._drive_faults[i] = tuple(labels) + # 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 +609,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,10 +648,23 @@ 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 + def set_status_rate(self, hz: float) -> None: + """Adopt a new broadcast rate. + + The cache differentiates TCP position against the broadcast period, + so a rate change that missed this would scale every reported speed. + """ + self._status_rate_hz = hz + + @property + def status_rate_hz(self) -> float: + return self._status_rate_hz + def mark_serial_observed(self) -> None: """Mark that a fresh serial frame was observed just now.""" self.last_serial_s = time.monotonic() diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 396d3cd..d9f732a 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}", 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/parol6/utils/errors.py b/parol6/utils/errors.py index a47d793..f2bfa19 100644 --- a/parol6/utils/errors.py +++ b/parol6/utils/errors.py @@ -29,6 +29,14 @@ def __init__(self, robot_error: RobotError): super().__init__(str(robot_error)) +class ConfigurationError(RuntimeError): + """A configuration command the controller cannot honour.""" + + def __init__(self, robot_error: RobotError): + self.robot_error = robot_error + super().__init__(str(robot_error)) + + class MotionError(RuntimeError): """Pipeline planning/execution error detected via status broadcast.""" 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_status_rate.py b/tests/integration/test_status_rate.py new file mode 100644 index 0000000..8e58fee --- /dev/null +++ b/tests/integration/test_status_rate.py @@ -0,0 +1,111 @@ +"""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 + + +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 + carry what the legal rates are, since that is all the operator needs.""" + 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 + + bogus = before.control_hz / 3 + 0.5 + assert bogus not in before.achievable() + + with pytest.raises(Exception) as caught: + await client.set_status_rate(bogus) + message = str(caught.value) + assert str(int(before.control_hz)) in message, message + assert "divide" in message.lower(), message + + after = await client.status_rate() + assert after is not None and after.hz == before.hz, ( + "a refused rate must leave the broadcast alone" + ) 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/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 From 7b04e401a0eb5c7a702817f0bc4be93a12dd9ccd Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sat, 5 Sep 2026 12:53:19 -0400 Subject: [PATCH 2/2] Refuse an unservable status rate without crashing on the way out The validator divided before it screened: control % int(hz) ran ahead of the integrality check, so 0.5 Hz floored to a zero divisor and NaN could not be made an int at all. Both escaped as a generic tick failure, which is the one thing a refusal must not do -- whoever asked for 0.5 Hz got a crash instead of the rates that would have worked. The guard now establishes finite, in range and integral before the modulo sees anything, and refuses through the command protocol rather than an exception the executor has to unwrap. make_error() formatted title and cause only, so the one template that puts its parameter in the remedy -- the remedy being where a refusal says what would have worked -- reached the client reading "Pick a rate that divides {control} Hz: {allowed}". All four fields format now. The rate also had more owners than one. STATUS_BROADCAST_INTERVAL was computed at import from the boot rate, the status cache kept a _status_rate_hz shadow of its own, and StatusBroadcaster took a rate_hz it never read. ControllerState owns it now: status_broadcast_interval(hz) derives the tick count on demand, and the TCP-speed derivative divides by the live broadcast period while keeping the period its previous sample was taken at, so the frame that straddles a rate change is not differentiated against a period it never spanned. Drive-fault labels are a pre-built lookup keyed by the two bits, because that path runs at 100 Hz and the hot path does not allocate. Co-Authored-By: Claude Opus 5 --- parol6/commands/utility_commands.py | 12 +-- parol6/config.py | 12 ++- parol6/server/controller.py | 20 ++--- parol6/server/status_broadcast.py | 6 +- parol6/server/status_cache.py | 69 ++++++++--------- parol6/utils/error_catalog.py | 12 ++- parol6/utils/errors.py | 8 -- .../test_status_broadcast_autofailover.py | 3 +- tests/integration/test_status_rate.py | 77 ++++++++++++++++--- 9 files changed, 139 insertions(+), 80 deletions(-) diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 18dccc7..6ec7821 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -20,10 +20,8 @@ ResetStateCmd, SetStatusRateCmd, ) -from parol6.server.status_cache import get_cache from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode -from parol6.utils.errors import ConfigurationError from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState @@ -112,11 +110,15 @@ class SetStatusRateCommand(SystemCommand[SetStatusRateCmd]): def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: hz = float(self.p.hz) control = int(CONTROL_RATE_HZ) - if hz <= 0.0 or hz > control or control % int(hz) != 0 or hz != int(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( str(control // n) for n in range(1, control + 1) if control % n == 0 ) - raise ConfigurationError( + self.fail( make_error( ErrorCode.SYS_STATUS_RATE_INVALID, requested=hz, @@ -124,8 +126,8 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: allowed=allowed, ) ) + return ExecutionStatusCode.FAILED state.status_rate_hz = hz - get_cache().set_status_rate(hz) logger.info("Status broadcast rate set to %g Hz", hz) self.finish() return ExecutionStatusCode.COMPLETED diff --git a/parol6/config.py b/parol6/config.py index 0f2b657..f3f3046 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -88,13 +88,23 @@ 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)) + + # 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/server/controller.py b/parol6/server/controller.py index c80ba3f..25d06b0 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -71,10 +71,8 @@ MCAST_PORT, MCAST_IF, MCAST_TTL, - STATUS_RATE_HZ, STATUS_STALE_S, - CONTROL_RATE_HZ, - STATUS_BROADCAST_INTERVAL, + status_broadcast_interval, ) import psutil @@ -194,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, @@ -202,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") @@ -539,11 +536,12 @@ def _main_control_loop(self): self._timer.start() pt = self._phase_timer tick_count = 0 - # Re-derived rather than captured once: SET_STATUS_RATE moves the rate - # mid-session, and a local snapshot would keep broadcasting at whatever - # the rate was at boot. + # 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 = STATUS_BROADCAST_INTERVAL + broadcast_interval = 1 while self.running: try: @@ -565,9 +563,7 @@ def _main_control_loop(self): if state.status_rate_hz != broadcast_rate_hz: broadcast_rate_hz = state.status_rate_hz - broadcast_interval = max( - 1, int(CONTROL_RATE_HZ) // int(broadcast_rate_hz) - ) + broadcast_interval = status_broadcast_interval(broadcast_rate_hz) if tick_count % broadcast_interval == 0: with pt.phase("status"): 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 d6c666f..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,16 +208,19 @@ 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. The firmware reports two bitfields; the - # label view is rebuilt only when a bit moves, so the 100 Hz path - # allocates nothing while the drives are healthy. 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._temp_fault_bits = np.zeros(6, dtype=np.uint8) - self._poserr_fault_bits = np.zeros(6, dtype=np.uint8) + # 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) @@ -450,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] @@ -458,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 @@ -541,22 +557,13 @@ def update_from_state(self, state: ControllerState) -> None: faults_changed = False for i in range(6): - temp_bit = 1 if state.Temperature_error_in[i] else 0 - pos_bit = 1 if state.Position_error_in[i] else 0 - if self._temp_fault_bits[i] != temp_bit: - self._temp_fault_bits[i] = temp_bit - faults_changed = True - if self._poserr_fault_bits[i] != pos_bit: - self._poserr_fault_bits[i] = pos_bit + 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 - if faults_changed: - for i in range(6): - labels = [] - if self._temp_fault_bits[i]: - labels.append("overtemperature") - if self._poserr_fault_bits[i]: - labels.append("following_error") - self._drive_faults[i] = tuple(labels) # Only a live HomeCommand owns homing_step; any cancel path that drops # the command clears action_current, so derive "idle" from that. @@ -653,18 +660,6 @@ def to_binary(self) -> bytes: self._binary_dirty = False return self._binary_cache - def set_status_rate(self, hz: float) -> None: - """Adopt a new broadcast rate. - - The cache differentiates TCP position against the broadcast period, - so a rate change that missed this would scale every reported speed. - """ - self._status_rate_hz = hz - - @property - def status_rate_hz(self) -> float: - return self._status_rate_hz - def mark_serial_observed(self) -> None: """Mark that a fresh serial frame was observed just now.""" self.last_serial_s = time.monotonic() diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index d9f732a..4a75378 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -164,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/errors.py b/parol6/utils/errors.py index f2bfa19..a47d793 100644 --- a/parol6/utils/errors.py +++ b/parol6/utils/errors.py @@ -29,14 +29,6 @@ def __init__(self, robot_error: RobotError): super().__init__(str(robot_error)) -class ConfigurationError(RuntimeError): - """A configuration command the controller cannot honour.""" - - def __init__(self, robot_error: RobotError): - self.robot_error = robot_error - super().__init__(str(robot_error)) - - class MotionError(RuntimeError): """Pipeline planning/execution error detected via status broadcast.""" 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 index 8e58fee..798786e 100644 --- a/tests/integration/test_status_rate.py +++ b/tests/integration/test_status_rate.py @@ -13,6 +13,10 @@ 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: @@ -88,24 +92,77 @@ async def test_raising_the_rate_delivers_more_frames(server_proc, ports): @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 - carry what the legal rates are, since that is all the operator needs.""" + """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 + achievable = before.achievable() - bogus = before.control_hz / 3 + 0.5 - assert bogus not in 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) - with pytest.raises(Exception) as caught: - await client.set_status_rate(bogus) - message = str(caught.value) - assert str(int(before.control_hz)) in message, message - assert "divide" in message.lower(), message + 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 str(int(hz)) 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 + at_50 = advance() + assert at_50 > 0.0, "a moving arm has to report a speed" + + state.status_rate_hz = 25.0 + straddling = advance() + settled = advance() + + assert straddling == pytest.approx(at_50, rel=1e-3), ( + "the sample taken before the rate changed spans the old period" + ) + assert settled == pytest.approx(at_50 / 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 {at_50}" + ) + finally: + cache.close()