diff --git a/README.md b/README.md index 1fe10b1..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 @@ -501,3 +509,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/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 21cfcda..5e9c64e 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, @@ -268,6 +275,7 @@ def skill_capabilities(self) -> frozenset[str]: "backend.parol6", "execution.speed", "observation.timed", + "world.attachments", "tool.gripper", "io.digital", } @@ -643,6 +651,7 @@ async def _request( await self._ensure_endpoint() assert self._transport is not None data = encode_command(cmd) + expected = QUERY_RESPONSE_TYPES[STRUCT_TO_CMDTYPE[type(cmd)]] wait = self.timeout if timeout is None else timeout attempts = self.retries + 1 if timeout is None else 1 for attempt in range(attempts): @@ -653,13 +662,20 @@ async def _request( end_time = time.monotonic() + wait 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): + # 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( @@ -701,10 +717,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: @@ -1208,15 +1222,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 ), @@ -1547,9 +1576,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). @@ -1577,17 +1607,42 @@ 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 + + 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 + ): + check_session(result.session_id) + 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/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..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.""" @@ -410,6 +429,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..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() # ============================================================================= @@ -691,6 +693,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( @@ -986,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), @@ -1372,11 +1400,32 @@ class ShapesResultStruct( installation: list[ShapeWire] program: list[ShapeWire] epoch: int + 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 915fc0e..5fca560 100644 --- a/parol6/server/command_executor.py +++ b/parol6/server/command_executor.py @@ -274,7 +274,7 @@ def _process_tick_result( state.action_current = "" state.action_params = "" state.action_state = ActionState.IDLE - 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 1e2e68a..2c50fe5 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, @@ -144,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, @@ -326,12 +328,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 ( @@ -401,9 +426,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: @@ -545,12 +568,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"): @@ -640,7 +665,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 @@ -680,6 +709,37 @@ 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, + ): + 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): reason = state.disabled_reason or "Controller disabled" @@ -830,6 +890,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) @@ -868,6 +940,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) @@ -879,6 +952,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/segment_player.py b/parol6/server/segment_player.py index 363861d..65a6bc8 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -409,14 +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 - 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 24da08c..da2c408 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 @@ -261,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) @@ -286,6 +290,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" @@ -344,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. @@ -351,6 +370,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.execution_paused = False @@ -399,6 +419,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 @@ -437,6 +460,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) @@ -453,10 +477,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[:6])): + 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/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_shapes_e2e.py b/tests/integration/test_shapes_e2e.py index 879078b..5b28148 100644 --- a/tests/integration/test_shapes_e2e.py +++ b/tests/integration/test_shapes_e2e.py @@ -27,6 +27,118 @@ 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) + # 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( + 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/integration/test_tool_operations.py b/tests/integration/test_tool_operations.py index fee8462..a8543f5 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, @@ -86,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"] @@ -109,6 +113,56 @@ 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 + ) + 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 + + # 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.""" diff --git a/tests/unit/test_async_client_lifecycle.py b/tests/unit/test_async_client_lifecycle.py index 27a82a4..bbbd710 100644 --- a/tests/unit/test_async_client_lifecycle.py +++ b/tests/unit/test_async_client_lifecycle.py @@ -84,3 +84,43 @@ async def consumer() -> None: finally: # Ensure cleanup even if assertions fail earlier await client.close() + + +@pytest.mark.asyncio +@pytest.mark.integration +@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) + 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_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 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]]) + ) 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]]) + )