Expose gripper skills and preserve commands after streaming

parol6/client/async_client.py

@@ -254,7 +254,7 @@ class AsyncRobotClient(_RobotClientABC):
 
     @property
     def skill_capabilities(self) -> frozenset[str]:
-        return super().skill_capabilities | {"backend.parol6"}
+        return super().skill_capabilities | {"backend.parol6", "tool.gripper"}
 
     def __init__(
         self,

parol6/client/dry_run_client.py

@@ -146,6 +146,22 @@ class _DryRunTool:
     def __init__(self, client: DryRunRobotClient) -> None:
         self._client = client
 
+    @property
+    def key(self) -> str:
+        return self._client._active_tool_key
+
+    @property
+    def tool_type(self) -> str:
+        from waldoctl.tools import ToolType
+        from parol6.tools import ElectricGripperConfig, PneumaticGripperConfig
+
+        spec = get_registry().get(self.key)
+        return (
+            ToolType.GRIPPER
+            if isinstance(spec, (ElectricGripperConfig, PneumaticGripperConfig))
+            else ToolType.NONE
+        )
+
     def __getattr__(self, name: str) -> Any:
         def method(*args: Any, **kwargs: Any) -> DryRunResult | None:
             return self._client.tool_action(
@@ -526,7 +542,9 @@ def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None:
 
     @property
     def skill_capabilities(self) -> frozenset[str]:
-        return frozenset({"motion.joint", "motion.linear", "backend.parol6"})
+        return frozenset(
+            {"motion.joint", "motion.linear", "tool.gripper", "backend.parol6"}
+        )
 
     def angles(self) -> list[float]:
         steps_to_rad(self._state.Position_in, self._q_rad_buf)

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

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,
@@ -609,11 +610,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)
 
@@ -701,10 +718,8 @@ def _handle_motion_command(
             self._segment_player.cancel(state)
             # Unconditional: a jog self-collision sets the viz but no state.error.
             state.clear_collision()
-            if self.udp_transport:
-                drained = self.udp_transport.drain_buffer()
-                if drained > 0:
-                    logger.log(TRACE, "udp_buffer_drained count=%d", drained)
+            # Coalesce decoded motion only: unread UDP packets can contain
+            # configuration, queries, or stop commands that must survive.
             self._executor.cancel_active_streamable()
             removed = self._executor.clear_streamable_commands(
                 "Streaming command prepare"

tests/unit/test_reset_enable_reaches_firmware.py

@@ -76,3 +76,58 @@ def tick_until(condition, message: str) -> None:
         "RESET's ENABLE never reached the firmware write phase — "
         f"written command codes: {sorted(set(written))}"
     )
+
+
+def test_streaming_packet_keeps_following_configuration_and_stop(controller):
+    from parol6.config import MAX_POLL_COUNT
+    from parol6.protocol.wire import PingCmd, SelectProfileCmd, TeleportCmd
+
+    state = controller.state_manager.get_state()
+    assert controller.udp_transport is not None
+    address = ("127.0.0.1", controller.udp_transport.socket.getsockname()[1])
+    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender:
+        sender.sendto(
+            encode_command(TeleportCmd(angles=[90, -90, 180, 0, 0, 180])), address
+        )
+        for _ in range(MAX_POLL_COUNT - 1):
+            sender.sendto(encode_command(PingCmd()), address)
+        sender.sendto(encode_command(SelectProfileCmd(profile="QUINTIC")), address)
+        sender.sendto(encode_command(EstopCmd()), address)
+        for _ in range(2):
+            controller._poll_commands(state)
+            controller._execute_commands(state)
+
+    assert state.motion_profile == "QUINTIC", (
+        "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"
+    )