From 631663481fd5102fc0e8c418df1c2878d8f15f88 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 22:26:05 -0400 Subject: [PATCH 1/6] Enforce held-object collision geometry and reference reconciliation --- README.md | 27 +++++++ parol6/PAROL6_ROBOT.py | 50 ++++++++++++- parol6/client/async_client.py | 20 ++++- parol6/client/dry_run_client.py | 49 +++++++++++++ parol6/commands/query_commands.py | 1 + parol6/commands/shape_commands.py | 9 ++- parol6/protocol/wire.py | 8 ++ parol6/server/controller.py | 68 ++++++++++++++++- parol6/server/state.py | 27 +++++++ tests/integration/test_shapes_e2e.py | 105 +++++++++++++++++++++++++++ tests/unit/test_attachment_wire.py | 51 +++++++++++++ 11 files changed, 409 insertions(+), 6 deletions(-) create mode 100644 tests/unit/test_attachment_wire.py diff --git a/README.md b/README.md index 1fe10b1..b5b8edc 100644 --- a/README.md +++ b/README.md @@ -501,3 +501,30 @@ The client advertises `io.digital` for typed named-signal skills, which can be imported from `waldo_commander.skills`; mappings are `waldoctl.signals.DigitalSignal` values stored in a setup snapshot. Dry-run clients advertise `execution.preview` so those skills require explicit observation fixtures during preview. + +## Held-object collision geometry + +Program shapes can be attached to the `L6` flange. `shape.attach(flange_pose=..., +epoch=world.attachment_epoch, allowed_contacts=(...))` creates a declaration from +a fresh `world = rbt.shapes()` readback; apply the complete program layer with +`rbt.set_shapes(...)`. Poses use metres and extrinsic XYZ radians (`Rz @ Ry @ Rx`) +relative to the flange, independently of the tool/TCP correction. A detachment +uses `shape.detach(world_pose=...)` and removes its contact exemptions. + +Only collision-enabled, nonphysical program shapes can attach. Changes require +idle motion and a fresh position reference. Exact allowed-contact names exempt +only pairs involving their declaring shape: URDF links, `tool:name`, +`shape:name`, or `install:name`, with at most 32 unique partners. Unknown names, +wildcards and self names are refused without changing the applied world. +Unrelated checks stay active during planned and streamed motion. + +Readback includes `attachment_epoch` and `attachments_valid`. Controller/session, +reference, source and selected-tool changes invalidate the old assumptions; +arm motion remains blocked until the declarations are removed or explicitly +reconciled against fresh state. For multiple stale attachments, reapply all +verified declarations together in one `set_shapes` call. Stored world files +do not restore a fresh context. Dry-run clients preserve these context gates. + +These declarations do not actuate a gripper, confirm a grasp or estimate payload. +Waldo Commander supplies `attach_object` / `detach_object` Python skills and +shape-menu controls that use this API and verify controller readback. diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py index d7f1185..cb7ff32 100644 --- a/parol6/PAROL6_ROBOT.py +++ b/parol6/PAROL6_ROBOT.py @@ -313,12 +313,35 @@ def apply_shapes(shapes: "Iterable[Any]") -> None: """ global _active_shape_names, _program_shapes shapes = _validate_shapes(shapes) - _program_shapes = shapes if collision is None: + if any(s.attachment is not None for s in shapes): + raise ValueError("attachments require an active collision checker") + _program_shapes = shapes return + names = { + reported + for name, reported in collision.geometry_link_names + if not name.startswith("shape:") + } | {f"shape:{s.name}" for s in shapes if s.collision} + for shape in shapes: + if shape.attachment is not None: + unknown = set(shape.attachment.allowed_contacts) - names + if unknown: + raise ValueError(f"unknown contact partners: {sorted(unknown)}") + previous = _program_shapes + try: + _replace_program_geometry(shapes) + except Exception: + _replace_program_geometry(previous) + raise + _program_shapes = shapes + + +def _replace_program_geometry(shapes: list) -> None: + assert collision is not None for name in _active_shape_names: collision.remove_geometry_by_name(name) - _active_shape_names = [] + _active_shape_names.clear() for s in shapes: if not s.collision: continue @@ -327,6 +350,27 @@ def apply_shapes(shapes: "Iterable[Any]") -> None: name, s.kind, s.params(), _pose_to_matrix(s.pose), margin=s.margin ) _active_shape_names.append(name) + if s.attachment is not None: + collision.reparent_geometry_by_name(name, "L6", _pose_to_matrix(s.pose)) + geom_names = collision.geometry_names + reports = dict(collision.geometry_link_names) + attached = { + f"shape:{s.name}": s.attachment for s in shapes if s.attachment is not None + } + for name, attachment in attached.items(): + index = geom_names.index(name) + for other_index, other_name in enumerate(geom_names): + if other_index == index: + continue + other_attachment = attached.get(other_name) + allowed = reports[other_name] in attachment.allowed_contacts or ( + other_attachment is not None + and reports[name] in other_attachment.allowed_contacts + ) + if allowed: + collision.remove_collision_pair(index, other_index) + else: + collision.add_collision_pair(index, other_index) def apply_installation_shapes(shapes: "Iterable[Any]") -> None: @@ -339,6 +383,8 @@ def apply_installation_shapes(shapes: "Iterable[Any]") -> None: """ global _installation_shapes shapes = _validate_shapes(shapes) + if any(s.attachment is not None for s in shapes): + raise ValueError("installation shapes cannot declare attachments") _installation_shapes = shapes if collision is None: return diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 529bda3..443e9f2 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -268,6 +268,7 @@ def skill_capabilities(self) -> frozenset[str]: "backend.parol6", "execution.speed", "observation.timed", + "world.attachments", "tool.gripper", "io.digital", } @@ -1173,15 +1174,30 @@ async def shapes(self) -> ShapeWorld | None: if not isinstance(resp, ShapesResultStruct): return None return ShapeWorld( + attachment_epoch=resp.attachment_epoch, installation=tuple( shape_from_wire( - w.kind, w.params, w.pose, w.collision, w.margin, w.name, w.physics + w.kind, + w.params, + w.pose, + w.collision, + w.margin, + w.name, + w.physics, + w.attachment, ) for w in resp.installation ), program=tuple( shape_from_wire( - w.kind, w.params, w.pose, w.collision, w.margin, w.name, w.physics + w.kind, + w.params, + w.pose, + w.collision, + w.margin, + w.name, + w.physics, + w.attachment, ) for w in resp.program ), diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 5dddfa9..e4559ff 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -42,6 +42,7 @@ import parol6.protocol.wire as _wire from ..protocol.wire import ( HomeCmd, + SetShapesCmd, SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd, @@ -282,6 +283,27 @@ def _snap_to_angles(self, angles_deg: list[float]) -> DryRunResult: def _dispatch(self, params: Any) -> DryRunResult | None: """Route a command struct through the trajectory planner.""" + self._state.Homed_in[:] = self._planner.state.Homed_in + if isinstance(params, (_wire.EstopCmd, _wire.ResetCmd)): + self._state.invalidate_attachments() + self._state.enabled = isinstance(params, _wire.ResetCmd) + if not self._state.enabled: + self._planner.cancel() + return None + if isinstance(params, _wire.ResetStateCmd): + self._planner.cancel() + self._state.reset() + self._planner.state.Position_in[:] = self._state.Position_in + self._planner.state.Homed_in[:] = self._state.Homed_in + return None + if isinstance(params, (_wire.SimulatorCmd, _wire.ConnectHardwareCmd)): + self._state.invalidate_attachments() + self._planner.cancel() + self._state.Homed_in.fill(0) + self._planner.state.Homed_in.fill(0) + return None + if isinstance(params, SetShapesCmd): + self._state.set_shapes(params.shapes) cmd_cls = self._registry.get_command_for_struct(type(params)) if ( cmd_cls is not None @@ -289,8 +311,27 @@ def _dispatch(self, params: Any) -> DryRunResult | None: and not cmd_cls.streamable ): self._require_running() + if not self._state.attachments_valid and isinstance( + params, + ( + _wire.MoveJCmd, + _wire.MoveJPoseCmd, + _wire.MoveLCmd, + _wire.MoveCCmd, + _wire.MoveSCmd, + _wire.MovePCmd, + _wire.JogJCmd, + _wire.JogLCmd, + _wire.ServoJCmd, + _wire.ServoJPoseCmd, + _wire.ServoLCmd, + _wire.TeleportCmd, + ), + ): + raise ValueError("attachment context changed; reconcile and reapply") if isinstance(params, HomeCmd): if params.calibrate or not self._planner.state.Homed_in[:6].all(): + self._state.invalidate_attachments() return self._snap_to_angles(HOME_ANGLES_DEG) # Already referenced → fall through: the planner fast-paths HOME # into a planned return move, so the preview renders the path. @@ -563,6 +604,7 @@ def skill_capabilities(self) -> frozenset[str]: "backend.parol6", "io.digital", "execution.preview", + "world.attachments", "execution.speed", } ) @@ -571,6 +613,10 @@ def angles(self) -> list[float]: steps_to_rad(self._state.Position_in, self._q_rad_buf) return np.degrees(self._q_rad_buf).tolist() + def set_shapes(self, shapes: list) -> int: + self._dispatch(SetShapesCmd(shapes=shapes)) + return 1 + def shapes(self): """The preview's collision world by layer (mirrors the live query). @@ -580,6 +626,7 @@ def shapes(self): from waldoctl import ShapeWorld return ShapeWorld( + attachment_epoch=self._state.attachment_epoch, installation=tuple(PAROL6_ROBOT.installation_shapes()), program=tuple(PAROL6_ROBOT.program_shapes()), ) @@ -630,6 +677,8 @@ def write_io(self, index: int, value: int, *, timeout: float | None = None) -> i return 0 def _require_running(self) -> None: + if not self._state.enabled: + raise ValueError("Controller disabled; reset before previewing motion") if self._state.execution_paused: raise UnresolvedPreview( "Queued execution is paused; preview needs an explicit resume " diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index bb21e55..c6de72a 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -410,6 +410,7 @@ def compute(self, state: "ControllerState") -> bytes: ], program=[ShapeWire(*s.to_wire()) for s in state.shapes], epoch=state.shapes_version, + attachment_epoch=state.attachment_epoch, ) ) diff --git a/parol6/commands/shape_commands.py b/parol6/commands/shape_commands.py index 362555f..85ead05 100644 --- a/parol6/commands/shape_commands.py +++ b/parol6/commands/shape_commands.py @@ -42,7 +42,14 @@ class SetShapesCommand(SystemCommand[SetShapesCmd]): def execute_step(self, state: ControllerState) -> ExecutionStatusCode: shapes = [ shape_from_wire( - w.kind, w.params, w.pose, w.collision, w.margin, w.name, w.physics + w.kind, + w.params, + w.pose, + w.collision, + w.margin, + w.name, + w.physics, + w.attachment, ) for w in self.p.shapes ] diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index c046034..cedbb42 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -691,6 +691,13 @@ class ShapeWire(msgspec.Struct, array_like=True, frozen=True, gc=False): margin: float | None name: str physics: tuple[float | None, list[float]] | None = None + attachment: tuple[int, list[str]] | None = None + + def __post_init__(self) -> None: + if self.attachment is not None: + from waldoctl.shapes import Attachment + + Attachment.from_wire(self.attachment) class SetShapesCmd( @@ -1372,6 +1379,7 @@ class ShapesResultStruct( installation: list[ShapeWire] program: list[ShapeWire] epoch: int + attachment_epoch: int = 0 # Tagged Union for responses diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 2dda415..621a5de 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -34,6 +34,7 @@ from parol6.server.segment_player import SegmentPlayer from parol6.protocol.wire import ( CommandCode, + CmdType, ToolActionCmd, pack_error, pack_ok, @@ -326,12 +327,35 @@ def _read_from_firmware(self, state: ControllerState) -> None: # Serial auto-reconnect when a port is known if self._transport_mgr.auto_reconnect(): + state.invalidate_attachments() # Flush stale commands so the robot doesn't replay old moves self._segment_player.cancel(state) self._planner.cancel() self._executor.cancel_active_command("Serial reconnect") self._executor.clear_queue("Serial reconnect") + def _check_attachments(self, state: ControllerState) -> None: + if not state.has_attachments: + return + healthy = state.enabled and self._transport_mgr.is_connected() + for i in range(6): + if not state.Homed_in[i]: + healthy = False + break + if not healthy and state.attachments_valid: + state.invalidate_attachments() + if not state.attachments_valid and not state.attachment_motion_stopped: + self._segment_player.cancel(state) + self._planner.cancel() + self._executor.cancel_active_command("Attachment context changed") + self._executor.clear_queue("Attachment context changed") + state.Speed_out.fill(0) + state.error = make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail="attachment context changed; reconcile the physical scene and reapply", + ) + state.attachment_motion_stopped = True + def _handle_estop(self, state: ControllerState) -> None: """Phase 2: Handle E-stop activation and recovery.""" if not ( @@ -551,12 +575,14 @@ def _main_control_loop(self): with pt.phase("read"): self._read_from_firmware(state) + self._check_attachments(state) with pt.phase("poll_cmd"): self._poll_commands(state) with pt.phase("estop"): self._handle_estop(state) + self._check_attachments(state) if not self.estop_active: with pt.phase("execute"): @@ -646,7 +672,11 @@ def _process_command( self._cmd_rate.record(time.perf_counter()) # Try stream fast-path first (avoids full command creation) - result = self._executor.try_stream_fast_path(data, state) + result = ( + self._executor.try_stream_fast_path(data, state) + if state.attachments_valid + else False + ) if result is True: return @@ -686,6 +716,28 @@ def _handle_motion_command( cmd_name = type(command).__name__ cmd_type = command._cmd_type + if not state.attachments_valid and cmd_type in ( + CmdType.MOVEJ, + CmdType.MOVEJ_POSE, + CmdType.MOVEL, + CmdType.MOVEC, + CmdType.MOVES, + CmdType.MOVEP, + CmdType.JOGJ, + CmdType.JOGL, + CmdType.SERVOJ, + CmdType.SERVOJ_POSE, + CmdType.SERVOL, + CmdType.TELEPORT, + ): + self._reply_error( + addr, + make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail="attachment context changed; reconcile the physical scene and reapply", + ), + ) + return if not state.enabled: if cmd_type and self._ack_policy.requires_ack(cmd_type): reason = state.disabled_reason or "Controller disabled" @@ -820,6 +872,18 @@ def _handle_system_command( ) -> None: """Execute system command, apply side effects, and send reply.""" try: + if ( + isinstance(command, SetShapesCommand) + and ( + state.has_attachments + or any(w.attachment is not None for w in command.p.shapes) + ) + and ( + self._segment_player.active + or self._executor.active_command is not None + ) + ): + raise ValueError("stop motion before changing attachments") command.setup(state) code = command.tick(state) @@ -860,6 +924,7 @@ def _handle_system_command( # Infrastructure side effects (only 2-3 commands trigger these) if command._switch_simulator is not None: + state.invalidate_attachments() state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) self._segment_player.cancel(state) @@ -871,6 +936,7 @@ def _handle_system_command( if not success: raise RuntimeError(error or "Simulator toggle failed") if command._switch_port is not None: + state.invalidate_attachments() self._transport_mgr.switch_to_port(command._switch_port) if command._sync_mock: self._transport_mgr.sync_mock_from_state(state) diff --git a/parol6/server/state.py b/parol6/server/state.py index b07b04f..3cd0901 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -2,6 +2,7 @@ import atexit import logging +import secrets from dataclasses import dataclass, field from typing import Any @@ -286,6 +287,10 @@ class ControllerState: # can mirror them to the IK worker's checker. shapes: list = field(default_factory=list) shapes_version: int = 0 + attachment_epoch: int = field(default_factory=lambda: secrets.randbits(64) or 1) + has_attachments: bool = False + attachments_valid: bool = True + attachment_motion_stopped: bool = False # Network setup and uptime ip: str = "127.0.0.1" @@ -351,6 +356,7 @@ def reset(self) -> None: Preserves: ser, ip, port, start_time, next_command_index Resets: positions, speeds, I/O, queues, tool, errors, etc. """ + self.invalidate_attachments() # Safety and control flags self.enabled = True self.soft_error = False @@ -436,6 +442,7 @@ def set_tool(self, tool_name: str, variant_key: str = "") -> None: Resets TCP offset to zero (changing tools invalidates any prior offset). """ if tool_name != self._current_tool or variant_key != self._current_tool_variant: + self.invalidate_attachments() self._current_tool = tool_name self._current_tool_variant = variant_key self._tcp_offset_m = (0.0, 0.0, 0.0) @@ -452,10 +459,30 @@ def set_shapes(self, shapes: list) -> None: to the IK worker's checker for enablement greying; the version doubles as the ``scene_epoch`` broadcast in status so displays re-query. """ + attached = [s for s in shapes if s.attachment is not None] + if any(s.attachment.epoch != self.attachment_epoch for s in attached): + raise ValueError( + "attachment context changed; reconcile the physical scene and reapply" + ) + if (attached or self.has_attachments) and self.queued_segments: + raise ValueError("stop queued motion before changing attachments") + if attached and (not self.enabled or not all(self.Homed_in)): + raise ValueError("attachments require enabled, referenced robot state") PAROL6_ROBOT.apply_shapes(shapes) + self.has_attachments = bool(attached) + self.attachments_valid = True + self.attachment_motion_stopped = False self.shapes = list(shapes) self.shapes_version += 1 + def invalidate_attachments(self) -> None: + """Require explicit reconciliation after a reference/source/tool change.""" + self.attachment_epoch = self.attachment_epoch % (2**64 - 1) + 1 + if self.has_attachments: + self.attachments_valid = False + self.attachment_motion_stopped = False + self.shapes_version += 1 + @property def tcp_offset_m(self) -> tuple[float, float, float]: """Current TCP offset in meters (tool-local frame).""" diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py index 879078b..b264d29 100644 --- a/tests/integration/test_shapes_e2e.py +++ b/tests/integration/test_shapes_e2e.py @@ -27,6 +27,111 @@ HOME_J1 = 90.0 +def test_preview_attachment_context_survives_only_explicit_reconciliation(): + from parol6.client.dry_run_client import DryRunRobotClient + from waldoctl import Sphere + + preview = DryRunRobotClient() + try: + world = preview.shapes() + part = Sphere(name="part", radius=0.01).attach( + flange_pose=(0, 0, 0.25, 0, 0, 0), + epoch=world.attachment_epoch, + ) + assert preview.set_shapes([part]) == 1 + preview.estop() + assert not preview.shapes().attachments_valid + preview.reset() + with pytest.raises(ValueError, match="attachment context"): + preview.move_j(preview.angles(), duration=1) + with pytest.raises(ValueError, match="attachment context"): + preview.set_shapes([part]) + part = part.attach( + flange_pose=part.pose, epoch=preview.shapes().attachment_epoch + ) + assert preview.set_shapes([part]) == 1 + assert preview.shapes().attachments_valid + assert preview.set_shapes([part.detach(world_pose=(1, 1, 1, 0, 0, 0))]) == 1 + assert preview.shapes().program[0].attachment is None + finally: + preview.set_shapes([]) + + +def test_attached_part_blocks_motion_except_for_declared_contacts(client: RobotClient): + from dataclasses import replace + + import parol6.PAROL6_ROBOT as model + from waldoctl import Sphere + + start = client.angles() + assert start is not None + target = list(start) + target[0] -= 40 + flange = model.robot.fkine(np.radians(target)) + local = (0.0, 0.0, 0.25, 0.0, 0.0, 0.0) + at = flange[:3, 3] + flange[:3, 2] * local[2] + fixture = Sphere(name="fixture", radius=0.025, pose=(*at, 0.0, 0.0, 0.0)) + fence = replace(fixture, name="fence") + world = client.shapes() + assert world is not None + part = Sphere(name="part", radius=0.025).attach( + flange_pose=local, + epoch=world.attachment_epoch, + allowed_contacts=("shape:fixture",), + ) + try: + assert client.set_shapes([fixture, fence, part]) == 1 + applied = client.shapes() + assert applied is not None and applied.program[-1] == part + with pytest.raises(MotionError, match="shape:fence"): + index = client.move_j(target, duration=1.0, wait=False) + client.wait_command(index, timeout=10.0) + assert abs(client.angles()[0] - start[0]) < 1.0 + assert client.set_shapes([fixture, part]) == 1 + index = client.move_j(target, duration=1.0, wait=False) + assert client.wait_command(index, timeout=10.0) + assert abs(client.angles()[0] - target[0]) < 1.0 + + with pytest.raises(MotionError, match="unknown contact"): + client.set_shapes( + [ + fixture, + part.attach( + flange_pose=local, + epoch=world.attachment_epoch, + allowed_contacts=("shape:typo",), + ), + ] + ) + assert client.shapes().program[-1] == part + + assert client.estop() == 1 + _wait_until( + lambda: not client.shapes().attachments_valid, + 3.0, + "attachment remained valid after stop", + ) + assert client.reset() == 1 + with pytest.raises(MotionError, match="attachment context"): + client.move_j(start, duration=1.0, wait=False) + fresh = client.shapes() + assert fresh is not None and fresh.attachment_epoch != world.attachment_epoch + reconciled = part.attach( + flange_pose=local, + epoch=fresh.attachment_epoch, + allowed_contacts=("shape:fixture",), + ) + assert client.set_shapes([fixture, reconciled]) == 1 + assert client.shapes().attachments_valid + released = reconciled.detach(world_pose=(*at, 0.0, 0.0, 0.0)) + assert client.set_shapes([fixture, released]) == 1 + index = client.move_j(start, duration=1.0, wait=False) + assert client.wait_command(index, timeout=10.0) + finally: + client.stop() + client.set_shapes([]) + + def _wrist_box(target_deg: list[float], name: str) -> Box: """A keep-out enveloping the wrist position of ``target_deg``.""" import parol6.PAROL6_ROBOT as PAROL6_ROBOT diff --git a/tests/unit/test_attachment_wire.py b/tests/unit/test_attachment_wire.py new file mode 100644 index 0000000..46bb85f --- /dev/null +++ b/tests/unit/test_attachment_wire.py @@ -0,0 +1,51 @@ +"""Attachment contexts and scoped contacts survive the command/reply codecs.""" + +import msgspec +import pytest +from waldoctl import Sphere + +from parol6.protocol.wire import ( + CmdType, + MsgType, + QueryType, + decode_command, + decode_message, + encode, +) + + +def test_attachment_wire_roundtrip_and_hostile_contexts(): + part = Sphere(name="part", radius=0.02).attach( + flange_pose=(0, 0, 0.25, 0, 0, 0), + epoch=2**64 - 1, + allowed_contacts=("shape:fixture",), + ) + wire = list(part.to_wire()) + command = [CmdType.SET_SHAPES, [wire]] + assert msgspec.msgpack.decode( + encode(decode_command(encode(command))) + ) == msgspec.msgpack.decode(encode(command)) + reply = [MsgType.RESPONSE, [QueryType.SHAPES, [], [wire], 2, 2**64 - 1]] + assert msgspec.msgpack.decode( + encode(decode_message(encode(reply))) + ) == msgspec.msgpack.decode(encode(reply)) + for binding in ( + [0, []], + [-1, []], + [True, []], + [1.5, []], + [1, "arm"], + [1, ["*"]], + [1, ["x", "x"]], + [1, [""]], + [1, [str(i) for i in range(33)]], + [1], + [1, [], 4], + ): + wire[-1] = binding + with pytest.raises(msgspec.ValidationError): + decode_command(encode([CmdType.SET_SHAPES, [wire]])) + with pytest.raises(msgspec.ValidationError): + decode_message( + encode([MsgType.RESPONSE, [QueryType.SHAPES, [], [wire], 2, 1]]) + ) From aa7a47f5381ffa442356b5a5b46dc2424a279fb3 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 8 Sep 2026 04:55:33 -0400 Subject: [PATCH 2/6] Preserve completed tool indices when older motion finishes --- parol6/server/command_executor.py | 4 +++- parol6/server/segment_player.py | 3 ++- tests/integration/test_tool_operations.py | 10 ++++++++++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py index 915fc0e..b3088a0 100644 --- a/parol6/server/command_executor.py +++ b/parol6/server/command_executor.py @@ -274,7 +274,9 @@ def _process_tick_result( state.action_current = "" state.action_params = "" state.action_state = ActionState.IDLE - state.completed_command_index = ac.command_index + state.completed_command_index = max( + state.completed_command_index, ac.command_index + ) self._update_queue_state(state) self.active_command = None diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 363861d..3ee5f18 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -416,7 +416,8 @@ def _complete_segment(self, seg: Segment, state: ControllerState) -> None: final_idx = idx state.queued_duration -= seg.duration state.queued_segments -= 1 - state.completed_command_index = final_idx + # The concurrent tool lane may already have completed a newer index. + state.completed_command_index = max(state.completed_command_index, final_idx) state.action_current = "" state.action_params = "" state.action_state = ActionState.IDLE diff --git a/tests/integration/test_tool_operations.py b/tests/integration/test_tool_operations.py index fee8462..2b4d2c4 100644 --- a/tests/integration/test_tool_operations.py +++ b/tests/integration/test_tool_operations.py @@ -109,6 +109,16 @@ async def test_pneumatic_open_close(self, async_client): assert idx >= 0 assert await client.wait_motion(timeout=5.0) + # A side-channel tool action can finish before an older planned + # command. Its completion must still be observable after that command. + earlier = await client.delay(0.5) + assert await client.wait_status( + lambda s: s.executing_index == earlier, timeout=5.0 + ) + opened = await tool.open(wait=False) + assert await client.wait_motion(timeout=5.0) + assert await client.wait_command(opened, timeout=1.0) + @pytest.mark.asyncio async def test_pneumatic_set_position_threshold(self, async_client): """set_position uses binary threshold: < 0.5 opens, >= 0.5 closes.""" From d542221a604ed74d0ccf2a260d78904f6004f0aa Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 8 Sep 2026 05:44:04 -0400 Subject: [PATCH 3/6] Confirm exact command results across concurrent execution --- README.md | 8 +++ parol6/ack_policy.py | 48 +++++++------- parol6/client/async_client.py | 57 +++++++++++----- parol6/commands/query_commands.py | 19 ++++++ parol6/protocol/wire.py | 41 ++++++++++++ parol6/server/command_executor.py | 4 +- parol6/server/controller.py | 4 +- parol6/server/segment_player.py | 8 +-- parol6/server/state.py | 17 +++++ parol6/server/status_broadcast.py | 1 + tests/integration/test_tool_operations.py | 30 ++++++++- tests/unit/test_async_client_lifecycle.py | 32 +++++++++ tests/unit/test_command_completion_wire.py | 75 ++++++++++++++++++++++ 13 files changed, 294 insertions(+), 50 deletions(-) create mode 100644 tests/unit/test_command_completion_wire.py diff --git a/README.md b/README.md index b5b8edc..e670e97 100644 --- a/README.md +++ b/README.md @@ -323,6 +323,14 @@ the pause request can be acknowledged while still decelerating. Queued delays retain their remaining time while paused; positive speed changes do not retime delays, tool actuators or homing routines already in progress. +Completion waits query the requested command's exact success. Tool actions run +concurrently with arm motion, so the highest completed index alone cannot prove +that an earlier command finished. The controller retains its latest 1024 +successful completions; an unknown, cancelled, or expired result remains +unconfirmed. A controller-session change during a wait raises `ConnectionError`. +This requires matching client and controller versions supporting the completion +query. + Standalone `wait_command()` keeps its wall-clock timeout and returns false if completion is unconfirmed. Blocking motion calls raise `TimeoutError` in that case. A timed-out wait leaves the motion queued; `stop()` cancels it. Planning diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index e13eb8f..c38dcd9 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -1,6 +1,6 @@ import os -from parol6.protocol.wire import CmdType +from parol6.protocol.wire import CmdType, QueryType # System command types (always require ACK) SYSTEM_CMD_TYPES: set[CmdType] = { @@ -19,29 +19,31 @@ } # Query command types (use request/response, not ACK) -QUERY_CMD_TYPES: set[CmdType] = { - CmdType.POSE, - CmdType.ANGLES, - CmdType.IO, - CmdType.JOINT_SPEEDS, - CmdType.STATUS, - CmdType.LOOP_STATS, - CmdType.ACTIVITY, - CmdType.QUEUE, - CmdType.TOOLS, - CmdType.TOOL_STATUS, - CmdType.PROFILE, - CmdType.REACHABLE, - CmdType.ERROR, - CmdType.TCP_SPEED, - CmdType.PING, - CmdType.IS_SIMULATOR, - CmdType.TCP_OFFSET, - CmdType.TCP_TRANSFORM, - CmdType.SHAPES, - CmdType.STATUS_RATE, - CmdType.EXECUTION_SPEED, +QUERY_RESPONSE_TYPES: dict[CmdType, QueryType] = { + CmdType.POSE: QueryType.POSE, + CmdType.ANGLES: QueryType.ANGLES, + CmdType.IO: QueryType.IO, + CmdType.JOINT_SPEEDS: QueryType.SPEEDS, + CmdType.STATUS: QueryType.STATUS, + CmdType.LOOP_STATS: QueryType.LOOP_STATS, + CmdType.ACTIVITY: QueryType.CURRENT_ACTION, + CmdType.QUEUE: QueryType.QUEUE, + CmdType.TOOLS: QueryType.TOOL, + CmdType.TOOL_STATUS: QueryType.TOOL_STATUS, + CmdType.PROFILE: QueryType.PROFILE, + CmdType.REACHABLE: QueryType.ENABLEMENT, + CmdType.ERROR: QueryType.ERROR, + CmdType.TCP_SPEED: QueryType.TCP_SPEED, + CmdType.PING: QueryType.PING, + CmdType.IS_SIMULATOR: QueryType.IS_SIMULATOR, + CmdType.TCP_OFFSET: QueryType.TCP_OFFSET, + CmdType.TCP_TRANSFORM: QueryType.TCP_TRANSFORM, + CmdType.SHAPES: QueryType.SHAPES, + CmdType.STATUS_RATE: QueryType.STATUS_RATE, + CmdType.EXECUTION_SPEED: QueryType.EXECUTION_SPEED, + CmdType.COMMAND_COMPLETION: QueryType.COMMAND_COMPLETION, } +QUERY_CMD_TYPES: set[CmdType] = set(QUERY_RESPONSE_TYPES) # Streaming commands are fire-and-forget (no ACK needed) FIRE_AND_FORGET: set[CmdType] = { diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 443e9f2..e09bcd1 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -30,7 +30,12 @@ from waldoctl.execution import ExecutionSpeed, validate_execution_scale from .. import config as cfg -from ..ack_policy import QUERY_CMD_TYPES, SYSTEM_CMD_TYPES, AckPolicy +from ..ack_policy import ( + QUERY_CMD_TYPES, + QUERY_RESPONSE_TYPES, + SYSTEM_CMD_TYPES, + AckPolicy, +) from ..utils.error_catalog import RobotError from ..utils.errors import MotionError from ..protocol.wire import ( @@ -41,6 +46,8 @@ decode_status_bin_into, CheckpointCmd, ConnectHardwareCmd, + CommandCompletionCmd, + CommandCompletionResultStruct, CurrentActionResultStruct, DelayCmd, EnablementResultStruct, @@ -615,6 +622,7 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None: await self._ensure_endpoint() assert self._transport is not None data = encode_command(cmd) + expected = QUERY_RESPONSE_TYPES[STRUCT_TO_CMDTYPE[type(cmd)]] for attempt in range(self.retries + 1): try: async with self._req_lock: @@ -629,6 +637,10 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None: try: parsed = decode_message(resp_data) if isinstance(parsed, ResponseMsg): + # A timed-out query can reply after the next + # query starts on this same UDP endpoint. + if parsed.result.__struct_config__.tag != expected: + continue return parsed.result if isinstance(parsed, ErrorMsg): raise MotionError( @@ -1528,9 +1540,10 @@ async def wait_status( async def wait_command(self, command_index: int, timeout: float = 10.0) -> bool: """Wait until a specific command index has been completed. - Uses status broadcasts to monitor the server's completed_command_index. - Raises MotionError if the pipeline reports a planning/execution failure - at or before the awaited command index. + Queries exact success in the controller's last 1024 completions. + A concurrent tool finishing does not complete an unfinished arm command. + Unknown, cancelled, or expired results are never inferred successful + from the status high-water mark. Pipeline failures raise MotionError. Args: command_index: The command index to wait for (returned by motion commands). @@ -1558,17 +1571,31 @@ def _blocking_error(s: StatusBuffer) -> RobotError | None: return err return None - def _done(s: StatusBuffer) -> bool: - if s.completed_index >= command_index: - return True - return _blocking_error(s) is not None - - ok = await self.wait_status(_done, timeout=timeout) - if ok: - err = _blocking_error(self._shared_status) - if err is not None: - raise MotionError(err) - return ok + command = CommandCompletionCmd(command_index) + session_id = self._shared_status.session_id or None + try: + async with asyncio.timeout(timeout): + while not self._closed: + result = await self._request(command) + if ( + isinstance(result, CommandCompletionResultStruct) + and result.command_index == command_index + ): + if session_id is None: + session_id = result.session_id + elif result.session_id != session_id: + raise ConnectionError( + "Controller session changed during completion wait" + ) + if result.completed: + return True + err = _blocking_error(self._shared_status) + if err is not None: + raise MotionError(err) + await asyncio.sleep(0.02) + except TimeoutError: + return False + return False # --------------- Move commands (queued, pre-computed trajectory) --------------- diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index c6de72a..d92da3f 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -13,6 +13,8 @@ AnglesCmd, AnglesResultStruct, CmdType, + CommandCompletionCmd, + CommandCompletionResultStruct, CurrentActionResultStruct, EnablementResultStruct, ErrorCmd, @@ -284,6 +286,23 @@ def compute(self, state: "ControllerState") -> bytes: ) +@register_command(CmdType.COMMAND_COMPLETION) +class CommandCompletionCommand(QueryCommand[CommandCompletionCmd]): + PARAMS_TYPE = CommandCompletionCmd + QUERY_TYPE = QueryType.COMMAND_COMPLETION + + __slots__ = () + + def compute(self, state: "ControllerState") -> bytes: + return pack_response( + CommandCompletionResultStruct( + command_index=self.p.command_index, + session_id=state.status_session_id, + completed=state.command_completed(self.p.command_index), + ) + ) + + @register_command(CmdType.QUEUE) class QueueCommand(QueryCommand[QueueCmd]): """Get the list of queued non-streamable commands.""" diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index cedbb42..4d4f6bc 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -97,6 +97,7 @@ class QueryType(IntEnum): STATUS_RATE = auto() TCP_TRANSFORM = auto() EXECUTION_SPEED = auto() + COMMAND_COMPLETION = auto() class CmdType(IntEnum): @@ -173,6 +174,7 @@ class CmdType(IntEnum): PAUSE = auto() SET_EXECUTION_SPEED = auto() EXECUTION_SPEED = auto() + COMMAND_COMPLETION = auto() # ============================================================================= @@ -993,6 +995,25 @@ class SetStatusRateCmd( hz: float +class CommandCompletionCmd( + msgspec.Struct, + tag=int(CmdType.COMMAND_COMPLETION), + array_like=True, + frozen=True, + gc=False, + forbid_unknown_fields=True, +): + """Query exact success of one command in the controller's bounded history.""" + + command_index: int + + def __post_init__(self) -> None: + if type(self.command_index) is not int or not 0 <= self.command_index < 2**63: + raise ValueError( + "Command index must be a nonnegative signed 64-bit integer" + ) + + class LoopStatsCmd( msgspec.Struct, tag=int(CmdType.LOOP_STATS), @@ -1382,9 +1403,29 @@ class ShapesResultStruct( attachment_epoch: int = 0 +class CommandCompletionResultStruct( + msgspec.Struct, + tag=int(QueryType.COMMAND_COMPLETION), + array_like=True, + frozen=True, + gc=False, + forbid_unknown_fields=True, +): + command_index: int + session_id: int + completed: bool + + def __post_init__(self) -> None: + if type(self.command_index) is not int or not 0 <= self.command_index < 2**63: + raise ValueError("Invalid command index in completion result") + if type(self.session_id) is not int or not 0 < self.session_id < 2**64: + raise ValueError("Invalid controller session in completion result") + + # Tagged Union for responses Response = ( StatusResultStruct + | CommandCompletionResultStruct | LoopStatsResultStruct | StatusRateResultStruct | ExecutionSpeedResultStruct diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py index b3088a0..5fca560 100644 --- a/parol6/server/command_executor.py +++ b/parol6/server/command_executor.py @@ -274,9 +274,7 @@ def _process_tick_result( state.action_current = "" state.action_params = "" state.action_state = ActionState.IDLE - state.completed_command_index = max( - state.completed_command_index, ac.command_index - ) + state.record_completion(ac.command_index) self._update_queue_state(state) self.active_command = None diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 621a5de..991d57e 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -431,9 +431,7 @@ def _tick_tool_cmd(self, state: ControllerState) -> None: code = self._tool_cmd.tick(state) if code == ExecutionStatusCode.COMPLETED: - state.completed_command_index = max( - state.completed_command_index, self._tool_cmd_index - ) + state.record_completion(self._tool_cmd_index) self._tool_cmd = None self._tool_cmd_activated = False elif code == ExecutionStatusCode.FAILED: diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 3ee5f18..65a6bc8 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -409,15 +409,13 @@ def _tick_inline(self, seg: InlineSegment, state: ControllerState) -> bool | Non def _complete_segment(self, seg: Segment, state: ControllerState) -> None: """Mark segment as completed and update tracking indices.""" - final_idx = seg.command_index if isinstance(seg, TrajectorySegment): for idx in seg.blend_consumed_indices: - if idx > final_idx: - final_idx = idx + if idx != seg.command_index: + state.record_completion(idx) state.queued_duration -= seg.duration state.queued_segments -= 1 - # The concurrent tool lane may already have completed a newer index. - state.completed_command_index = max(state.completed_command_index, final_idx) + state.record_completion(seg.command_index) state.action_current = "" state.action_params = "" state.action_state = ActionState.IDLE diff --git a/parol6/server/state.py b/parol6/server/state.py index 3cd0901..d0fc3b1 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -262,6 +262,9 @@ class ControllerState: next_command_index: int = 0 executing_command_index: int = -1 completed_command_index: int = -1 + status_session_id: int = field(default_factory=lambda: secrets.randbits(64) or 1) + _recent_completions: list[int] = field(default_factory=lambda: [-1] * 1024) + _completion_cursor: int = 0 last_checkpoint: str = "" # Planning behavior (stop on first IK failure vs solve all for diagnostic) @@ -349,6 +352,17 @@ def clear_collision(self) -> None: self.collision_active = False self.collision_pairs = () + def record_completion(self, index: int) -> None: + """Retain exact successes; concurrent lanes do not finish in index order.""" + self.completed_command_index = max(self.completed_command_index, index) + self._recent_completions[self._completion_cursor] = index + self._completion_cursor = (self._completion_cursor + 1) % len( + self._recent_completions + ) + + def command_completed(self, index: int) -> bool: + return index >= 0 and index in self._recent_completions + def reset(self) -> None: """ Reset robot state to initial values without losing connection state. @@ -404,6 +418,9 @@ def reset(self) -> None: # can never satisfy a wait on a post-reset command. self.executing_command_index = -1 self.completed_command_index = -1 + for i in range(len(self._recent_completions)): + self._recent_completions[i] = -1 + self._completion_cursor = 0 self.last_checkpoint = "" # Error and pipeline depth diff --git a/parol6/server/status_broadcast.py b/parol6/server/status_broadcast.py index fb800d5..0114017 100644 --- a/parol6/server/status_broadcast.py +++ b/parol6/server/status_broadcast.py @@ -63,6 +63,7 @@ def __init__( self._max_send_failures = 3 self._last_fail_log_time = 0.0 self._session_id = secrets.randbits(64) or 1 + state_mgr.get_state().status_session_id = self._session_id self._seq = 0 self._setup_socket() diff --git a/tests/integration/test_tool_operations.py b/tests/integration/test_tool_operations.py index 2b4d2c4..4d7485b 100644 --- a/tests/integration/test_tool_operations.py +++ b/tests/integration/test_tool_operations.py @@ -5,9 +5,13 @@ with a running controller (FAKE_SERIAL mode). """ +import asyncio + import pytest import pytest_asyncio +from parol6.protocol.wire import CommandCompletionCmd, encode_command + from waldoctl import ( ElectricGripperTool, GripperType, @@ -115,10 +119,34 @@ async def test_pneumatic_open_close(self, async_client): assert await client.wait_status( lambda s: s.executing_index == earlier, timeout=5.0 ) - opened = await tool.open(wait=False) + assert await client.pause() == 1 + try: + opened = await tool.open(wait=False) + assert await client.wait_command(opened, timeout=1.0) + assert not await client.wait_command(earlier, timeout=0.05), ( + "a completed tool action must not confirm the paused delay" + ) + finally: + assert await client.resume() == 1 assert await client.wait_motion(timeout=5.0) assert await client.wait_command(opened, timeout=1.0) + cancelled = await client.delay(1.0) + assert await client.wait_status( + lambda s: s.executing_index == cancelled, timeout=5.0 + ) + assert await client.stop() == 1 + closed = await tool.close(wait=False) + assert await client.wait_command(closed, timeout=1.0) + assert not await client.wait_command(cancelled, timeout=0.05) + + # Deliver a real completion reply late, ahead of a different query. + assert client._transport is not None + client._transport.sendto(encode_command(CommandCompletionCmd(closed))) + reply = await asyncio.wait_for(client._rx_queue.get(), timeout=1.0) + client._rx_queue.put_nowait(reply) + assert await client.status() is not None + @pytest.mark.asyncio async def test_pneumatic_set_position_threshold(self, async_client): """set_position uses binary threshold: < 0.5 opens, >= 0.5 closes.""" diff --git a/tests/unit/test_async_client_lifecycle.py b/tests/unit/test_async_client_lifecycle.py index 27a82a4..f36274d 100644 --- a/tests/unit/test_async_client_lifecycle.py +++ b/tests/unit/test_async_client_lifecycle.py @@ -84,3 +84,35 @@ async def consumer() -> None: finally: # Ensure cleanup even if assertions fail earlier await client.close() + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_completion_wait_refuses_a_restarted_controller(ports, server_proc): + client = AsyncRobotClient( + host=ports.server_ip, port=ports.server_port, timeout=0.25, retries=0 + ) + waiting = None + try: + assert await client.wait_status(lambda s: s.session_id != 0, timeout=5.0) + waiting = asyncio.create_task(client.wait_command(999999, timeout=20.0)) + await asyncio.sleep(0) + await asyncio.to_thread(server_proc.stop) + await asyncio.to_thread( + server_proc.start, + timeout=15.0, + extra_env={ + "PAROL6_FAKE_SERIAL": "1", + "PAROL6_NOAUTOHOME": "1", + "PAROL6_CONTROLLER_IP": ports.server_ip, + "PAROL6_CONTROLLER_PORT": str(ports.server_port), + "PAROL6_MCAST_PORT": str(ports.mcast_port), + }, + ) + with pytest.raises(ConnectionError, match="session changed"): + await asyncio.wait_for(waiting, timeout=5.0) + finally: + if waiting is not None: + waiting.cancel() + await asyncio.gather(waiting, return_exceptions=True) + await client.close() diff --git a/tests/unit/test_command_completion_wire.py b/tests/unit/test_command_completion_wire.py new file mode 100644 index 0000000..a7bba44 --- /dev/null +++ b/tests/unit/test_command_completion_wire.py @@ -0,0 +1,75 @@ +"""Exact completion readback remains bounded and rejects malformed indices.""" + +import math + +import msgspec +import pytest + +from parol6.protocol.wire import ( + CmdType, + CommandCompletionCmd, + MsgType, + QueryType, + decode_command, + decode_message, + encode, +) +from parol6.server.command_registry import create_command +from parol6.server.state import ControllerState + + +def test_completion_history_is_exact_expires_and_resets_through_wire_query(): + state = ControllerState() + + def completed(index): + command, _, error = create_command(encode(CommandCompletionCmd(index))) + assert command is not None, error + command.setup(state) + result = decode_message(command.compute(state)).result + assert result.command_index == index + assert result.session_id == state.status_session_id + return result.completed + + state.record_completion(10) + assert completed(10) and not completed(9) and not completed(11) + # Completion order can differ from acceptance order. + state.record_completion(9) + assert completed(9) and completed(10) + for index in range(11, 1034): + state.record_completion(index) + assert not completed(10) and completed(9) and completed(1033) + state.record_completion(1034) + assert not completed(9) and completed(1034) + state.reset() + assert not completed(1034) + + +def test_completion_packets_reject_invalid_indices_sessions_and_verdicts(): + for value in (0, 2**63 - 1): + command = CommandCompletionCmd(value) + assert decode_command(encode(command)) == command + invalid = [ + [CmdType.COMMAND_COMPLETION], + [CmdType.COMMAND_COMPLETION, 0, 1], + *( + [CmdType.COMMAND_COMPLETION, value] + for value in (-1, 2**63, True, "1", 0.5, math.nan, math.inf, None) + ), + ] + for packet in invalid: + with pytest.raises(msgspec.ValidationError): + decode_command(encode(packet)) + for values in ( + (-1, 1, True), + (2**63, 1, True), + (1, 0, True), + (1, -1, True), + (1, True, True), + (1, 1, 1), + (1, 1), + (1, 1, True, 0), + ): + with pytest.raises(msgspec.ValidationError): + decode_message( + encode([MsgType.RESPONSE, [QueryType.COMMAND_COMPLETION, *values]]) + ) From 33d7f0bf9fedb584dc71439834a64f8ef93bb74b Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 8 Sep 2026 06:00:42 -0400 Subject: [PATCH 4/6] Detect restarted controllers through independent status broadcasts --- parol6/client/async_client.py | 23 +++++++++++++++++------ tests/unit/test_async_client_lifecycle.py | 10 +++++++++- 2 files changed, 26 insertions(+), 7 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index e09bcd1..bce9c33 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -1573,20 +1573,31 @@ def _blocking_error(s: StatusBuffer) -> RobotError | None: command = CommandCompletionCmd(command_index) session_id = self._shared_status.session_id or None + + def check_session(candidate: int) -> None: + nonlocal session_id + if not candidate: + return + if session_id is None: + session_id = candidate + elif candidate != session_id: + raise ConnectionError( + "Controller session changed during completion wait" + ) + try: async with asyncio.timeout(timeout): while not self._closed: + check_session(self._shared_status.session_id) result = await self._request(command) + # Status has its own socket and can survive a command + # socket that stopped receiving after a peer restart. + check_session(self._shared_status.session_id) if ( isinstance(result, CommandCompletionResultStruct) and result.command_index == command_index ): - if session_id is None: - session_id = result.session_id - elif result.session_id != session_id: - raise ConnectionError( - "Controller session changed during completion wait" - ) + check_session(result.session_id) if result.completed: return True err = _blocking_error(self._shared_status) diff --git a/tests/unit/test_async_client_lifecycle.py b/tests/unit/test_async_client_lifecycle.py index f36274d..bbbd710 100644 --- a/tests/unit/test_async_client_lifecycle.py +++ b/tests/unit/test_async_client_lifecycle.py @@ -88,13 +88,21 @@ async def consumer() -> None: @pytest.mark.asyncio @pytest.mark.integration -async def test_completion_wait_refuses_a_restarted_controller(ports, server_proc): +@pytest.mark.parametrize("close_commands", [False, True]) +async def test_completion_wait_refuses_a_restarted_controller( + ports, server_proc, close_commands +): client = AsyncRobotClient( host=ports.server_ip, port=ports.server_port, timeout=0.25, retries=0 ) waiting = None try: assert await client.wait_status(lambda s: s.session_id != 0, timeout=5.0) + # The command socket can stop receiving after a peer reset on Windows. + # Session broadcasts must still invalidate its outstanding wait. + if close_commands: + assert client._transport is not None + client._transport.close() waiting = asyncio.create_task(client.wait_command(999999, timeout=20.0)) await asyncio.sleep(0) await asyncio.to_thread(server_proc.stop) From 595ccdf636fab8f7fa1d7ea4ed89c83428a173fe Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 8 Sep 2026 06:21:05 -0400 Subject: [PATCH 5/6] Preserve cancellation when controller replies arrive --- parol6/client/async_client.py | 17 +++++++++-------- tests/integration/test_tool_operations.py | 18 +++++++++++++++++- 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index bce9c33..e3beba3 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -630,10 +630,13 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None: end_time = time.monotonic() + self.timeout while time.monotonic() < end_time: try: - resp_data, _ = await asyncio.wait_for( - self._rx_queue.get(), - timeout=max(0.0, end_time - time.monotonic()), - ) + # Keep the receive in this task: Python 3.11's + # wait_for can swallow an outer cancellation when + # its child receives a reply in the same turn. + async with asyncio.timeout( + max(0.0, end_time - time.monotonic()) + ): + resp_data, _ = await self._rx_queue.get() try: parsed = decode_message(resp_data) if isinstance(parsed, ResponseMsg): @@ -681,10 +684,8 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg: self._transport.sendto(data) while time.monotonic() < end_time: try: - resp_data, _addr = await asyncio.wait_for( - self._rx_queue.get(), - timeout=max(0.0, end_time - time.monotonic()), - ) + async with asyncio.timeout(max(0.0, end_time - time.monotonic())): + resp_data, _addr = await self._rx_queue.get() try: match decode_message(resp_data): case OkMsg() as ok: diff --git a/tests/integration/test_tool_operations.py b/tests/integration/test_tool_operations.py index 4d7485b..a8543f5 100644 --- a/tests/integration/test_tool_operations.py +++ b/tests/integration/test_tool_operations.py @@ -90,7 +90,7 @@ class TestPneumaticGripperMethods: """Test pneumatic gripper via client.tool().""" @pytest.mark.asyncio - async def test_pneumatic_open_close(self, async_client): + async def test_pneumatic_open_close(self, async_client, monkeypatch): """Open and close pneumatic gripper via tool methods.""" robot, client = async_client spec = robot.tools["PNEUMATIC"] @@ -147,6 +147,22 @@ async def test_pneumatic_open_close(self, async_client): client._rx_queue.put_nowait(reply) assert await client.status() is not None + # Cancel as an actual controller reply arrives. The reply must not + # swallow cancellation of a caller's completion budget or task. + receive = client._rx_queue.get + + async def receive_and_cancel(): + packet = await receive() + request.cancel() + return packet + + with monkeypatch.context() as patch: + patch.setattr(client._rx_queue, "get", receive_and_cancel) + request = asyncio.create_task(client.status()) + with pytest.raises(asyncio.CancelledError): + await request + assert await client.status() is not None + @pytest.mark.asyncio async def test_pneumatic_set_position_threshold(self, async_client): """set_position uses binary threshold: < 0.5 opens, >= 0.5 closes.""" From 1ce4c4899a05bb9c6e5ba0f6746958e9be152c03 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 05:30:42 +0000 Subject: [PATCH 6/6] Gate attachments on the six joints and stop answering streamed datagrams with ERROR set_shapes checked all eight slots of the homed array, but the firmware byte only carries six joints, so every attachment declaration was refused on hardware (the fake serial path fills all eight, which is why tests were green). While the attachment context is stale, every fire-and-forget jog or servo datagram was answered with an ERROR nobody awaited; the client dequeued those replies on its next unrelated request and raised for a command the server never refused. Streamed datagrams are now dropped (and logged once per epoch); acked commands are still refused explicitly. Co-Authored-By: Claude Fable 5.1 --- parol6/server/controller.py | 24 +++++++++++++++++------- parol6/server/state.py | 2 +- tests/integration/test_shapes_e2e.py | 7 +++++++ tests/unit/test_attachment_gate.py | 19 +++++++++++++++++++ 4 files changed, 44 insertions(+), 8 deletions(-) create mode 100644 tests/unit/test_attachment_gate.py diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 991d57e..118b05e 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -145,6 +145,7 @@ def __init__(self, config: ControllerConfig): self._cmd_rate = EventRateMetrics() self._gc_tracker = GCTracker() self._ack_policy = AckPolicy() + self._stale_attachment_logged_epoch = -1 self._async_log = AsyncLogHandler() self._transport_mgr = TransportManager( shutdown_event=self.shutdown_event, @@ -728,13 +729,22 @@ def _handle_motion_command( CmdType.SERVOL, CmdType.TELEPORT, ): - self._reply_error( - addr, - make_error( - ErrorCode.COMM_VALIDATION_ERROR, - detail="attachment context changed; reconcile the physical scene and reapply", - ), - ) + if self._ack_policy.requires_ack(cmd_type): + self._reply_error( + addr, + make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail="attachment context changed; reconcile the physical scene and reapply", + ), + ) + elif self._stale_attachment_logged_epoch != state.attachment_epoch: + # Nothing awaits a reply to a streamed datagram; an ERROR sent + # anyway is dequeued by the client's next unrelated request. + self._stale_attachment_logged_epoch = state.attachment_epoch + logger.warning( + "Dropping streamed %s: attachment context changed; reconcile the physical scene and reapply", + cmd_name, + ) return if not state.enabled: if cmd_type and self._ack_policy.requires_ack(cmd_type): diff --git a/parol6/server/state.py b/parol6/server/state.py index d0fc3b1..3cc05aa 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -483,7 +483,7 @@ def set_shapes(self, shapes: list) -> None: ) if (attached or self.has_attachments) and self.queued_segments: raise ValueError("stop queued motion before changing attachments") - if attached and (not self.enabled or not all(self.Homed_in)): + if attached and (not self.enabled or not all(self.Homed_in[:6])): raise ValueError("attachments require enabled, referenced robot state") PAROL6_ROBOT.apply_shapes(shapes) self.has_attachments = bool(attached) diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py index b264d29..5b28148 100644 --- a/tests/integration/test_shapes_e2e.py +++ b/tests/integration/test_shapes_e2e.py @@ -114,6 +114,13 @@ def test_attached_part_blocks_motion_except_for_declared_contacts(client: RobotC assert client.reset() == 1 with pytest.raises(MotionError, match="attachment context"): client.move_j(start, duration=1.0, wait=False) + # Streamed datagrams are dropped while the context is stale, not + # answered: nothing awaits a reply, and an ERROR sent anyway would be + # dequeued by the next unrelated request on this client. + for _ in range(50): + client.jog_j(0, speed=0.1, duration=0.02) + time.sleep(0.3) + assert client._inner._rx_queue.empty(), "unsolicited ERROR replies queued" fresh = client.shapes() assert fresh is not None and fresh.attachment_epoch != world.attachment_epoch reconciled = part.attach( diff --git a/tests/unit/test_attachment_gate.py b/tests/unit/test_attachment_gate.py new file mode 100644 index 0000000..1af23a1 --- /dev/null +++ b/tests/unit/test_attachment_gate.py @@ -0,0 +1,19 @@ +"""Attachment declarations gate on the six joints, not the padded homed byte.""" + +import numpy as np +from waldoctl import Sphere + +from parol6.server.state import ControllerState + + +def test_attachments_accept_a_homed_arm_with_unused_homed_slots_clear(): + state = ControllerState() + state.enabled = True + # The firmware byte carries six joints; slots 6-7 are always zero on + # hardware (the fake serial path fills all eight). + state.Homed_in[:] = np.array([1, 1, 1, 1, 1, 1, 0, 0], dtype=np.uint8) + part = Sphere(name="part", radius=0.02).attach( + flange_pose=(0.0, 0.0, 0.1, 0.0, 0.0, 0.0), epoch=state.attachment_epoch + ) + state.set_shapes([part]) + assert state.has_attachments and state.attachments_valid