diff --git a/README.md b/README.md index 5ca62de..3969fed 100644 --- a/README.md +++ b/README.md @@ -330,6 +330,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 @@ -508,3 +516,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 ee0ce19..fde0584 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -40,6 +40,7 @@ CmdType.SHAPES, CmdType.STATUS_RATE, CmdType.EXECUTION_SPEED, + CmdType.COMMAND_COMPLETION, } # Streaming commands are fire-and-forget (no ACK needed) @@ -71,6 +72,25 @@ CmdType.TOOL_ACTION, } +# Commands that move the arm: refused while the attachment context is stale +ARM_MOTION_CMD_TYPES: frozenset[CmdType] = frozenset( + { + CmdType.HOME, + 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, + } +) + class AckPolicy: """ diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index a256b05..2deb928 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -31,7 +31,11 @@ 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, + SYSTEM_CMD_TYPES, + AckPolicy, +) from ..utils.error_catalog import RobotError from ..utils.errors import MotionError from ..protocol.wire import ( @@ -42,6 +46,8 @@ decode_status_bin_into, CheckpointCmd, ConnectHardwareCmd, + CommandCompletionCmd, + CommandCompletionResultStruct, CurrentActionResultStruct, DelayCmd, EnablementResultStruct, @@ -677,10 +683,13 @@ 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 parsed.req_id != req_id: @@ -732,10 +741,8 @@ async def _request_ok_raw(self, data: bytes, timeout: float, req_id: int) -> OkM 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(reply_id) as ok if reply_id == req_id: @@ -1245,15 +1252,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 ), @@ -1585,9 +1607,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). @@ -1615,17 +1638,60 @@ 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 self._await_completion_hint(command_index, 0.25) + except TimeoutError: + return False + return False + + async def _await_completion_hint(self, command_index: int, timeout: float) -> None: + """Return once a status frame reports the command complete or an + error standing, or after ``timeout`` without one, so the completion + query is paced by the status stream and still re-asked without it.""" + end_time = time.monotonic() + timeout + while True: + self._status_event.clear() + status = self._shared_status + if status.completed_index >= command_index or status.error is not None: + return + remaining = end_time - time.monotonic() + if remaining <= 0: + return + try: + await asyncio.wait_for(self._status_event.wait(), remaining) + except asyncio.TimeoutError: + return # --------------- Move commands (queued, pre-computed trajectory) --------------- diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index e155c91..9ffba28 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -4,19 +4,32 @@ Delegates trajectory planning to TrajectoryPlanner (diagnostic=True) — the same logic used by the real PlannerWorker subprocess. Jog commands are simulated separately since the planner doesn't handle streaming. + +Every command a program issues becomes one block of a commanded +``TickIndex``: a motion at the planner's tick resolution, a delay as rows +holding the pose, a tool action as rows over its estimated travel, and a +command that plans nothing (a checkpoint, a tool selection, a refusal) as +a zero-row block that still carries its place and its error. ``plan()`` +lays the blocks on one 50 Hz axis; ``simulate()`` returns the same record, +because a planner has no plant to predict with and the plan is the honest +answer to what the arm will do. """ from __future__ import annotations +import hashlib import logging +import math from dataclasses import dataclass from typing import TYPE_CHECKING, Any import numpy as np from waldoctl.execution import ExecutionSpeed, validate_execution_scale from waldoctl.skills import UnresolvedPreview +from waldoctl.ticks import TickBlock, TickIndex import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from ..ack_policy import ARM_MOTION_CMD_TYPES from ..commands.base import MotionCommand from ..commands.cartesian_commands import ( JogLCommand, @@ -30,6 +43,7 @@ from ..config import ( CONTROL_RATE_HZ, HOME_ANGLES_DEG, + INTERVAL_S, deg_to_steps, rad_to_steps, steps_to_rad, @@ -44,6 +58,7 @@ from waldoctl.commands import CommandKind, command_table from ..protocol.wire import ( HomeCmd, + SetShapesCmd, SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd, @@ -60,8 +75,7 @@ TrajectorySegment, ) from ..server.state import ControllerState, get_fkine_se3 -from ..utils.error_catalog import RobotError, make_error -from ..utils.error_codes import ErrorCode +from ..utils.error_catalog import RobotError from parol6.tools import ElectricGripperConfig, PneumaticGripperConfig, get_registry from waldoctl.tools import ToolType @@ -110,42 +124,82 @@ def build_cmd(name: str, *args: Any, **kwargs: Any) -> Any: logger = logging.getLogger(__name__) -@dataclass -class DryRunResult: - """Result from a dry-run motion command.""" - - tcp_poses: np.ndarray # (N, 6) [x_m, y_m, z_m, rx_rad, ry_rad, rz_rad] - end_joints_rad: np.ndarray # (6,) final joint angles - duration: float # trajectory duration in seconds +#: Row spacing of the commanded record: the rate par6's engine keeps too, +#: so a host scrubs both backends on one axis. +_ROW_RATE_HZ = 50.0 +_STRIDE = max(1, int(round(CONTROL_RATE_HZ / _ROW_RATE_HZ))) +_ROW_DT_S = _STRIDE * INTERVAL_S +_MOVE_TYPE: dict[str, str | None] = { + name: spec.move_type for name, spec in _COMMANDS.items() +} + + +def _tcp_from_joints(q_rad: np.ndarray) -> np.ndarray: + """``(N, 6)`` TCP ``[x, y, z, rx, ry, rz]`` in metres and radians for joint + rows in radians, under the tool applied right now.""" + if q_rad.shape[0] == 0: + return np.empty((0, 6), dtype=np.float64) + tcp = joint_path_to_tcp_poses(q_rad) + tcp[:, :3] /= 1000.0 + np.deg2rad(tcp[:, 3:], out=tcp[:, 3:]) + return tcp + + +def _digest(joints: np.ndarray, tcp: np.ndarray) -> bytes: + """Identity over what reaches the screen, quantised below what a display + resolves, so two runs that paint the same picture hash the same.""" + h = hashlib.blake2b(digest_size=16) + h.update(np.round(joints / 1e-4).astype(np.int64).tobytes()) + h.update(np.round(tcp / 1e-5).astype(np.int64).tobytes()) + return h.digest() + + +@dataclass(slots=True) +class _Chunk: + """One program command at control-tick resolution, before the record + decimates it onto the row axis. Zero ticks for a command that plans + nothing: folded into a blend chain, refused, or state-only.""" + + command: int + method: str + q_rad: np.ndarray + tcp: np.ndarray + tool_closed: np.ndarray + valid: np.ndarray | None = None error: RobotError | None = None - valid: np.ndarray | None = None # (N,) per-pose bool; None = all valid - joint_trajectory_rad: np.ndarray | None = None # (N, 6) full joint trajectory - - -def _error_result(error: RobotError) -> DryRunResult: - """Build a DryRunResult for an error (empty trajectory).""" - return DryRunResult( - tcp_poses=np.empty((0, 6)), - end_joints_rad=np.empty(6), - duration=0.0, - error=error, - joint_trajectory_rad=None, - ) - -def _build_result(radians: np.ndarray, duration: float) -> DryRunResult: - """Build a DryRunResult from joint radians (N, 6) and duration. - - Converts joint radians → TCP poses in meters + radians. - """ - tcp_poses = joint_path_to_tcp_poses(radians) - tcp_poses[:, :3] /= 1000.0 # mm → m - np.deg2rad(tcp_poses[:, 3:], out=tcp_poses[:, 3:]) # deg → rad - return DryRunResult( - tcp_poses=tcp_poses, - end_joints_rad=radians[-1].copy(), - duration=duration, - joint_trajectory_rad=radians.copy(), + @property + def ticks(self) -> int: + return int(self.q_rad.shape[0]) + + +def _truncated(record: TickIndex, max_seconds: float) -> TickIndex: + limit = math.ceil(max_seconds / record.row_dt_s) + if limit >= record.rows: + return record + blocks = tuple( + TickBlock( + command=b.command, + start_row=min(b.start_row, limit), + rows=max(0, min(b.rows, limit - b.start_row)), + line_number=b.line_number, + error=b.error, + move_type=b.move_type, + ) + for b in record.blocks + ) + joints = record.joints_rad[:limit] + tcp = record.tcp[:limit] + return TickIndex( + row_dt_s=record.row_dt_s, + joints_rad=joints, + tcp=tcp, + tool_closed=record.tool_closed[:limit], + tool_gripping=record.tool_gripping[:limit], + blocks=blocks, + stop="budget_exhausted", + digest=_digest(joints, tcp), + valid=None if record.valid is None else record.valid[:limit], ) @@ -169,7 +223,7 @@ def tool_type(self) -> str: ) def __getattr__(self, name: str) -> Any: - def method(*args: Any, **kwargs: Any) -> DryRunResult | None: + def method(*args: Any, **kwargs: Any) -> int: return self._client.tool_action( self._client._active_tool_key, name, list(args), **kwargs ) @@ -184,8 +238,11 @@ class DryRunRobotClient: delegated to TrajectoryPlanner in diagnostic mode. Jog commands are simulated separately since the planner doesn't handle streaming. - Most methods are auto-dispatched via __getattr__ using CMD_MAP. - Execution controls change the planning clock; observations read local state. + Command methods answer as the live client does — a program index for + queued work, a code for the rest — and the record of what they planned + comes back from ``plan()``. Most methods are auto-dispatched via + __getattr__ using CMD_MAP; execution controls change the planning + clock; observations read local state. """ _robot: Robot | None = None @@ -208,7 +265,6 @@ def robot(self, value: Robot | None) -> None: def __init__( self, initial_joints_deg: list[float] | None = None, - max_snapshot_points: int = 200, initial_homed: bool = True, robot: Robot | None = None, ) -> None: @@ -243,11 +299,18 @@ def __init__( self._registry = CommandRegistry() self._q_rad_buf = np.zeros(6, dtype=np.float64) self._rpy_buf = np.zeros(3, dtype=np.float64) - self._max_snapshot_points = max_snapshot_points self._active_tool_key: str = "NONE" self._active_variant_key: str = "" self._tool_proxy = _DryRunTool(self) + # The commanded record: one chunk per program command, filled as the + # planner answers. Rows are recorded at submit time, under the tool + # and execution speed in force then, so a later change cannot + # rewrite an earlier command's path. + self._chunks: list[_Chunk] = [] + self._tool_position = 0.0 + self._plan_cache: TickIndex | None = None + @property def state(self) -> ControllerState: """Access the simulated controller state.""" @@ -258,6 +321,11 @@ def tool(self) -> _DryRunTool: """Tool proxy that routes actions through the planner.""" return self._tool_proxy + @property + def program_length(self) -> int: + """Commands recorded so far — one block each in ``plan()``.""" + return len(self._chunks) + def tcp_offset(self) -> list[float]: """Return current TCP offset in mm.""" return [ @@ -269,42 +337,224 @@ def tcp_offset(self) -> list[float]: def tcp_transform(self) -> list[float]: return self.tcp_offset() + [degrees(v) for v in self._state.tcp_rotation_rad] - def flush(self) -> list[DryRunResult]: - """Flush pending blend buffer. Call after script completion.""" + # ---- The record ---- + + def _open(self, method: str) -> int: + """Reserve the next program index for *method* with an empty chunk.""" + idx = len(self._chunks) + empty = np.empty((0, 6), dtype=np.float64) + self._chunks.append( + _Chunk(idx, method, empty, empty.copy(), np.empty(0, dtype=np.float64)) + ) + self._plan_cache = None + return idx + + def _current_q(self) -> np.ndarray: + steps_to_rad(self._state.Position_in, self._q_rad_buf) + return self._q_rad_buf.copy() + + def _fill( + self, + idx: int, + q_rad: np.ndarray, + *, + tcp: np.ndarray | None = None, + valid: np.ndarray | None = None, + tool_closed: np.ndarray | None = None, + error: RobotError | None = None, + ) -> None: + """Give chunk *idx* its rows, FK'd under the tool applied now.""" + c = self._chunks[idx] + c.q_rad = np.ascontiguousarray(q_rad, dtype=np.float64).reshape(-1, 6) + c.tcp = tcp if tcp is not None else _tcp_from_joints(c.q_rad) + c.tool_closed = ( + tool_closed + if tool_closed is not None + else np.full(c.ticks, self._tool_position, dtype=np.float64) + ) + c.valid = valid + c.error = error + self._plan_cache = None + + def _hold(self, idx: int, ticks: int, *, error: RobotError | None = None) -> None: + """Chunk *idx* holds the current pose for *ticks* control ticks.""" + q = np.repeat(self._current_q()[np.newaxis], max(0, ticks), axis=0) + self._fill(idx, q, error=error) + + def _stretched(self, q_rad: np.ndarray) -> np.ndarray: + """*q_rad* replayed at the execution speed in force: the segment player + indexes the same waypoints at a scaled rate, so the path is the same + and only the row count changes.""" + scale = self._state.execution_speed + if scale == 1.0 or q_rad.shape[0] < 2: + return q_rad + ticks = max(1, int(round(q_rad.shape[0] / scale))) + at = np.clip( + np.round(np.arange(ticks) * scale).astype(np.intp), 0, q_rad.shape[0] - 1 + ) + return q_rad[at] + + def _tool_target(self, action: str, params: list) -> float: + if action == "open": + return 0.0 + if action == "close": + return 1.0 + if action in ("move", "set_position") and params: + return float(min(1.0, max(0.0, float(params[0])))) + return self._tool_position + + def _fill_tool_action(self, idx: int, cmd: ToolActionCmd) -> None: + """A tool action holds the arm for the tool's estimated travel while + the jaws ramp to their target.""" + cfg = get_registry().get(cmd.tool_key.strip().upper()) + params = list(cmd.params) + seconds = cfg.estimate_duration(cmd.action, params) if cfg is not None else 0.0 + ticks = int(round(seconds / INTERVAL_S)) + target = self._tool_target(cmd.action, params) + q = np.repeat(self._current_q()[np.newaxis], ticks, axis=0) + closed = ( + np.linspace(self._tool_position, target, ticks, dtype=np.float64) + if ticks + else np.empty(0, dtype=np.float64) + ) + self._fill(idx, q, tool_closed=closed) + self._tool_position = target + + def _absorb(self, segments: list[Segment]) -> None: + """Record what the planner produced, each segment under its own + command. A blend chain lands under its head; the folded commands' + chunks stay at zero ticks.""" + for seg in segments: + if isinstance(seg, TrajectorySegment): + self._fill(seg.command_index, self._stretched(seg.trajectory_rad)) + elif isinstance(seg, ErrorSegment): + if seg.cartesian_path is not None and seg.ik_valid is not None: + path = np.asarray(seg.cartesian_path, dtype=np.float64) + q = np.repeat(self._current_q()[np.newaxis], path.shape[0], axis=0) + self._fill( + seg.command_index, + q, + tcp=path, + valid=np.asarray(seg.ik_valid, dtype=np.bool_), + error=seg.error, + ) + else: + self._fill(seg.command_index, np.empty((0, 6)), error=seg.error) + elif isinstance(seg, InlineSegment) and isinstance( + seg.params, ToolActionCmd + ): + self._fill_tool_action(seg.command_index, seg.params) + # Any other InlineSegment (select_tool, checkpoint, write_io …) + # plans nothing: its chunk keeps its place at zero ticks. + + def _assemble(self) -> TickIndex: + joints_parts: list[np.ndarray] = [] + tcp_parts: list[np.ndarray] = [] + closed_parts: list[np.ndarray] = [] + valid_parts: list[np.ndarray] = [] + blocks: list[TickBlock] = [] + any_valid = any(c.valid is not None for c in self._chunks) + failed = False + tick0 = 0 + row0 = 0 + for c in self._chunks: + rows = 0 + if c.ticks: + # Keep the control ticks that fall on the row grid, counted + # from the program's first tick so blocks abut exactly. + keep = np.arange((-tick0) % _STRIDE, c.ticks, _STRIDE) + rows = int(keep.shape[0]) + if rows: + joints_parts.append(c.q_rad[keep]) + tcp_parts.append(c.tcp[keep]) + closed_parts.append(c.tool_closed[keep]) + if any_valid: + valid_parts.append( + c.valid[keep] + if c.valid is not None + else np.ones(rows, dtype=np.bool_) + ) + blocks.append( + TickBlock( + command=c.command, + start_row=row0, + rows=rows, + error=c.error, + move_type=_MOVE_TYPE.get(c.method), + ) + ) + failed = failed or c.error is not None + row0 += rows + tick0 += c.ticks + joints = ( + np.concatenate(joints_parts).astype(np.float32) + if joints_parts + else np.empty((0, 6), dtype=np.float32) + ) + tcp = ( + np.concatenate(tcp_parts).astype(np.float32) + if tcp_parts + else np.empty((0, 6), dtype=np.float32) + ) + closed = ( + np.concatenate(closed_parts).astype(np.float32) + if closed_parts + else np.empty(0, dtype=np.float32) + ) + return TickIndex( + row_dt_s=_ROW_DT_S, + joints_rad=joints, + tcp=tcp, + tool_closed=closed, + tool_gripping=np.zeros(row0, dtype=np.bool_), + blocks=tuple(blocks), + stop="failed" if failed else "completed", + digest=_digest(joints, tcp), + valid=np.concatenate(valid_parts) if valid_parts else None, + ) + + def plan(self, max_seconds: float | None = None) -> TickIndex: + """The commanded record for everything submitted so far.""" + self.flush() + if self._plan_cache is None: + self._plan_cache = self._assemble() + record = self._plan_cache + return record if max_seconds is None else _truncated(record, max_seconds) + + def simulate(self, max_seconds: float | None = None) -> TickIndex: + """The predicted record — the plan itself. A planner has no plant to + drive, so where the controller would send the arm is the honest + answer to where the arm goes.""" + return self.plan(max_seconds) + + # ---- Dispatch ---- + + def flush(self) -> None: + """Plan any pending blend chain. Call after script completion.""" if self._planner._blend_buffer: self._require_running() - segments = self._planner.flush() + self._absorb(self._planner.flush()) self._state.Position_in[:] = self._planner.state.Position_in - results: list[DryRunResult] = [] - for seg in segments: - r = self._segment_to_result(seg) - if r is not None: - results.append(r) - return results - def _snap_to_angles(self, angles_deg: list[float]) -> DryRunResult: + def _snap_to_angles(self, idx: int, angles_deg: list[float]) -> None: """Snap to angles instantly (no trajectory) — used by Home and Teleport. Both establish position references, so subsequent planned moves pass the homed gate. Blended moves still buffered in the planner are - planned first and lead the returned result, so their paths — and any - refusal — reach the caller exactly as the live controller would run - them before the snap.""" - pending = [ - r - for seg in self._planner.flush() - if (r := self._segment_to_result(seg)) is not None - ] + planned first, under their own commands — the live controller runs + them before the snap — and the snap itself lands as one row at the + new pose, so the record shows where the arm is once it is there.""" + self._absorb(self._planner.flush()) deg = np.asarray(angles_deg, dtype=np.float64) deg_to_steps(deg, self._state.Position_in) self._planner.state.Position_in[:] = self._state.Position_in self._planner.state.Homed_in.fill(1) - rad = np.radians(deg).reshape(1, -1) - snap = _build_result(rad, duration=0.0) - return self._merge_results([*pending, snap]) if pending else snap + self._hold(idx, _STRIDE) - def _dispatch(self, params: Any) -> DryRunResult | None: - """Route a command struct through the trajectory planner.""" + def _dispatch(self, params: Any, method: str) -> int: + """Route a command struct through the trajectory planner, recording + it as the next program command. Returns its program index.""" + self._state.Homed_in[:] = self._planner.state.Homed_in cmd_cls = self._registry.get_command_for_struct(type(params)) if ( cmd_cls is not None @@ -312,17 +562,45 @@ def _dispatch(self, params: Any) -> DryRunResult | None: and not cmd_cls.streamable ): self._require_running() + if ( + not self._state.attachments_valid + and _wire.STRUCT_TO_CMDTYPE.get(type(params)) in ARM_MOTION_CMD_TYPES + ): + raise ValueError("attachment context changed; reconcile and reapply") + idx = self._open(method) + 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 idx + 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 idx + 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 idx + if isinstance(params, SetShapesCmd): + self._state.set_shapes(params.shapes) if isinstance(params, HomeCmd): if params.calibrate or not self._planner.state.Homed_in[:6].all(): - return self._snap_to_angles(HOME_ANGLES_DEG) + self._state.invalidate_attachments() + self._snap_to_angles(idx, HOME_ANGLES_DEG) + return idx # Already referenced → fall through: the planner fast-paths HOME # into a planned return move, so the preview renders the path. if isinstance(params, TeleportCmd): - return self._snap_to_angles(params.angles) - results: list[DryRunResult] = [] + self._snap_to_angles(idx, params.angles) + return idx if isinstance(params, (SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd)): # Resolve pending paths against their original TCP before changing it. - results.extend(self.flush()) + self.flush() if isinstance(params, SelectToolCmd): self._active_tool_key = params.tool_name.strip().upper() self._active_variant_key = params.variant_key @@ -340,129 +618,28 @@ def _dispatch(self, params: Any) -> DryRunResult | None: # Other non-trajectory MotionCommands (SelectTool, Home) fall through # to the planner which handles them as inline segments. if cmd_cls is not None and issubclass(cmd_cls, (JogJCommand, JogLCommand)): - self._planner.flush() - self._state.Position_in[:] = self._planner.state.Position_in + self.flush() cmd = cmd_cls(params) assert isinstance(cmd, MotionCommand) - result = self._simulate_jog(cmd) + path = self._simulate_jog(cmd) + if path is not None: + self._fill(idx, path) self._planner.state.Position_in[:] = self._state.Position_in - return result + return idx # Everything else → planner - segments = self._planner.process(params) + self._absorb(self._planner.process(params, command_index=idx)) self._state.Position_in[:] = self._planner.state.Position_in + return idx - for seg in segments: - r = self._segment_to_result(seg) - if r is not None: - results.append(r) - - if not results: - return None - if len(results) == 1: - return results[0] - return self._merge_results(results) - - def _segment_to_result(self, seg: Segment) -> DryRunResult | None: - """Convert a planner segment to a DryRunResult.""" - if isinstance(seg, TrajectorySegment): - return self._trajectory_segment_to_result(seg) - if isinstance(seg, ErrorSegment): - return self._error_segment_to_result(seg) - if isinstance(seg, InlineSegment) and isinstance(seg.params, ToolActionCmd): - return self._tool_action_segment_to_result(seg.params) - # Other InlineSegments (SelectTool, Home, etc.) — no visualization - return None - - def _trajectory_segment_to_result(self, seg: TrajectorySegment) -> DryRunResult: - """Convert a TrajectorySegment to a DryRunResult.""" - steps = seg.trajectory_steps - stride = max(1, len(steps) // self._max_snapshot_points) - sampled = steps[::stride] - if not np.array_equal(sampled[-1], steps[-1]): - sampled = np.vstack([sampled, steps[-1:]]) - - radians = np.empty((len(sampled), 6), dtype=np.float64) - for i in range(len(sampled)): - steps_to_rad(sampled[i], radians[i]) - - return _build_result(radians, seg.duration / self._state.execution_speed) - - def _error_segment_to_result(self, seg: ErrorSegment) -> DryRunResult: - """Convert an ErrorSegment to a DryRunResult with per-pose validity.""" - if seg.cartesian_path is not None and seg.ik_valid is not None: - return DryRunResult( - tcp_poses=seg.cartesian_path, - end_joints_rad=np.zeros(6, dtype=np.float64), - duration=0.0, - error=seg.error, - valid=seg.ik_valid, - joint_trajectory_rad=None, - ) - return _error_result(seg.error) - - def _tool_action_segment_to_result(self, cmd: ToolActionCmd) -> DryRunResult: - """Return a single-point DryRunResult at the current TCP pose.""" - steps_to_rad(self._state.Position_in, self._q_rad_buf) - duration = 0.0 - cfg = get_registry().get(cmd.tool_key.strip().upper()) - if cfg is not None: - duration = cfg.estimate_duration(cmd.action, cmd.params) - return _build_result(self._q_rad_buf[np.newaxis], duration) - - def _merge_results(self, results: list[DryRunResult]) -> DryRunResult: - """Merge multiple DryRunResults into one (for multi-segment blends).""" - non_empty = [r for r in results if r.tcp_poses.shape[0] > 0] - first_error = next((r.error for r in results if r.error is not None), None) - if not non_empty: - if first_error is not None: - return _error_result(first_error) - return _error_result(make_error(ErrorCode.TRAJ_EMPTY_RESULT, detail="")) - - tcp_all = np.vstack([r.tcp_poses for r in non_empty]) - total_duration = sum(r.duration for r in results) - last = non_empty[-1] - - has_any_valid = any(r.valid is not None for r in non_empty) - if has_any_valid: - valids = [ - r.valid - if r.valid is not None - else np.ones(r.tcp_poses.shape[0], dtype=np.bool_) - for r in non_empty - ] - merged_valid = np.concatenate(valids) - else: - merged_valid = None - - has_any_joints = any(r.joint_trajectory_rad is not None for r in non_empty) - if has_any_joints: - joint_parts = [ - r.joint_trajectory_rad - if r.joint_trajectory_rad is not None - else np.broadcast_to( - r.end_joints_rad[np.newaxis, :], - (r.tcp_poses.shape[0], r.end_joints_rad.shape[0]), - ).copy() - for r in non_empty - ] - merged_joints = np.vstack(joint_parts) - else: - merged_joints = None - - return DryRunResult( - tcp_poses=tcp_all, - end_joints_rad=last.end_joints_rad, - duration=total_duration, - error=first_error, - valid=merged_valid, - joint_trajectory_rad=merged_joints, - ) + def _failed(self, idx: int) -> bool: + return self._chunks[idx].error is not None # ---- Jog simulation (planner doesn't handle streaming) ---- - def _simulate_jog(self, cmd: MotionCommand) -> DryRunResult | None: - """Simulate jog commands by computing linear displacement.""" + def _simulate_jog(self, cmd: MotionCommand) -> np.ndarray | None: + """Simulate jog commands by computing linear displacement, one row + per control tick.""" # Run do_setup so speeds_out / _axis_index / etc. are computed cmd.setup(self._state) @@ -472,13 +649,10 @@ def _simulate_jog(self, cmd: MotionCommand) -> DryRunResult | None: return self._simulate_cartesian_jog(cmd) return None - def _simulate_joint_jog(self, cmd: JogJCommand) -> DryRunResult: + def _simulate_joint_jog(self, cmd: JogJCommand) -> np.ndarray: """Simulate joint jog by computing linear displacement in joint space.""" duration = cmd.p.duration - n_points = min( - self._max_snapshot_points, - max(2, int(duration * CONTROL_RATE_HZ)), - ) + n_points = max(1, int(round(duration * CONTROL_RATE_HZ))) # Compute total displacement (steps/tick * ticks_in_duration) ticks = duration * CONTROL_RATE_HZ @@ -496,16 +670,12 @@ def _simulate_joint_jog(self, cmd: JogJCommand) -> DryRunResult: radians = np.empty((n_points, 6), dtype=np.float64) for i in range(n_points): steps_to_rad(trajectory[i], radians[i]) + return radians - return _build_result(radians, duration) - - def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None: + def _simulate_cartesian_jog(self, cmd: JogLCommand) -> np.ndarray: """Simulate cartesian jog by displacing along a Cartesian axis and solving IK.""" duration = cmd.p.duration - n_points = min( - self._max_snapshot_points, - max(2, int(duration * CONTROL_RATE_HZ)), - ) + n_points = max(1, int(round(duration * CONTROL_RATE_HZ))) current_se3 = get_fkine_se3(self._state) se3_rpy(current_se3, self._rpy_buf) @@ -569,8 +739,7 @@ def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None: rad_to_steps(last_valid_q, steps_buf) self._state.Position_in[:] = steps_buf - - return _build_result(radians, duration) + return radians # ---- Explicit methods for state reads ---- @@ -578,6 +747,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), "set_shapes") + return 1 + def shapes(self): """The preview's collision world by layer (mirrors the live query). @@ -587,6 +760,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()), ) @@ -604,16 +778,24 @@ def pose(self) -> list[float]: float(np.degrees(self._rpy_buf[2])), ] + # ---- Explicit command methods: the live client's signatures ---- + + def home(self, **kwargs: Any) -> int: + return self._dispatch(build_cmd("home", **kwargs), "home") + def move_j( self, angles: list[float] | None = None, *, pose: list[float] | None = None, **kwargs: Any, - ) -> DryRunResult | None: + ) -> int: if pose is not None: - return self._dispatch(build_cmd("move_j_pose", pose, **kwargs)) - return self._dispatch(build_cmd("move_j", angles or [], **kwargs)) + return self._dispatch(build_cmd("move_j_pose", pose, **kwargs), "move_j") + return self._dispatch(build_cmd("move_j", angles or [], **kwargs), "move_j") + + def move_l(self, pose: list[float], **kwargs: Any) -> int: + return self._dispatch(build_cmd("move_l", pose, **kwargs), "move_l") def servo_j( self, @@ -621,22 +803,54 @@ def servo_j( *, pose: list[float] | None = None, **kwargs: Any, - ) -> DryRunResult | None: + ) -> int: if pose is not None: - return self._dispatch(build_cmd("servo_j_pose", pose, **kwargs)) - return self._dispatch(build_cmd("servo_j", angles or [], **kwargs)) + idx = self._dispatch(build_cmd("servo_j_pose", pose, **kwargs), "servo_j") + else: + idx = self._dispatch( + build_cmd("servo_j", angles or [], **kwargs), "servo_j" + ) + return -1 if self._failed(idx) else 1 + + def checkpoint(self, label: str) -> int: + return self._dispatch(build_cmd("checkpoint", label), "checkpoint") def write_io(self, index: int, value: int, *, timeout: float | None = None) -> int: if type(index) is not int or index not in (0, 1): raise ValueError("Output index must be 0 or 1") if type(value) not in (int, bool) or value not in (0, 1): raise ValueError("Digital output must be 0 or 1") - result = self._dispatch(WriteIOCmd(port_index=index + 2, value=int(value))) - if result is not None and result.error is not None: - raise RuntimeError(str(result.error)) - return 0 + return self._dispatch( + WriteIOCmd(port_index=index + 2, value=int(value)), "write_io" + ) + + def delay(self, seconds: float) -> int: + """Hold the pose for *seconds*: rows on the commanded record, so the + timeline carries the wait as the controller will.""" + self._require_running() + if not math.isfinite(seconds) or seconds <= 0: + raise ValueError("delay needs a positive, finite number of seconds") + # The live planner runs a pending blend chain before a delay; hold + # the pose the chain ends at, not the one before it. + self.flush() + idx = self._open("delay") + self._hold(idx, int(round(seconds / INTERVAL_S))) + return idx + + def wait_command(self, command_index: int, timeout: float = 10.0) -> bool: + """Whether the block for *command_index* planned without error.""" + self.flush() + return 0 <= command_index < len(self._chunks) and not self._failed( + command_index + ) + + def wait_motion(self, **kwargs: Any) -> bool: + self.flush() + return True 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 " @@ -645,6 +859,7 @@ def _require_running(self) -> None: def set_execution_speed(self, scale: float, *, timeout: float = 3.0) -> int: self._state.execution_speed = validate_execution_scale(scale) + self._open("set_execution_speed") return 1 def execution_speed(self, *, timeout: float = 3.0) -> ExecutionSpeed: @@ -654,10 +869,12 @@ def execution_speed(self, *, timeout: float = 3.0) -> ExecutionSpeed: def pause(self, *, timeout: float = 3.0) -> int: self._state.execution_paused = True + self._open("pause") return 1 def resume(self, *, timeout: float = 3.0) -> int: self._state.execution_paused = False + self._open("resume") return 1 def jog_j( @@ -669,7 +886,7 @@ def jog_j( joints: list[int] | None = None, speeds: list[float] | None = None, accel: float = 1.0, - ) -> DryRunResult | None: + ) -> int: """The live client's signature, so a script's jog previews as written.""" speed_arr = [0.0] * 6 if joints is not None and speeds is not None: @@ -679,9 +896,10 @@ def jog_j( speed_arr[joint] = speed else: raise ValueError("jog_j requires either joint= or joints=/speeds=") - return self._dispatch( - _wire.JogJCmd(speeds=speed_arr, duration=duration, accel=accel) + idx = self._dispatch( + _wire.JogJCmd(speeds=speed_arr, duration=duration, accel=accel), "jog_j" ) + return -1 if self._failed(idx) else 1 def jog_l( self, @@ -693,7 +911,7 @@ def jog_l( axes: list[str] | None = None, speeds_list: list[float] | None = None, accel: float = 1.0, - ) -> DryRunResult | None: + ) -> int: vel = [0.0] * 6 if axes is not None and speeds_list is not None: for a, s in zip(axes, speeds_list): @@ -702,15 +920,11 @@ def jog_l( vel[_AXIS_INDEX[axis]] = speed else: raise ValueError("jog_l requires either axis= or axes=/speeds_list=") - return self._dispatch( - _wire.JogLCmd(frame=frame, velocities=vel, duration=duration, accel=accel) + idx = self._dispatch( + _wire.JogLCmd(frame=frame, velocities=vel, duration=duration, accel=accel), + "jog_l", ) - - def delay(self, seconds: float = 0.0) -> None: - self._require_running() - - def wait_motion(self, **kwargs: Any) -> None: - self.flush() + return -1 if self._failed(idx) else 1 # ---- Auto-dispatch for everything else ---- @@ -721,18 +935,21 @@ def __getattr__(self, name: str) -> Any: raise AttributeError(f"'{type(self).__name__}' has no attribute '{name}'") spec = _COMMANDS.get(name) - applies = spec is not None and spec.kind in ( - CommandKind.SYSTEM, - CommandKind.CONTROL, - ) + if spec is None or spec.kind in ( + CommandKind.QUERY, + CommandKind.OBSERVATION, + CommandKind.SYNC, + ): + raise AttributeError( + f"'{type(self).__name__}' previews no '{name}': the query reads " + "live state the dry run does not keep" + ) - def method(*args: Any, **kwargs: Any) -> DryRunResult | int | None: - result = self._dispatch(build_cmd(name, *args, **kwargs)) - if not applies: - return result - # A system or control command answers as the live client does: - # 1 when it applied, negative when the planner refused it. Its - # planner result carries no path a program could wait on. - return -1 if result is not None and result.error is not None else 1 + def method(*args: Any, **kwargs: Any) -> int: + idx = self._dispatch(build_cmd(name, *args, **kwargs), name) + # Queued work answers with its program index; everything else + # answers as the live client does: 1 when it applied, -1 when + # the planner refused it. + return idx if spec.mints_index else (-1 if self._failed(idx) else 1) return method diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index e9a5c7b..8e77828 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -14,6 +14,8 @@ AnglesCmd, AnglesResultStruct, CmdType, + CommandCompletionCmd, + CommandCompletionResultStruct, CurrentActionResultStruct, EnablementResultStruct, ErrorCmd, @@ -273,6 +275,21 @@ def compute(self, state: "ControllerState") -> Response: ) +@register_command(CmdType.COMMAND_COMPLETION) +class CommandCompletionCommand(QueryCommand[CommandCompletionCmd]): + PARAMS_TYPE = CommandCompletionCmd + QUERY_TYPE = QueryType.COMMAND_COMPLETION + + __slots__ = () + + def compute(self, state: "ControllerState") -> Response: + return 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.""" @@ -392,6 +409,7 @@ def compute(self, state: "ControllerState") -> Response: ], 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 d9d3edc..690dbce 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -35,6 +35,7 @@ from parol6.config import LIMITS from waldoctl import ActionState, ToolStatus from waldoctl.execution import ExecutionSpeed, validate_execution_scale +from waldoctl.shapes import Attachment from waldoctl.tools import ToolState from parol6.tools import get_registry, list_tools @@ -119,6 +120,7 @@ class QueryType(IntEnum): STATUS_RATE = auto() TCP_TRANSFORM = auto() EXECUTION_SPEED = auto() + COMMAND_COMPLETION = auto() class CmdType(IntEnum): @@ -195,6 +197,7 @@ class CmdType(IntEnum): PAUSE = auto() SET_EXECUTION_SPEED = auto() EXECUTION_SPEED = auto() + COMMAND_COMPLETION = auto() # ============================================================================= @@ -713,6 +716,11 @@ 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: + Attachment.from_wire(self.attachment) class SetShapesCmd( @@ -1008,6 +1016,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), @@ -1394,11 +1421,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/robot.py b/parol6/robot.py index 2fa4650..1bc2385 100644 --- a/parol6/robot.py +++ b/parol6/robot.py @@ -988,7 +988,7 @@ def create_sync_client(self, **kwargs: Any) -> SyncRobotClient: def create_dry_run_client(self, **kwargs: Any) -> DryRunClient | None: initial_joints_deg: list[float] | None = kwargs.get("initial_joints_deg") initial_homed: bool = bool(kwargs.get("initial_homed", True)) - return DryRunRobotClient( # ty: ignore[invalid-return-type] + return DryRunRobotClient( initial_joints_deg=initial_joints_deg, initial_homed=initial_homed, robot=self, 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 7312938..a252672 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -14,7 +14,7 @@ from typing import Any -from parol6.ack_policy import AckPolicy +from parol6.ack_policy import ARM_MOTION_CMD_TYPES, AckPolicy from parol6.commands.base import ( CommandBase, ExecutionStatusCode, @@ -147,6 +147,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, @@ -329,12 +330,39 @@ 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() + # A hardware E-stop already cancelled all motion and owns the error + # until it is released; the attachment error latches on that tick. + if state.error is not None and state.error.code == ErrorCode.SYS_ESTOP_ACTIVE: + return + 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 ( @@ -404,9 +432,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: @@ -548,12 +574,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"): @@ -667,7 +695,11 @@ def _process_command( return # Try stream fast-path first (avoids full command creation) - result = self._executor.try_stream_fast_path(payload, state) + result = ( + self._executor.try_stream_fast_path(payload, state) + if state.attachments_valid + else False + ) if result is True: return @@ -715,6 +747,25 @@ def _handle_motion_command( cmd_name = type(command).__name__ cmd_type = command._cmd_type + if not state.attachments_valid and cmd_type in ARM_MOTION_CMD_TYPES: + if self._ack_policy.requires_ack(cmd_type): + self._reply_error( + req_id, + 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" @@ -825,6 +876,7 @@ def _handle_motion_command( homed=homed_snapshot, ) ) + state.plan_submitted_index = cmd_index if cmd_type and self._ack_policy.requires_ack(cmd_type): self._reply_ok_index(req_id, addr, cmd_index) @@ -910,6 +962,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) @@ -921,6 +974,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 4e8ac57..65106d8 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -108,8 +108,13 @@ def tick(self, state: ControllerState) -> bool: while seg is not None: self._buffer.append(seg) state.queued_segments += 1 + if seg.command_index > state.plan_received_index: + state.plan_received_index = seg.command_index if isinstance(seg, TrajectorySegment): state.queued_duration += seg.duration + for idx in seg.blend_consumed_indices: + if idx > state.plan_received_index: + state.plan_received_index = idx seg = self._planner.poll_segment() # MoveIt-style invalidation: a world change (SET_SHAPES bumps @@ -417,14 +422,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 @@ -512,3 +516,4 @@ def _drain_planner_queue(self, state: ControllerState) -> None: pass state.queued_segments = 0 state.queued_duration = 0.0 + state.plan_received_index = state.plan_submitted_index diff --git a/parol6/server/state.py b/parol6/server/state.py index 24da08c..e84fc61 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,14 @@ 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 + # Highest command index handed to the planner, and the highest one a + # returned segment accounts for (a blend head answers for its chain). + plan_submitted_index: int = -1 + plan_received_index: int = -1 # Network setup and uptime ip: str = "127.0.0.1" @@ -344,6 +356,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 +374,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 +423,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 @@ -406,6 +433,8 @@ def reset(self) -> None: self.clear_collision() self.queued_segments = 0 self.queued_duration = 0.0 + self.plan_submitted_index = -1 + self.plan_received_index = -1 # Gripper mode tracker self.gripper_mode_tracker = GripperModeResetTracker() @@ -437,6 +466,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 +483,39 @@ 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.action_state == ActionState.EXECUTING + or self.queued_segments + or self.plan_in_flight + ): + raise ValueError("stop 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 + @property + def plan_in_flight(self) -> bool: + """True while a submitted plan has not come back from the planner.""" + return self.plan_submitted_index > self.plan_received_index + + 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..e9fa43a 100644 --- a/parol6/server/status_broadcast.py +++ b/parol6/server/status_broadcast.py @@ -2,7 +2,6 @@ import logging import socket -import secrets import sys import time @@ -62,7 +61,7 @@ def __init__( self._send_failures = 0 self._max_send_failures = 3 self._last_fail_log_time = 0.0 - self._session_id = secrets.randbits(64) or 1 + self._session_id = state_mgr.get_state().status_session_id self._seq = 0 self._setup_socket() diff --git a/parol6/server/transports/mock_serial_transport.py b/parol6/server/transports/mock_serial_transport.py index 5f43392..07f24be 100644 --- a/parol6/server/transports/mock_serial_transport.py +++ b/parol6/server/transports/mock_serial_transport.py @@ -72,9 +72,6 @@ def _simulate_motion_jit( speed_in[i] = 0 command_out = CommandCode.IDLE - # Ensure E-stop stays released - io_in[4] = 1 - if command_out == CommandCode.HOME: if homing_countdown == 0: for i in range(6): @@ -466,6 +463,10 @@ def sync_from_controller_state(self, state: ControllerState) -> None: "MockSerialTransport: failed to sync from controller state: %s", e ) + def press_estop(self, pressed: bool) -> None: + """Drive the simulated E-stop input (bit 4: 0 pressed, 1 released).""" + self._state.io_in[4] = 0 if pressed else 1 + def disconnect(self) -> None: """Simulate serial port disconnection.""" self._connected = False diff --git a/tests/conftest.py b/tests/conftest.py index bd0c932..83bea68 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -349,6 +349,26 @@ def client(ports: TestPorts): ) +@pytest.fixture +def controller(monkeypatch): + """An in-process Controller on the fake serial with an ephemeral UDP port, + ticked by the test through the loop's phases; the planner is not started.""" + from parol6.server.controller import Controller, ControllerConfig + + monkeypatch.setenv("PAROL6_FAKE_SERIAL", "1") + ctl = Controller(ControllerConfig(udp_host="127.0.0.1", udp_port=0)) + try: + yield ctl + finally: + ctl._planner.stop() + if ctl.udp_transport is not None: + ctl.udp_transport.close_socket() + if ctl._status_broadcaster is not None: + ctl._status_broadcaster.close() + ctl._transport_mgr.disconnect() + ctl.state_manager.reset_state() + + def pytest_sessionfinish(session, exitstatus): """Called after whole test run finished.""" logger.info( diff --git a/tests/integration/test_attachment_estop.py b/tests/integration/test_attachment_estop.py new file mode 100644 index 0000000..fe2e9c7 --- /dev/null +++ b/tests/integration/test_attachment_estop.py @@ -0,0 +1,92 @@ +"""A hardware E-stop with a part attached: the E-stop owns the error while it +is pressed, and the stale-attachment error latches on the tick it is released. + +Driven through the control loop's phases in loop order against the fake +serial, whose E-stop input the test presses and releases. +""" + +import socket + +import pytest + +from parol6.protocol.wire import SetShapesCmd, ShapeWire, encode_command +from parol6.server.controller import Controller +from parol6.server.transports.mock_serial_transport import MockSerialTransport +from parol6.utils.error_codes import ErrorCode +from waldoctl import Sphere + +pytestmark = pytest.mark.integration + + +def _tick(controller: Controller, state) -> None: + controller._read_from_firmware(state) + controller._check_attachments(state) + controller._poll_commands(state) + controller._handle_estop(state) + controller._check_attachments(state) + if not controller.estop_active: + controller._execute_commands(state) + controller._write_to_firmware(state) + controller._transport_mgr.tick_simulation(state.current_tool, tool_teleport_pos=-1) + + +def _tick_until(controller: Controller, state, condition, message: str) -> None: + for _ in range(50): + _tick(controller, state) + if condition(): + return + pytest.fail(message) + + +def test_estop_owns_the_error_until_release_then_the_attachment_latches(controller): + state = controller.state_manager.get_state() + robot = controller._transport_mgr.transport + assert isinstance(robot, MockSerialTransport) + state.Homed_in[:] = 1 + robot.sync_from_controller_state(state) + _tick_until( + controller, + state, + lambda: state.enabled and all(state.Homed_in[:6]), + "the fake serial never reported a referenced, enabled robot", + ) + + part = Sphere(name="part", radius=0.01).attach( + flange_pose=(0.0, 0.0, 0.25, 0.0, 0.0, 0.0), epoch=state.attachment_epoch + ) + assert controller.udp_transport is not None + address = ("127.0.0.1", controller.udp_transport.socket.getsockname()[1]) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender: + sender.sendto( + encode_command(SetShapesCmd(shapes=[ShapeWire(*part.to_wire())])), address + ) + _tick_until( + controller, + state, + lambda: state.has_attachments, + "the part was never attached", + ) + + robot.press_estop(True) + _tick_until( + controller, + state, + lambda: controller.estop_active, + "the E-stop press was never seen", + ) + for _ in range(5): + _tick(controller, state) + assert state.error is not None + assert state.error.code == ErrorCode.SYS_ESTOP_ACTIVE, state.error + assert not state.attachments_valid + + robot.press_estop(False) + _tick_until( + controller, + state, + lambda: state.error is not None + and state.error.code == ErrorCode.COMM_VALIDATION_ERROR, + "the stale attachment never surfaced after the E-stop released", + ) + assert state.error is not None and "attachment context" in state.error.cause + assert not controller.estop_active and state.enabled diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py index 879078b..c67dfea 100644 --- a/tests/integration/test_shapes_e2e.py +++ b/tests/integration/test_shapes_e2e.py @@ -27,6 +27,148 @@ 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) + with pytest.raises(MotionError, match="attachment context"): + client.home(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 test_attaching_a_part_is_refused_while_a_plan_is_in_flight( + client: RobotClient, server_proc +): + """A move accepted an instant ago may still be in the planner: attaching a + part waits for that plan to come back, not only for a queued segment.""" + from waldoctl import Sphere + + start = client.angles() + assert start is not None + target = list(start) + target[0] -= 20 + world = client.shapes() + assert world is not None + part = Sphere(name="part", radius=0.01).attach( + flange_pose=(0.0, 0.0, 0.25, 0.0, 0.0, 0.0), epoch=world.attachment_epoch + ) + try: + index = client.move_j(target, duration=1.0, wait=False) + assert index >= 0 + with pytest.raises(MotionError, match="stop motion"): + client.set_shapes([part]) + assert client.wait_command(index, timeout=10.0) + assert client.set_shapes([part]) == 1 + 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_tcp_transform.py b/tests/integration/test_tcp_transform.py index 3764c64..4321a12 100644 --- a/tests/integration/test_tcp_transform.py +++ b/tests/integration/test_tcp_transform.py @@ -61,10 +61,12 @@ async def test_full_tcp_transform_agrees_across_wire_fk_preview_and_motion(ports preview = DryRunRobotClient(initial_joints_deg=angles) preview.set_tcp_transform(*values) - predicted = preview.move_l([0, 0, 5, 0, 0, 0], frame="TRF", rel=True, speed=0.2) - assert predicted is not None and predicted.error is None + planned = preview.move_l([0, 0, 5, 0, 0, 0], frame="TRF", rel=True, speed=0.2) + assert preview.wait_command(planned) target = expected @ Pose((0, 0, 5, 0, 0, 0)).matrix() - predicted_pose = predicted.tcp_poses[-1] + record = preview.plan() + block = record.blocks[planned] + predicted_pose = record.tcp[block.start_row + block.rows - 1].astype(float) predicted_matrix = Pose( tuple([*(predicted_pose[:3] * 1000), *np.degrees(predicted_pose[3:])]) ).matrix() 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/integration/test_wait_command.py b/tests/integration/test_wait_command.py new file mode 100644 index 0000000..1cf2d12 --- /dev/null +++ b/tests/integration/test_wait_command.py @@ -0,0 +1,20 @@ +"""wait_command is paced by the status stream and still completes without it.""" + +import pytest + +from parol6 import RobotClient, config as cfg +from tests.conftest import free_udp_port + +pytestmark = pytest.mark.integration + + +def test_wait_command_completes_without_status_frames(ports, server_proc, monkeypatch): + monkeypatch.setattr(cfg, "MCAST_PORT", free_udp_port()) + with RobotClient(host=ports.server_ip, port=ports.server_port, timeout=5.0) as deaf: + assert not deaf.wait_status(lambda s: True, timeout=0.5), "frames still arrive" + start = deaf.angles() + assert start is not None + target = list(start) + target[0] += 5 + assert deaf.move_j(target, duration=0.5, wait=True, timeout=5.0) >= 0 + assert deaf.move_j(start, duration=0.5, wait=True, timeout=5.0) >= 0 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..207df3b --- /dev/null +++ b/tests/unit/test_attachment_gate.py @@ -0,0 +1,24 @@ +"""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 + ) + # The set lands in the process-wide collision world; leaving the part on + # the flange would put every later test's arm in collision at home. + try: + state.set_shapes([part]) + assert state.has_attachments and state.attachments_valid + finally: + state.set_shapes([]) diff --git a/tests/unit/test_attachment_wire.py b/tests/unit/test_attachment_wire.py new file mode 100644 index 0000000..7dd4906 --- /dev/null +++ b/tests/unit/test_attachment_wire.py @@ -0,0 +1,52 @@ +"""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)) + # The reply carries the request id it answers. + reply = [MsgType.RESPONSE, 7, [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..c265ded --- /dev/null +++ b/tests/unit/test_command_completion_wire.py @@ -0,0 +1,76 @@ +"""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, + pack_response, +) +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(pack_response(command.compute(state), 7)).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]]) + ) diff --git a/tests/unit/test_dry_run_blend.py b/tests/unit/test_dry_run_blend.py index 5de568c..19c0998 100644 --- a/tests/unit/test_dry_run_blend.py +++ b/tests/unit/test_dry_run_blend.py @@ -22,83 +22,56 @@ def client(): class TestDryRunBlend: - """Tests for blend buffering in DryRunRobotClient.""" - - def test_blend_produces_composite(self, client): - """3x move_j with r > 0 should buffer, then flush returns a single composite.""" - r1 = client.move_j(angles=W1, speed=0.5, r=10) - assert r1 is None, "r > 0 should buffer, not return immediately" - - r2 = client.move_j(angles=W2, speed=0.5, r=10) - assert r2 is None, "r > 0 should buffer" - - # r=0 terminates the chain → flush returns composite result - r3 = client.move_j(angles=W3, speed=0.5, r=0) - assert r3 is not None, "r=0 after buffered commands should flush and return" - assert r3.tcp_poses.shape[0] > 0 - assert r3.tcp_poses.shape[1] == 6 - assert r3.error is None + """Blend buffering: a chain lands under its head command.""" + + def test_blend_chain_lands_under_its_head(self, client): + """3x move_j with r > 0 buffer; the head block owns the chain's rows + and the folded commands keep their place at zero rows.""" + first = client.move_j(angles=W1, speed=0.5, r=10) + second = client.move_j(angles=W2, speed=0.5, r=10) + third = client.move_j(angles=W3, speed=0.5, r=0) + assert (first, second, third) == (0, 1, 2) + record = client.plan() + assert record.blocks[first].rows > 0 + assert record.blocks[second].rows == 0 and record.blocks[third].rows == 0 + assert record.tcp.shape == (record.rows, 6) + assert all(b.error is None for b in record.blocks) + np.testing.assert_allclose(np.degrees(record.joints_rad[-1]), W3, atol=0.5) def test_no_blend_without_radius(self, client): - """move_j with r=0 should return immediately (no buffering).""" - result = client.move_j(angles=W1, speed=0.5, r=0) - assert result is not None, "r=0 should return immediately" - assert result.tcp_poses.shape[0] > 0 - assert result.error is None - - def test_flush_returns_buffered(self, client): - """Explicit flush() after buffered commands should return results list.""" - r1 = client.move_j(angles=W1, speed=0.5, r=10) - assert r1 is None - - r2 = client.move_j(angles=W2, speed=0.5, r=10) - assert r2 is None + index = client.move_j(angles=W1, speed=0.5, r=0) + block = client.plan().blocks[index] + assert block.rows > 0 and block.error is None - results = client.flush() - assert len(results) > 0, "flush() should return buffered results" - assert results[0].tcp_poses.shape[0] > 0 - assert results[0].error is None + def test_flush_plans_the_pending_chain(self, client): + client.move_j(angles=W1, speed=0.5, r=10) + client.move_j(angles=W2, speed=0.5, r=10) + assert client.program_length == 2 + client.flush() + assert client.plan().blocks[0].rows > 0 - def test_flush_empty_returns_empty_list(self, client): - """flush() with no buffered commands should return empty list.""" - assert client.flush() == [] + def test_empty_program_is_an_empty_record(self, client): + assert client.flush() is None + record = client.plan() + assert record.rows == 0 and record.blocks == () - def test_blended_trajectory_is_longer(self, client): - """Composite blended trajectory should have longer duration than a single move.""" + def test_blended_chain_is_longer_than_one_move(self, client): single = DryRunRobotClient() - single_result = single.move_j(angles=W3, speed=0.3, r=0) - assert single_result is not None - - # Blended chain of 3 moves + single.move_j(angles=W3, speed=0.3, r=0) client.move_j(angles=W1, speed=0.3, r=10) client.move_j(angles=W2, speed=0.3, r=10) - r3 = client.move_j(angles=W3, speed=0.3, r=0) - assert r3 is not None - - assert r3.duration > single_result.duration, ( - f"Blended ({r3.duration:.3f}s) should be longer than single ({single_result.duration:.3f}s)" - ) + client.move_j(angles=W3, speed=0.3, r=0) + assert client.plan().duration_s > single.plan().duration_s def test_state_updated_after_blend(self, client): - """Position should reflect the final waypoint after a blended chain.""" client.move_j(angles=W1, speed=0.5, r=10) client.move_j(angles=W2, speed=0.5, r=0) + np.testing.assert_allclose(client.angles(), W2, atol=0.5) - angles_after = client.angles() - assert len(angles_after) == 6 - np.testing.assert_allclose(angles_after, W2, atol=0.5) - - def test_execution_override_preserves_path_and_pause(self): - normal = DryRunRobotClient(initial_joints_deg=W0) + def test_pause_holds_the_program_until_resume(self): slow = DryRunRobotClient(initial_joints_deg=W0) - normal_result = normal.move_j(W1, duration=2) assert slow.set_execution_speed(0.5) == 1 - slow_result = slow.move_j(W1, duration=2) - assert normal_result is not None and slow_result is not None - assert slow_result.duration == pytest.approx(normal_result.duration * 2) - np.testing.assert_allclose( - slow_result.joint_trajectory_rad, normal_result.joint_trajectory_rad - ) + assert slow.move_j(W1, duration=2) >= 0 assert slow.pause() == 1 assert slow.set_execution_speed(0.3) == 1 assert slow.execution_speed().paused @@ -108,7 +81,7 @@ def test_execution_override_preserves_path_and_pause(self): np.testing.assert_allclose(slow.angles(), W1, atol=0.05) assert slow.resume() == 1 assert slow.execution_speed().applied_scale == 0.3 - assert slow.move_j(W2, duration=2).error is None + assert slow.wait_command(slow.move_j(W2, duration=2)) for invalid in (0, True, 2, float("nan")): with pytest.raises(ValueError): slow.set_execution_speed(invalid) diff --git a/tests/unit/test_dry_run_record.py b/tests/unit/test_dry_run_record.py new file mode 100644 index 0000000..44ace1e --- /dev/null +++ b/tests/unit/test_dry_run_record.py @@ -0,0 +1,126 @@ +"""The commanded record a parol6 dry run returns: one block per command on +one row axis, with delays, tool travel and refusals all on it.""" + +import math + +import numpy as np +import pytest +from waldoctl import following_error + +from parol6.client.dry_run_client import _STRIDE, DryRunRobotClient +from parol6.config import INTERVAL_S +from parol6.tools import get_registry + +HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] +W1 = [80.0, -80.0, 190.0, 10.0, 10.0, 190.0] +W2 = [70.0, -70.0, 200.0, 20.0, 20.0, 200.0] + + +def _rows_for(seconds: float) -> int: + """Rows a hold of *seconds* occupies from the program's first tick.""" + return math.ceil(round(seconds / INTERVAL_S) / _STRIDE) + + +def _span(record, block): + return slice(block.start_row, block.start_row + block.rows) + + +def test_delay_holds_the_pose_for_its_rows(): + client = DryRunRobotClient(initial_joints_deg=HOME) + index = client.delay(2.0) + record = client.plan() + block = record.blocks[index] + assert block.command == index and block.rows == _rows_for(2.0) == 100 + held = record.joints_rad[_span(record, block)] + # Motor-step quantisation moves the pose by well under a hundredth of a + # degree; what matters is that every row holds the same pose. + np.testing.assert_allclose( + np.degrees(held), np.broadcast_to(HOME, held.shape), atol=0.01 + ) + assert np.ptp(held, axis=0).max() == 0 + assert block.error is None and block.move_type is None + assert record.duration_s == pytest.approx(2.0, abs=record.row_dt_s) + for bad in (0, -1.0, float("nan"), float("inf")): + with pytest.raises(ValueError): + client.delay(bad) + + +def test_gripper_close_ramps_the_jaws_over_the_tools_travel(): + client = DryRunRobotClient(initial_joints_deg=HOME) + assert client.select_tool("SSG-48") == 1 + index = client.tool.close() + record = client.plan() + block = record.blocks[index] + expected = get_registry().get("SSG-48").estimate_duration("close", []) + assert expected > 0 + assert block.rows == pytest.approx(_rows_for(expected), abs=1) + closed = record.tool_closed[_span(record, block)] + assert closed[0] == pytest.approx(0.0, abs=0.05) + assert np.all(np.diff(closed) >= 0) and closed[-1] > 0.9 + # The arm holds still while the jaws move. + assert np.ptp(record.joints_rad[_span(record, block)], axis=0).max() == 0 + # Once the action is over the jaws are closed: the row grid may skip the + # ramp's final tick, but everything after it holds the closed state. + after = client.delay(0.1) + record = client.plan() + assert np.all(record.tool_closed[_span(record, record.blocks[after])] == 1.0) + + +def test_a_refused_move_keeps_its_place_and_its_error(): + client = DryRunRobotClient(initial_joints_deg=[0.0] * 6, initial_homed=False) + first = client.move_j(W1, speed=0.5) + assert first == 0 and client.wait_command(first) is False + record = client.plan() + assert record.stop == "failed" + assert "not homed" in str(record.blocks[first].error) + assert record.blocks[first].rows == 0 + # A later command still lands after it, in order. + homed = client.home() + assert client.wait_command(homed) + later = client.move_j(W1, speed=0.5) + assert client.wait_command(later) + record = client.plan() + assert [b.command for b in record.blocks] == [0, 1, 2] + assert record.blocks[later].rows > 1 and record.blocks[later].move_type == "joints" + + +def test_simulate_is_the_plan_on_a_planner_only_backend(): + client = DryRunRobotClient(initial_joints_deg=HOME) + client.move_j(W1, speed=0.5) + client.delay(0.5) + client.move_j(W2, speed=0.5) + plan = client.plan() + predicted = client.simulate() + assert predicted.digest == plan.digest and predicted.rows == plan.rows + assert not following_error(plan, predicted).any() + assert [b.move_type for b in plan.blocks] == ["joints", None, "joints"] + assert sum(b.rows for b in plan.blocks) == plan.rows + assert plan.blocks[2].start_row == plan.blocks[1].start_row + plan.blocks[1].rows + + +def test_execution_speed_stretches_the_rows_not_the_path(): + normal = DryRunRobotClient(initial_joints_deg=HOME) + slow = DryRunRobotClient(initial_joints_deg=HOME) + n = normal.move_j(W1, duration=2) + assert slow.set_execution_speed(0.5) == 1 + s = slow.move_j(W1, duration=2) + nb = normal.plan().blocks[n] + sb = slow.plan().blocks[s] + assert sb.rows == pytest.approx(2 * nb.rows, abs=1) + np.testing.assert_allclose( + slow.plan().joints_rad[sb.start_row + sb.rows - 1], + normal.plan().joints_rad[nb.start_row + nb.rows - 1], + atol=1e-5, + ) + + +def test_budget_truncates_the_record_and_says_so(): + client = DryRunRobotClient(initial_joints_deg=HOME) + client.delay(2.0) + client.move_j(W1, speed=0.5) + full = client.plan() + cut = client.plan(max_seconds=1.0) + assert cut.stop == "budget_exhausted" + assert cut.rows == _rows_for(1.0) < full.rows + assert cut.blocks[0].rows == cut.rows and cut.blocks[1].rows == 0 + assert full.stop == "completed" diff --git a/tests/unit/test_dry_run_script_compat.py b/tests/unit/test_dry_run_script_compat.py index 0a81fe5..4872e84 100644 --- a/tests/unit/test_dry_run_script_compat.py +++ b/tests/unit/test_dry_run_script_compat.py @@ -13,7 +13,8 @@ import pytest from waldoctl import CommandKind, command_table -from parol6.client.dry_run_client import _CMD_STRUCTS, DryRunRobotClient +from parol6.client.dry_run_client import _CMD_STRUCTS, _STRIDE, DryRunRobotClient +from parol6.config import INTERVAL_S HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] POSE_A = [0.0, 280.0, 200.0, 90.0, 0.0, 90.0] @@ -23,6 +24,12 @@ ANGLES_B = [70.0, -70.0, 200.0, 20.0, 20.0, 200.0] +def _rows_for(seconds: float) -> int: + import math + + return math.ceil(round(seconds / INTERVAL_S) / _STRIDE) + + @pytest.fixture def client(): return DryRunRobotClient() @@ -31,73 +38,63 @@ def client(): class TestDryRunScriptCompat: """Every call signature a user can write in a script must work in dry run.""" + def _planned(self, client, index): + assert isinstance(index, int) and index >= 0 + assert client.wait_command(index) + return client.plan().blocks[index] + def test_home(self, client): - result = client.home() - assert result is not None - assert result.error is None + self._planned(client, client.home()) def test_move_j_positional(self, client): - result = client.move_j(ANGLES_A, speed=0.5) - assert result is not None - assert result.error is None + self._planned(client, client.move_j(ANGLES_A, speed=0.5)) def test_move_j_angles_kwarg(self, client): - result = client.move_j(angles=ANGLES_A, speed=0.5) - assert result is not None - assert result.error is None + self._planned(client, client.move_j(angles=ANGLES_A, speed=0.5)) def test_move_j_with_accel(self, client): - result = client.move_j(ANGLES_A, speed=0.5, accel=0.8) - assert result is not None - assert result.error is None + self._planned(client, client.move_j(ANGLES_A, speed=0.5, accel=0.8)) def test_move_j_with_duration(self, client): - result = client.move_j(ANGLES_A, duration=2.0) - assert result is not None - assert result.error is None + self._planned(client, client.move_j(ANGLES_A, duration=2.0)) def test_move_j_relative(self, client): - result = client.move_j(ANGLES_A, speed=0.5, rel=True) - assert result is not None + assert client.move_j(ANGLES_A, speed=0.5, rel=True) >= 0 def test_move_l_positional(self, client): - result = client.move_l(POSE_A, speed=0.5) - assert result is not None - assert result.error is None + self._planned(client, client.move_l(POSE_A, speed=0.5)) def test_move_l_with_frame(self, client): - result = client.move_l(POSE_A, speed=0.5, frame="WRF") - assert result is not None + assert client.move_l(POSE_A, speed=0.5, frame="WRF") >= 0 def test_move_c(self, client): client.move_l(POSE_A, speed=0.5) - result = client.move_c(via=POSE_B, end=POSE_A, speed=0.5) - assert result is not None + assert client.move_c(via=POSE_B, end=POSE_A, speed=0.5) >= 0 def test_move_s(self, client): client.move_l(POSE_A, speed=0.5) waypoints = [POSE_A, POSE_B, POSE_C, POSE_A] - result = client.move_s(waypoints=waypoints, speed=0.5) - assert result is not None + assert client.move_s(waypoints=waypoints, speed=0.5) >= 0 def test_move_p(self, client): client.move_l(POSE_A, speed=0.5) waypoints = [POSE_A, POSE_B, POSE_C, POSE_A] - result = client.move_p(waypoints=waypoints, speed=0.5) - assert result is not None + assert client.move_p(waypoints=waypoints, speed=0.5) >= 0 def test_move_j_blend_radius(self, client): - """Blend radius queues commands; r=0 flushes.""" + """A blend radius buffers the move; the index comes back at once and + the rows land under the chain's head once r=0 closes it.""" r1 = client.move_j(ANGLES_A, speed=0.5, r=10) - assert r1 is None # buffered r2 = client.move_j(ANGLES_B, speed=0.5, r=0) - assert r2 is not None # flushed + assert (r1, r2) == (0, 1) + record = client.plan() + assert record.blocks[r1].rows > 0 and record.blocks[r2].rows == 0 def test_move_l_blend_radius(self, client): r1 = client.move_l(POSE_A, speed=0.5, r=15) - assert r1 is None r2 = client.move_l(POSE_B, speed=0.5, r=0) - assert r2 is not None + assert (r1, r2) == (0, 1) + assert client.plan().blocks[r1].rows > 0 def test_angles(self, client): angles = client.angles() @@ -110,16 +107,15 @@ def test_pose(self, client): assert len(pose) == 6 def test_flush(self, client): - results = client.flush() - assert isinstance(results, list) + assert client.flush() is None def test_delay(self, client): - # Should be a no-op, not raise - client.delay(1.0) + index = client.delay(1.0) + assert client.plan().blocks[index].rows == _rows_for(1.0) def test_wait_motion(self, client): client.move_j(ANGLES_A, speed=0.5) - client.wait_motion() + assert client.wait_motion() is True class TestDryRunHomedGate: @@ -132,51 +128,55 @@ class TestDryRunHomedGate: def test_unhomed_seed_gates_planned_moves_until_home(self): client = DryRunRobotClient(initial_joints_deg=[0.0] * 6, initial_homed=False) - result = client.move_j(ANGLES_A, speed=0.5) - assert result is not None and result.error is not None - assert "not homed" in str(result.error) + refused = client.move_j(ANGLES_A, speed=0.5) + assert client.wait_command(refused) is False + assert "not homed" in str(client.plan().blocks[refused].error) # home() snaps to the home pose and establishes references — # the first move after it must NOT error. - assert client.home().error is None - result = client.move_j(ANGLES_A, speed=0.5) - assert result is not None and result.error is None + assert client.wait_command(client.home()) + assert client.wait_command(client.move_j(ANGLES_A, speed=0.5)) def test_homed_seed_plans_immediately(self): client = DryRunRobotClient(initial_joints_deg=HOME, initial_homed=True) - result = client.move_j(ANGLES_A, speed=0.5) - assert result is not None and result.error is None + assert client.wait_command(client.move_j(ANGLES_A, speed=0.5)) def test_referenced_home_previews_as_return_move(self): - import numpy as np - client = DryRunRobotClient(initial_joints_deg=ANGLES_A, initial_homed=True) - result = client.home() - assert result is not None and result.error is None - assert result.duration > 0.0 - assert len(result.joint_trajectory_rad) > 1 - assert np.allclose(np.degrees(result.end_joints_rad), HOME, atol=0.5) + index = client.home() + record = client.plan() + block = record.blocks[index] + assert block.error is None and block.rows > 1 + assert np.allclose( + np.degrees(record.joints_rad[block.start_row + block.rows - 1]), + HOME, + atol=0.5, + ) # Unreferenced seed keeps the instant snap — the switch-seek can't - # be previewed from unreferenced positions. + # be previewed from unreferenced positions — as one row at home. client = DryRunRobotClient(initial_joints_deg=[0.0] * 6, initial_homed=False) - result = client.home() - assert result is not None and result.error is None - assert result.duration == 0.0 + index = client.home() + record = client.plan() + block = record.blocks[index] + assert block.error is None and block.rows == 1 + assert np.allclose( + np.degrees(record.joints_rad[block.start_row]), HOME, atol=0.5 + ) def test_snap_carries_the_pending_blend_chain(self): """A blended move still buffered when the script homes with calibrate - (or teleports) is planned and leads the returned result — the live - controller runs it before the snap, so the preview must show it.""" + (or teleports) is planned under its own command before the snap — + the live controller runs it before the snap, so the preview must + show it.""" client = DryRunRobotClient(initial_joints_deg=HOME, initial_homed=True) - assert client.move_j(ANGLES_A, speed=0.5, r=10) is None # buffered - - result = client.home(calibrate=True) - assert result is not None and result.error is None - assert result.duration > 0.0 - assert len(result.joint_trajectory_rad) > 1 - assert np.allclose(np.degrees(result.end_joints_rad), HOME, atol=0.5) - assert client.flush() == [] + chain = client.move_j(ANGLES_A, speed=0.5, r=10) + snap = client.home(calibrate=True) + record = client.plan() + assert record.blocks[chain].rows > 1 and record.blocks[chain].error is None + assert record.blocks[snap].rows == 1 + assert np.allclose(np.degrees(record.joints_rad[-1]), HOME, atol=0.5) + assert client.flush() is None def test_jogs_take_the_live_clients_arguments(client): @@ -184,17 +184,19 @@ def test_jogs_take_the_live_clients_arguments(client): forms the docs show; the preview must plan them, not the wire struct's field order.""" before = np.asarray(client.angles()) - result = client.jog_j(0, 0.5, 1.0) - assert result is not None and result.error is None - after = np.degrees(result.end_joints_rad) + assert client.jog_j(0, 0.5, 1.0) == 1 + record = client.plan() + block = record.blocks[client.program_length - 1] + after = np.degrees(record.joints_rad[block.start_row + block.rows - 1]) assert after[0] > before[0] + 1.0 assert np.allclose(after[1:], before[1:], atol=1e-6) client = DryRunRobotClient() x_before = client.pose()[0] - result = client.jog_l("WRF", "X", 0.5, 1.0) - assert result is not None and result.error is None - assert result.tcp_poses[-1][0] * 1000.0 > x_before + 1.0 + assert client.jog_l("WRF", "X", 0.5, 1.0) == 1 + record = client.plan() + block = record.blocks[client.program_length - 1] + assert record.tcp[block.start_row + block.rows - 1][0] * 1000.0 > x_before + 1.0 assert client.pose()[0] > x_before + 1.0 with pytest.raises(ValueError, match="joint="): client.jog_j(speed=0.5) diff --git a/tests/unit/test_reset_enable_reaches_firmware.py b/tests/unit/test_reset_enable_reaches_firmware.py index 973f272..ceb212b 100644 --- a/tests/unit/test_reset_enable_reaches_firmware.py +++ b/tests/unit/test_reset_enable_reaches_firmware.py @@ -13,21 +13,6 @@ import pytest from parol6.protocol.wire import CommandCode, EstopCmd, ResetCmd, encode_command -from parol6.server.controller import Controller, ControllerConfig - - -@pytest.fixture -def controller(): - ctl = Controller(ControllerConfig(udp_host="127.0.0.1", udp_port=0)) - try: - yield ctl - finally: - if ctl.udp_transport is not None: - ctl.udp_transport.close_socket() - if ctl._status_broadcaster is not None: - ctl._status_broadcaster.close() - ctl._transport_mgr.disconnect() - ctl.state_manager.reset_state() def test_reset_enable_reaches_firmware_write(controller):