Merge commit '8fb7f26cb80b78ee4f0e1b41ca1b011f40d93340' into feat/tcp-calibration

Author: Jepson2k

parol6/client/async_client.py

@@ -968,7 +968,11 @@ async def status_rate(self) -> StatusRate | None:
         resp = await self._request(StatusRateCmd())
         if not isinstance(resp, StatusRateResultStruct):
             return None
-        return StatusRate(hz=resp.hz, control_hz=resp.control_hz)
+        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.

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,
@@ -78,6 +79,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."""
@@ -601,6 +605,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
 
@@ -615,8 +665,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

parol6/commands/query_commands.py

@@ -200,7 +200,11 @@ 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),
+                # 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(),
             )
         )
 

parol6/commands/utility_commands.py

@@ -11,7 +11,7 @@
     MotionCommand,
     SystemCommand,
 )
-from parol6.config import CONTROL_RATE_HZ
+from parol6.config import CONTROL_RATE_HZ, servable_status_rates
 from parol6.protocol.wire import (
     CheckpointCmd,
     CmdType,
@@ -115,9 +115,7 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
         # 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
-            )
+            allowed = ", ".join(f"{hz:g}" for hz in servable_status_rates())
             self.fail(
                 make_error(
                     ErrorCode.SYS_STATUS_RATE_INVALID,

parol6/config.py

@@ -22,6 +22,11 @@
 MAX_COMMAND_QUEUE_SIZE: int = 100
 MAX_BLEND_LOOKAHEAD: int = int(os.getenv("PAROL6_MAX_BLEND_LOOKAHEAD", "100"))
 MAX_POLL_COUNT: int = 25  # Max UDP messages to read per control tick
+# Further messages read in a tick whose batch filled up. A client streaming
+# faster than the tick leaves a backlog in the socket; it is already stale, so
+# carrying it to later ticks makes the arm chase old targets and delays the
+# stop behind them by as many ticks as the backlog is deep.
+MAX_BACKLOG_COUNT: int = int(os.getenv("PAROL6_MAX_BACKLOG_COUNT", "500"))
 
 # Serial transport defaults
 SERIAL_RX_RING_DEFAULT: int = 262144
@@ -99,6 +104,19 @@ def status_broadcast_interval(hz: float) -> int:
     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(

parol6/protocol/wire.py

@@ -1078,10 +1078,13 @@ class StatusRateResultStruct(
     frozen=True,
     gc=False,
 ):
-    """Broadcast rate, and the control rate it divides."""
+    """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(

parol6/server/controller.py

@@ -66,6 +66,7 @@
 from parol6.config import (
     TRACE,
     INTERVAL_S,
+    MAX_BACKLOG_COUNT,
     MAX_POLL_COUNT,
     MCAST_GROUP,
     MCAST_PORT,
@@ -604,11 +605,27 @@ def _main_control_loop(self):
                 state.Speed_out.fill(0)
 
     def _poll_commands(self, state: ControllerState) -> None:
-        """Poll and process UDP commands (non-blocking)."""
+        """Poll and process UDP commands (non-blocking).
+
+        A full batch means a client outran the tick, so the rest of the socket
+        is read in this tick as well: each streaming command supersedes the one
+        before it, so the arm ends the tick on the newest target instead of
+        following a queue of old ones for as many ticks as the backlog is deep,
+        and the configuration, queries and stops mixed into it are still seen,
+        in order -- which a blind socket drain threw away.
+        """
         assert self.udp_transport is not None
 
         state.command_out_locked = False
-        msgs = self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT)
+        # Copied: the transport hands back a buffer it reuses on the next call.
+        msgs = list(self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT))
+        if len(msgs) == MAX_POLL_COUNT:
+            backlog = self.udp_transport.poll_receive_all(max_count=MAX_BACKLOG_COUNT)
+            if len(backlog) == MAX_BACKLOG_COUNT:
+                logger.log(
+                    TRACE, "udp_backlog_capped count=%d", MAX_BACKLOG_COUNT
+                )
+            msgs.extend(backlog)
         for data, addr in msgs:
             self._process_command(data, addr, state)
 

tests/integration/test_status_rate.py

@@ -104,6 +104,14 @@ async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports)
         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")):
@@ -116,7 +124,7 @@ async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports)
                 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]
+            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}"
@@ -150,19 +158,23 @@ def advance() -> float:
             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
+        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(at_50, rel=1e-3), (
+        assert straddling == pytest.approx(started, rel=1e-3), (
             "the sample taken before the rate changed spans the old period"
         )
-        assert settled == pytest.approx(at_50 / 2, rel=1e-3), (
+        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 {at_50}"
+            f"movement per frame is half the speed: {settled} vs {started}"
         )
     finally:
         cache.close()

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

tests/unit/test_reset_enable_reaches_firmware.py

@@ -101,3 +101,33 @@ def test_streaming_packet_keeps_following_configuration_and_stop(controller):
         "streaming must not discard pending configuration"
     )
     assert not state.enabled, "a queued stop must survive a streaming batch boundary"
+
+
+def test_a_streaming_flood_is_consumed_in_the_tick_it_arrived_in(controller):
+    """A client streaming faster than one tick's batch leaves a backlog in the
+    socket. It is stale the moment the tick runs, so the tick has to consume
+    it: otherwise the arm follows superseded targets for backlog/batch ticks
+    and anything queued behind them — here a stop — waits just as long."""
+    from parol6.config import MAX_POLL_COUNT
+    from parol6.protocol.wire import JogJCmd
+
+    state = controller.state_manager.get_state()
+    assert controller.udp_transport is not None
+    address = ("127.0.0.1", controller.udp_transport.socket.getsockname()[1])
+    flood = MAX_POLL_COUNT * 4
+    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender:
+        for _ in range(flood):
+            sender.sendto(
+                encode_command(
+                    JogJCmd(speeds=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], duration=0.2)
+                ),
+                address,
+            )
+        sender.sendto(encode_command(EstopCmd()), address)
+        controller._poll_commands(state)
+        controller._execute_commands(state)
+
+    assert not state.enabled, (
+        f"one tick left {flood} streamed targets unread, so the stop behind "
+        f"them was not seen either"
+    )