From 2c71f4eb300268fb1e47d1876a88cb72beb5b26c Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 00:48:36 -0400 Subject: [PATCH 1/3] Expose gripper capabilities to reusable skills --- parol6/client/async_client.py | 2 +- parol6/client/dry_run_client.py | 20 +++++++++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index ad6d339..378fc3c 100644 --- a/parol6/client/async_client.py +++ b/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, diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index d6d34fd..c965a4b 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -142,6 +142,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( @@ -522,7 +538,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) From 6ac733b26c0a0ef373503a9850c2d4882410d627 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 01:09:36 -0400 Subject: [PATCH 2/3] Preserve pending configuration and stop packets during streaming --- parol6/server/controller.py | 6 ++--- .../test_reset_enable_reaches_firmware.py | 25 +++++++++++++++++++ 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 25d06b0..61ba9ca 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -701,10 +701,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" diff --git a/tests/unit/test_reset_enable_reaches_firmware.py b/tests/unit/test_reset_enable_reaches_firmware.py index e73be64..71b33bd 100644 --- a/tests/unit/test_reset_enable_reaches_firmware.py +++ b/tests/unit/test_reset_enable_reaches_firmware.py @@ -76,3 +76,28 @@ 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" From 0e2d8d65a5b869a1a37d1f7f88766438e1069d5d Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 21:43:35 +0000 Subject: [PATCH 3/3] Consume a streaming backlog in the tick it arrived in A client streaming faster than one tick's 25-message batch leaves the rest in the socket, and that backlog is stale by definition: replaying 25 of it per tick makes the arm follow superseded targets for as many ticks as the backlog is deep, and a stop queued behind them waits just as long. When the batch fills, the rest of the buffer is now read in the same tick, bounded and logged at the cap. This restores the bound the removed blind socket drain used to provide without its cost: configuration, queries and stops mixed into the backlog are still processed, in order, because each streaming command supersedes its predecessor through the executor rather than by discarding datagrams nobody decoded. Co-Authored-By: Claude Opus 5 --- parol6/config.py | 5 ++++ parol6/server/controller.py | 21 +++++++++++-- .../test_reset_enable_reaches_firmware.py | 30 +++++++++++++++++++ 3 files changed, 54 insertions(+), 2 deletions(-) diff --git a/parol6/config.py b/parol6/config.py index f3f3046..aba4300 100644 --- a/parol6/config.py +++ b/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 diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 61ba9ca..5ec66dc 100644 --- a/parol6/server/controller.py +++ b/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) diff --git a/tests/unit/test_reset_enable_reaches_firmware.py b/tests/unit/test_reset_enable_reaches_firmware.py index 71b33bd..973f272 100644 --- a/tests/unit/test_reset_enable_reaches_firmware.py +++ b/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" + )