diff --git a/README.md b/README.md index 3969fed..034966b 100644 --- a/README.md +++ b/README.md @@ -301,12 +301,15 @@ Note: RUCKIG is point-to-point only and cannot follow Cartesian paths. When RUCK ### Speed and acceleration ```python -client.moveJ(target, speed=0.5, accel=0.5) # 50% of joint limits -client.moveL(target, speed=0.25, accel=1.0) # 25% cart speed, full accel -client.moveL(target, duration=2.0) # Fixed duration (uses TOPPRA) +client.move_j(target) # default speed=0.5, accel=0.5 +client.move_l(target, speed=0.25, accel=1.0) # 25% cart speed, full accel +client.move_l(target, duration=2.0) # Fixed duration; speed is unused ``` -Speed and accel are fractions of maximum (0.0–1.0), not percentages. +Speed and accel are fractions of maximum in (0, 1], not percentages. A +`duration` > 0 times the move and `speed` is ignored; otherwise `speed` times +it. `servo_j`/`servo_l` default to `speed=0.5, accel=0.5`, and jogs to +`accel=0.5`. For Cartesian moves, joint limits stay at 100% as hard bounds—the speed fraction only affects the Cartesian velocity constraint. @@ -330,17 +333,22 @@ 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. +Completion waits query the requested command's exact success. A jog, a servo +stream or a `tool.stop()` can finish ahead of commands queued before it, so the +highest completed index alone cannot prove that an earlier command finished. +The controller retains its latest 1024 +successful completions and 1024 failures. A wait on a command that was +discarded — by `stop()`, an E-stop, `reset_state()`, a teleport or a failure +queued ahead of it — raises `MotionError` (`MOTN_CANCELLED`) at once, and one +on a command the pipeline failed raises `MotionError` with that failure. An +unknown 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 +completion is still unconfirmed when it runs out. Blocking motion calls raise +`TimeoutError` in that case. A timed-out wait leaves the motion queued; +`stop()` cancels it. Planning preview retimes trajectories and reports paused queued operations as `UnresolvedPreview` instead of claiming completion. @@ -360,7 +368,7 @@ checks; no continuous recorded-trajectory command is added. ## Command system -Jog and servo commands (JogJ, JogL, ServoJ, ServoL) automatically use the streaming fast-path — the server de-duplicates stale inputs, reduces ACK chatter, and reuses the active command. Use jog/servo for UI-driven motion or teleoperation; use planned moves (MoveJ, MoveL, etc.) for discrete motions and queued programs. +Jog and servo commands (JogJ, JogL, ServoJ, ServoL) automatically use the streaming fast-path — the server de-duplicates stale inputs, reduces ACK chatter, and reuses the active command. A servo stream stops about 0.25 s after its last target arrives: the arm brakes to rest and holds. Use jog/servo for UI-driven motion or teleoperation; use planned moves (MoveJ, MoveL, etc.) for discrete motions and queued programs. ### Command categories @@ -411,6 +419,32 @@ with RobotClient() as c: Add a new tool by creating a `ToolConfig` subclass (or using `ToolConfig` directly) and calling `register_tool("KEY", config)` in `parol6/tools.py`. +### Tool actions + +Tool actions (`tool.open()`, `close()`, `set_position()`, `calibrate()`, +`release()`, `tool_action(...)`) are queued work, in the same queue as planned +motion: + +- **They take their turn.** A tool action waits for the motion queued ahead of + it, the arm holds still while it runs, and the motion queued after it waits + for it. A tool action sent after a blended move ends the blend there. A wait + on a tool action therefore covers the motion queued ahead of it, and + `queued_duration` counts the tool's estimated travel. +- **A failure cancels what follows.** A `move` on a gripper never calibrated is + refused when its turn comes, under its own index; like any command that + fails, it cancels the commands queued behind it (`MOTN_CANCELLED`). +- **What discards the queue discards them.** A pause holds them. `stop()`, + `estop()`, `reset_state()`, a teleport, or a jog or servo stream taking the + arm fails the queued ones as cancelled and halts the one running where the + jaws are, keeping the grip. +- **`tool.stop()` is immediate.** It is not queued: it halts the tool action + running, failing it as cancelled by a tool stop, keeps everything queued + behind it, and holds the next queued tool action until the jaws are still. + +`release()` drops the grip without moving the jaws. A pneumatic action lasts +its estimated stroke; an electric `calibrate` lasts 200 control ticks (two +seconds at the default 100 Hz). + **Security note:** The controller has no authentication — it accepts any correctly parsed command on its UDP port. Multiple senders are supported by design (e.g., GUI + orchestrator), but deploy only on trusted networks. diff --git a/examples/async_client_quickstart.py b/examples/async_client_quickstart.py index 6bc7c0f..769acb0 100644 --- a/examples/async_client_quickstart.py +++ b/examples/async_client_quickstart.py @@ -26,6 +26,11 @@ async def run_client() -> int: ok = await client.simulator(True) print(f"simulator(True): {ok}") + # Planned motion is refused until the robot is referenced, and a + # freshly started one is not — however sensible its reported + # angles look. + await client.home(wait=True) + print("ping:", await client.ping()) pose_xyz = (await client.pose())[:3] print("pose xyz:", pose_xyz) @@ -36,9 +41,11 @@ async def run_client() -> int: print(status.speeds) break - # Small relative move (safe in simulator) - # Move +5mm in Z over 1.0s - moved = await client.move_l([0, 0, 5, 0, 0, 0], rel=True, duration=1.0) + # Small relative move (safe in simulator): +5mm in Z over 1.0s, + # waited on so a refusal raises instead of passing unnoticed + moved = await client.move_l( + [0, 0, 5, 0, 0, 0], rel=True, duration=1.0, wait=True + ) print("move_l ->", moved) return 0 diff --git a/examples/demo_showcase.py b/examples/demo_showcase.py index 31f5ce9..b70f6f7 100644 --- a/examples/demo_showcase.py +++ b/examples/demo_showcase.py @@ -10,17 +10,12 @@ rbt = RobotClient() HOME_ANGLES = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] -HOME_TOLERANCE_DEG = 2.0 -# Select tool, and home only if not already near the home pose +# Select the tool and home: an arm that is already homed returns to the +# home pose with a planned move, and one already there does not move. rbt.select_tool("SSG-48") rbt.tool.calibrate() -current = rbt.angles() -if ( - current is None - or max(abs(a - h) for a, h in zip(current, HOME_ANGLES)) > HOME_TOLERANCE_DEG -): - rbt.home() +rbt.home() # move_j vs move_l (joint-space then linear-cartesian to nearby pose) rbt.move_j(pose=[100, 340, 334, 90, 0, 90], speed=0.5) @@ -90,8 +85,11 @@ def circle_pt(cx, cz, angle_deg): is_last = row == ROWS - 1 y_start, y_end = (Y_MIN, Y_MAX) if row % 2 == 0 else (Y_MAX, Y_MIN) rbt.move_l([X, y_start, z] + ZZ_ORI, speed=1.0, r=BLEND, wait=False) - rbt.move_l([X, y_end, z] + ZZ_ORI, speed=1.0, r=0 if is_last else BLEND, wait=False) -rbt.wait_motion() + last = rbt.move_l( + [X, y_end, z] + ZZ_ORI, speed=1.0, r=0 if is_last else BLEND, wait=False + ) +# The whole blended scan runs as one path: wait for its last move. +rbt.wait_command(last, timeout=30) # ── Precision demo: pencil pick-up and TCP-offset rotations ────────── PRECISION_POSE = [0, -250, 350, -90, 0, -90] diff --git a/examples/draw_circle.py b/examples/draw_circle.py index e22eec5..f36ced4 100644 --- a/examples/draw_circle.py +++ b/examples/draw_circle.py @@ -22,7 +22,9 @@ SPEED = 0.4 CIRCLE_Y = 340 ORIENTATION = [90, 0, 90] -CENTERS = [(0, CIRCLE_Y, 280), (0, CIRCLE_Y, 210), (0, CIRCLE_Y, 140)] +# The top circle sits just below where following it would need the wrist +# to flip mid-arc, which a cartesian move refuses. +CENTERS = [(0, CIRCLE_Y, 270), (0, CIRCLE_Y, 210), (0, CIRCLE_Y, 140)] def circle_pt(cx, cz, angle_deg): @@ -46,7 +48,11 @@ def circle_pt(cx, cz, angle_deg): cx, _, cz = CENTERS[0] rbt.move_j(pose=circle_pt(cx, cz, 0), speed=0.5, wait=True) rbt.move_c( - via=circle_pt(cx, cz, 180), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True + via=circle_pt(cx, cz, 180), + end=circle_pt(cx, cz, 0), + speed=SPEED, + wait=True, + timeout=30, ) # Circle 2: two half-circle move_c arcs diff --git a/examples/precision.py b/examples/precision.py index 36c493d..001b227 100644 --- a/examples/precision.py +++ b/examples/precision.py @@ -10,20 +10,16 @@ rbt = RobotClient() HOME_ANGLES = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] -HOME_TOLERANCE_DEG = 2.0 -# Select tool, and home only if not already near the home pose +# Select the tool and home: an arm that is already homed returns to the +# home pose with a planned move, and one already there does not move. rbt.select_tool("SSG-48") -rbt.tool.calibrate() -current = rbt.angles() -if ( - current is None - or max(abs(a - h) for a, h in zip(current, HOME_ANGLES)) > HOME_TOLERANCE_DEG -): - rbt.home() +rbt.tool.calibrate(wait=True, timeout=30.0) +rbt.home(wait=True, timeout=30.0) PRECISION_POSE = [0, -250, 350, -90, 0, -90] -rbt.move_j(pose=PRECISION_POSE, speed=0.5) +# The first Cartesian target also warms the controller's planner. +rbt.move_j(pose=PRECISION_POSE, speed=0.5, timeout=30.0) # Test gripper: two quick close/open cycles rbt.tool.close(speed=1.0) diff --git a/examples/zigzag_scan.py b/examples/zigzag_scan.py index 37c0f6a..b0d90d0 100644 --- a/examples/zigzag_scan.py +++ b/examples/zigzag_scan.py @@ -39,9 +39,12 @@ is_last = row == ROWS - 1 y_start, y_end = (Y_MIN, Y_MAX) if row % 2 == 0 else (Y_MAX, Y_MIN) rbt.move_l([X, y_start, z] + ZZ_ORI, speed=0.5, r=BLEND, wait=False) - rbt.move_l( + last = rbt.move_l( [X, y_end, z] + ZZ_ORI, speed=0.5, r=0 if is_last else BLEND, wait=False ) - rbt.wait_motion() + # The blended scan runs as one path: wait for its last move. A move that + # fails anywhere in the scan fails the ones behind it too, and raises. + if not rbt.wait_command(last, timeout=60.0): + raise SystemExit("the scan did not finish") print("Done!") diff --git a/parol6/__init__.py b/parol6/__init__.py index e539744..9e84d58 100644 --- a/parol6/__init__.py +++ b/parol6/__init__.py @@ -15,7 +15,7 @@ from . import PAROL6_ROBOT __version__: str = _pkg_version("parol6") -from .client.async_client import AsyncRobotClient +from .client.async_client import AsyncRobotClient, StatusSnapshot from .client.dry_run_client import DryRunRobotClient from .client.sync_client import RobotClient from .protocol.wire import ( @@ -33,7 +33,7 @@ # Type aliases for backward compatibility CurrentActionResult = CurrentActionResultStruct LoopStatsResult = LoopStatsResultStruct -StatusResult = StatusResultStruct +StatusResult = StatusSnapshot ToolResult = ToolResultStruct __all__ = [ @@ -43,6 +43,7 @@ "DryRunRobotClient", "RobotClient", "PAROL6_ROBOT", + "StatusSnapshot", # Result types (msgspec structs) "CurrentActionResultStruct", "LoopStatsResultStruct", diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index fde0584..adacabc 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -15,6 +15,7 @@ CmdType.SET_STATUS_RATE, CmdType.SET_EXECUTION_SPEED, CmdType.PAUSE, + CmdType.TELEPORT, } # Query command types (use request/response, not ACK) @@ -50,7 +51,6 @@ CmdType.SERVOL, CmdType.JOGJ, CmdType.JOGL, - CmdType.TELEPORT, CmdType.RESET_LOOP_STATS, } diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 3ad438b..18cce13 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -5,6 +5,7 @@ import asyncio import contextlib import logging +from dataclasses import dataclass import math import random import socket @@ -26,7 +27,7 @@ StatusRate, ToolResult, ) -from waldoctl.tools import ToolSpec +from waldoctl.tools import ToolSpec, ToolState from waldoctl.execution import ExecutionSpeed, validate_execution_scale @@ -128,6 +129,7 @@ decode_message, encode_command, encode_command_into, + planned_move_timing, ) from waldoctl.types import Axis, Frame from waldoctl import PingResult @@ -135,6 +137,16 @@ logger = logging.getLogger(__name__) + +def _no_wait_kwargs(wait_kwargs: dict[str, Any]) -> None: + """Refuse keywords the wait does not take; it takes none beyond + ``timeout``.""" + if wait_kwargs: + raise TypeError( + f"unexpected keyword argument(s): {', '.join(sorted(wait_kwargs))}" + ) + + _AXIS_MAP: dict[str, int] = {"X": 0, "Y": 1, "Z": 2, "RX": 3, "RY": 4, "RZ": 5} _ACTION_STATE_MAP: dict[str, ActionState] = { "idle": ActionState.IDLE, @@ -274,6 +286,19 @@ def connection_lost(self, exc: Exception | None) -> None: pass +@dataclass(frozen=True, slots=True) +class StatusSnapshot: + """One ``status()`` reading: the TCP transform (flattened row-major 4×4, + translation in mm), joint angles (deg), joint velocities (deg/s), digital + I/O, and the fitted tool's status.""" + + pose: list[float] + angles: list[float] + speeds: list[float] + io: list[int] + tool_status: ToolStatus + + class AsyncRobotClient(_RobotClientABC): """ Async UDP client for the PAROL6 headless controller. @@ -342,8 +367,10 @@ def __init__( self._status_generation: int = 0 self._status_event: asyncio.Event = asyncio.Event() - # Last command index returned by server for queued commands + # Last command index returned by server for queued commands, and the + # status session it was acknowledged in (0 before any status). self._last_command_index: int | None = None + self._last_command_session: int = 0 self._active_tool_key: str | None = None self._active_variant_key: str = "" @@ -636,7 +663,9 @@ async def _send(self, cmd: msgspec.Struct, *, timeout: float | None = None) -> i ok = await self._request_ok_raw( encode_command(cmd, req_id), wait, req_id ) - self._last_command_index = ok.index + if ok.index is not None: + self._last_command_index = ok.index + self._last_command_session = self._shared_status.session_id return ok.index if ok.index is not None else 0 except TimeoutError: if timeout is not None: @@ -786,6 +815,7 @@ async def home( calibrate: If True, always run the referencing sequence timeout: Maximum time to wait in seconds (only used when wait=True) """ + _no_wait_kwargs(wait_kwargs) index = await self._send(HomeCmd(calibrate=calibrate)) assert isinstance(index, int) if wait and index >= 0: @@ -865,11 +895,25 @@ async def teleport( ) -> int: """Instantly set joint angles and optional tool positions (simulator only). + The pose is exact, so the arm counts as homed afterwards and + planned motion may follow. Whatever was driving the arm is + cancelled with ``MOTN_CANCELLED``. + Category: Control Example: - rbt.teleport([0, -90, 0, 0, 0, 0]) - rbt.teleport([0, -90, 0, 0, 0, 0], tool_positions=[1.0]) + rbt.teleport([90, -90, 180, 0, 0, 180]) + rbt.teleport([90, -90, 180, 0, 0, 180], tool_positions=[1.0]) + + Returns: + 1 once the pose is applied, 0 when no reply arrives. + + Raises: + ValueError: for a non-finite angle, one outside the hard joint + limits, or a tool position outside ``[0, 1]``. + MotionError: off the simulator (``SYS_NOT_SIMULATOR``), when the + tool positions do not match the fitted tool's DOF count, or + while the controller is disabled. """ return await self._send( TeleportCmd(angles=angles_deg, tool_positions=tool_positions) @@ -897,10 +941,13 @@ async def connect_hardware(self, port_str: str) -> int: return await self._send(ConnectHardwareCmd(port_str=port_str)) async def reset_state(self) -> int: - """Reset controller state to initial values. + """Reset program state — world shapes, tool selection, errors, pause, + motion profile and execution speed — and discard the queue, each + discarded command failing with ``MOTN_CANCELLED``. - Instantly resets positions to home, clears queues, resets tool/errors. - Preserves serial connection. Useful for fast test isolation. + The arm holds where it is. A protective stop stays latched (only + ``reset()`` clears it), and homed state, digital outputs and the + serial connection are kept. Category: Control @@ -958,7 +1005,8 @@ async def io(self, *, timeout: float | None = None) -> list[int] | None: return resp.io if isinstance(resp, IOResultStruct) else None async def joint_speeds(self) -> list[float] | None: - """Current joint speeds in steps/sec [J1, J2, J3, J4, J5, J6]. + """Current joint velocities in deg/s [J1, J2, J3, J4, J5, J6], the + units of ``StatusBuffer.speeds``. Category: Query @@ -1002,8 +1050,10 @@ async def pose(self, frame: Frame = "WRF") -> list[float] | None: except (ValueError, IndexError): return None - async def status(self) -> StatusResultStruct | None: + async def status(self) -> StatusSnapshot | None: """Aggregate status snapshot (pose, angles, speeds, io, tool_status). + Its ``tool_status`` is always a ``ToolStatus``, key ``"NONE"`` when + no tool is fitted. Category: Query @@ -1011,7 +1061,34 @@ async def status(self) -> StatusResultStruct | None: status = rbt.status() """ resp = await self._request(StatusCmd()) - return resp if isinstance(resp, StatusResultStruct) else None + if not isinstance(resp, StatusResultStruct): + return None + ( + key, + tool_state, + engaged, + part_detected, + fault_code, + positions, + channels, + variant, + ) = resp.tool_status + return StatusSnapshot( + pose=resp.pose, + angles=resp.angles, + speeds=resp.speeds, + io=resp.io, + tool_status=ToolStatus( + key=key, + state=ToolState(tool_state), + engaged=bool(engaged), + part_detected=bool(part_detected), + fault_code=int(fault_code), + positions=tuple(positions), + channels=tuple(channels), + variant_key=variant, + ), + ) async def loop_stats(self) -> LoopStatsResult | None: """Fetch control-loop runtime metrics. @@ -1172,11 +1249,12 @@ async def select_tool(self, tool_name: str, variant_key: str = "") -> int: Returns: Command index (>= 0) if queued, 0 on failure. """ - self._active_tool_key = tool_name.upper() - self._active_variant_key = variant_key - return await self._send( - SelectToolCmd(tool_name=self._active_tool_key, variant_key=variant_key) - ) + key = tool_name.upper() + index = await self._send(SelectToolCmd(tool_name=key, variant_key=variant_key)) + if index >= 0: + self._active_tool_key = key + self._active_variant_key = variant_key + return index async def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int: """Set TCP offset in mm, composed on top of the current tool transform. @@ -1307,7 +1385,10 @@ async def select_profile(self, profile: str) -> int: Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. Returns: - True if successful + 1 once the profile is selected, 0 when no reply arrives. + + Raises: + MotionError: when ``profile`` is not a profile name. """ return await self._send(SelectProfileCmd(profile=profile.upper())) @@ -1434,7 +1515,7 @@ async def is_estop_pressed(self) -> bool: return io_status[4] == 0 # E-stop at index 4, 0 means pressed return False - async def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: + async def is_robot_stopped(self, threshold_speed: float = 0.5) -> bool: """Check if robot has stopped moving. Category: Query @@ -1447,7 +1528,7 @@ async def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: stopped = rbt.is_robot_stopped() Args: - threshold_speed: Speed threshold in steps/sec + threshold_speed: Speed threshold in deg/s Returns: True if all joints below threshold @@ -1461,17 +1542,17 @@ async def wait_motion( self, timeout: float = 10.0, settle_window: float = 0.25, - speed_threshold: float = 0.01, + speed_threshold: float = 0.5, angle_threshold: float = 0.5, - motion_start_timeout: float = 1.0, **kwargs: Any, ) -> bool: - """Wait for robot to stop moving using multicast status broadcasts. + """Wait until the queue has run out and the arm has come to rest. - This method first waits for motion to START (speeds above threshold), - then waits for motion to COMPLETE (speeds below threshold for settle_window). - This avoids a race condition where the method returns immediately if - called before motion has begun. + First waits for the latest command the controller has accepted — + every command this client queued, and each newer one the status + stream reports accepted — to end, then for the arm to hold still + for ``settle_window``. A command that failed or was cancelled ends + the wait like one that completed; ``wait_command()`` tells which. Category: Synchronization @@ -1481,60 +1562,63 @@ async def wait_motion( Args: timeout: Maximum time to wait in seconds settle_window: How long robot must be stable to be considered stopped - speed_threshold: Max joint speed to be considered stopped (rad/s) + speed_threshold: Max joint speed to be considered stopped (deg/s) angle_threshold: Max angle change to be considered stopped (degrees) - motion_start_timeout: Max time to wait for motion to start (seconds) Returns: True if robot stopped, False if timeout + + Raises: + ConnectionError: If the controller session changes while a + command is still being waited on. """ await self._ensure_endpoint() + # An index acknowledged by a controller that has since restarted + # will never end on this one. + own = self._last_command_index + barrier = -1 + if own is not None and self._last_command_session in ( + 0, + self._shared_status.session_id, + ): + barrier = own + ended = -1 last_angles: np.ndarray | None = None settle_start: float | None = None - motion_started = False - start_time = time.monotonic() try: async with asyncio.timeout(timeout): async for status in self.stream_status_shared(): - speeds = status.speeds + barrier = max(barrier, status.accepted_index) + if ended < barrier: + if await self._command_outcome(barrier) is False: + return False + ended = barrier + last_angles = None + settle_start = None + continue + + max_speed = float(np.abs(status.speeds).max()) angles = status.angles - - max_speed = float(np.abs(speeds).max()) - - max_angle_change = 0.0 - if last_angles is not None: + if last_angles is None: + last_angles = angles.copy() + max_angle_change = 0.0 + else: max_angle_change = float(np.abs(angles - last_angles).max()) last_angles[:] = angles - else: - last_angles = angles.copy() + if ( + max_speed >= speed_threshold + or max_angle_change >= angle_threshold + ): + settle_start = None + continue now = time.monotonic() - - # Phase 1: Wait for motion to start - if not motion_started: - if ( - max_speed >= speed_threshold - or max_angle_change >= angle_threshold - ): - motion_started = True - settle_start = None - elif now - start_time > motion_start_timeout: - motion_started = True - - # Phase 2: Wait for motion to complete - if motion_started: - if ( - max_speed < speed_threshold - and max_angle_change < angle_threshold - ): - if settle_start is None: - settle_start = now - elif now - settle_start > settle_window: - return True - else: - settle_start = None + if settle_start is None: + settle_start = now + elif now - settle_start > settle_window: + return True except TimeoutError: return False @@ -1607,10 +1691,15 @@ 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. - 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. + Queries exact success in the controller's last 1024 completions: a + later command that finishes first — a jog, a ``tool.stop()`` — does + not complete an unfinished one queued before it. Queued commands, + tool actions among them, run in index order, so a wait on one covers + everything queued ahead of it. + Unknown or expired results are never inferred successful from the + status high-water mark. A command that ended as a failure — cancelled + by ``stop()``/``estop()`` (``MOTN_CANCELLED``) or failed by the + pipeline — raises MotionError. Args: command_index: The command index to wait for (returned by motion commands). @@ -1622,6 +1711,23 @@ async def wait_command(self, command_index: int, timeout: float = 10.0) -> bool: Raises: MotionError: If the pipeline errored at or before command_index. """ + try: + async with asyncio.timeout(timeout): + outcome = await self._command_outcome(command_index) + except TimeoutError: + return False + if isinstance(outcome, RobotError): + raise MotionError(outcome) + return outcome + + async def _command_outcome(self, command_index: int) -> RobotError | bool: + """Wait for *command_index* to end: True once it completed, the + error it failed with, or False if the client closed first. Runs + until it has an answer; the caller's deadline bounds it. + + Raises: + ConnectionError: If the controller session changes. + """ def _blocking_error(s: StatusBuffer) -> RobotError | None: # A standing error fails this wait only when the frame proves it @@ -1652,27 +1758,25 @@ def check_session(candidate: int) -> None: "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 + 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 + if result.error is not None: + return RobotError.from_wire(result.error) + err = _blocking_error(self._shared_status) + if err is not None: + return err + await self._await_completion_hint(command_index, 0.25) return False async def _await_completion_hint(self, command_index: int, timeout: float) -> None: @@ -1705,8 +1809,8 @@ async def move_j( *, pose: list[float] | None = None, duration: float = 0.0, - speed: float = 0.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, r: float = 0.0, rel: bool = False, wait: bool = False, @@ -1725,17 +1829,31 @@ async def move_j( Args: angles: 6 joint angles in degrees (ignored if pose= is set) pose: If set, Cartesian target [x,y,z,rx,ry,rz] — dispatches to MOVEJ_POSE - duration: Motion duration in seconds (mutually exclusive with speed) - speed: Speed fraction 0-1 (mutually exclusive with duration) - accel: Acceleration fraction 0-1 + duration: Motion duration in seconds; > 0 times the move and speed is unused + speed: Speed fraction in (0, 1], used when duration is 0 + accel: Acceleration fraction in (0, 1] r: Blend radius in mm (0 = stop at target) rel: If True, angles are relative to current position wait: If True, block until motion completes """ + _no_wait_kwargs(wait_kwargs) + duration, speed = planned_move_timing(duration, speed, accel) if pose is not None: + if rel: + # A pose target is absolute on the wire; planning it as + # though the offset had been honoured would send the arm + # to a world pose near the origin. + raise ValueError( + "move_j(pose=..., rel=True) is not supported: a pose target is " + "absolute. Use move_j(angles, rel=True) for a relative joint move." + ) index = await self._send( MoveJPoseCmd( - pose=pose, duration=duration, speed=speed, accel=accel, r=r + pose=pose, + duration=duration, + speed=speed, + accel=accel, + r=r, ) ) else: @@ -1759,8 +1877,8 @@ async def move_l( *, frame: Frame = "WRF", duration: float = 0.0, - speed: float = 0.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, r: float = 0.0, rel: bool = False, wait: bool = False, @@ -1779,13 +1897,15 @@ async def move_l( Args: pose: Target [x,y,z,rx,ry,rz] in mm and degrees frame: Reference frame ("WRF" or "TRF") - duration: Motion duration in seconds - speed: Speed fraction 0-1 - accel: Acceleration fraction 0-1 + duration: Motion duration in seconds; > 0 times the move and speed is unused + speed: Speed fraction in (0, 1], used when duration is 0 + accel: Acceleration fraction in (0, 1] r: Blend radius in mm rel: If True, pose is relative delta wait: If True, block until motion completes """ + _no_wait_kwargs(wait_kwargs) + duration, speed = planned_move_timing(duration, speed, accel) cmd = MoveLCmd( pose=pose, frame=frame, @@ -1806,9 +1926,9 @@ async def move_c( end: list[float], *, frame: Frame = "WRF", - duration: float | None = None, - speed: float | None = None, - accel: float = 1.0, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, r: float = 0.0, wait: bool = False, timeout: float = 10.0, @@ -1827,18 +1947,21 @@ async def move_c( via: Via-point pose [x,y,z,rx,ry,rz] end: End-point pose [x,y,z,rx,ry,rz] frame: Reference frame - duration: Motion duration in seconds - speed: Speed fraction 0-1 - accel: Acceleration fraction 0-1 + duration: Motion duration in seconds; > 0 times the move and speed is unused + speed: Speed fraction in (0, 1], used when duration is 0 + accel: Acceleration fraction in (0, 1] r: Blend radius in mm wait: If True, block until motion completes """ + _no_wait_kwargs(wait_kwargs) + duration, speed = planned_move_timing(duration, speed, accel) + # The arc and spline wires mark their unused timing field None, not 0. cmd = MoveCCmd( via=via, end=end, frame=frame, - duration=duration, - speed=speed, + duration=duration or None, + speed=speed or None, accel=accel, r=r, ) @@ -1852,9 +1975,9 @@ async def move_s( waypoints: list[list[float]], *, frame: Frame = "WRF", - duration: float | None = None, - speed: float | None = None, - accel: float = 1.0, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, wait: bool = False, timeout: float = 10.0, **wait_kwargs: Any, @@ -1871,16 +1994,18 @@ async def move_s( Args: waypoints: List of poses [[x,y,z,rx,ry,rz], ...] frame: Reference frame - duration: Motion duration in seconds - speed: Speed fraction 0-1 - accel: Acceleration fraction 0-1 + duration: Motion duration in seconds; > 0 times the move and speed is unused + speed: Speed fraction in (0, 1], used when duration is 0 + accel: Acceleration fraction in (0, 1] wait: If True, block until motion completes """ + _no_wait_kwargs(wait_kwargs) + duration, speed = planned_move_timing(duration, speed, accel) cmd = MoveSCmd( waypoints=waypoints, frame=frame, - duration=duration, - speed=speed, + duration=duration or None, + speed=speed or None, accel=accel, ) index = await self._send(cmd) @@ -1893,9 +2018,9 @@ async def move_p( waypoints: list[list[float]], *, frame: Frame = "WRF", - duration: float | None = None, - speed: float | None = None, - accel: float = 1.0, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, wait: bool = False, timeout: float = 10.0, **wait_kwargs: Any, @@ -1912,16 +2037,18 @@ async def move_p( Args: waypoints: List of poses [[x,y,z,rx,ry,rz], ...] frame: Reference frame - duration: Motion duration in seconds - speed: Speed fraction 0-1 - accel: Acceleration fraction 0-1 + duration: Motion duration in seconds; > 0 times the move and speed is unused + speed: Speed fraction in (0, 1], used when duration is 0 + accel: Acceleration fraction in (0, 1] wait: If True, block until motion completes """ + _no_wait_kwargs(wait_kwargs) + duration, speed = planned_move_timing(duration, speed, accel) cmd = MovePCmd( waypoints=waypoints, frame=frame, - duration=duration, - speed=speed, + duration=duration or None, + speed=speed or None, accel=accel, ) index = await self._send(cmd) @@ -1969,11 +2096,14 @@ async def servo_j( angles: list[float], *, pose: list[float] | None = None, - speed: float = 1.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, ) -> int: """Streaming joint position target. Fire-and-forget. + The stream stops about 0.25 s after the last target arrives: the arm + brakes to rest and holds there. + Category: Streaming Example: @@ -1993,11 +2123,14 @@ async def servo_l( self, pose: list[float], *, - speed: float = 1.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, ) -> int: """Streaming linear Cartesian position target. Fire-and-forget. + The stream stops about 0.25 s after the last target arrives: the tool + brakes along its line to rest and holds there. + Category: Streaming Example: @@ -2020,7 +2153,7 @@ async def jog_j( *, joints: list[int] | None = None, speeds: list[float] | None = None, - accel: float = 1.0, + accel: float = 0.5, ) -> int: """Joint velocity jog. Single-joint or multi-joint. @@ -2061,7 +2194,7 @@ async def jog_l( *, axes: list[Axis] | None = None, speeds_list: list[float] | None = None, - accel: float = 1.0, + accel: float = 0.5, ) -> int: """Cartesian velocity jog. Single-axis or multi-axis. @@ -2082,9 +2215,11 @@ async def jog_l( speeds_list: List of signed speed fractions for multi-axis jog accel: Acceleration fraction 0-1 """ + if frame not in ("WRF", "TRF"): + raise ValueError(f"jog_l frame must be 'WRF' or 'TRF', got {frame!r}") vel = [0.0] * 6 if axes is not None and speeds_list is not None: - for a, s in zip(axes, speeds_list): + for a, s in zip(axes, speeds_list, strict=True): vel[_AXIS_MAP[a]] = s elif axis is not None: vel[_AXIS_MAP[axis]] = speed @@ -2150,12 +2285,39 @@ async def tool_action( action: str, params: list | None = None, *, - wait: bool = True, + wait: bool = False, timeout: float = 10.0, ) -> int: """Send a generic tool action command. - Returns the command index (>= 0) on success, -1 on failure. + Returns the command index (>= 0) on success, -1 on failure. The + action and its parameters are checked before anything is sent: a + malformed one raises ``ValueError``. A key naming a tool other than + the selected one is refused by the controller. + + A tool action is queued work: it runs in queue order with motion, + the arm holding still while it does, so a wait on it covers the + motion queued ahead of it, and one sent after a blended move ends + the blend there. A ``move`` before a completed ``calibrate`` is + refused when its turn comes, failing its own index; a tool action + that fails cancels what is queued behind it (``MOTN_CANCELLED``). + A pause holds the tool actions still queued. ``stop()``, + ``estop()``, ``reset_state()``, a teleport, or a jog or servo stream + taking the arm discards them and halts the one running where the + jaws are, keeping the grip. + + ``stop`` alone acts at once, on the tool fitted now: it halts the + action running, failing it as cancelled by a tool stop, keeps what + is queued behind it, and holds the next queued tool action until + the jaws are still. + + Electric grippers take ``move [position, speed, current]`` + (exactly three fractions in ``[0, 1]``; current spans the tool's + ``current_range``), ``calibrate``, ``stop`` (halt in place, keep + grip) and ``idle`` (release, the jaws left where they are); + ``set_position``/``open``/``close`` map onto ``move``. Pneumatic + grippers take ``open``, ``close``, and ``move``/``set_position + [position]``. Category: I/O diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 9ffba28..b0be8a6 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -17,41 +17,43 @@ from __future__ import annotations +import copy +import functools import hashlib import logging import math +from collections.abc import Coroutine from dataclasses import dataclass from typing import TYPE_CHECKING, Any +import msgspec import numpy as np +from waldoctl.discovery import iter_plugin_tool_specs from waldoctl.execution import ExecutionSpeed, validate_execution_scale from waldoctl.skills import UnresolvedPreview +from waldoctl.sync_tools import make_sync_tool from waldoctl.ticks import TickBlock, TickIndex +from waldoctl.tools import ComposedToolsSpec, ToolSpec, ToolState, ToolStatus import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from ..ack_policy import ARM_MOTION_CMD_TYPES +from ..ack_policy import ARM_MOTION_CMD_TYPES, FIRE_AND_FORGET from ..commands.base import MotionCommand -from ..commands.cartesian_commands import ( - JogLCommand, - _CART_ANG_JOG_MAX_RAD, - _CART_ANG_JOG_MIN_RAD, - _CART_LIN_JOG_MAX_MS, - _CART_LIN_JOG_MIN_MS, - _linmap_frac, -) +from ..commands.cartesian_commands import JogLCommand, jog_twist from ..commands.basic_commands import JogJCommand +from ..commands.system_commands import VALID_PROFILES from ..config import ( CONTROL_RATE_HZ, HOME_ANGLES_DEG, INTERVAL_S, + LIMITS, deg_to_steps, rad_to_steps, steps_to_rad, ) from ..motion.geometry import joint_path_to_tcp_poses +from ..motion.streaming_executors import cap_twist from ..utils.ik import solve_ik -from pinokin import se3_from_rpy, se3_rpy -import re as _re +from pinokin import se3_exp, se3_rpy, so3_exp from math import degrees, radians import parol6.protocol.wire as _wire @@ -65,6 +67,7 @@ WriteIOCmd, TeleportCmd, ToolActionCmd, + planned_move_timing, ) from ..server.command_registry import CommandRegistry from ..server.motion_planner import ( @@ -75,27 +78,25 @@ TrajectorySegment, ) from ..server.state import ControllerState, get_fkine_se3 -from ..utils.error_catalog import RobotError -from parol6.tools import ElectricGripperConfig, PneumaticGripperConfig, get_registry -from waldoctl.tools import ToolType +from ..utils.error_catalog import RobotError, make_error +from ..utils.error_codes import ErrorCode +from ..utils.errors import MotionError, TrajectoryPlanningError +from parol6.tools import ( + PneumaticGripperConfig, + get_registry, + tool_action_refusal, +) if TYPE_CHECKING: from parol6.robot import Robot -def _pascal_to_snake(name: str) -> str: - """Convert PascalCase to snake_case: MoveJPose → move_j_pose""" - s = _re.sub(r"([A-Z]+)([A-Z][a-z])", r"\1_\2", name) - s = _re.sub(r"([a-z0-9])([A-Z])", r"\1_\2", s) - return s.lower() - - # Auto-derive method_name → struct_class from wire module. # E.g. MoveJCmd → move_j, IsSimulatorCmd → is_simulator _CMD_STRUCTS: dict[str, type] = {} for _attr in dir(_wire): if _attr.endswith("Cmd") and isinstance(getattr(_wire, _attr), type): - _CMD_STRUCTS[_pascal_to_snake(_attr.removesuffix("Cmd"))] = getattr( + _CMD_STRUCTS[_wire.pascal_to_snake(_attr.removesuffix("Cmd"))] = getattr( _wire, _attr ) @@ -105,12 +106,26 @@ def _pascal_to_snake(name: str) -> str: _AXIS_INDEX: dict[str, int] = {"X": 0, "Y": 1, "Z": 2, "RX": 3, "RY": 4, "RZ": 5} +#: Planned moves, whose keywords are the struct's own plus the wait's: the +#: live client refuses anything else with a TypeError, and so does this. +_PLANNED_MOVES = frozenset( + {"home", "move_j", "move_j_pose", "move_l", "move_c", "move_s", "move_p"} +) +_WAIT_KEYWORDS = frozenset({"wait", "timeout"}) + + def build_cmd(name: str, *args: Any, **kwargs: Any) -> Any: """Build a command struct by method name.""" struct_cls = _CMD_STRUCTS.get(name) if struct_cls is None: raise ValueError(f"Unknown command: {name}") struct_fields: tuple[str, ...] = getattr(struct_cls, "__struct_fields__", ()) + if name in _PLANNED_MOVES: + unknown = sorted( + k for k in kwargs if k not in struct_fields and k not in _WAIT_KEYWORDS + ) + if unknown: + raise TypeError(f"unexpected keyword argument(s): {', '.join(unknown)}") filtered = {} for k, v in kwargs.items(): if v is None or k not in struct_fields: @@ -121,9 +136,65 @@ def build_cmd(name: str, *args: Any, **kwargs: Any) -> Any: return struct_cls(*args, **filtered) +def _decode_refusal(params: Any) -> RobotError | None: + """The refusal the controller answers *params* with when its decoder + rejects them, or None. msgspec checks a field's declared constraints — + a frame spelled exactly ``WRF`` or ``TRF``, six-value poses, ranges — + on decode only, so a struct that constructs can still be refused.""" + if isinstance(params, SetShapesCmd): + # The preview's copy carries the shapes themselves, not their wire form. + return None + try: + _wire.decode_command(_wire.split_request(_wire.encode_command(params))[1]) + except msgspec.ValidationError as e: + return make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=str(e)) + return None + + +@functools.cache +def _plugin_tool_specs() -> tuple[ToolSpec, ...]: + """The ``waldoctl.tools`` plugin specs, instantiated once per process: + entry points are static, and scanning them costs more than the rest of + a preview's setup.""" + return tuple(iter_plugin_tool_specs()) + + +def _drive(coro: Coroutine[Any, Any, Any]) -> Any: + """Run a tool verb's coroutine to completion. The preview answers every + call at once, so it finishes on its first step with no event loop, which + keeps it usable in a worker process and inside a host's running loop.""" + try: + coro.send(None) + except StopIteration as done: + return done.value + coro.close() + raise RuntimeError("a dry-run tool verb suspended; the preview never awaits") + + logger = logging.getLogger(__name__) +def _twist_pose( + start: np.ndarray, twist: np.ndarray, t: float, wrf: bool +) -> np.ndarray: + """The pose a TCP driven at `twist` for `t` seconds from `start` + reaches, the twist resolved against the tool as it stands at every + instant, as the live jog resolves it. In world axes when `wrf`: the TCP + runs a straight line while the tool turns about a fixed world axis + through it. In the tool's axes otherwise: the tool's own axes turn with + it, so a twist that both moves and turns the tool traces the screw + ``start · exp(t · twist)``.""" + out = np.eye(4, dtype=np.float64) + if wrf: + rot = np.empty((3, 3), dtype=np.float64) + so3_exp(twist[3:] * t, rot) + out[:3, :3] = rot @ start[:3, :3] + out[:3, 3] = start[:3, 3] + twist[:3] * t + return out + se3_exp(twist * t, out) + return start @ out + + #: 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 @@ -203,34 +274,6 @@ def _truncated(record: TickIndex, max_seconds: float) -> TickIndex: ) -class _DryRunTool: - """Tool proxy for dry-run. Routes actions through the planner.""" - - def __init__(self, client: DryRunRobotClient) -> None: - self._client = client - - @property - def key(self) -> str: - return self._client._active_tool_key - - @property - def tool_type(self) -> str: - spec = get_registry().get(self.key) - return ( - ToolType.GRIPPER - if isinstance(spec, (ElectricGripperConfig, PneumaticGripperConfig)) - else ToolType.NONE - ) - - def __getattr__(self, name: str) -> Any: - def method(*args: Any, **kwargs: Any) -> int: - return self._client.tool_action( - self._client._active_tool_key, name, list(args), **kwargs - ) - - return method - - class DryRunRobotClient: """Runs commands through the trajectory planner without UDP/serial. @@ -266,6 +309,7 @@ def __init__( self, initial_joints_deg: list[float] | None = None, initial_homed: bool = True, + initial_gripper_calibrated: bool = False, robot: Robot | None = None, ) -> None: self._robot = robot @@ -283,6 +327,10 @@ def __init__( register_plugin_tools() self._state = ControllerState() + # The controller refuses an electric gripper's jaw move until a + # calibrate, and status does not say whether one ran: the host seeds + # what it knows, and False (as after power-on) previews that refusal. + self._state.gripper_calibrated = bool(initial_gripper_calibrated) init_deg = np.asarray( initial_joints_deg if initial_joints_deg is not None else HOME_ANGLES_DEG, dtype=np.float64, @@ -301,7 +349,18 @@ def __init__( self._rpy_buf = np.zeros(3, dtype=np.float64) self._active_tool_key: str = "NONE" self._active_variant_key: str = "" - self._tool_proxy = _DryRunTool(self) + # The robot's own tool specs, bound to this preview as the live + # client binds them: every verb and property is the live tool's, and + # only the action it sends lands here. Typed Any: the sync wrapper + # turns each async verb into a plain call. + from parol6.robot import _build_tools + + self._tools: dict[str, Any] = {} + for spec in ComposedToolsSpec(_build_tools(), _plugin_tool_specs()).available: + bound: Any = copy.copy(spec) + bound._execute = self._tool_execute + bound._get_status = self._tool_status + self._tools[spec.key] = make_sync_tool(bound, _drive) # The commanded record: one chunk per program command, filled as the # planner answers. Rows are recorded at submit time, under the tool @@ -317,9 +376,34 @@ def state(self) -> ControllerState: return self._state @property - def tool(self) -> _DryRunTool: - """Tool proxy that routes actions through the planner.""" - return self._tool_proxy + def tool(self) -> Any: + """The selected tool, as the live client hands it out: its verbs + send their actions through the planner.""" + return self._tools[self._active_tool_key] + + async def _tool_execute( + self, tool_key: str, action: str, params: list[Any], **kwargs: Any + ) -> int: + return self.tool_action(tool_key, action, params, **kwargs) + + async def _tool_status(self) -> ToolStatus: + """The previewed tool state: the tool fitted, and its jaws where the + program last sent them.""" + return ToolStatus( + key=self._state.current_tool, + variant_key=self._state.current_tool_variant, + state=ToolState.IDLE, + positions=(self._tool_position,) * self._tool_dof(), + ) + + def _tool_dof(self) -> int: + """How many positions status reports for the fitted tool, as the + controller counts them.""" + reported = ToolStatus() + cfg = get_registry().get(self._state.current_tool) + if cfg is not None: + cfg.populate_status(self._state, reported) + return len(reported.positions) @property def program_length(self) -> int: @@ -394,23 +478,32 @@ def _stretched(self, q_rad: np.ndarray) -> np.ndarray: ) return q_rad[at] - def _tool_target(self, action: str, params: list) -> float: - if action == "open": + def _tool_target(self, tool_key: str, action: str, params: list) -> float: + # ``idle`` drops the grip without moving the jaws. + if action in ("open", "calibrate"): 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])))) + position = float(params[0]) + # A valve is open or shut: it closes at half a position or more. + if isinstance( + get_registry().get(tool_key.strip().upper()), PneumaticGripperConfig + ): + return 0.0 if position < 0.5 else 1.0 + return min(1.0, max(0.0, position)) 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()) + def _fill_tool_action(self, idx: int, cmd: ToolActionCmd, seconds: float) -> None: + """A tool action holds the arm for *seconds*, the tool's estimated + travel as the planner hands it to the live queue too, while the jaws + ramp to their target.""" + action = cmd.action.strip().lower() 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) + target = self._tool_target(cmd.tool_key, action, params) + if action == "calibrate": + self._state.gripper_calibrated = True q = np.repeat(self._current_q()[np.newaxis], ticks, axis=0) closed = ( np.linspace(self._tool_position, target, ticks, dtype=np.float64) @@ -443,7 +536,7 @@ def _absorb(self, segments: list[Segment]) -> None: elif isinstance(seg, InlineSegment) and isinstance( seg.params, ToolActionCmd ): - self._fill_tool_action(seg.command_index, seg.params) + self._fill_tool_action(seg.command_index, seg.params, seg.duration) # Any other InlineSegment (select_tool, checkpoint, write_io …) # plans nothing: its chunk keeps its place at zero ticks. @@ -540,21 +633,60 @@ 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, 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()) + the homed gate. The snap lands as one row at the new pose, so the + record shows where the arm is once it is there.""" 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) self._hold(idx, _STRIDE) + def _cancel_pending(self, scope: str) -> None: + """Discard the blend chain the planner still holds, failing each of + its commands with ``MOTN_CANCELLED`` as the controller fails what a + cancel discards, so a wait on one reads the cancel.""" + for index, _ in self._planner._blend_buffer: + self._fill( + index, + np.empty((0, 6)), + error=make_error(ErrorCode.MOTN_CANCELLED, index, scope=scope), + ) + self._planner.cancel() + + def _teleport_refusal(self, params: TeleportCmd) -> RobotError | None: + """Why the controller would refuse this teleport, or None. Whether + the live controller runs the simulator, as a teleport also needs, is + not the preview's to know.""" + if params.tool_positions is not None: + dof = self._tool_dof() + if len(params.tool_positions) != dof: + return make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail=( + f"tool_positions has {len(params.tool_positions)} entries; " + f"the fitted tool {self._state.current_tool} reports {dof} " + "positions" + ), + ) + if not self._state.enabled: + return make_error( + ErrorCode.SYS_CONTROLLER_DISABLED, + detail=self._state.disabled_reason or "Controller disabled", + ) + return None + 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 + refusal = _decode_refusal(params) + if refusal is not None: + idx = self._open(method) + self._fill(idx, np.empty((0, 6)), error=refusal) + if _wire.STRUCT_TO_CMDTYPE.get(type(params)) in FIRE_AND_FORGET: + # Live, a streamed datagram's refusal reaches nobody. + return idx + raise MotionError(refusal) cmd_cls = self._registry.get_command_for_struct(type(params)) if ( cmd_cls is not None @@ -568,21 +700,75 @@ def _dispatch(self, params: Any, method: str) -> int: ): raise ValueError("attachment context changed; reconcile and reapply") idx = self._open(method) + if isinstance(params, _wire.StopCmd): + # A stop discards the blends still buffered and lifts a pause, + # as the controller's does. + self._cancel_pending("stop") + self._state.execution_paused = False + return idx + if isinstance(params, ToolActionCmd): + stop = params.action.strip().lower() == "stop" + if not stop: + # A tool action takes its turn in the queue with the arm at + # rest: a pending blend chain ends before it, and it holds + # the pose the chain ends at. + self.flush() + refusal = tool_action_refusal( + params.tool_key, + params.action, + current_tool=self._state.current_tool, + gripper_calibrated=self._state.gripper_calibrated, + ) + if refusal is not None: + self._fill( + idx, + np.empty((0, 6)), + error=make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=refusal), + ) + return idx + if stop: + # Live, a stop acts at once, beside the queue: it takes no + # turn in it and leaves a blend chain whole. + self._fill(idx, np.empty((0, 6))) + return idx 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() + self._cancel_pending("estop") return idx if isinstance(params, _wire.ResetStateCmd): - self._planner.cancel() + self._cancel_pending("reset_state") self._state.reset() self._planner.state.Position_in[:] = self._state.Position_in self._planner.state.Homed_in[:] = self._state.Homed_in + self._planner.state.motion_profile = self._state.motion_profile + return idx + if isinstance(params, _wire.SelectProfileCmd): + profile = params.profile.upper() + if profile not in VALID_PROFILES: + self._fill( + idx, + np.empty((0, 6)), + error=make_error( + ErrorCode.SYS_PROFILE_INVALID, detail=params.profile + ), + ) + return idx + # The moves after it are planned with it, as the controller + # hands the planner the profile it selects. + self._state.motion_profile = profile + self._planner.state.motion_profile = profile return idx if isinstance(params, (_wire.SimulatorCmd, _wire.ConnectHardwareCmd)): self._state.invalidate_attachments() - self._planner.cancel() + self._cancel_pending( + "a simulator toggle" + if isinstance(params, _wire.SimulatorCmd) + else "a hardware connect" + ) + # The gripper on the new transport has not been calibrated. + self._state.gripper_calibrated = False self._state.Homed_in.fill(0) self._planner.state.Homed_in.fill(0) return idx @@ -591,11 +777,24 @@ def _dispatch(self, params: Any, method: str) -> int: if isinstance(params, HomeCmd): if params.calibrate or not self._planner.state.Homed_in[:6].all(): self._state.invalidate_attachments() + # A home is queued: the controller runs a pending blend chain + # before it, each move under its own command. + self._absorb(self._planner.flush()) 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): + refusal = self._teleport_refusal(params) + if refusal is not None: + self._fill(idx, np.empty((0, 6)), error=refusal) + raise MotionError(refusal) + # The pose jumps: whatever was driving the arm is void, and a + # pause held a queue that is gone. + self._cancel_pending("a teleport") + self._state.execution_paused = False + if params.tool_positions: + self._tool_position = float(params.tool_positions[0]) self._snap_to_angles(idx, params.angles) return idx if isinstance(params, (SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd)): @@ -621,7 +820,11 @@ def _dispatch(self, params: Any, method: str) -> int: self.flush() cmd = cmd_cls(params) assert isinstance(cmd, MotionCommand) - path = self._simulate_jog(cmd) + try: + path = self._simulate_jog(cmd) + except TrajectoryPlanningError as refused: + self._fill(idx, np.empty((0, 6)), error=refused.robot_error) + return idx if path is not None: self._fill(idx, path) self._planner.state.Position_in[:] = self._state.Position_in @@ -640,7 +843,7 @@ def _failed(self, idx: int) -> bool: 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 + # Run setup so the jog's gates apply and speeds_out is computed cmd.setup(self._state) if isinstance(cmd, JogJCommand): @@ -654,9 +857,8 @@ def _simulate_joint_jog(self, cmd: JogJCommand) -> np.ndarray: duration = cmd.p.duration n_points = max(1, int(round(duration * CONTROL_RATE_HZ))) - # Compute total displacement (steps/tick * ticks_in_duration) - ticks = duration * CONTROL_RATE_HZ - displacements = cmd.speeds_out.astype(np.int64) * int(ticks) + # The jog speeds are steps per second, held for the duration. + displacements = np.round(cmd.speeds_out * duration).astype(np.int64) start_pos = self._state.Position_in.copy() fracs = np.arange(1, n_points + 1, dtype=np.float64) / n_points @@ -665,70 +867,46 @@ def _simulate_joint_jog(self, cmd: JogJCommand) -> np.ndarray: fracs[:, np.newaxis] * displacements[np.newaxis, :] ).astype(np.int64) - self._state.Position_in[:] = start_pos + displacements - radians = np.empty((n_points, 6), dtype=np.float64) for i in range(n_points): steps_to_rad(trajectory[i], radians[i]) + # A joint jogged into its limit stops there, as on the arm; one + # already past it is held where it is. + start_rad = np.empty(6, dtype=np.float64) + steps_to_rad(start_pos, start_rad) + lo = np.minimum(LIMITS.joint.position.rad[:, 0], start_rad) + hi = np.maximum(LIMITS.joint.position.rad[:, 1], start_rad) + np.clip(radians, lo, hi, out=radians) + end_steps = np.empty(6, dtype=np.int32) + rad_to_steps(radians[-1], end_steps) + self._state.Position_in[:] = end_steps return radians def _simulate_cartesian_jog(self, cmd: JogLCommand) -> np.ndarray: - """Simulate cartesian jog by displacing along a Cartesian axis and solving IK.""" + """Simulate a cartesian jog by integrating its TCP twist over the + duration and solving IK along the way.""" duration = cmd.p.duration n_points = max(1, int(round(duration * CONTROL_RATE_HZ))) - current_se3 = get_fkine_se3(self._state) - se3_rpy(current_se3, self._rpy_buf) - # pose = [x_m, y_m, z_m, rx_rad, ry_rad, rz_rad] - pose = np.array( - [ - current_se3[0, 3], - current_se3[1, 3], - current_se3[2, 3], - self._rpy_buf[0], - self._rpy_buf[1], - self._rpy_buf[2], - ], - dtype=np.float64, + start_se3 = get_fkine_se3(self._state).copy() + twist = np.zeros(6, dtype=np.float64) + jog_twist(cmd.p.velocities, twist) + # The ceilings the live executor holds the tool to, at the full + # velocity scale a jog runs at. + cap_twist( + twist, LIMITS.cart.jog.velocity.linear, LIMITS.cart.jog.velocity.angular ) - - # Compute velocity along dominant axis using same mapping as production - vels = cmd.p.velocities - speed_mag = abs(vels[cmd._axis_index + (3 if cmd.is_rotation else 0)]) - if cmd.is_rotation: - vel = _linmap_frac(speed_mag, _CART_ANG_JOG_MIN_RAD, _CART_ANG_JOG_MAX_RAD) - total_disp = vel * cmd._axis_sign * duration - else: - vel = _linmap_frac(speed_mag, _CART_LIN_JOG_MIN_MS, _CART_LIN_JOG_MAX_MS) - total_disp = vel * cmd._axis_sign * duration - - # Determine which component of the 6-element pose to displace - pose_index = (3 + cmd._axis_index) if cmd.is_rotation else cmd._axis_index + wrf = cmd.p.frame == "WRF" # Get current joint angles for IK seed steps_to_rad(self._state.Position_in, self._q_rad_buf) - q_current = self._q_rad_buf.copy() + last_valid_q = self._q_rad_buf.copy() steps_buf = np.zeros_like(self._state.Position_in) - # Generate trajectory by interpolating and solving IK at each point radians = np.empty((n_points, 6), dtype=np.float64) - last_valid_q = q_current.copy() - target_se3 = np.zeros((4, 4), dtype=np.float64) - for i in range(n_points): - t = (i + 1) / n_points - target_pose = pose.copy() - target_pose[pose_index] += total_disp * t - - se3_from_rpy( - target_pose[0], - target_pose[1], - target_pose[2], - target_pose[3], - target_pose[4], - target_pose[5], - target_se3, - ) + t = duration * (i + 1) / n_points + target_se3 = _twist_pose(start_se3, twist, t, wrf) ik_result = solve_ik( PAROL6_ROBOT.robot, target_se3, last_valid_q, quiet_logging=True ) @@ -788,33 +966,143 @@ def move_j( angles: list[float] | None = None, *, pose: list[float] | None = None, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, **kwargs: Any, ) -> int: + duration, speed = planned_move_timing(duration, speed, accel) if pose is not None: - return self._dispatch(build_cmd("move_j_pose", pose, **kwargs), "move_j") - return self._dispatch(build_cmd("move_j", angles or [], **kwargs), "move_j") + if kwargs.pop("rel", False): + raise ValueError( + "move_j(pose=..., rel=True) is not supported: a pose target is " + "absolute. Use move_j(angles, rel=True) for a relative joint move." + ) + cmd = build_cmd( + "move_j_pose", + pose, + duration=duration, + speed=speed, + accel=accel, + **kwargs, + ) + return self._dispatch(cmd, "move_j") + cmd = build_cmd( + "move_j", + angles or [], + duration=duration, + speed=speed, + accel=accel, + **kwargs, + ) + return self._dispatch(cmd, "move_j") - def move_l(self, pose: list[float], **kwargs: Any) -> int: - return self._dispatch(build_cmd("move_l", pose, **kwargs), "move_l") + def move_l( + self, + pose: list[float], + *, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, + **kwargs: Any, + ) -> int: + duration, speed = planned_move_timing(duration, speed, accel) + cmd = build_cmd( + "move_l", pose, duration=duration, speed=speed, accel=accel, **kwargs + ) + return self._dispatch(cmd, "move_l") + + def move_c( + self, + via: list[float], + end: list[float], + *, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, + **kwargs: Any, + ) -> int: + return self._path_move("move_c", [via, end], duration, speed, accel, kwargs) + + def move_s( + self, + waypoints: list[list[float]], + *, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, + **kwargs: Any, + ) -> int: + return self._path_move("move_s", [waypoints], duration, speed, accel, kwargs) + + def move_p( + self, + waypoints: list[list[float]], + *, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, + **kwargs: Any, + ) -> int: + return self._path_move("move_p", [waypoints], duration, speed, accel, kwargs) + + def _path_move( + self, + name: str, + args: list[Any], + duration: float, + speed: float, + accel: float, + kwargs: dict[str, Any], + ) -> int: + duration, speed = planned_move_timing(duration, speed, accel) + # The arc and spline wires mark their unused timing field None, not 0. + cmd = build_cmd( + name, + *args, + duration=duration or None, + speed=speed or None, + accel=accel, + **kwargs, + ) + return self._dispatch(cmd, name) def servo_j( self, - angles: list[float] | None = None, + angles: list[float], *, pose: list[float] | None = None, - **kwargs: Any, + speed: float = 0.5, + accel: float = 0.5, ) -> int: if pose is not None: - idx = self._dispatch(build_cmd("servo_j_pose", pose, **kwargs), "servo_j") + cmd: Any = _wire.ServoJPoseCmd(pose=pose, speed=speed, accel=accel) else: - idx = self._dispatch( - build_cmd("servo_j", angles or [], **kwargs), "servo_j" - ) + cmd = _wire.ServoJCmd(angles=angles, speed=speed, accel=accel) + idx = self._dispatch(cmd, "servo_j") + return -1 if self._failed(idx) else 1 + + def servo_l( + self, + pose: list[float], + *, + speed: float = 0.5, + accel: float = 0.5, + ) -> int: + idx = self._dispatch( + _wire.ServoLCmd(pose=pose, speed=speed, accel=accel), "servo_l" + ) return -1 if self._failed(idx) else 1 def checkpoint(self, label: str) -> int: return self._dispatch(build_cmd("checkpoint", label), "checkpoint") + def select_tool(self, tool_name: str, variant_key: str = "") -> int: + return self._dispatch( + SelectToolCmd(tool_name=tool_name.upper(), variant_key=variant_key), + "select_tool", + ) + 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") @@ -824,6 +1112,22 @@ def write_io(self, index: int, value: int, *, timeout: float | None = None) -> i WriteIOCmd(port_index=index + 2, value=int(value)), "write_io" ) + def tool_action( + self, + tool_key: str, + action: str, + params: list | None = None, + *, + wait: bool = False, + timeout: float = 10.0, + ) -> int: + cmd = ToolActionCmd( + tool_key=tool_key.strip().upper(), + action=action.strip().lower(), + params=params or [], + ) + return self._dispatch(cmd, "tool_action") + 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.""" @@ -885,7 +1189,7 @@ def jog_j( *, joints: list[int] | None = None, speeds: list[float] | None = None, - accel: float = 1.0, + accel: float = 0.5, ) -> int: """The live client's signature, so a script's jog previews as written.""" speed_arr = [0.0] * 6 @@ -910,11 +1214,13 @@ def jog_l( *, axes: list[str] | None = None, speeds_list: list[float] | None = None, - accel: float = 1.0, + accel: float = 0.5, ) -> int: + if frame not in ("WRF", "TRF"): + raise ValueError(f"jog_l frame must be 'WRF' or 'TRF', got {frame!r}") vel = [0.0] * 6 if axes is not None and speeds_list is not None: - for a, s in zip(axes, speeds_list): + for a, s in zip(axes, speeds_list, strict=True): vel[_AXIS_INDEX[a]] = s elif axis is not None: vel[_AXIS_INDEX[axis]] = speed diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index fc153b9..23d20aa 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -26,10 +26,9 @@ from ..protocol.wire import ( EnablementResultStruct, StatusBuffer, - StatusResultStruct, ) from ..utils.error_catalog import RobotError -from .async_client import AsyncRobotClient +from .async_client import AsyncRobotClient, StatusSnapshot if TYPE_CHECKING: from parol6.robot import Robot @@ -276,7 +275,8 @@ def connect_hardware(self, port_str: str) -> int: return _run(self._inner.connect_hardware(port_str)) def reset_state(self) -> int: - """Reset controller state to initial values.""" + """Reset program state and discard the queue; the arm holds where it + is and a protective stop stays latched.""" return _run(self._inner.reset_state()) # ---------- status / queries ---------- @@ -305,10 +305,10 @@ def io(self, *, timeout: float | None = None) -> list[int] | None: return _run(self._inner.io(timeout=timeout)) def joint_speeds(self) -> list[float] | None: - """Current joint speeds in steps per second. + """Current joint velocities in deg/s. Returns: - List of 6 joint speeds [J1-J6] in steps/sec, or None on timeout. + List of 6 joint velocities [J1-J6] in deg/s, or None on timeout. """ return _run(self._inner.joint_speeds()) @@ -323,11 +323,12 @@ def pose(self, frame: str = "WRF") -> list[float] | None: """ return _run(self._inner.pose(frame=frame)) - def status(self) -> StatusResultStruct | None: + def status(self) -> StatusSnapshot | None: """Aggregate status snapshot. Returns: - StatusResultStruct with pose, angles, speeds, io, tool_status, or None on timeout. + StatusSnapshot with pose, angles, speeds, io and a ToolStatus + (key ``"NONE"`` when no tool is fitted), or None on timeout. """ return _run(self._inner.status()) @@ -430,7 +431,10 @@ def select_profile(self, profile: str) -> int: Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. Returns: - True if successful + 1 once the profile is selected, 0 when no reply arrives. + + Raises: + MotionError: when ``profile`` is not a profile name. """ return _run(self._inner.select_profile(profile)) @@ -496,7 +500,7 @@ def is_estop_pressed(self) -> bool: """ return _run(self._inner.is_estop_pressed()) - def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: + def is_robot_stopped(self, threshold_speed: float = 0.5) -> bool: """Check if robot has stopped moving. Prefer ``wait_command()`` for waiting on specific commands. @@ -504,7 +508,7 @@ def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: diagnostics or manual stopping logic. Args: - threshold_speed: Speed threshold in steps/sec. + threshold_speed: Speed threshold in deg/s. Returns: True if all joints below threshold. @@ -515,18 +519,18 @@ def wait_motion( self, timeout: float = 10.0, settle_window: float = 0.25, - speed_threshold: float = 0.01, + speed_threshold: float = 0.5, angle_threshold: float = 0.5, - motion_start_timeout: float = 1.0, ) -> bool: - """Wait for robot to stop moving. + """Wait until the queue has run out and the arm has come to rest: + the latest accepted command has ended (failed and cancelled ones + included) and the arm has held still for ``settle_window``. Args: timeout: Maximum time to wait in seconds. settle_window: How long robot must be stable. - speed_threshold: Max joint speed to be considered stopped (rad/s). + speed_threshold: Max joint speed to be considered stopped (deg/s). angle_threshold: Max angle change to be considered stopped. - motion_start_timeout: Max time to wait for motion to start. Returns: True if robot stopped, False if timeout. @@ -537,7 +541,6 @@ def wait_motion( settle_window=settle_window, speed_threshold=speed_threshold, angle_threshold=angle_threshold, - motion_start_timeout=motion_start_timeout, ) ) @@ -607,8 +610,8 @@ def move_j( *, pose: list[float] | None = None, duration: float = 0.0, - speed: float = 0.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, r: float = 0.0, rel: bool = False, wait: bool = True, @@ -622,6 +625,7 @@ def move_j( speed=speed, accel=accel, r=r, + rel=rel, wait=wait, timeout=timeout, ) @@ -645,8 +649,8 @@ def move_l( *, frame: Frame = "WRF", duration: float = 0.0, - speed: float = 0.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, r: float = 0.0, rel: bool = False, wait: bool = True, @@ -672,9 +676,9 @@ def move_c( end: list[float], *, frame: Frame = "WRF", - duration: float | None = None, - speed: float | None = None, - accel: float = 1.0, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, r: float = 0.0, wait: bool = True, timeout: float = 10.0, @@ -698,9 +702,9 @@ def move_s( waypoints: list[list[float]], *, frame: Frame = "WRF", - duration: float | None = None, - speed: float | None = None, - accel: float = 1.0, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, wait: bool = True, timeout: float = 10.0, ) -> int: @@ -721,9 +725,9 @@ def move_p( waypoints: list[list[float]], *, frame: Frame = "WRF", - duration: float | None = None, - speed: float | None = None, - accel: float = 1.0, + duration: float = 0.0, + speed: float = 0.5, + accel: float = 0.5, wait: bool = True, timeout: float = 10.0, ) -> int: @@ -763,8 +767,8 @@ def servo_j( angles: list[float] | None = None, *, pose: list[float] | None = None, - speed: float = 1.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, ) -> int: if pose is not None: return _run( @@ -778,8 +782,8 @@ def servo_l( self, pose: list[float], *, - speed: float = 1.0, - accel: float = 1.0, + speed: float = 0.5, + accel: float = 0.5, ) -> int: return _run(self._inner.servo_l(pose, speed=speed, accel=accel)) @@ -811,7 +815,7 @@ def jog_j( *, joints: list[int] | None = None, speeds: list[float] | None = None, - accel: float = 1.0, + accel: float = 0.5, ) -> int: if joints is not None and speeds is not None: return _run( @@ -854,7 +858,7 @@ def jog_l( *, axes: list[Axis] | None = None, speeds_list: list[float] | None = None, - accel: float = 1.0, + accel: float = 0.5, ) -> int: if axes is not None and speeds_list is not None: return _run( @@ -892,7 +896,7 @@ def tool_action( action: str, params: list | None = None, *, - wait: bool = True, + wait: bool = False, timeout: float = 10.0, ) -> int: return _run( diff --git a/parol6/commands/_collision_guard.py b/parol6/commands/_collision_guard.py index d6e0d27..87f7bf6 100644 --- a/parol6/commands/_collision_guard.py +++ b/parol6/commands/_collision_guard.py @@ -10,19 +10,103 @@ from __future__ import annotations +from typing import TYPE_CHECKING + import numpy as np +from numba import njit from numpy.typing import NDArray import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.config import COLLISION_PATH_SAMPLES -from parol6.utils.error_catalog import make_error +from parol6.config import COLLISION_JOG_LOOKAHEAD_S, COLLISION_PATH_SAMPLES +from parol6.utils.error_catalog import RobotError, make_error from parol6.utils.error_codes import ErrorCode from parol6.utils.errors import TrajectoryPlanningError +if TYPE_CHECKING: + from parol6.motion.trajectory import JointPath + from parol6.server.state import ControllerState + # Escape-check tolerance (m): min-distance drops within this count as "not # deeper" (absorbs signed-distance jitter). _ESCAPE_TOL = 1e-4 +_QLIM_ROWS: tuple[np.ndarray, np.ndarray] | None = None + + +def _qlim_rows() -> tuple[np.ndarray, np.ndarray]: + """Joint-limit rows, fetched once per process — ``robot.qlim`` allocates a + fresh matrix per access and this is consumed on the 100 Hz stream path.""" + global _QLIM_ROWS + if _QLIM_ROWS is None: + qlim = PAROL6_ROBOT.robot.qlim + if qlim is None: + _QLIM_ROWS = (np.full(6, -np.inf), np.full(6, np.inf)) + else: + _QLIM_ROWS = ( + np.ascontiguousarray(qlim[0], dtype=np.float64), + np.ascontiguousarray(qlim[1], dtype=np.float64), + ) + return _QLIM_ROWS + + +@njit(cache=True) +def _lookahead_jit( + q: np.ndarray, + qd: np.ndarray, + horizon: float, + lo: np.ndarray, + hi: np.ndarray, + target: np.ndarray, + toward_target: bool, + out: np.ndarray, +) -> None: + for j in range(q.shape[0]): + v = q[j] + qd[j] * horizon + if toward_target: + t = target[j] + if q[j] <= t < v or v < t <= q[j]: + v = t + if v < lo[j]: + v = lo[j] + elif v > hi[j]: + v = hi[j] + out[j] = v + + +def stream_lookahead( + q: np.ndarray, + qd: np.ndarray, + out: np.ndarray, + target: np.ndarray | None = None, +) -> np.ndarray: + """Where a stream at ``q`` moving at ``qd`` (rad/s) will be one collision + lookahead horizon on, written into ``out``: faster motion is checked + further ahead of contact. A stream tracking ``target`` stops there, so + no joint is projected past it. Clamped to the joint limits, so a pose at + a mechanical stop cannot trip the checker on travel the arm does not + have.""" + lo, hi = _qlim_rows() + if target is None: + _lookahead_jit(q, qd, COLLISION_JOG_LOOKAHEAD_S, lo, hi, q, False, out) + else: + _lookahead_jit(q, qd, COLLISION_JOG_LOOKAHEAD_S, lo, hi, target, True, out) + return out + + +def collision_stop(state: ControllerState, checker, q: np.ndarray) -> RobotError: + """The error a stream stopped short of a contact at ``q`` ends with. + The pairs are recorded as the standing collision the display draws. + Allocates: a stop path, never a clean tick.""" + pairs = tuple(PAROL6_ROBOT.display_pairs(checker.colliding_pairs(q))) + state.collision_pairs = pairs + state.collision_active = True + return make_error( + ErrorCode.SYS_SELF_COLLISION, + sample="1", + total="1", + pairs=_format_pairs(list(pairs)), + ) + def collision_blocked(checker, current_q, target_q) -> bool: """Whether streaming toward ``target_q`` must stop. @@ -131,3 +215,12 @@ def _raise(sample: int, pairs: list[tuple[str, str]]) -> None: sorted(new_pairs) if new_pairs else checker.colliding_pairs(pos[sample]) ) _raise(sample, pairs) + + +def guard_cartesian_path(joint_path: JointPath) -> None: + """``guard_joint_path`` over a cartesian move's joints, with the wrist + turn ahead of it checked on its own: the turn's rows are few and close + together, and sampling the whole path could step over all of them.""" + if joint_path.prefix: + guard_joint_path(joint_path.positions[: joint_path.prefix + 1]) + guard_joint_path(joint_path.positions) diff --git a/parol6/commands/base.py b/parol6/commands/base.py index 9982dd8..960207b 100644 --- a/parol6/commands/base.py +++ b/parol6/commands/base.py @@ -10,7 +10,7 @@ import numpy as np -from parol6.config import TRACE +from parol6.config import INTERVAL_S, TRACE from parol6.protocol.wire import CmdType, Command, CommandCode, QueryType, Response from parol6.server.state import ControllerState from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error @@ -20,15 +20,26 @@ logger = logging.getLogger(__name__) +def arm_homed(state: ControllerState) -> bool: + """Whether every arm joint holds its reference; a loop rather than a + slice, so a per-tick caller allocates no view.""" + for i in range(6): + if not state.Homed_in[i]: + return False + return True + + def guard_homed(state: ControllerState) -> None: - """Refuse planned motion while the robot is not homed. + """Refuse motion that works from the reported pose while the robot is + not homed. Reported joint positions are unreferenced until homing (the boot state is - all-zeros steps — outside J2/J3's limits), so building or collision-checking - a trajectory from them is meaningless. Called at the top of every planned - command's ``do_setup``, like ``guard_joint_path``. Jog/servo/home are - deliberately not gated: they don't plan a path from the reported pose, and - an unhomed arm may need to be jogged clear of an obstruction before homing. + all-zeros steps — outside J2/J3's limits), so a trajectory, an IK solve + or a collision check built from them is meaningless. Called at the top + of every planned command's ``do_setup``, like ``guard_joint_path``, and + of every cartesian or servo stream's. Only ``jog_j`` and ``home`` stay + open: neither needs the pose, and an unhomed arm may need to be jogged + clear of an obstruction before homing. """ for i in range(6): if not state.Homed_in[i]: @@ -106,6 +117,7 @@ class CommandBase(ABC, Generic[P]): "robot_error", "_t0", "_t_end", + "_ticks_left", "_q_rad_buf", "_steps_buf", ) @@ -117,6 +129,8 @@ def __init__(self, p: P) -> None: self.robot_error: RobotError | None = None self._t0: float | None = None self._t_end: float | None = None + # Control ticks left on the tick timer; -1 before it starts. + self._ticks_left = -1 # Pre-allocated buffers for zero-allocation unit conversions self._q_rad_buf: np.ndarray = np.zeros(6, dtype=np.float64) self._steps_buf: np.ndarray = np.zeros(6, dtype=np.int32) @@ -232,6 +246,21 @@ def timer_expired(self) -> bool: """Check if the timer has expired.""" return self._t_end is not None and time.perf_counter() >= self._t_end + def start_tick_timer(self, duration_s: float) -> None: + """Start a timer for ``duration_s`` counted in control ticks, one per + :meth:`tick_timer_expired`, as the motion it times advances: a loop + that drops periods stretches it in wall time instead of cutting the + motion short.""" + self._ticks_left = max(0, round(duration_s / INTERVAL_S)) + + def tick_timer_expired(self) -> bool: + """Whether the tick timer has run out; counts one tick off it, so it + is called once per tick.""" + if self._ticks_left > 0: + self._ticks_left -= 1 + return False + return self._ticks_left == 0 + def progress01(self, duration_s: float) -> float: """Get progress as a value between 0 and 1.""" if self._t0 is None: @@ -355,14 +384,13 @@ class SystemCommand(CommandBase[P]): and can execute even when the controller is disabled. Side-effect signaling: commands that need infrastructure changes (simulator toggle, - port switch, mock sync) set the corresponding attribute. The controller reads these + port switch) set the corresponding attribute. The controller reads these after tick() and orchestrates the actual change. """ - __slots__ = ("_switch_simulator", "_switch_port", "_sync_mock") + __slots__ = ("_switch_simulator", "_switch_port") def __init__(self, p: P) -> None: super().__init__(p) self._switch_simulator: bool | None = None self._switch_port: str | None = None - self._sync_mock: bool = False diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py index 073575c..dedb694 100644 --- a/parol6/commands/basic_commands.py +++ b/parol6/commands/basic_commands.py @@ -6,9 +6,9 @@ import logging from enum import Enum, auto import numpy as np +from numba import njit from parol6.config import ( - COLLISION_JOG_LOOKAHEAD_S, JOG_MIN_STEPS, LIMITS, rad_to_steps, @@ -25,44 +25,115 @@ from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState -from parol6.commands._collision_guard import collision_blocked +from parol6.commands._collision_guard import collision_blocked, stream_lookahead +from parol6.motion.streaming_executors import below_speed from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode from parol6.config import deg_to_steps -from parol6.server.transports.transport_factory import is_simulation_mode import parol6.PAROL6_ROBOT as PAROL6_ROBOT # noqa: N811 from .base import ( ExecutionStatusCode, MotionCommand, + SystemCommand, + arm_homed, ) logger = logging.getLogger(__name__) -_QLIM_ROWS: tuple[np.ndarray, np.ndarray] | None = None - - -def _qlim_rows() -> tuple[np.ndarray, np.ndarray]: - """Joint-limit rows, fetched once per process — ``robot.qlim`` allocates a - fresh matrix per access and this is consumed on the 100 Hz jog path.""" - global _QLIM_ROWS - if _QLIM_ROWS is None: - qlim = PAROL6_ROBOT.robot.qlim - if qlim is None: - _QLIM_ROWS = (np.full(6, -np.inf), np.full(6, np.inf)) - else: - _QLIM_ROWS = ( - np.ascontiguousarray(qlim[0], dtype=np.float64), - np.ascontiguousarray(qlim[1], dtype=np.float64), +# A jog stops this far short of a joint's limit, on top of its own +# stopping distance, so a joint that overshoots its ramp by a hair never +# touches the stop. +_JOG_LIMIT_MARGIN_RAD: float = 0.005 +# The jerk-limited stopping distance is over-estimated by this factor. The +# limit itself is held by StreamingExecutor.hold_inside, whatever the +# lookahead predicted; this only decides how far short the ramp starts. +_JOG_STOP_MARGIN: float = 1.05 +# The measured position trails the commanded one by this many ticks +# (write, firmware, read back); the lookahead counts that travel too. +_JOG_LAG_TICKS: float = 2.0 +# Joint travel the jog stops short of, read once and contiguous: the +# per-tick kernels take them as they are. +_POS_LO_RAD = np.ascontiguousarray(LIMITS.joint.position.rad[:, 0]) +_POS_HI_RAD = np.ascontiguousarray(LIMITS.joint.position.rad[:, 1]) +_ACCEL_MAX = np.ascontiguousarray(LIMITS.joint.hard.acceleration, dtype=np.float64) +_JERK_MAX = np.ascontiguousarray(LIMITS.joint.hard.jerk, dtype=np.float64) + + +@njit(cache=True) +def _jog_lookahead_jit( + jog_vel: np.ndarray, + stopping: bool, + vel: np.ndarray, + acc: np.ndarray, + q_meas: np.ndarray, + lo: np.ndarray, + hi: np.ndarray, + accel_max: np.ndarray, + jerk_max: np.ndarray, + accel_frac: float, + dt: float, + blocked: np.ndarray, + target_vel: np.ndarray, +) -> int: + """Fill the target velocities, latching the direction of any joint + whose stopping distance reaches its limit and zeroing a joint driven + into its latch. Returns bit 0 set while any joint is still driven, and + bit ``j + 1`` for each joint latched this tick. + + The stopping distance is the jerk-limited ramp's: from the speed and + acceleration the executor is at, the acceleration first reverses at the + jerk limit, peaking the speed, then the ramp runs at the acceleration + limit and rounds off at the jerk limit again. The remaining travel is + measured: the arm is what approaches the limit, not the integrator. + """ + status = 0 + for j in range(target_vel.shape[0]): + v_t = 0.0 if stopping else jog_vel[j] + v = vel[j] + probe = v if v != 0.0 else v_t + if probe != 0.0: + if probe > 0.0: + remaining = hi[j] - q_meas[j] + sgn = 1 + else: + remaining = q_meas[j] - lo[j] + sgn = -1 + a = accel_max[j] * accel_frac + jk = jerk_max[j] + speed = abs(v) + a0 = acc[j] * sgn + if a0 < 0.0: + a0 = 0.0 + v_peak = speed + a0 * a0 / (2.0 * jk) + stop = ( + speed * a0 / jk + + a0 * a0 * a0 / (3.0 * jk * jk) + + v_peak * v_peak / (2.0 * a) + + v_peak * a / (2.0 * jk) ) - return _QLIM_ROWS - - -def _limit_hit_mask(pos_steps: np.ndarray, speeds: np.ndarray) -> np.ndarray: - return ((speeds > 0) & (pos_steps >= LIMITS.joint.position.steps[:, 1])) | ( - (speeds < 0) & (pos_steps <= LIMITS.joint.position.steps[:, 0]) - ) + stop = _JOG_STOP_MARGIN * stop + _JOG_LAG_TICKS * speed * dt + if stop + _JOG_LIMIT_MARGIN_RAD >= remaining and blocked[j] != sgn: + blocked[j] = sgn + status |= 1 << (j + 1) + if v_t != 0.0 and blocked[j] == (1 if v_t > 0.0 else -1): + v_t = 0.0 + target_vel[j] = v_t + if v_t != 0.0: + status |= 1 + return status + + +@njit(cache=True) +def _track_rates_jit( + vel: np.ndarray, vel_prev: np.ndarray, acc_prev: np.ndarray, dt: float +) -> None: + """The executor's speed and acceleration as the lookahead reads them.""" + inv = 1.0 / dt + for j in range(vel.shape[0]): + acc_prev[j] = (vel[j] - vel_prev[j]) * inv + vel_prev[j] = vel[j] class HomeState(Enum): @@ -118,6 +189,7 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: self.state = HomeState.WAITING_FOR_HOMED self.timeout_counter -= 1 if self.timeout_counter <= 0: + state.homing_step = 0 self.fail(make_error(ErrorCode.MOTN_HOME_TIMEOUT)) self.stop_and_idle(state) return ExecutionStatusCode.FAILED @@ -127,11 +199,13 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: state.Command_out = CommandCode.IDLE if np.all(state.Homed_in[:6] == 1): self.log_info("Homing sequence complete. All joints reported home.") + state.homing_step = 0 self.finish() self.stop_and_idle(state) return ExecutionStatusCode.COMPLETED self.timeout_counter -= 1 if self.timeout_counter <= 0: + state.homing_step = 0 self.fail(make_error(ErrorCode.MOTN_HOME_TIMEOUT)) self.stop_and_idle(state) return ExecutionStatusCode.FAILED @@ -144,6 +218,15 @@ class JogJCommand(MotionCommand[JogJCmd]): """ A non-blocking command to jog joints for a specific duration. Uses static 6-element speed array on the wire (all joints, zeros for inactive). + + Each joint runs its own lookahead against its position limits: a joint + whose stopping distance would reach its limit is ramped to rest there + — that joint alone; the others carry on — and held while the jog + drives it that way. The hold is the joint's own: it lets go when the + jog drives that joint the other way or stops driving it. Whatever the + lookahead predicted, the commanded position never steps across a + limit. The jog ends when its duration, counted in control ticks, runs + out and the joints have come to rest. """ PARAMS_TYPE = JogJCmd @@ -151,22 +234,43 @@ class JogJCommand(MotionCommand[JogJCmd]): __slots__ = ( "speeds_out", - "_jog_initialized", + "_synced", + "_accel_applied", "_jog_vel_rad", + "_target_vel", + "_vel_prev", + "_acc_prev", + "_q_meas", + "_blocked", "_lookahead_buf", ) def __init__(self, p: JogJCmd): super().__init__(p) self.speeds_out = np.zeros(6, dtype=np.int32) - self._jog_initialized = False + self._synced = False + self._accel_applied = -1.0 self._jog_vel_rad = np.zeros(6, dtype=np.float64) + self._target_vel = np.zeros(6, dtype=np.float64) + self._vel_prev = np.zeros(6, dtype=np.float64) + self._acc_prev = np.zeros(6, dtype=np.float64) + self._q_meas = np.zeros(6, dtype=np.float64) + # Per joint: the direction a limit has blocked (±1), or 0. + self._blocked = np.zeros(6, dtype=np.int8) self._lookahead_buf = np.zeros(6, dtype=np.float64) def do_setup(self, state: "ControllerState") -> None: - """Pre-compute step speeds and rad/s velocities for all 6 joints.""" + """Pre-compute step speeds and rad/s velocities for all 6 joints, + releasing the limit hold of a joint this datagram stops driving or + drives the other way.""" for i in range(6): s = self.p.speeds[i] + held = self._blocked[i] + if held != 0 and ( + (s == 0.0 and self._jog_vel_rad[i] != 0.0) + or (s != 0.0 and (1 if s > 0.0 else -1) != held) + ): + self._blocked[i] = 0 if s == 0.0: self.speeds_out[i] = 0 self._jog_vel_rad[i] = 0.0 @@ -180,41 +284,52 @@ def do_setup(self, state: "ControllerState") -> None: self._jog_vel_rad[i] = speed_steps_to_rad_scalar(step_speed, i) * ( 1 if s > 0 else -1 ) - self.start_timer(self.p.duration) - self._jog_initialized = False + self.start_tick_timer(self.p.duration) def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: """Execute one tick of joint jogging via StreamingExecutor.""" se = state.streaming_executor - # Sync position on first tick - if not self._jog_initialized: + # A new jog starts from the arm at rest; one continued by the next + # datagram keeps the motion it is in, and the lookahead the speed + # and acceleration it has measured of it. + if not self._synced: steps_to_rad(state.Position_in, self._q_rad_buf) se.sync_position(self._q_rad_buf) - self._jog_initialized = True - - stop_reason = self._check_stop_conditions(state) - - if stop_reason: - self._jog_vel_rad.fill(0.0) - se.set_jog_velocity(self._jog_vel_rad) - pos_rad, vel, finished = se.tick() - self._q_rad_buf[:] = pos_rad - rad_to_steps(self._q_rad_buf, self._steps_buf) - self.set_move_position(state, self._steps_buf) - - if finished or np.dot(vel, vel) < 1e-6: - if stop_reason.startswith("Limit"): - logger.warning(stop_reason) - else: - self.log_trace(stop_reason) - se.active = False - self.finish() - return ExecutionStatusCode.COMPLETED - return ExecutionStatusCode.EXECUTING - - se.set_jog_velocity(self._jog_vel_rad) - pos_rad, _vel, _finished = se.tick() + self._vel_prev.fill(0.0) + self._acc_prev.fill(0.0) + self._synced = True + if self.p.accel != self._accel_applied: + se.set_limits(1.0, self.p.accel) + self._accel_applied = self.p.accel + + steps_to_rad(state.Position_in, self._q_meas) + stopping = self.tick_timer_expired() + status = _jog_lookahead_jit( + self._jog_vel_rad, + stopping, + self._vel_prev, + self._acc_prev, + self._q_meas, + _POS_LO_RAD, + _POS_HI_RAD, + _ACCEL_MAX, + _JERK_MAX, + self.p.accel, + se.dt, + self._blocked, + self._target_vel, + ) + if status > 1: + for j in range(6): + if status & (1 << (j + 1)): + logger.info("[JOGJ] joint %d stopping short of its limit", j + 1) + + se.set_jog_velocity(self._target_vel) + pos_rad, vel, finished = se.tick() + # _q_rad_buf still holds the position commanded last tick. + se.hold_inside(self._q_rad_buf, self._q_meas, _POS_LO_RAD, _POS_HI_RAD) + _track_rates_jit(vel, self._vel_prev, self._acc_prev, se.dt) # Never stream a config that collides or approaches collision: the # streamed config itself is checked (catches anything inside the @@ -225,16 +340,12 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: # the planner guard's start-in-collision semantics. An abrupt stop is # acceptable when the alternative is driving deeper. (The Cartesian jog # uses a graceful CSE-based stop; JogJ has no smoother, so it halts.) + # An unreferenced arm's positions are not its own, so there is no + # configuration to check: the jog that nudges it clear before it + # can home runs unchecked, as does par6's. checker = PAROL6_ROBOT.collision - if checker is not None: - # In-place to keep the hot path allocation-free; clamped to joint - # limits so a pose past the mechanical stop can't phantom-trip. - la = self._lookahead_buf - la[:] = self._jog_vel_rad - la *= COLLISION_JOG_LOOKAHEAD_S - la += pos_rad - lo, hi = _qlim_rows() - np.clip(la, lo, hi, out=la) + if checker is not None and status & 1 and arm_homed(state): + la = stream_lookahead(pos_rad, self._target_vel, self._lookahead_buf) if collision_blocked(checker, pos_rad, la): logger.warning("[JOGJ] collision predicted - stopping jog") # Allocate only here (the rare stop), never on the clean tick. @@ -250,18 +361,13 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: rad_to_steps(self._q_rad_buf, self._steps_buf) self.set_move_position(state, self._steps_buf) - return ExecutionStatusCode.EXECUTING - - def _check_stop_conditions(self, state: "ControllerState") -> str | None: - """Check if jog should stop. Returns stop reason or None.""" - if self.timer_expired(): - return "Timed jog finished." - - limit_mask = _limit_hit_mask(state.Position_in, self.speeds_out) - if np.any(limit_mask): - return f"Limit reached on joint {int(np.argmax(limit_mask)) + 1}." + if stopping and (finished or below_speed(vel, 1e-6)): + self.log_trace("Timed jog finished.") + se.active = False + self.finish() + return ExecutionStatusCode.COMPLETED - return None + return ExecutionStatusCode.EXECUTING @register_command(CmdType.SELECT_TOOL) @@ -285,39 +391,38 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: @register_command(CmdType.TELEPORT) -class TeleportCommand(MotionCommand[TeleportCmd]): - """Instantly set joint angles (simulator only, no trajectory).""" +class TeleportCommand(SystemCommand[TeleportCmd]): + """Set the simulated arm's joint angles, and optionally its tool's + positions, in one tick — no trajectory. The pose is exact afterwards, + so the arm counts as homed. Refused on hardware, and when the tool + positions are not as many as status reports for the fitted tool.""" PARAMS_TYPE = TeleportCmd - streamable = True - __slots__ = ("_target_steps", "_deg_buf", "_sim_mode") + __slots__ = ("_target_steps",) def __init__(self, p: TeleportCmd): super().__init__(p) self._target_steps = np.empty(6, dtype=np.int32) - self._deg_buf = np.empty(6, dtype=np.float64) - self._sim_mode = False def do_setup(self, state: ControllerState) -> None: - self._sim_mode = is_simulation_mode() - self._deg_buf[:] = self.p.angles - deg_to_steps(self._deg_buf, self._target_steps) + # The controller refuses what the simulator cannot apply before + # setup runs (off the simulator, tool positions other than the ones + # status reports for the fitted tool). + deg_to_steps(np.asarray(self.p.angles, dtype=np.float64), self._target_steps) def execute_step(self, state: ControllerState) -> ExecutionStatusCode: - if not self._sim_mode: - logger.warning("TELEPORT rejected: only allowed in simulator mode") - self.finish() - return ExecutionStatusCode.COMPLETED - state.Position_out[:] = self._target_steps state.Speed_out.fill(0) state.Command_out = CommandCode.TELEPORT + # The pose is exact: the arm is referenced there from this tick on, + # and the simulator reports it so on the next frame. A move read + # after the teleport, in the same batch, is planned from the landing. + state.Position_in[:] = self._target_steps + state.Homed_in[:6] = 1 if self.p.tool_positions: - state.tool_teleport_pos = ( - max(0.0, min(1.0, self.p.tool_positions[0])) * 255.0 - ) + state.tool_teleport_pos = self.p.tool_positions[0] * 255.0 # Clear gripper command bits so write_frame's JIT doesn't re-arm the ramp state.Gripper_data_out[3] = 0 diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py index c1ce792..a497680 100644 --- a/parol6/commands/cartesian_commands.py +++ b/parol6/commands/cartesian_commands.py @@ -4,15 +4,18 @@ """ import logging +from collections.abc import Sequence from typing import cast import numpy as np import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.commands._collision_guard import collision_blocked, guard_joint_path +from parol6.commands._collision_guard import ( + collision_blocked, + collision_stop, + guard_cartesian_path, +) from parol6.config import ( - CART_ANG_JOG_MIN, - CART_LIN_JOG_MIN, INTERVAL_S, LIMITS, PATH_SAMPLES, @@ -20,6 +23,11 @@ steps_to_rad, ) from parol6.motion import JointPath, TrajectoryBuilder +from parol6.motion.geometry import ( + ArcSegment, + LineSegment, + build_blended_path, +) from parol6.protocol.wire import ( CmdType, JogLCmd, @@ -27,12 +35,14 @@ ) from parol6.server.command_registry import register_command from parol6.server.state import ControllerState, get_fkine_se3 -from parol6.utils.error_catalog import make_error +from parol6.utils.error_catalog import RobotError, make_error from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import IKError, TrajectoryPlanningError from parol6.utils.ik import RateLimitedWarning, solve_ik -from pinokin import se3_from_rpy, se3_interp, se3_rpy +from pinokin import se3_from_rpy, se3_rpy -from parol6.commands.servo_commands import _max_vel_ratio_jit +from parol6.commands.servo_commands import _step_toward_jit +from parol6.motion.streaming_executors import below_speed from .base import ( ExecutionStatusCode, @@ -43,19 +53,23 @@ logger = logging.getLogger(__name__) -# Pre-computed Cartesian jog limit constants (avoid per-tick recomputation) -_CART_ANG_JOG_MIN_RAD: float = float(np.deg2rad(CART_ANG_JOG_MIN)) +# Full-scale jog_l rates: a velocity fraction of ±1 maps to these. _CART_ANG_JOG_MAX_RAD: float = float(LIMITS.cart.jog.velocity.angular) -_CART_LIN_JOG_MIN_MS: float = CART_LIN_JOG_MIN / 1000.0 _CART_LIN_JOG_MAX_MS: float = float(LIMITS.cart.jog.velocity.linear) -def _linmap_frac(frac: float, lo: float, hi: float) -> float: - if frac < 0.0: - frac = 0.0 - elif frac > 1.0: - frac = 1.0 - return lo + (hi - lo) * frac +def jog_twist(fractions: list[float], out: np.ndarray) -> None: + """The TCP twist `[vx, vy, vz, wx, wy, wz]` (m/s, rad/s) a jog_l's six + signed fractions ask for: each component is a pure linear map from + zero onto its full-scale rate, so an all-zero vector asks for rest and + a diagonal keeps the direction it was given.""" + for i in range(6): + f = fractions[i] + if f > 1.0: + f = 1.0 + elif f < -1.0: + f = -1.0 + out[i] = f * (_CART_LIN_JOG_MAX_MS if i < 3 else _CART_ANG_JOG_MAX_RAD) _ik_warn = RateLimitedWarning() @@ -66,104 +80,135 @@ class JogLCommand(MotionCommand[JogLCmd]): """ A non-blocking command to jog the robot's end-effector in Cartesian space. - CSE drives Cartesian velocity (Ruckig-smoothed). IK converts each - smoothed pose to joint space. Velocity clamping and commanded-position - tracking match servo_l for smooth, deterministic joint trajectories. + The CSE drives the commanded 6-DOF TCP twist (Ruckig-smoothed); IK + converts each smoothed pose to joint space. Velocity clamping and + commanded-position tracking match servo_l for smooth, deterministic + joint trajectories. The twist is resolved against the tool as it now + stands, every tick: a tool-frame jog moves along the tool's axes as + they are after any turn before it, a world-frame turn is about the + world axis. An unreachable pose brakes the tool along its twist; a + predicted collision brakes it and ends the jog with the collision + latched, as a planned move is refused. The duration is counted in + control ticks. """ PARAMS_TYPE = JogLCmd streamable = True __slots__ = ( - "is_rotation", + "_initialized", + "_accel_applied", "_ik_stopping", - "_axis_index", - "_axis_sign", - "_dot_buf", + "_released", + "_collision_error", + "_twist", + "_scaled_twist", "_q_commanded", "_q_ik_seed", - "_dq_buf", - "_pos_rad_buf", "_vel_ratio", ) def __init__(self, p: JogLCmd): super().__init__(p) - self.is_rotation = False + self._initialized = False + self._accel_applied = -1.0 self._ik_stopping = False - self._axis_index = 0 - self._axis_sign = 1.0 + # The duration ran out and the brake has been handed to the CSE. + self._released = False + # Set once a contact stops the jog; latched across datagrams. + self._collision_error: RobotError | None = None self._vel_ratio = 1.0 - self._dot_buf = np.zeros((), dtype=np.float64) + self._twist = np.zeros(6, dtype=np.float64) + self._scaled_twist = np.zeros(6, dtype=np.float64) self._q_commanded = np.zeros(6, dtype=np.float64) self._q_ik_seed = np.zeros(6, dtype=np.float64) - self._dq_buf = np.zeros(6, dtype=np.float64) - self._pos_rad_buf = np.zeros(6, dtype=np.float64) def do_setup(self, state: "ControllerState") -> None: - """Find dominant axis and start timer.""" - vels = self.p.velocities - max_idx = 0 - max_abs = abs(vels[0]) - for i in range(1, 6): - a = abs(vels[i]) - if a > max_abs: - max_abs = a - max_idx = i - self.is_rotation = max_idx >= 3 - self._axis_index = max_idx - 3 if max_idx >= 3 else max_idx - self._axis_sign = 1.0 if vels[max_idx] >= 0 else -1.0 - self.start_timer(self.p.duration) + """Resolve the twist and start the timer. A collision stop stays + latched across the datagrams that keep the stream alive: the jog + ends where it braked, like a refused planned move.""" + guard_homed(state) + jog_twist(self.p.velocities, self._twist) + self.start_tick_timer(self.p.duration) self._ik_stopping = False + def _sync(self, state: "ControllerState") -> None: + """Start the CSE and the joint tracking from the arm, at rest.""" + steps_to_rad(state.Position_in, self._q_rad_buf) + state.cartesian_streaming_executor.sync_pose(get_fkine_se3(state)) + self._q_commanded[:] = self._q_rad_buf + self._q_ik_seed[:] = self._q_rad_buf + self._vel_ratio = 1.0 + def _track_and_send(self, state: "ControllerState", ik_q: np.ndarray) -> None: """Velocity-clamp IK result, update tracked position, send MOVE.""" self._q_ik_seed[:] = ik_q - dq = self._dq_buf - for i in range(6): - dq[i] = float(ik_q[i]) - self._q_commanded[i] - ratio = _max_vel_ratio_jit(ik_q, self._q_commanded) - if ratio > 1.0: - for i in range(6): - self._q_commanded[i] += dq[i] / ratio - self._vel_ratio = ratio - else: - self._q_commanded[:] = ik_q - self._vel_ratio = 1.0 - self._pos_rad_buf[:] = self._q_commanded - rad_to_steps(self._pos_rad_buf, self._steps_buf) + ratio = _step_toward_jit(self._q_commanded, ik_q) + self._vel_ratio = ratio if ratio > 1.0 else 1.0 + rad_to_steps(self._q_commanded, self._steps_buf) self.set_move_position(state, self._steps_buf) + def _send_if_clear(self, state: "ControllerState", pose: np.ndarray) -> None: + """Solve *pose* and send it, unless the step there would reach a + contact: a brake's configurations are gated like the jog's own, and + the arm holds the last clear one instead.""" + ik_result = solve_ik(PAROL6_ROBOT.robot, pose, self._q_ik_seed) + if not ik_result.success or ik_result.q is None: + return + checker = PAROL6_ROBOT.collision + if checker is None or not collision_blocked( + checker, self._q_commanded, ik_result.q + ): + self._track_and_send(state, ik_result.q) + + def _command_twist(self, cse) -> None: + """Command the twist, held back by the factor the joints were: the + tool keeps its direction and loses only speed.""" + self._released = False + if self._vel_ratio != 1.0: + np.multiply(self._twist, 1.0 / self._vel_ratio, out=self._scaled_twist) + cse.set_jog_twist(self._scaled_twist, self.p.frame == "WRF") + else: + cse.set_jog_twist(self._twist, self.p.frame == "WRF") + def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: """Execute one tick of Cartesian jogging.""" cse = state.cartesian_streaming_executor - # Initialize only if not already active (preserve velocity across streaming) - if not cse.active: - steps_to_rad(state.Position_in, self._q_rad_buf) - cse.sync_pose(get_fkine_se3(state)) + # A new jog starts from the arm; one continued by the next datagram + # keeps the velocity it is at. + if not self._initialized or not cse.active: + self._sync(state) + self._accel_applied = -1.0 + self._initialized = True + if self.p.accel != self._accel_applied: cse.set_limits(1.0, self.p.accel) - self._q_commanded[:] = self._q_rad_buf - self._q_ik_seed[:] = self._q_rad_buf - self._vel_ratio = 1.0 + self._accel_applied = self.p.accel + + if self._collision_error is not None: + # Braking to rest with the collision latched, whatever the timer + # says; the jog ends there in error and does not resume on its + # own, like a refused planned move. + smoothed_pose, smoothed_vel, _finished = cse.tick() + if below_speed(smoothed_vel, 1e-8): + cse.active = False + self.fail_and_idle(state, self._collision_error) + return ExecutionStatusCode.FAILED + self._send_if_clear(state, smoothed_pose) + return ExecutionStatusCode.EXECUTING # Handle timer expiry - stop smoothly - if self.timer_expired(): - cse.set_jog_velocity_1dof(self._axis_index, 0.0, self.is_rotation) + if self.tick_timer_expired(): + if not self._released: + cse.stop() + self._released = True smoothed_pose, smoothed_vel, finished = cse.tick() - np.dot(smoothed_vel, smoothed_vel, out=self._dot_buf) - if not finished and self._dot_buf > 1e-8: - ik_result = solve_ik(PAROL6_ROBOT.robot, smoothed_pose, self._q_ik_seed) - if ik_result.success and ik_result.q is not None: - # Keep streaming while escaping from inside a keep-out, - # else the target freezes at release and the arm jerks. - checker = PAROL6_ROBOT.collision - if checker is None or not collision_blocked( - checker, self._q_commanded, ik_result.q - ): - self._track_and_send(state, ik_result.q) + if not finished and not below_speed(smoothed_vel, 1e-8): + # Keep streaming while escaping from inside a keep-out, + # else the target freezes at release and the arm jerks. + self._send_if_clear(state, smoothed_pose) return ExecutionStatusCode.EXECUTING cse.active = False @@ -171,34 +216,10 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: self.stop_and_idle(state) return ExecutionStatusCode.COMPLETED - # Compute target velocity based on speed fraction from velocity vector - vels = self.p.velocities - speed_mag = abs(vels[self._axis_index + (3 if self.is_rotation else 0)]) - if self.is_rotation: - velocity = ( - _linmap_frac(speed_mag, _CART_ANG_JOG_MIN_RAD, _CART_ANG_JOG_MAX_RAD) - * self._axis_sign - ) - else: - velocity = ( - _linmap_frac(speed_mag, _CART_LIN_JOG_MIN_MS, _CART_LIN_JOG_MAX_MS) - * self._axis_sign - ) - - # Scale velocity by previous tick's clamping ratio to keep CSE - # in sync with joint-velocity-limited motion - if self._vel_ratio > 1.0: - velocity /= self._vel_ratio - - # While stopping, leave the CSE target at zero — re-commanding full - # velocity every tick would defeat cse.stop()'s deceleration. + # While stopping, leave the CSE target at zero — re-commanding the + # twist every tick would defeat cse.stop()'s deceleration. if not self._ik_stopping: - if self.p.frame == "WRF": - cse.set_jog_velocity_1dof_wrf( - self._axis_index, velocity, self.is_rotation - ) - else: - cse.set_jog_velocity_1dof(self._axis_index, velocity, self.is_rotation) + self._command_twist(cse) smoothed_pose, smoothed_vel, _finished = cse.tick() @@ -211,66 +232,222 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: if not self._ik_stopping: _ik_warn( logger, - "[CARTJOG] IK failed - initiating graceful stop: pos=%s", + "[JOGL] IK failed - initiating graceful stop: pos=%s", smoothed_pose[:3, 3], ) cse.stop() self._ik_stopping = True - else: - # Still failing, check if we've stopped decelerating - np.dot(smoothed_vel, smoothed_vel, out=self._dot_buf) - if self._dot_buf < 1e-8: - cse.sync_pose(get_fkine_se3(state)) - cse.active = False - self.finish() - return ExecutionStatusCode.COMPLETED + elif below_speed(smoothed_vel, 1e-8): + cse.active = False + self.finish() + return ExecutionStatusCode.COMPLETED return ExecutionStatusCode.EXECUTING - # Predicted collision decelerates like an IK failure (no mid-jog - # raise); escaping from inside a keep-out stays allowed, mirroring the - # planner guard. + # A predicted collision brakes like an IK failure (no mid-jog + # raise) but does not resume: the jog ends where it stopped with the + # collision latched. Escaping from inside a keep-out stays allowed, + # mirroring the planner guard. checker = PAROL6_ROBOT.collision if checker is not None and collision_blocked( checker, self._q_commanded, ik_result.q ): - if not self._ik_stopping: - _ik_warn( - logger, - "[CARTJOG] collision predicted - initiating stop", - ) - # Capture once on the stop transition (not every decel tick). - state.collision_pairs = tuple( - PAROL6_ROBOT.display_pairs(checker.colliding_pairs(ik_result.q)) - ) - state.collision_active = True - cse.stop() - self._ik_stopping = True + _ik_warn( + logger, + "[JOGL] collision predicted - stopping", + ) + self._collision_error = collision_stop(state, checker, ik_result.q) + cse.stop() return ExecutionStatusCode.EXECUTING - # Reachable + collision-free again — resume jogging. + # Reachable again — resume jogging. if self._ik_stopping: - logger.info("[CARTJOG] constraint cleared - resuming jog") - state.clear_collision() - steps_to_rad(state.Position_in, self._q_rad_buf) - cse.sync_pose(get_fkine_se3(state)) - self._q_commanded[:] = self._q_rad_buf - self._q_ik_seed[:] = self._q_rad_buf - self._vel_ratio = 1.0 + logger.info("[JOGL] pose reachable again - resuming jog") + self._sync(state) self._ik_stopping = False - if self.p.frame == "WRF": - cse.set_jog_velocity_1dof_wrf( - self._axis_index, velocity, self.is_rotation - ) - else: - cse.set_jog_velocity_1dof(self._axis_index, velocity, self.is_rotation) + self._command_twist(cse) self._track_and_send(state, ik_result.q) return ExecutionStatusCode.EXECUTING +def pose6_to_se3(pose: Sequence[float], out: np.ndarray) -> np.ndarray: + """Write the SE3 of a wire pose ``[x, y, z, rx, ry, rz]`` (mm, degrees) + into ``out`` and return it.""" + se3_from_rpy( + pose[0] / 1000.0, + pose[1] / 1000.0, + pose[2] / 1000.0, + np.radians(pose[3]), + np.radians(pose[4]), + np.radians(pose[5]), + out, + ) + return out + + +def resolve_pose( + start: np.ndarray, pose: "list[float]", frame: str, rel: bool +) -> np.ndarray: + """The SE3 target a wire pose ``[x, y, z, rx, ry, rz]`` (mm, degrees) + names, resolved against the pose the move starts from. + + A TRF pose is an offset in the tool frame at the start of the move, + with or without ``rel``. A WRF pose is absolute unless ``rel``, in which + case its rotation is applied about the TCP and its translation is added + in world coordinates. + """ + delta_se3 = pose6_to_se3(pose, np.zeros((4, 4), dtype=np.float64)) + if frame == "TRF": + return start @ delta_se3 + if rel: + target = np.eye(4, dtype=np.float64) + target[:3, :3] = delta_se3[:3, :3] @ start[:3, :3] + target[:3, 3] = start[:3, 3] + delta_se3[:3, 3] + return target + return delta_se3 + + +def cartesian_diagnostic(poses: np.ndarray, ik_valid: np.ndarray) -> dict: + """What a dry run draws of a cartesian path IK could not solve all of: + the path's TCP poses (x, y, z in m, roll, pitch, yaw in rad) and which + of them solved.""" + n = len(poses) + tcp_poses = np.empty((n, 6), dtype=np.float64) + rpy = np.empty(3, dtype=np.float64) + for i in range(n): + tcp_poses[i, :3] = poses[i][:3, 3] + se3_rpy(poses[i], rpy) + tcp_poses[i, 3:] = rpy + return {"tcp_poses": tcp_poses, "ik_valid": ik_valid} + + +class CartesianChainLink: + """A cartesian move that can join a blend chain: it contributes one + segment, resolved against the pose the move before it ends at.""" + + #: Where the move ends, once planning has resolved it: a dry run that + #: fails the move carries on from there. + target_pose: np.ndarray | None = None + #: The path a dry run draws for a move that fails in IK. + cartesian_diagnostic: dict | None = None + + def chain_segment( + self, previous: np.ndarray, state: "ControllerState" + ) -> tuple[LineSegment | ArcSegment, np.ndarray]: + """The segment this move traces from ``previous``, and its end pose.""" + raise NotImplementedError + + def do_setup_with_blend( + self, + state: "ControllerState", + next_cmds: "list[TrajectoryMoveCommandBase]", + ) -> int: + """Build one cartesian trajectory through the moves blended behind + this one, straight or circular, with the junctions rounded.""" + assert isinstance(self, TrajectoryMoveCommandBase) + guard_homed(state) + return setup_cartesian_chain(self, state, next_cmds) + + +def setup_cartesian_chain( + head: "TrajectoryMoveCommandBase", + state: "ControllerState", + next_cmds: "list[TrajectoryMoveCommandBase]", +) -> int: + """Plan ``head`` and the cartesian moves blended behind it as ONE path + whose junctions are rounded. Returns how many of ``next_cmds`` the chain + consumed; the head's trajectory covers them all. Falls back to the + head's own setup when there is nothing to chain. + + A move whose segment cannot be built (a move_c whose via names no + circle) ends the chain ahead of it: the moves before it run, stopping + where it would have started, and it fails on its own setup as it + would alone.""" + assert isinstance(head, CartesianChainLink) + chain: list[TrajectoryMoveCommandBase] = [head] + if head.blend_radius > 0: + for cmd in next_cmds: + if not isinstance(cmd, CartesianChainLink): + break + chain.append(cmd) + if cmd.blend_radius <= 0: + break + if len(chain) < 2: + head.do_setup(state) + return 0 + + segments: list[LineSegment | ArcSegment] = [] + previous = get_fkine_se3(state).copy() + for i, cmd in enumerate(chain): + assert isinstance(cmd, CartesianChainLink) + try: + segment, end = cmd.chain_segment(previous, state) + except TrajectoryPlanningError: + if i == 0: + raise + del chain[i:] + break + segments.append(segment) + previous = end + if i == 0: + head.target_pose = end + if len(chain) < 2: + head.do_setup(state) + return 0 + blend_radii = [cmd.blend_radius for cmd in chain[:-1]] + + composite_poses = build_blended_path( + segments, blend_radii, samples_per_segment=PATH_SAMPLES + ) + + steps_to_rad(state.Position_in, head._q_rad_buf) + joint_path = JointPath.from_poses( + composite_poses, head._q_rad_buf, stop_on_failure=state.stop_on_failure + ) + if joint_path.is_partial: + assert joint_path.valid is not None + head.cartesian_diagnostic = cartesian_diagnostic( + composite_poses, joint_path.valid + ) + raise IKError( + make_error( + ErrorCode.IK_PARTIAL_PATH, + valid=str(int(joint_path.valid.sum())), + total=str(len(joint_path)), + ) + ) + guard_cartesian_path(joint_path) + + # The chain runs under the slowest speed and acceleration fraction in + # it; durations add up when every move carries one. + min_speed = min(c.p.resolved_speed for c in chain) + min_accel = min(c.p.accel for c in chain) + durations = [c.p.resolved_duration for c in chain] + total_duration = ( + sum(cast(list[float], durations)) if None not in durations else None + ) + + builder = TrajectoryBuilder( + joint_path=joint_path, + profile=state.motion_profile, + velocity_frac=min_speed, + accel_frac=min_accel, + duration=total_duration, + dt=INTERVAL_S, + cart_vel_limit=LIMITS.cart.hard.velocity.linear * min_speed, + cart_acc_limit=LIMITS.cart.hard.acceleration.linear * min_accel, + path_knots=joint_path.knots, + ) + trajectory = builder.build() + head.trajectory_steps = trajectory.steps + head.trajectory_rad = trajectory.positions_rad + head._duration = trajectory.duration + return len(chain) - 1 + + @register_command(CmdType.MOVEL) -class MoveLCommand(TrajectoryMoveCommandBase[MoveLCmd]): +class MoveLCommand(CartesianChainLink, TrajectoryMoveCommandBase[MoveLCmd]): """Move the robot's end-effector in a straight line to a Cartesian pose. Supports absolute and relative modes via the `rel` field, and WRF/TRF frames. @@ -301,17 +478,15 @@ def do_setup(self, state: "ControllerState") -> None: def _precompute_trajectory(self, state: "ControllerState") -> None: """Pre-compute joint trajectory that follows straight-line Cartesian path.""" - from parol6.utils.errors import IKError - assert self.initial_pose is not None and self.target_pose is not None steps_to_rad(state.Position_in, self._q_rad_buf) current_rad = self._q_rad_buf cart_poses = self._cart_poses_buf - for i in range(PATH_SAMPLES): - s = i / (PATH_SAMPLES - 1) - se3_interp(self.initial_pose, self.target_pose, s, cart_poses[i]) + LineSegment(self.initial_pose, self.target_pose).sample_into( + cart_poses, 0.0, 1.0, 0 + ) stop_on_failure = state.stop_on_failure joint_path = JointPath.from_poses( @@ -321,23 +496,12 @@ def _precompute_trajectory(self, state: "ControllerState") -> None: ) if not joint_path.is_partial: - guard_joint_path(joint_path.positions) + guard_cartesian_path(joint_path) if joint_path.is_partial: ik_valid = joint_path.valid assert ik_valid is not None - # Extract TCP poses (x,y,z,rx,ry,rz) in meters+radians from SE3 - n = len(cart_poses) - tcp_poses = np.empty((n, 6), dtype=np.float64) - _rpy_buf = np.empty(3, dtype=np.float64) - for i in range(n): - tcp_poses[i, :3] = cart_poses[i][:3, 3] - se3_rpy(cart_poses[i], _rpy_buf) - tcp_poses[i, 3:] = _rpy_buf - self.cartesian_diagnostic = { - "tcp_poses": tcp_poses, - "ik_valid": ik_valid, - } + self.cartesian_diagnostic = cartesian_diagnostic(cart_poses, ik_valid) raise IKError( make_error( ErrorCode.IK_PARTIAL_PATH, @@ -355,6 +519,7 @@ def _precompute_trajectory(self, state: "ControllerState") -> None: dt=INTERVAL_S, cart_vel_limit=LIMITS.cart.hard.velocity.linear * self.p.resolved_speed, cart_acc_limit=LIMITS.cart.hard.acceleration.linear * self.p.accel, + path_knots=joint_path.knots, ) trajectory = builder.build() @@ -370,164 +535,12 @@ def _precompute_trajectory(self, state: "ControllerState") -> None: ) def _compute_target_pose(self, state: "ControllerState") -> None: - """Compute target pose - absolute or relative based on rel flag.""" - pose = self.p.pose - - if self.p.rel: - # Relative move: compute delta SE3, then apply in tool frame (TRF) - # or world frame (WRF) depending on self.p.frame - delta_se3 = np.zeros((4, 4), dtype=np.float64) - se3_from_rpy( - pose[0] / 1000.0, - pose[1] / 1000.0, - pose[2] / 1000.0, - np.radians(pose[3]), - np.radians(pose[4]), - np.radians(pose[5]), - delta_se3, - ) - if self.p.frame == "TRF": - # Post-multiply for tool-relative motion - self.target_pose = cast(np.ndarray, self.initial_pose) @ delta_se3 - else: - # Pre-multiply for world-relative motion - self.target_pose = delta_se3 @ cast(np.ndarray, self.initial_pose) - else: - # Absolute target pose - self.target_pose = np.zeros((4, 4), dtype=np.float64) - se3_from_rpy( - pose[0] / 1000.0, - pose[1] / 1000.0, - pose[2] / 1000.0, - np.radians(pose[3]), - np.radians(pose[4]), - np.radians(pose[5]), - self.target_pose, - ) - - def do_setup_with_blend( - self, - state: "ControllerState", - next_cmds: "list[TrajectoryMoveCommandBase]", - ) -> int: - """Build composite Cartesian trajectory with blend zones.""" - guard_homed(state) - if self.blend_radius <= 0 or not next_cmds: - self.do_setup(state) - return 0 - - chain: list[MoveLCommand] = [self] - for cmd in next_cmds: - if isinstance(cmd, MoveLCommand): - chain.append(cmd) - else: - break - if len(chain) < 2: - self.do_setup(state) - return 0 - - from parol6.motion.geometry import build_composite_cartesian_path - - initial_pose = get_fkine_se3(state) - self.initial_pose = initial_pose - - waypoints = [initial_pose] - blend_radii: list[float] = [] - prev_pose = initial_pose - - for i, movel in enumerate(chain): - movel.initial_pose = prev_pose - movel._compute_target_pose(state) - if movel.target_pose is None: - if i < 2: - self.do_setup(state) - return 0 - chain = chain[:i] - break - waypoints.append(movel.target_pose) - prev_pose = movel.target_pose - if i < len(chain) - 1: - blend_radii.append(movel.blend_radius) - - if len(waypoints) < 3: - self.do_setup(state) - return 0 - - composite_poses = build_composite_cartesian_path( - waypoints, - blend_radii, - samples_per_segment=PATH_SAMPLES, + self.target_pose = resolve_pose( + cast(np.ndarray, self.initial_pose), self.p.pose, self.p.frame, self.p.rel ) - if len(composite_poses) == 0: - self.do_setup(state) - return 0 - - steps_to_rad(state.Position_in, self._q_rad_buf) - - try: - joint_path = JointPath.from_poses(composite_poses, self._q_rad_buf) - except Exception: - self.log_warning( - "Blend IK failed for %d-segment Cartesian path, falling back", - len(waypoints) - 1, - ) - self.do_setup(state) - return 0 - - if joint_path.is_partial: - self.log_warning( - "Blend IK partial for %d-segment Cartesian path, falling back", - len(waypoints) - 1, - ) - self.do_setup(state) - return 0 - - guard_joint_path(joint_path.positions) - - # Use minimum speed/accel across chain, sum durations when all duration-based - min_speed = self.p.resolved_speed - min_accel = self.p.accel - total_duration = self.p.resolved_duration - all_have_duration = total_duration is not None - - for i in range(1, len(chain)): - cmd = chain[i] - s = cmd.p.resolved_speed - a = cmd.p.accel - if s < min_speed: - min_speed = s - if a < min_accel: - min_accel = a - d = cmd.p.resolved_duration - if all_have_duration and d is not None: - assert total_duration is not None - total_duration += d - else: - all_have_duration = False - total_duration = None - - builder = TrajectoryBuilder( - joint_path=joint_path, - profile=state.motion_profile, - velocity_frac=min_speed, - accel_frac=min_accel, - duration=total_duration, - dt=INTERVAL_S, - cart_vel_limit=LIMITS.cart.hard.velocity.linear * min_speed, - cart_acc_limit=LIMITS.cart.hard.acceleration.linear * min_accel, - ) - - trajectory = builder.build() - self.trajectory_steps = trajectory.steps - self.trajectory_rad = trajectory.positions_rad - self._duration = trajectory.duration - - consumed = len(chain) - 1 - self.log_info( - " -> Blended Cartesian trajectory: %d segments, steps=%d, duration=%.3fs", - len(waypoints) - 1, - len(self.trajectory_steps), - trajectory.duration, - ) - return consumed + def chain_segment( + self, previous: np.ndarray, state: "ControllerState" + ) -> tuple[LineSegment | ArcSegment, np.ndarray]: + end = resolve_pose(previous, self.p.pose, self.p.frame, self.p.rel) + return LineSegment(previous, end), end diff --git a/parol6/commands/curved_commands.py b/parol6/commands/curved_commands.py index 368fc54..18b6b25 100644 --- a/parol6/commands/curved_commands.py +++ b/parol6/commands/curved_commands.py @@ -11,10 +11,10 @@ import numpy as np -from parol6.commands._collision_guard import guard_joint_path +from parol6.commands._collision_guard import guard_cartesian_path from parol6.commands.base import TrajectoryMoveCommandBase, guard_homed -from parol6.config import INTERVAL_S, LIMITS, steps_to_rad -from parol6.motion import CircularMotion, JointPath, SplineMotion, TrajectoryBuilder +from parol6.config import INTERVAL_S, LIMITS, PATH_SAMPLES, steps_to_rad +from parol6.motion import JointPath, TrajectoryBuilder from parol6.protocol.wire import ( CmdType, MoveCCmd, @@ -22,13 +22,23 @@ MovePCmd, MoveSCmd, ) -from parol6.motion.geometry import compute_circle_from_3_points +from parol6.commands.cartesian_commands import ( + CartesianChainLink, + resolve_pose, +) +from parol6.motion.geometry import ( + ArcSegment, + LineSegment, + build_blended_path, + build_composite_cartesian_path, + build_spline_path, + pose_distance_m, +) from parol6.server.command_registry import register_command from parol6.server.state import get_fkine_se3 from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode from parol6.utils.errors import IKError, TrajectoryPlanningError -from pinokin import se3_from_rpy, se3_interp, se3_rpy _MP = TypeVar("_MP", bound=MotionParamsMixin) @@ -38,49 +48,21 @@ logger = logging.getLogger(__name__) -# ============================================================================= -# TRF/WRF Transformation Utilities -# ============================================================================= +#: A waypoint list's first entry stands in for the start pose within this, +#: on the combined translation and rotation metric [mm]. +WAYPOINT_SNAP_MM: float = 5.0 + -# Pre-allocated workspace buffers for TRF/WRF transformations (command setup phase) -_pose_trf_buf: np.ndarray = np.zeros((4, 4), dtype=np.float64) -_pose_wrf_buf: np.ndarray = np.zeros((4, 4), dtype=np.float64) -_rpy_rad_buf: np.ndarray = np.zeros(3, dtype=np.float64) - - -def _pose6_trf_to_wrf( - pose6_mm_deg: Sequence[float], tool_pose: np.ndarray, out: np.ndarray -) -> None: - """Convert 6D pose [x,y,z,rx,ry,rz] from TRF to WRF (mm, degrees).""" - se3_from_rpy( - pose6_mm_deg[0] / 1000.0, - pose6_mm_deg[1] / 1000.0, - pose6_mm_deg[2] / 1000.0, - np.radians(pose6_mm_deg[3]), - np.radians(pose6_mm_deg[4]), - np.radians(pose6_mm_deg[5]), - _pose_trf_buf, - ) - np.matmul(tool_pose, _pose_trf_buf, out=_pose_wrf_buf) - se3_rpy(_pose_wrf_buf, _rpy_rad_buf) - out[:3] = _pose_wrf_buf[:3, 3] * 1000.0 - np.degrees(_rpy_rad_buf, out=out[3:]) - - -def _transform_waypoints_trf_to_wrf( - waypoints: Sequence[Sequence[float]], frame: str, state: "ControllerState" -) -> np.ndarray: - """Transform 6D waypoint poses from TRF to WRF. Returns (N, 6) array.""" - n = len(waypoints) - result = np.empty((n, 6), dtype=np.float64) - if frame == "WRF": - for i in range(n): - result[i] = waypoints[i] - return result - tool_pose = get_fkine_se3(state) - for i in range(n): - _pose6_trf_to_wrf(waypoints[i], tool_pose, out=result[i]) - return result +def _waypoint_chain( + start: np.ndarray, waypoints: Sequence[Sequence[float]], frame: str +) -> list[np.ndarray]: + """The SE3 poses a waypoint list names, resolved against ``start`` as + ``resolve_pose`` resolves a move's target, led by ``start`` itself: a + first waypoint within ``WAYPOINT_SNAP_MM`` of it is the start.""" + poses = [start] + [resolve_pose(start, list(wp), frame, False) for wp in waypoints] + if len(poses) > 1 and pose_distance_m(start, poses[1]) * 1000.0 <= WAYPOINT_SNAP_MM: + del poses[1] + return poses # ============================================================================= @@ -89,44 +71,32 @@ def _transform_waypoints_trf_to_wrf( class BaseSmoothMotionCommand(TrajectoryMoveCommandBase[_MP]): - """Base class for smooth geometry commands (circle, arc, helix, spline). + """Base class for smooth geometry commands (arc, spline, process move). Subclasses implement generate_main_trajectory() to create Cartesian geometry. This base class handles IK conversion and trajectory building. """ - __slots__ = ( - "_rpy_rad_buf", - "_pose6_buf", - ) + #: Hold the tool to one speed along the whole path (a process move) + #: rather than as fast as the joints allow under the cartesian ceiling. + constant_tool_speed: bool = False - def __init__(self, p: _MP) -> None: - super().__init__(p) - self._rpy_rad_buf = np.zeros(3, dtype=np.float64) - self._pose6_buf = np.zeros(6, dtype=np.float64) - - def get_current_pose(self, state: "ControllerState") -> np.ndarray: - """Get current TCP pose as [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg].""" - current_se3 = get_fkine_se3(state) - se3_rpy(current_se3, self._rpy_rad_buf) - self._pose6_buf[:3] = current_se3[:3, 3] * 1000 # m -> mm - np.degrees(self._rpy_rad_buf, out=self._pose6_buf[3:]) - return self._pose6_buf + __slots__ = () def do_setup(self, state: "ControllerState") -> None: """Pre-compute trajectory from current position.""" guard_homed(state) self.log_debug(" -> Preparing %s...", self.name) - current_pose = self.get_current_pose(state) + start = get_fkine_se3(state).copy() self.log_info( " -> Generating %s from position: %s", self.name, - [round(p, 1) for p in current_pose[:3]], + [round(float(p) * 1000.0, 1) for p in start[:3, 3]], ) - cartesian_trajectory = self.generate_main_trajectory(current_pose) - if cartesian_trajectory is None or len(cartesian_trajectory) == 0: + cartesian_trajectory = self.generate_main_trajectory(start, state) + if len(cartesian_trajectory) == 0: raise TrajectoryPlanningError( make_error( ErrorCode.TRAJ_EMPTY_RESULT, detail="empty cartesian trajectory" @@ -156,7 +126,7 @@ def do_setup(self, state: "ControllerState") -> None: ) ) - guard_joint_path(joint_path.positions) + guard_cartesian_path(joint_path) builder = TrajectoryBuilder( joint_path=joint_path, @@ -167,6 +137,8 @@ def do_setup(self, state: "ControllerState") -> None: dt=INTERVAL_S, cart_vel_limit=LIMITS.cart.hard.velocity.linear * self.p.resolved_speed, cart_acc_limit=LIMITS.cart.hard.acceleration.linear * self.p.accel, + path_knots=joint_path.knots, + constant_tool_speed=self.constant_tool_speed, ) trajectory = builder.build() @@ -180,53 +152,44 @@ def do_setup(self, state: "ControllerState") -> None: trajectory.duration, ) - def generate_main_trajectory(self, effective_start_pose) -> np.ndarray: - """Override this in subclasses to generate the specific motion trajectory.""" + def generate_main_trajectory( + self, start: np.ndarray, state: "ControllerState" + ) -> np.ndarray: + """The (N, 4, 4) SE3 poses the move runs through from ``start``.""" raise NotImplementedError("Subclasses must implement generate_main_trajectory") @register_command(CmdType.MOVEC) -class MoveCCommand(BaseSmoothMotionCommand[MoveCCmd]): +class MoveCCommand(CartesianChainLink, BaseSmoothMotionCommand[MoveCCmd]): """Execute circular arc motion through current → via → end (3-point arc). - Computes circle center and normal from the 3 points, then delegates to - CircularMotion.generate_arc(). + Via and end resolve against the pose the move starts from: absolute in + WRF, tool-frame offsets in TRF. With a blend radius the arc joins a + cartesian blend chain and rounds into the move after it. """ PARAMS_TYPE = MoveCCmd - __slots__ = ("_via", "_end") - - def __init__(self, p: MoveCCmd) -> None: - super().__init__(p) - self._via: np.ndarray = np.asarray(p.via, dtype=np.float64) - self._end: np.ndarray = np.asarray(p.end, dtype=np.float64) + __slots__ = () - def do_setup(self, state: "ControllerState") -> None: - """Transform via/end from TRF if needed, then compute arc.""" - if self.p.frame == "TRF": - tool_pose = get_fkine_se3(state) - _pose6_trf_to_wrf(self.p.via, tool_pose, out=self._via) - _pose6_trf_to_wrf(self.p.end, tool_pose, out=self._end) - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose) -> np.ndarray: - """Generate arc geometry from current position through via to end.""" - start_xyz = effective_start_pose[:3] - via_xyz = self._via[:3] - end_xyz = self._end[:3] - - center, _radius, normal = compute_circle_from_3_points( - start_xyz, via_xyz, end_xyz - ) + def generate_main_trajectory( + self, start: np.ndarray, state: "ControllerState" + ) -> np.ndarray: + """The arc from ``start`` through the via to the end.""" + arc, self.target_pose = self.chain_segment(start, state) + return build_blended_path([arc], [], samples_per_segment=PATH_SAMPLES) - return CircularMotion().generate_arc( - start_pose=effective_start_pose, - end_pose=self._end, - center=center, - normal=normal, - clockwise=False, - ) + def chain_segment( + self, previous: np.ndarray, state: "ControllerState" + ) -> tuple[LineSegment | ArcSegment, np.ndarray]: + via = resolve_pose(previous, self.p.via, self.p.frame, False) + end = resolve_pose(previous, self.p.end, self.p.frame, False) + try: + return ArcSegment(previous, via, end), end + except ValueError as e: + raise TrajectoryPlanningError( + make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=str(e)) + ) from e @register_command(CmdType.MOVES) @@ -235,133 +198,57 @@ class MoveSCommand(BaseSmoothMotionCommand[MoveSCmd]): PARAMS_TYPE = MoveSCmd - __slots__ = ("_waypoints",) - - def __init__(self, p: MoveSCmd) -> None: - super().__init__(p) - self._waypoints: np.ndarray | None = None - - def do_setup(self, state: "ControllerState") -> None: - """Transform parameters if in TRF.""" - self._waypoints = _transform_waypoints_trf_to_wrf( - self.p.waypoints, self.p.frame, state - ) - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose) -> np.ndarray: - """Generate spline starting from actual position.""" - assert self._waypoints is not None - - wps = self._waypoints - motion_gen = SplineMotion() - - first_wp_error = float(np.linalg.norm(wps[0, :3] - effective_start_pose[:3])) - - if first_wp_error > 5.0: - modified_waypoints = np.vstack([effective_start_pose[np.newaxis], wps]) - logger.info( - f" Added start position as first waypoint (distance: {first_wp_error:.1f}mm)" - ) - else: - modified_waypoints = np.vstack([effective_start_pose[np.newaxis], wps[1:]]) - logger.info(" Replaced first waypoint with actual start position") - - duration = self.p.resolved_duration - trajectory = motion_gen.generate_spline( - waypoints=modified_waypoints, - duration=duration, - ) - - logger.debug(f" Generated spline with {len(trajectory)} points") - + __slots__ = () + + def generate_main_trajectory( + self, start: np.ndarray, state: "ControllerState" + ) -> np.ndarray: + """The spline from ``start`` through the waypoints, sampled by its + length whatever duration times it: a duration too short to keep is + stretched to one the arm can, never met by cutting the path.""" + poses = _waypoint_chain(start, self.p.waypoints, self.p.frame) + trajectory = build_spline_path(poses) + logger.debug(" Generated spline with %d poses", len(trajectory)) return trajectory -# Number of SE3 samples per linear segment for move_p -_MOVEP_SAMPLES_PER_SEGMENT: int = 20 +#: Each interior corner of a process move is rounded with this fraction of +#: the shorter adjoining segment. +MOVEP_AUTO_BLEND_FRAC: float = 0.25 @register_command(CmdType.MOVEP) class MovePCommand(BaseSmoothMotionCommand[MovePCmd]): - """Process move — constant TCP speed through waypoints with piecewise linear segments. - - Phase 3 will add auto-blending at corners (Bézier blend zones). - Currently uses sharp piecewise-linear interpolation. - """ + """Process move — the waypoint list as straight segments with every + interior corner rounded, run at one constant tool speed: the TCP sweeps + the path without stopping at a single waypoint.""" PARAMS_TYPE = MovePCmd - - __slots__ = ("_waypoints", "_se3_buf_a", "_se3_buf_b") - - def __init__(self, p: MovePCmd) -> None: - super().__init__(p) - self._waypoints: np.ndarray | None = None - self._se3_buf_a = np.zeros((4, 4), dtype=np.float64) - self._se3_buf_b = np.zeros((4, 4), dtype=np.float64) - - def do_setup(self, state: "ControllerState") -> None: - """Transform parameters if TRF, build trajectory with constant TCP speed.""" - self._waypoints = _transform_waypoints_trf_to_wrf( - self.p.waypoints, self.p.frame, state - ) - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose) -> np.ndarray: - """Generate piecewise-linear Cartesian path through waypoints. - - Each segment is linearly interpolated in SE3 space. - Phase 3 adds Bézier blend zones at corner points. - """ - assert self._waypoints is not None - - wps = self._waypoints - - first_wp_error = float(np.linalg.norm(wps[0, :3] - effective_start_pose[:3])) - if first_wp_error > 5.0: - all_waypoints = np.vstack([effective_start_pose[np.newaxis], wps]) - else: - all_waypoints = np.vstack([effective_start_pose[np.newaxis], wps[1:]]) - - # Pre-compute total SE3 poses for single allocation - n = _MOVEP_SAMPLES_PER_SEGMENT - n_segs = len(all_waypoints) - 1 - total = n_segs * n - (n_segs - 1) # first segment full, rest skip junction - cart_poses = np.empty((total, 4, 4), dtype=np.float64) - cursor = 0 - - for seg_idx in range(n_segs): - wp_a = all_waypoints[seg_idx] - wp_b = all_waypoints[seg_idx + 1] - - se3_from_rpy( - wp_a[0] / 1000.0, - wp_a[1] / 1000.0, - wp_a[2] / 1000.0, - np.radians(wp_a[3]), - np.radians(wp_a[4]), - np.radians(wp_a[5]), - self._se3_buf_a, - ) - se3_from_rpy( - wp_b[0] / 1000.0, - wp_b[1] / 1000.0, - wp_b[2] / 1000.0, - np.radians(wp_b[3]), - np.radians(wp_b[4]), - np.radians(wp_b[5]), - self._se3_buf_b, - ) - - start_i = 0 if seg_idx == 0 else 1 - for i in range(start_i, n): - s = i / (n - 1) - se3_interp(self._se3_buf_a, self._se3_buf_b, s, cart_poses[cursor]) - cursor += 1 + constant_tool_speed = True + + __slots__ = () + + def generate_main_trajectory( + self, start: np.ndarray, state: "ControllerState" + ) -> np.ndarray: + """The polyline through the waypoints with each interior corner + rounded by a quarter of the shorter adjoining segment, sampled by + its length.""" + poses = _waypoint_chain(start, self.p.waypoints, self.p.frame) + lengths = [ + float(np.linalg.norm(poses[i + 1][:3, 3] - poses[i][:3, 3])) * 1000.0 + for i in range(len(poses) - 1) + ] + radii = [ + MOVEP_AUTO_BLEND_FRAC * min(lengths[i], lengths[i + 1]) + for i in range(len(lengths) - 1) + ] + cart_poses = build_composite_cartesian_path(poses, radii) logger.debug( " Generated process move path with %d SE3 poses across %d segments", - cursor, - n_segs, + len(cart_poses), + len(lengths), ) - return cart_poses[:cursor] + return cart_poses diff --git a/parol6/commands/gripper_commands.py b/parol6/commands/gripper_commands.py index 8115d67..aab24a6 100644 --- a/parol6/commands/gripper_commands.py +++ b/parol6/commands/gripper_commands.py @@ -11,12 +11,18 @@ from enum import Enum from parol6.commands.base import Debouncer, ExecutionStatusCode, MotionCommand +from parol6.config import INTERVAL_S from parol6.server.state import ControllerState from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode logger = logging.getLogger(__name__) +#: Control ticks an electric gripper's calibration is given to finish. The +#: calibrated bit in the gripper's status byte is not read: it is unverified +#: across the supported grippers (see ``ControllerState.gripper_calibrated``). +CALIBRATE_TICKS = 200 + class ElectricGripperState(Enum): """State machine states for the electric gripper command engine.""" @@ -25,6 +31,7 @@ class ElectricGripperState(Enum): SEND_CALIBRATE = "SEND_CALIBRATE" WAITING_CALIBRATION = "WAITING_CALIBRATION" WAIT_FOR_POSITION = "WAIT_FOR_POSITION" + HALTING = "HALTING" @dataclass(frozen=True) @@ -33,57 +40,61 @@ class PneumaticGripperParams: action: str # "open" or "close" port: int # 1 or 2 + dwell_s: float # the jaws' estimated stroke, which the action waits out @dataclass(frozen=True) class ElectricGripperParams: """Parameters for electric gripper action.""" - action: str # "move" or "calibrate" + action: str # "move", "calibrate", "stop" or "idle" position: float speed: float current: int class PneumaticGripperCommand(MotionCommand[PneumaticGripperParams]): - """Control pneumatic gripper (open/close).""" + """Control pneumatic gripper (open/close). The valve switches on the + first tick; the action lasts the jaws' estimated stroke, as the dry run + previews it, since the valve reports nothing when they arrive.""" PARAMS_TYPE = None # Not wire-registered — instantiated by ToolActionCommand __slots__ = ( - "timeout_counter", "_state_to_set", "_port_index", + "_ticks_left", ) def __init__(self, p: PneumaticGripperParams): super().__init__(p) - self.timeout_counter = 1000 self._state_to_set: int = 0 self._port_index: int = 0 + self._ticks_left: int = 1 @classmethod - def from_tool_action(cls, *, action: str, port: int) -> PneumaticGripperCommand: - return cls(PneumaticGripperParams(action=action, port=port)) + def from_tool_action( + cls, *, action: str, port: int, dwell_s: float + ) -> PneumaticGripperCommand: + return cls(PneumaticGripperParams(action=action, port=port, dwell_s=dwell_s)) def do_setup(self, state: ControllerState) -> None: self._state_to_set = 1 if self.p.action == "open" else 0 # port 1 -> index 2, port 2 -> index 3 self._port_index = 2 if self.p.port == 1 else 3 + self._ticks_left = max(1, round(self.p.dwell_s / INTERVAL_S)) def execute_step(self, state: ControllerState) -> ExecutionStatusCode: - self.timeout_counter -= 1 - if self.timeout_counter <= 0: - self.fail(make_error(ErrorCode.MOTN_GRIPPER_TIMEOUT)) - return ExecutionStatusCode.FAILED - state.InOut_out[self._port_index] = self._state_to_set + self._ticks_left -= 1 + if self._ticks_left > 0: + return ExecutionStatusCode.EXECUTING self.finish() return ExecutionStatusCode.COMPLETED class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): - """Control electric gripper (move/calibrate).""" + """Control electric gripper (move/calibrate/stop/idle).""" PARAMS_TYPE = None # Not wire-registered — instantiated by ToolActionCommand @@ -98,6 +109,9 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): _STALL_TICKS: int = 20 # 200 ms at 100 Hz _STALL_DEAD_BAND: int = 2 # progress threshold, 2/255 ≈ 0.8% of full range _GRACE_TICKS: int = 10 # 100 ms before stall checks begin + # A stop is done once the jaws hold still, within one position byte, + # for this many ticks. + _STILL_TICKS: int = 5 __slots__ = ( "state", @@ -109,6 +123,8 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): "_stall_best_distance", "_stall_remaining", "_grace_remaining", + "_last_feedback", + "_still_remaining", ) def __init__(self, p: ElectricGripperParams): @@ -122,6 +138,8 @@ def __init__(self, p: ElectricGripperParams): self._stall_best_distance = 256 # larger than any possible distance self._stall_remaining = self._STALL_TICKS self._grace_remaining = self._GRACE_TICKS + self._last_feedback = -1 + self._still_remaining = self._STILL_TICKS @classmethod def from_tool_action( @@ -138,11 +156,43 @@ def from_tool_action( ) ) + def halt(self, state: ControllerState) -> None: + """Stop the jaws where they are (stop/estop), keeping the grip: + re-target the reported position with the move bit still set, so the + firmware is already in tolerance and holds there. Clearing the bit + would release a part the jaws are holding. A calibration has no + position to hold and is simply ended; an uncalibrated gripper has + none either and is released, as its own stop action releases it.""" + if self.state in ( + ElectricGripperState.SEND_CALIBRATE, + ElectricGripperState.WAITING_CALIBRATION, + ): + state.gripper_hw.mode = 0 + return + if not state.gripper_calibrated: + self._release(state) + return + self._hold_in_place(state) + + @staticmethod + def _hold_in_place(state: ControllerState) -> None: + hw = state.gripper_hw + hw.target_position = hw.feedback_position + hw.mode = 0 + hw.set_command_bits(move_active=True, estop=not state.InOut_in[4]) + + @staticmethod + def _release(state: ControllerState) -> None: + state.gripper_hw.mode = 0 + state.gripper_hw.set_command_bits( + move_active=False, estop=not state.InOut_in[4] + ) + def do_setup(self, state: ControllerState) -> None: self._hw_position = int(round(self.p.position * 255)) self._hw_speed = max(1, int(round(self.p.speed * 255))) if self.p.action == "calibrate": - self.wait_counter = 200 + self.wait_counter = CALIBRATE_TICKS def execute_step(self, state: ControllerState) -> ExecutionStatusCode: self.timeout_counter -= 1 @@ -153,11 +203,35 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: hw = state.gripper_hw if self.state == ElectricGripperState.START: - if self.p.action == "calibrate": + action = self.p.action + if action == "calibrate": self.state = ElectricGripperState.SEND_CALIBRATE + elif action == "idle" or ( + action == "stop" and not state.gripper_calibrated + ): + # An uncalibrated gripper has no position to hold: re-targeting + # its reported 0 would drive it fully open. + self._release(state) + self.finish() + return ExecutionStatusCode.COMPLETED + elif action == "stop": + self._hold_in_place(state) + self.state = ElectricGripperState.HALTING else: self.state = ElectricGripperState.WAIT_FOR_POSITION + if self.state == ElectricGripperState.HALTING: + position = hw.feedback_position + if abs(position - self._last_feedback) <= 1: + self._still_remaining -= 1 + if self._still_remaining <= 0: + self.finish() + return ExecutionStatusCode.COMPLETED + else: + self._still_remaining = self._STILL_TICKS + self._last_feedback = position + return ExecutionStatusCode.EXECUTING + if self.state == ElectricGripperState.SEND_CALIBRATE: logger.debug(" -> Sending one-shot calibrate command...") hw.mode = 1 @@ -169,6 +243,7 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: if self.wait_counter <= 0: logger.info(" -> Calibration delay finished.") hw.mode = 0 + state.gripper_calibrated = True self.finish() return ExecutionStatusCode.COMPLETED return ExecutionStatusCode.EXECUTING diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py index fe109a7..1de1a64 100644 --- a/parol6/commands/joint_commands.py +++ b/parol6/commands/joint_commands.py @@ -19,6 +19,7 @@ from parol6.commands._collision_guard import guard_joint_path from parol6.commands.base import TrajectoryMoveCommandBase, guard_homed from parol6.config import ( + LIMITS, INTERVAL_S, MAX_BLEND_LOOKAHEAD, steps_to_rad, @@ -30,6 +31,7 @@ from parol6.utils.error_codes import ErrorCode from parol6.utils.errors import IKError, TrajectoryPlanningError from parol6.utils.ik import solve_ik +from parol6.utils.joint_limits import joint_outside_travel_rad from pinokin import se3_from_rpy _MP = TypeVar("_MP", bound=MotionParamsMixin) @@ -40,6 +42,25 @@ logger = logging.getLogger(__name__) +def _require_inside_limits(target_rad: np.ndarray) -> None: + """Refuse a joint target outside a joint's travel rather than plan a + move into the stop (par6's ``require_inside_soft``). A joint parked on + its limit and left where it is stays inside, however its motor step + rounds.""" + j = joint_outside_travel_rad(target_rad) + if j >= 0: + lo, hi = LIMITS.joint.position.deg[j] + raise TrajectoryPlanningError( + make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail=( + f"joint {j + 1} target {np.degrees(target_rad[j]):.2f} deg " + f"is outside [{lo:.2f}, {hi:.2f}] deg" + ), + ) + ) + + class JointMoveCommandBase(TrajectoryMoveCommandBase[_MP]): """Base class for joint-space trajectory commands. @@ -83,6 +104,7 @@ def do_setup(self, state: ControllerState) -> None: steps_to_rad(state.Position_in, self._q_rad_buf) target_rad = self._get_target_rad(state, self._q_rad_buf) current_rad = self._q_rad_buf + _require_inside_limits(target_rad) joint_path = JointPath.interpolate(current_rad, target_rad, n_samples=50) guard_joint_path(joint_path.positions) @@ -144,6 +166,9 @@ def do_setup_with_blend( for i, cmd in enumerate(chain): target_rad = cmd._get_target_rad(state, current_rad) + # A relative target resolves against the one before it, so the + # limit check a lone move makes has to run on every link. + _require_inside_limits(target_rad) waypoints_rad.append(target_rad) if i < len(chain) - 1: blend_radii_mm.append(cmd.blend_radius) diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index bc56679..4ea2fa9 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -116,7 +116,7 @@ def compute(self, state: "ControllerState") -> Response: @register_command(CmdType.JOINT_SPEEDS) class JointSpeedsCommand(QueryCommand[JointSpeedsCmd]): - """Get current joint speeds.""" + """Current joint velocities in deg/s, the units of the status stream.""" PARAMS_TYPE = JointSpeedsCmd QUERY_TYPE = QueryType.SPEEDS @@ -124,7 +124,9 @@ class JointSpeedsCommand(QueryCommand[JointSpeedsCmd]): __slots__ = () def compute(self, state: "ControllerState") -> Response: - return SpeedsResultStruct(speeds=state.Speed_in.tolist()) + cache = get_cache() + cache.update_from_state(state) + return SpeedsResultStruct(speeds=cache.speeds_deg_s.tolist()) @register_command(CmdType.STATUS) @@ -143,7 +145,7 @@ def compute(self, state: "ControllerState") -> Response: return StatusResultStruct( pose=cache.pose.tolist(), angles=cache.angles_deg.tolist(), - speeds=cache.speeds_rad_s.tolist(), + speeds=cache.speeds_deg_s.tolist(), io=cache.io.tolist(), tool_status=[ ts.key, @@ -283,10 +285,12 @@ class CommandCompletionCommand(QueryCommand[CommandCompletionCmd]): __slots__ = () def compute(self, state: "ControllerState") -> Response: + failure = state.command_failure(self.p.command_index) return CommandCompletionResultStruct( command_index=self.p.command_index, session_id=state.status_session_id, completed=state.command_completed(self.p.command_index), + error=None if failure is None else failure.to_wire(), ) diff --git a/parol6/commands/servo_commands.py b/parol6/commands/servo_commands.py index c5f300f..3028d65 100644 --- a/parol6/commands/servo_commands.py +++ b/parol6/commands/servo_commands.py @@ -4,34 +4,51 @@ ServoJ: joint-space position target via StreamingExecutor ServoJPose: joint-space target from Cartesian pose (IK + StreamingExecutor) ServoL: Cartesian-space target via CartesianStreamingExecutor + IK + +Every servo stream keeps out of collision as a jog does: a target that +would collide is refused on arrival, and each tick the configuration the +stream is heading for one lookahead horizon on is checked; a contact ahead +brakes the stream to rest short of it, and it ends there with the +collision as its error. No step of that brake is commanded into contact. """ import logging import math +from typing import TypeVar import numpy as np from numba import njit import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.commands._collision_guard import ( + collision_blocked, + collision_stop, + stream_lookahead, +) from parol6.config import ( INTERVAL_S, LIMITS, rad_to_steps, steps_to_rad, ) +from parol6.motion.streaming_executors import below_speed from parol6.protocol.wire import CmdType, ServoJCmd, ServoJPoseCmd, ServoLCmd from parol6.server.command_registry import register_command from parol6.server.state import ControllerState, get_fkine_se3 -from parol6.utils.error_catalog import make_error +from parol6.utils.error_catalog import RobotError, make_error from parol6.utils.error_codes import ErrorCode -from parol6.utils.errors import IKError +from parol6.utils.errors import IKError, TrajectoryPlanningError from parol6.utils.ik import RateLimitedWarning, solve_ik from pinokin import se3_from_rpy -from .base import ExecutionStatusCode, MotionCommand +from .base import ExecutionStatusCode, MotionCommand, guard_homed logger = logging.getLogger(__name__) +# A servo stream that goes silent for this long is braked to rest and +# held, rather than driven on to a target the client stopped refreshing. +SERVO_GRACE_S: float = 0.25 + # Velocity ratio uses hardware limits (jog limits only apply to jog_j/jog_l) _JOINT_MAX_STEP_INV = 1.0 / ( np.array(LIMITS.joint.hard.velocity, dtype=np.float64) * INTERVAL_S @@ -49,6 +66,9 @@ ) _ik_warn = RateLimitedWarning() +# Squared speed under which a braking stream counts as at rest. +_REST_SQ = 1e-8 + @njit(cache=True) def _max_vel_ratio_jit( @@ -78,35 +98,172 @@ def _max_vel_ratio_jit( return max_ratio -def _streaming_joint_step( - cmd: "ServoJCommand | ServoJPoseCommand", state: ControllerState -) -> ExecutionStatusCode: - """Shared execute_step for ServoJ and ServoJPose commands.""" - se = state.streaming_executor +@njit(cache=True) +def _step_toward_jit(q_commanded: np.ndarray, q_target: np.ndarray) -> float: + """Move ``q_commanded`` toward ``q_target`` in place, as far as one tick + of every joint's hardware speed allows: every joint's step is divided by + the worst joint's share of its budget, so the arm keeps its direction + through joint space and only loses speed. Returns that share; at or + under 1 the step landed on ``q_target``.""" + ratio = _max_vel_ratio_jit(q_target, q_commanded) + if ratio > 1.0: + inv = 1.0 / ratio + for i in range(q_commanded.shape[0]): + q_commanded[i] += (q_target[i] - q_commanded[i]) * inv + else: + for i in range(q_commanded.shape[0]): + q_commanded[i] = q_target[i] + return ratio + + +#: The target a braking joint stream ramps toward. Read, never written: +#: ``set_jog_velocity`` copies it into the executor's own buffer. +_ZERO_JOINT_VEL = np.zeros(6, dtype=np.float64) + + +_JP = TypeVar("_JP", ServoJCmd, ServoJPoseCmd) + + +class _JointServoCommand(MotionCommand[_JP]): + """A joint-space servo stream: the StreamingExecutor interpolates to + each target; a contact ahead, an unreachable target or a client gone + silent brakes it in joint space to a hold.""" + + streamable = True + + __slots__ = ( + "_initialized", + "_retarget", + "_speed_applied", + "_accel_applied", + "_braking", + "_collision", + "_brake_error", + "_target_rad", + "_target_q", + "_q_sent", + "_la_buf", + ) + + def __init__(self, p: _JP): + super().__init__(p) + self._initialized = False + self._retarget = True + self._speed_applied = -1.0 + self._accel_applied = -1.0 + self._braking = False + # A collision brake stays latched across the datagrams that keep + # the stream alive: the stream ends where it stopped, in error. + self._collision = False + self._brake_error: RobotError | None = None + self._target_rad = [0.0] * 6 + self._target_q = np.zeros(6, dtype=np.float64) + # The configuration last commanded. + self._q_sent = np.zeros(6, dtype=np.float64) + self._la_buf = np.zeros(6, dtype=np.float64) + + def _set_target( + self, q: "list[float] | np.ndarray", state: ControllerState + ) -> None: + """Aim the stream at ``q`` [rad], refusing it on arrival if it would + collide: a running stream brakes to rest with the collision as its + error, one not yet running never starts.""" + for i in range(6): + v = float(q[i]) + self._target_rad[i] = v + self._target_q[i] = v + checker = PAROL6_ROBOT.collision + if checker is None: + return + running = self._initialized and state.streaming_executor.active + if not running: + steps_to_rad(state.Position_in, self._q_sent) + if not collision_blocked(checker, self._q_sent, self._target_q): + return + error = collision_stop(state, checker, self._target_q) + if not running: + raise TrajectoryPlanningError(error) + self._braking = True + self._collision = True + self._brake_error = error + + def _step_clear( + self, state: ControllerState, pos: np.ndarray, vel: np.ndarray + ) -> bool: + """Whether the stream may command ``pos``, reached at ``vel``. While + it tracks its target, a contact one lookahead horizon ahead — never + past the target, where it stops — starts the collision brake. A + brake comes to rest short of any horizon, so each of its steps is + checked on its own instead; one that would reach a contact is held + back, and the stream ends there in collision.""" + checker = PAROL6_ROBOT.collision + if checker is None: + return True + if self._braking: + if not collision_blocked(checker, self._q_sent, pos): + return True + if not self._collision: + self._brake_error = collision_stop(state, checker, pos) + self._collision = True + return False + stream_lookahead(pos, vel, self._la_buf, self._target_q) + if not collision_blocked(checker, self._q_sent, self._la_buf): + return True + logger.warning("[%s] collision predicted - braking", self.name) + self._brake_error = collision_stop(state, checker, self._la_buf) + self._braking = True + self._collision = True + return not collision_blocked(checker, self._q_sent, pos) - if not cmd._initialized or not se.active: - steps_to_rad(state.Position_in, cmd._q_rad_buf) - se.sync_position(cmd._q_rad_buf) - se.set_limits(cmd.p.speed, cmd.p.accel) - cmd._initialized = True + def execute_step(self, state: ControllerState) -> ExecutionStatusCode: + se = state.streaming_executor - se.set_position_target(cmd._target_rad) - pos_rad, _vel, finished = se.tick() + if not self._initialized or not se.active: + steps_to_rad(state.Position_in, self._q_sent) + se.sync_position(self._q_sent) + self._initialized = True + self._retarget = True + self._speed_applied = -1.0 + self._accel_applied = -1.0 + if self.p.speed != self._speed_applied or self.p.accel != self._accel_applied: + # A stream re-targets through assign_params + do_setup, so a + # change of speed or accel mid-stream reaches the limiter here. + se.set_limits(self.p.speed, self.p.accel) + self._speed_applied = self.p.speed + self._accel_applied = self.p.accel + + if self._braking or self.timer_expired(): + self._braking = True + se.set_jog_velocity(_ZERO_JOINT_VEL) + elif self._retarget: + se.set_position_target(self._target_rad) + self._retarget = False + pos_rad, vel, finished = se.tick() + if self._step_clear(state, pos_rad, vel): + self._q_sent[:] = pos_rad + rad_to_steps(self._q_sent, self._steps_buf) + self.set_move_position(state, self._steps_buf) - cmd._pos_rad_buf[:] = pos_rad - rad_to_steps(cmd._pos_rad_buf, cmd._steps_buf) - cmd.set_move_position(state, cmd._steps_buf) + if self._braking: + if not (finished or below_speed(vel, _REST_SQ)): + return ExecutionStatusCode.EXECUTING + se.active = False + if self._brake_error is not None: + self.fail(self._brake_error) + return ExecutionStatusCode.FAILED + self.finish() + return ExecutionStatusCode.COMPLETED - if finished: - se.active = False - cmd.finish() - return ExecutionStatusCode.COMPLETED + if finished: + se.active = False + self.finish() + return ExecutionStatusCode.COMPLETED - return ExecutionStatusCode.EXECUTING + return ExecutionStatusCode.EXECUTING @register_command(CmdType.SERVOJ) -class ServoJCommand(MotionCommand[ServoJCmd]): +class ServoJCommand(_JointServoCommand[ServoJCmd]): """Streaming joint position target. Uses StreamingExecutor with set_position_target() for smooth Ruckig- @@ -114,55 +271,69 @@ class ServoJCommand(MotionCommand[ServoJCmd]): """ PARAMS_TYPE = ServoJCmd - streamable = True - __slots__ = ( - "_initialized", - "_target_rad", - "_pos_rad_buf", - ) + __slots__ = ("_angles_seen", "_angles_rad") def __init__(self, p: ServoJCmd): super().__init__(p) - self._initialized = False - self._target_rad = [0.0] * 6 - self._pos_rad_buf = np.zeros(6, dtype=np.float64) + self._angles_seen: list[float] | None = None + self._angles_rad = np.zeros(6, dtype=np.float64) def do_setup(self, state: ControllerState) -> None: - # Target arrives in degrees; convert into pre-allocated radian buffer + guard_homed(state) + self.start_timer(SERVO_GRACE_S) + if self._collision: + return + if self._braking: + # A client heard from again ends the brake its silence began. + self._braking = False + self._retarget = True + angles = self.p.angles + if angles == self._angles_seen: + return + self._angles_seen = angles for i in range(6): - self._target_rad[i] = math.radians(self.p.angles[i]) - - def execute_step(self, state: ControllerState) -> ExecutionStatusCode: - return _streaming_joint_step(self, state) + self._angles_rad[i] = math.radians(angles[i]) + self._retarget = True + self._set_target(self._angles_rad, state) @register_command(CmdType.SERVOJ_POSE) -class ServoJPoseCommand(MotionCommand[ServoJPoseCmd]): +class ServoJPoseCommand(_JointServoCommand[ServoJPoseCmd]): """Streaming joint position target via Cartesian pose. Solves IK for the target pose, then uses StreamingExecutor like ServoJ. """ PARAMS_TYPE = ServoJPoseCmd - streamable = True - __slots__ = ( - "_initialized", - "_target_rad", - "_pos_rad_buf", - "_target_se3", - ) + __slots__ = ("_target_se3", "_pose_seen", "_unreachable") def __init__(self, p: ServoJPoseCmd): super().__init__(p) - self._initialized = False - self._target_rad = [0.0] * 6 - self._pos_rad_buf = np.zeros(6, dtype=np.float64) self._target_se3 = np.zeros((4, 4), dtype=np.float64) + self._pose_seen: list[float] | None = None + # The refusal of the pose last seen, when the solver could not + # reach it: a client resending it gets the same answer unsolved. + self._unreachable: RobotError | None = None def do_setup(self, state: ControllerState) -> None: + guard_homed(state) + self.start_timer(SERVO_GRACE_S) + if self._collision: + return pose = self.p.pose + if pose == self._pose_seen: + if self._braking and self._unreachable is None: + # A client heard from again ends the brake its silence began. + self._braking = False + self._retarget = True + return + self._pose_seen = pose + self._unreachable = None + self._braking = False + self._brake_error = None + self._retarget = True # Build target SE3 from [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg] se3_from_rpy( @@ -179,18 +350,21 @@ def do_setup(self, state: ControllerState) -> None: steps_to_rad(state.Position_in, self._q_rad_buf) ik_result = solve_ik(PAROL6_ROBOT.robot, self._target_se3, self._q_rad_buf) if not ik_result.success or ik_result.q is None: - raise IKError( - make_error( - ErrorCode.IK_TARGET_UNREACHABLE, - detail=f"SERVOJ_POSE: IK failed for pose {[round(v, 1) for v in pose]}", - ) + # Unreachable: the stream brakes to rest where it is and ends in + # error there, rather than stopping dead on a setup failure; the + # next reachable target starts it again. A stream that was not + # running has nothing to brake and is refused outright. + error = make_error( + ErrorCode.IK_TARGET_UNREACHABLE, + detail=f"SERVOJ_POSE: IK failed for pose {[round(v, 1) for v in pose]}", ) - - for i in range(6): - self._target_rad[i] = float(ik_result.q[i]) - - def execute_step(self, state: ControllerState) -> ExecutionStatusCode: - return _streaming_joint_step(self, state) + if not (self._initialized and state.streaming_executor.active): + raise IKError(error) + self._unreachable = error + self._braking = True + self._brake_error = error + return + self._set_target(ik_result.q, state) @register_command(CmdType.SERVOL) @@ -200,7 +374,12 @@ class ServoLCommand(MotionCommand[ServoLCmd]): CSE drives the Cartesian path (with its own internal Ruckig for smooth TCP motion). IK converts each smoothed pose to joint space. If any joint's per-tick delta exceeds its hardware velocity limit, all deltas - are scaled proportionally. + are scaled proportionally — on every path, the brakes included — and + the stream ends only once the joints have caught up with the tool. + + A pose the solver cannot reach brakes the tool along its line and holds + it there; the next target the client sends that differs from the one + it braked away from resumes the stream from where the tool is. """ PARAMS_TYPE = ServoLCmd @@ -209,25 +388,61 @@ class ServoLCommand(MotionCommand[ServoLCmd]): __slots__ = ( "_initialized", "_ik_stopping", + "_held", + "_silent", + "_collision_error", "_target_se3", - "_pos_rad_buf", + "_pose_seen", + "_brake_pose", "_q_commanded", + "_q_prev", "_q_ik_seed", + "_q_target", + "_target_solved", "_dq_buf", + "_la_buf", ) def __init__(self, p: ServoLCmd): super().__init__(p) self._initialized = False self._ik_stopping = False + # Braked to rest short of an unreachable target: nothing moves + # until the client names somewhere new or goes silent. + self._held = False + self._silent = False + # Set once a contact stops the stream; latched across datagrams. + self._collision_error: RobotError | None = None self._target_se3 = np.zeros((4, 4), dtype=np.float64) - self._pos_rad_buf = np.zeros(6, dtype=np.float64) + self._pose_seen: list[float] | None = None + # The wire pose the running brake gave up on. + self._brake_pose: list[float] | None = None self._q_commanded = np.zeros(6, dtype=np.float64) + self._q_prev = np.zeros(6, dtype=np.float64) self._q_ik_seed = np.zeros(6, dtype=np.float64) + # The joints the target solves to, when the solver reached it. + self._q_target = np.zeros(6, dtype=np.float64) + self._target_solved = False self._dq_buf = np.zeros(6, dtype=np.float64) + self._la_buf = np.zeros(6, dtype=np.float64) def do_setup(self, state: ControllerState) -> None: + guard_homed(state) + self.start_timer(SERVO_GRACE_S) + self._silent = False + running = self._initialized and state.cartesian_streaming_executor.active pose = self.p.pose + if self._ik_stopping and pose != self._brake_pose: + # Somewhere new ends the brake; the same pose keeps it. The + # brake ran on past what the solver reaches while the arm held, + # so the stream resumes from the arm, not from the brake. + self._ik_stopping = False + self._held = False + self._initialized = False + if pose == self._pose_seen: + return + self._pose_seen = pose + self._target_solved = False # Build target SE3 from [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg] se3_from_rpy( @@ -239,6 +454,65 @@ def do_setup(self, state: ControllerState) -> None: math.radians(pose[5]), self._target_se3, ) + if self._collision_error is None: + self._refuse_colliding_target(state, running) + + def _refuse_colliding_target(self, state: ControllerState, running: bool) -> None: + """Refuse a target whose solution would collide, on arrival: a + ``running`` stream brakes to rest with the collision as its error, + one not yet running never starts. A target the solver cannot reach + is left to the stream, which brakes short of it.""" + checker = PAROL6_ROBOT.collision + if checker is None: + return + cse = state.cartesian_streaming_executor + if not running: + steps_to_rad(state.Position_in, self._q_commanded) + self._q_ik_seed[:] = self._q_commanded + ik_result = solve_ik(PAROL6_ROBOT.robot, self._target_se3, self._q_ik_seed) + if not ik_result.success: + return + self._q_target[:] = ik_result.q + self._target_solved = True + if not collision_blocked(checker, self._q_commanded, self._q_target): + return + error = collision_stop(state, checker, self._q_target) + if not running: + raise TrajectoryPlanningError(error) + self._collision_error = error + cse.stop() + + def _step_clear(self, state: ControllerState, braking: bool) -> bool: + """Whether the step just taken, from ``_q_prev`` to ``_q_commanded``, + may be commanded; a blocked one is taken back. While the stream + tracks its target, a contact one lookahead horizon ahead — never + past the target's joints, where it stops — starts the collision + brake. A brake comes to rest short of any horizon, so each of its + steps is checked on its own instead; one that would reach a contact + ends the stream there in collision.""" + checker = PAROL6_ROBOT.collision + if checker is None: + return True + if not braking: + np.subtract(self._q_commanded, self._q_prev, out=self._dq_buf) + self._dq_buf *= 1.0 / INTERVAL_S + stream_lookahead( + self._q_commanded, + self._dq_buf, + self._la_buf, + self._q_target if self._target_solved else None, + ) + if not collision_blocked(checker, self._q_prev, self._la_buf): + return True + logger.warning("[SERVOL] collision predicted - braking") + self._collision_error = collision_stop(state, checker, self._la_buf) + state.cartesian_streaming_executor.stop() + if not collision_blocked(checker, self._q_prev, self._q_commanded): + return True + if self._collision_error is None: + self._collision_error = collision_stop(state, checker, self._q_commanded) + self._q_commanded[:] = self._q_prev + return False def execute_step(self, state: ControllerState) -> ExecutionStatusCode: cse = state.cartesian_streaming_executor @@ -249,9 +523,27 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: cse.set_limits(self.p.speed, self.p.accel) self._q_commanded[:] = self._q_rad_buf self._q_ik_seed[:] = self._q_rad_buf + self._held = False self._initialized = True - cse.set_pose_target(self._target_se3) + # A client that has gone silent stops refreshing its target: the + # tool brakes along its line and holds where it stops. + if not self._silent and self.timer_expired(): + self._silent = True + cse.stop() + if self._held: + self._send(state) + if self._silent: + cse.active = False + self.finish() + return ExecutionStatusCode.COMPLETED + return ExecutionStatusCode.EXECUTING + + # A brake owns the limiter: re-aiming it at the pose it is braking + # away from would undo the brake on the next tick. + braking = self._collision_error is not None or self._ik_stopping or self._silent + if not braking: + cse.set_pose_target(self._target_se3) smoothed_pose, vel, finished = cse.tick() # Solve IK seeded from previous IK result (branch continuity) @@ -260,50 +552,51 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: smoothed_pose, self._q_ik_seed, ) + # Whether the joints have caught up with the tool. + landed = True if ik_result.success and ik_result.q is not None: - if self._ik_stopping: - logger.info("[SERVOL] IK recovered — resuming") - steps_to_rad(state.Position_in, self._q_rad_buf) - cse.sync_pose(get_fkine_se3(state)) - self._q_commanded[:] = self._q_rad_buf - self._q_ik_seed[:] = self._q_rad_buf - self._ik_stopping = False - # Let next tick handle normal tracking - else: - self._q_ik_seed[:] = ik_result.q - - dq = self._dq_buf - for i in range(6): - dq[i] = float(ik_result.q[i]) - self._q_commanded[i] - - # Velocity ratio: worst-case joint vs its per-tick hard limit - ratio = _max_vel_ratio_jit(ik_result.q, self._q_commanded) - - if ratio > 1.0: - for i in range(6): - self._q_commanded[i] += dq[i] / ratio - cse.set_limits(self.p.speed / ratio, self.p.accel) - else: - self._q_commanded[:] = ik_result.q - cse.set_limits(self.p.speed, self.p.accel) - else: - # IK failed — graceful deceleration - if not self._ik_stopping: - _ik_warn( - logger, - "[SERVOL] IK failed — decelerating: pos=%s", - smoothed_pose[:3, 3], - ) - cse.stop() - self._ik_stopping = True - - self._pos_rad_buf[:] = self._q_commanded - rad_to_steps(self._pos_rad_buf, self._steps_buf) - self.set_move_position(state, self._steps_buf) - - if finished and not self._ik_stopping: + self._q_ik_seed[:] = ik_result.q + self._q_prev[:] = self._q_commanded + ratio = _step_toward_jit(self._q_commanded, ik_result.q) + landed = ratio <= 1.0 + if not self._step_clear(state, braking): + landed = True + elif not braking: + # Slow the tool by the factor the joints were held back. + cse.set_limits(self.p.speed / max(ratio, 1.0), self.p.accel) + elif not braking: + _ik_warn( + logger, + "[SERVOL] IK failed — decelerating: pos=%s", + smoothed_pose[:3, 3], + ) + cse.stop() + self._ik_stopping = True + self._brake_pose = self.p.pose + + self._send(state) + + if self._collision_error is not None or self._silent or self._ik_stopping: + if not landed or not (finished or below_speed(vel, _REST_SQ)): + return ExecutionStatusCode.EXECUTING + if self._collision_error is not None: + cse.active = False + self.fail_and_idle(state, self._collision_error) + return ExecutionStatusCode.FAILED + if self._silent: + cse.active = False + self.finish() + return ExecutionStatusCode.COMPLETED + self._held = True + return ExecutionStatusCode.EXECUTING + + if finished and landed: self.finish() cse.active = False return ExecutionStatusCode.COMPLETED return ExecutionStatusCode.EXECUTING + + def _send(self, state: ControllerState) -> None: + rad_to_steps(self._q_commanded, self._steps_buf) + self.set_move_position(state, self._steps_buf) diff --git a/parol6/commands/tool_action_command.py b/parol6/commands/tool_action_command.py index 723ff3e..1849d39 100644 --- a/parol6/commands/tool_action_command.py +++ b/parol6/commands/tool_action_command.py @@ -5,10 +5,11 @@ import logging from parol6.commands.base import ExecutionStatusCode, MotionCommand +from parol6.commands.gripper_commands import ElectricGripperCommand from parol6.protocol.wire import CmdType, ToolActionCmd from parol6.server.command_registry import register_command from parol6.server.state import ControllerState -from parol6.tools import get_registry +from parol6.tools import get_registry, tool_action_refusal from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode @@ -32,6 +33,18 @@ def do_setup(self, state: ControllerState) -> None: action = self.p.action.strip().lower() params = self.p.params + # Judged when the action's turn comes: the selection or calibration + # it needs may be the command queued just ahead of it. + refusal = tool_action_refusal( + key, + action, + current_tool=state.current_tool, + gripper_calibrated=state.gripper_calibrated, + ) + if refusal is not None: + self.fail(make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=refusal)) + return + cfg = get_registry().get(key) if cfg is None: raise ValueError(f"Unknown tool '{key}'") @@ -43,6 +56,13 @@ def do_setup(self, state: ControllerState) -> None: delegate.setup(state) self._delegate = delegate + def halt(self, state: ControllerState) -> None: + """Stop the tool where it is, keeping its grip, for whatever discards + the action mid-way. Only an electric gripper has motion to halt; a + pneumatic valve, once switched, strokes to its end.""" + if isinstance(self._delegate, ElectricGripperCommand): + self._delegate.halt(state) + def execute_step(self, state: ControllerState) -> ExecutionStatusCode: if self._delegate is None: self.fail(make_error(ErrorCode.MOTN_GRIPPER_UNKNOWN)) diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 213d17d..55f1f07 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -76,7 +76,8 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: @register_command(CmdType.RESET_STATE) class ResetStateCommand(SystemCommand[ResetStateCmd]): """ - Instantly reset controller state to initial values. + Reset the program-level controller state (see ``ControllerState.reset``). + The arm, the protective-stop latch and the I/O are left as they are. """ PARAMS_TYPE = ResetStateCmd @@ -85,7 +86,6 @@ class ResetStateCommand(SystemCommand[ResetStateCmd]): def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: state.reset() - self._sync_mock = True self.finish() return ExecutionStatusCode.COMPLETED diff --git a/parol6/config.py b/parol6/config.py index c3056c5..c29fd3c 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -21,6 +21,14 @@ # Command queue limits MAX_COMMAND_QUEUE_SIZE: int = 100 MAX_BLEND_LOOKAHEAD: int = int(os.getenv("PAROL6_MAX_BLEND_LOOKAHEAD", "100")) +# How long the planner waits for the next command of a blend chain, once +# nothing plays ahead of it, before planning what it has: a corner cannot be +# rounded until both of its moves have arrived. +BLEND_HOLD_S: float = float(os.getenv("PAROL6_BLEND_HOLD_S", "0.1")) +# It times the planner's queue waits: NaN or more than a lock wait takes +# kills the planner, and zero or less spins it and plans every move alone. +if not 0.0 < BLEND_HOLD_S <= threading.TIMEOUT_MAX: + raise ValueError("PAROL6_BLEND_HOLD_S must be a positive, finite duration") MAX_POLL_COUNT: int = 25 # Max UDP messages to read per control tick # Further messages read in a tick whose batch filled up. A client streaming # faster than the tick leaves a backlog in the socket; it is already stale, so @@ -584,10 +592,8 @@ def _build_cart_kinodynamic( ): raise ValueError("Joint limits must be positive. Check PAROL6_ROBOT config.") -# Jog min speeds - derived from control rate (1 step per tick minimum) +# Jog min speed - derived from control rate (1 step per tick minimum) JOG_MIN_STEPS: int = int(CONTROL_RATE_HZ) # steps/s -CART_LIN_JOG_MIN: float = CONTROL_RATE_HZ / 100 # mm/s (scales with control rate) -CART_ANG_JOG_MIN: float = 1.0 # deg/s # Per-joint IK safety margins (radians) - [min_margin, max_margin] per joint # Direction-aware: J3 backwards bend (max) is a trap, but inward (min) is safe diff --git a/parol6/motion/__init__.py b/parol6/motion/__init__.py index db520f5..e9631b9 100644 --- a/parol6/motion/__init__.py +++ b/parol6/motion/__init__.py @@ -15,11 +15,9 @@ """ from parol6.motion.geometry import ( - CircularMotion, - SplineMotion, - blend_path_into, build_composite_cartesian_path, build_composite_joint_path, + build_spline_path, compute_circle_from_3_points, joint_path_to_tcp_poses, ) @@ -46,11 +44,9 @@ "CartesianStreamingExecutor", "RuckigExecutorBase", # Geometry generators - "CircularMotion", - "SplineMotion", + "build_spline_path", "joint_path_to_tcp_poses", # Blend infrastructure - "blend_path_into", "build_composite_cartesian_path", "build_composite_joint_path", "compute_circle_from_3_points", diff --git a/parol6/motion/geometry.py b/parol6/motion/geometry.py index b581a79..ae7717f 100644 --- a/parol6/motion/geometry.py +++ b/parol6/motion/geometry.py @@ -9,213 +9,85 @@ """ import logging -from typing import TYPE_CHECKING, Any +import math +from collections.abc import Sequence +from typing import TYPE_CHECKING import numpy as np from numpy.typing import NDArray -from pinokin import batch_se3_interp, se3_from_rpy, se3_interp, so3_rpy +from pinokin import batch_se3_interp, se3_interp, so3_rpy from scipy.interpolate import CubicSpline from scipy.spatial.transform import Rotation, Slerp if TYPE_CHECKING: from pinokin import Robot -from parol6.config import CONTROL_RATE_HZ, PATH_SAMPLES - logger = logging.getLogger(__name__) -DEFAULT_CONTROL_RATE = CONTROL_RATE_HZ - - -class _ShapeGenerator: - """Base class for geometry generation.""" - - def __init__(self, control_rate: float | None = None): - self.control_rate = ( - control_rate if control_rate is not None else DEFAULT_CONTROL_RATE - ) - - -class CircularMotion(_ShapeGenerator): - """Generate arc trajectories in 3D space. - - Returns (N, 6) arrays of [x, y, z, rx, ry, rz] poses. - Position units match input units (typically mm). - Orientation is in degrees. - """ - - def generate_arc( - self, - start_pose: NDArray, - end_pose: NDArray, - center: NDArray, - normal: NDArray | None = None, - clockwise: bool = False, - n_samples: int = PATH_SAMPLES, - ) -> np.ndarray: - """Generate a 3D circular arc trajectory (uniformly sampled geometry). - - Args: - start_pose: Start pose [x, y, z, rx, ry, rz] (mm, degrees) - end_pose: End pose [x, y, z, rx, ry, rz] (mm, degrees) - center: Arc center point [x, y, z] (mm) - normal: Normal vector defining arc plane (auto-computed if None) - clockwise: If True, arc goes clockwise when viewed from normal - n_samples: Number of sample points along the arc - - Returns: - (N, 4, 4) array of SE3 poses along the arc (meters, radians). - """ - start_pos = start_pose[:3] - end_pos = end_pose[:3] - - r1 = start_pos - center - r2 = end_pos - center - - if normal is None: - normal = np.cross(r1, r2) - if np.linalg.norm(normal) < 1e-6: - normal = np.array([0, 0, 1]) - normal_unit = normal / np.linalg.norm(normal) - - r1_norm = r1 / np.linalg.norm(r1) - r2_norm = r2 / np.linalg.norm(r2) - cos_angle = np.clip(np.dot(r1_norm, r2_norm), -1, 1) - arc_angle = np.arccos(cos_angle) - - # Full circle: start ≈ end → 2π arc, not zero - if arc_angle < 1e-6 and float(np.linalg.norm(r1 - r2)) < 1.0: - arc_angle = 2 * np.pi - - cross = np.cross(r1_norm, r2_norm) - if np.dot(cross, normal_unit) < 0: - arc_angle = 2 * np.pi - arc_angle - if clockwise: - arc_angle = -arc_angle - - num_points = max(2, n_samples) - - t_values = np.linspace(0, 1, num_points) if num_points > 1 else np.array([1.0]) - angles = t_values * arc_angle - - rotvecs = np.outer(angles, normal_unit) # (num_points, 3) - rotations = Rotation.from_rotvec(rotvecs) - positions = center + rotations.apply(r1) # (num_points, 3) in mm - - # Build SE3 start/end from 6D poses, then interpolate orientation in - # SE3 space (Lie-algebra geodesic) to avoid gimbal-lock Euler issues. - start_se3 = np.empty((4, 4), dtype=np.float64) - end_se3 = np.empty((4, 4), dtype=np.float64) - se3_from_rpy( - start_pose[0] / 1000.0, - start_pose[1] / 1000.0, - start_pose[2] / 1000.0, - np.radians(start_pose[3]), - np.radians(start_pose[4]), - np.radians(start_pose[5]), - start_se3, - ) - se3_from_rpy( - end_pose[0] / 1000.0, - end_pose[1] / 1000.0, - end_pose[2] / 1000.0, - np.radians(end_pose[3]), - np.radians(end_pose[4]), - np.radians(end_pose[5]), - end_se3, - ) - - trajectory = np.empty((num_points, 4, 4), dtype=np.float64) - batch_se3_interp(start_se3, end_se3, t_values, trajectory) - - # Override translations with the arc-geometry positions (mm → meters) - trajectory[:, :3, 3] = positions / 1000.0 - - return trajectory +#: Rotation weight in the multi-segment path metric sqrt(t² + (w·θ)²) [m/rad]. +PATH_ROT_WEIGHT_M_PER_RAD: float = 0.15 +#: Sampling pitch of a spline or process move on that metric [mm] (par6's +#: ``path_step_m``). +PATH_STEP_MM: float = 2.0 +#: Most poses a spline or process move is sampled into (par6's +#: ``CART_PATH_MAX_STEPS``): bounds the IK and timing work of one path. +PATH_MAX_POINTS: int = 3000 + +#: An arc's end within this of its start asks for a full circle [mm]: the +#: arm settles a hair off the pose it was sent to, so the start the arc is +#: planned from is never exactly the end a script wrote back. +FULL_CIRCLE_MM: float = 1.0 +#: A via within this of the line through an arc's start and end names no +#: circle [mm]. The arm settles within a motor step of a commanded pose, a +#: few hundredths of a millimetre at the tool, so a start read back from +#: it stands that far off a line a script drew through it. +ARC_COLLINEAR_MM: float = 0.1 + +_PATH_ROT_WEIGHT_MM_PER_RAD: float = PATH_ROT_WEIGHT_M_PER_RAD * 1000.0 +# Legs shorter than this on the combined metric repeat the waypoint before [mm]. +_REPEAT_MM: float = 1e-6 + + +def _rotation_angle(a: NDArray[np.float64], b: NDArray[np.float64]) -> float: + """Angle between the rotations of two SE3 poses [rad]. + + The atan2 of the relative rotation's sine and cosine: exact to rounding + at zero, where the arccos of a rounded cosine reads a repeated pose as + turned by up to 3e-8 rad.""" + r = a[:3, :3].T @ b[:3, :3] + sine = 0.5 * math.sqrt( + (r[2, 1] - r[1, 2]) ** 2 + (r[0, 2] - r[2, 0]) ** 2 + (r[1, 0] - r[0, 1]) ** 2 + ) + cosine = (r[0, 0] + r[1, 1] + r[2, 2] - 1.0) / 2.0 + return math.atan2(sine, cosine) -class SplineMotion(_ShapeGenerator): - """Generate smooth spline trajectories through waypoints. - - Uses cubic spline interpolation for position and SLERP for orientation. - """ - def generate_spline( - self, - waypoints: NDArray, - timestamps: NDArray | None = None, - duration: float | None = None, - velocity_start: NDArray | None = None, - velocity_end: NDArray | None = None, - ) -> np.ndarray: - """Generate spline trajectory (uniformly sampled geometry). - - Args: - waypoints: (N, 6) array of [x, y, z, rx, ry, rz] waypoints - timestamps: Optional timestamps for each waypoint - duration: Total duration (overrides timestamps scaling) - velocity_start: Start velocity for position [vx, vy, vz] - velocity_end: End velocity for position [vx, vy, vz] - - Returns: - (N, 6) array of poses along the spline - """ - waypoints_arr = np.asarray(waypoints, dtype=float) - num_waypoints = len(waypoints_arr) - - if num_waypoints < 2: - return waypoints_arr - - if timestamps is None: - total_dist = 0.0 - for i in range(1, num_waypoints): - dist = np.linalg.norm(waypoints_arr[i, :3] - waypoints_arr[i - 1, :3]) - total_dist += float(dist) - - if duration is not None: - total_time = duration - else: - total_time = max(0.1, total_dist / 50.0) +def pose_distance_m(a: NDArray[np.float64], b: NDArray[np.float64]) -> float: + """Distance between two SE3 poses on the combined metric + sqrt(translation² + (w·rotation)²) [m]: a reorientation in place still + covers distance.""" + translation = float(np.linalg.norm(b[:3, 3] - a[:3, 3])) + return math.hypot(translation, PATH_ROT_WEIGHT_M_PER_RAD * _rotation_angle(a, b)) - timestamps_arr = np.linspace(0, total_time, num_waypoints) - else: - timestamps_arr = np.asarray(timestamps, dtype=float) - if duration is not None: - scale = duration / timestamps_arr[-1] if timestamps_arr[-1] > 0 else 1.0 - timestamps_arr = timestamps_arr * scale - - if len(timestamps_arr) != len(waypoints_arr): - raise ValueError( - f"Timestamps length ({len(timestamps_arr)}) must match " - f"waypoints length ({len(waypoints_arr)})" - ) - pos_splines = [] - for i in range(3): - # Annotated assignment keeps bc as Any: scipy-stubs' bc_type rejects - # the scalar derivative values scipy requires for 1-D y - if velocity_start is not None and velocity_end is not None: - bc: Any = ((1, float(velocity_start[i])), (1, float(velocity_end[i]))) - else: - bc = "not-a-knot" - spline = CubicSpline(timestamps_arr, waypoints_arr[:, i], bc_type=bc) - pos_splines.append(spline) +def _intervals(length_mm: float, angle_rad: float) -> int: + """Sample intervals a piece of path wants at ``PATH_STEP_MM`` on the + combined metric, at least one.""" + metric = math.hypot(length_mm, _PATH_ROT_WEIGHT_MM_PER_RAD * angle_rad) + return max(1, math.ceil(metric / PATH_STEP_MM)) - # Batch convert euler angles to rotations (vectorized) - euler_angles = waypoints_arr[:, 3:] - key_rots = Rotation.from_euler("xyz", euler_angles, degrees=True) - slerp = Slerp(timestamps_arr, key_rots) - total_time = float(timestamps_arr[-1]) - num_points = max(2, int(total_time * self.control_rate)) - t_eval = np.linspace(0, total_time, num_points) - - trajectory = np.empty((num_points, 6), dtype=np.float64) - for i, spline in enumerate(pos_splines): - trajectory[:, i] = spline(t_eval) - trajectory[:, 3:] = slerp(t_eval).as_euler("xyz", degrees=True) - - return trajectory +def _fit_budget(counts: list[int]) -> None: + """Scale per-piece interval counts down so the whole path stays within + ``PATH_MAX_POINTS`` poses, every piece keeping at least one interval.""" + total = sum(counts) + budget = max(PATH_MAX_POINTS, len(counts) + 1) + if total < budget: + return + factor = (budget - 1) / total + for i, c in enumerate(counts): + counts[i] = max(1, round(c * factor)) def joint_path_to_tcp_poses( @@ -259,10 +131,13 @@ def compute_circle_from_3_points( p2: NDArray[np.float64], p3: NDArray[np.float64], ) -> tuple[NDArray[np.float64], float, NDArray[np.float64]]: - """Compute the circumscribed circle through 3 non-collinear 3D points. + """Compute the circumscribed circle through 3 non-collinear 3D points (mm). + + An end within ``FULL_CIRCLE_MM`` of the start is a full circle through + the via opposite the start. Args: - p1, p2, p3: 3D points (shape (3,)) + p1, p2, p3: 3D points (shape (3,)) in mm Returns: (center, radius, normal): @@ -271,7 +146,9 @@ def compute_circle_from_3_points( normal: Unit normal of the plane containing the circle (3,) Raises: - ValueError: If the 3 points are collinear (no unique circle). + ValueError: If the via lies within ``ARC_COLLINEAR_MM`` of the line + through the start and the end (no unique circle), or all three + points coincide. """ p1 = np.asarray(p1, dtype=np.float64) p2 = np.asarray(p2, dtype=np.float64) @@ -279,10 +156,9 @@ def compute_circle_from_3_points( a = p2 - p1 b = p3 - p1 + b_len = float(np.linalg.norm(b)) - # Full circle: start ≈ end (p1 ≈ p3), via is diametrically opposite. - # Threshold accounts for FK/IK precision (~0.1 mm). - if float(np.linalg.norm(b)) < 1.0: + if b_len < FULL_CIRCLE_MM: a_len = float(np.linalg.norm(a)) if a_len < 1e-12: raise ValueError("All three points are coincident.") @@ -298,7 +174,8 @@ def compute_circle_from_3_points( normal = np.asarray(np.cross(a, b), dtype=np.float64) normal_len = float(np.linalg.norm(normal)) - if normal_len < 1e-12: + # |a × b| / |b| is how far the via stands off the start-end line. + if normal_len < ARC_COLLINEAR_MM * b_len: raise ValueError("Points are collinear; no unique circle exists.") np.divide(normal, normal_len, out=normal) @@ -310,10 +187,7 @@ def compute_circle_from_3_points( aa = float(np.dot(a, a)) bb = float(np.dot(b, b)) ab = float(np.dot(a, b)) - det = aa * bb - ab * ab - if abs(det) < 1e-20: - raise ValueError("Degenerate configuration; cannot compute circle center.") s = (bb * aa - ab * bb) / (2.0 * det) t = (aa * bb - ab * aa) / (2.0 * det) @@ -323,96 +197,214 @@ def compute_circle_from_3_points( return center, radius, normal -def blend_path_into( +class LineSegment: + """A straight cartesian segment: position lerp, orientation geodesic. + + The two are interpolated apart, not as one screw motion: a screw that + turns the tool bows its position off the line between the ends.""" + + __slots__ = ("start", "end", "_length_m", "_angle_rad") + + def __init__(self, start: NDArray[np.float64], end: NDArray[np.float64]) -> None: + self.start = start + self.end = end + self._length_m = float(np.linalg.norm(end[:3, 3] - start[:3, 3])) + self._angle_rad = _rotation_angle(start, end) + + def length_mm(self) -> float: + return self._length_m * 1000.0 + + def angle_rad(self) -> float: + return self._angle_rad + + def sample_into( + self, out: NDArray[np.float64], s_start: float, s_end: float, skip: int + ) -> None: + """Poses at evenly spaced ``t`` from ``s_start`` to ``s_end``, the + first ``skip`` of them left out, written into ``out``.""" + n_total = out.shape[0] + skip + t_values = np.linspace(s_start, s_end, n_total)[skip:] + # The screw's rotation is the geodesic; its translation is replaced. + batch_se3_interp(self.start, self.end, t_values, out) + p0 = self.start[:3, 3] + out[:, :3, 3] = p0 + np.outer(t_values, self.end[:3, 3] - p0) + + def sample(self, t: float, out: NDArray[np.float64]) -> None: + se3_interp(self.start, self.end, t, out) + p0 = self.start[:3, 3] + out[:3, 3] = p0 + t * (self.end[:3, 3] - p0) + + def tangent(self, t: float) -> NDArray[np.float64]: + d = self.end[:3, 3] - self.start[:3, 3] + n = float(np.linalg.norm(d)) + return d / n if n > 1e-12 else np.zeros(3) + + +class ArcSegment: + """A circular arc as a segment: position sweeps the circle through the + via point from start to end, orientation is the geodesic between the + two end poses.""" + + __slots__ = ( + "start", + "end", + "_center_m", + "_r1_m", + "_normal", + "_sweep", + "_angle_rad", + ) + + def __init__( + self, + start: NDArray[np.float64], + via: NDArray[np.float64], + end: NDArray[np.float64], + ) -> None: + self.start = start + self.end = end + start_mm = start[:3, 3] * 1000.0 + end_mm = end[:3, 3] * 1000.0 + center_mm, _radius, normal = compute_circle_from_3_points( + start_mm, via[:3, 3] * 1000.0, end_mm + ) + self._center_m = center_mm / 1000.0 + self._normal = normal + r1 = start[:3, 3] - self._center_m + r2 = end[:3, 3] - self._center_m + n1, n2 = float(np.linalg.norm(r1)), float(np.linalg.norm(r2)) + if n1 < 1e-9 or n2 < 1e-9: + raise ValueError("the arc has no radius") + self._r1_m = r1 + u1, u2 = r1 / n1, r2 / n2 + sweep = float(np.arccos(np.clip(np.dot(u1, u2), -1.0, 1.0))) + if float(np.linalg.norm(end_mm - start_mm)) < FULL_CIRCLE_MM: + sweep = 2.0 * np.pi + elif float(np.dot(np.cross(u1, u2), normal)) < 0.0: + sweep = 2.0 * np.pi - sweep + self._sweep = sweep + self._angle_rad = _rotation_angle(start, end) + + def length_mm(self) -> float: + return float(np.linalg.norm(self._r1_m)) * self._sweep * 1000.0 + + def angle_rad(self) -> float: + return self._angle_rad + + def _position(self, t: float) -> NDArray[np.float64]: + rotation = Rotation.from_rotvec(self._normal * (t * self._sweep)) + return self._center_m + rotation.apply(self._r1_m) + + def sample(self, t: float, out: NDArray[np.float64]) -> None: + se3_interp(self.start, self.end, t, out) + out[:3, 3] = self._position(t) + + def sample_into( + self, out: NDArray[np.float64], s_start: float, s_end: float, skip: int + ) -> None: + n_total = out.shape[0] + skip + t_values = np.linspace(s_start, s_end, n_total)[skip:] + batch_se3_interp(self.start, self.end, t_values, out) + rotations = Rotation.from_rotvec(np.outer(t_values * self._sweep, self._normal)) + out[:, :3, 3] = self._center_m + rotations.apply(self._r1_m) + + def tangent(self, t: float) -> NDArray[np.float64]: + r = self._position(t) - self._center_m + d = np.cross(self._normal, r) + n = float(np.linalg.norm(d)) + return d / n if n > 1e-12 else np.zeros(3) + + +def _cubic_blend_into( entry_pose: NDArray[np.float64], - waypoint_pose: NDArray[np.float64], exit_pose: NDArray[np.float64], + p1: NDArray[np.float64], + p2: NDArray[np.float64], out: NDArray[np.float64], skip: int = 0, ) -> None: - """Write quadratic Bezier blend zone into pre-allocated buffer. - - The blend zone smoothly rounds a corner between two linear Cartesian - segments. It is tangent to the incoming segment at t=0 and to the outgoing - segment at t=1. - - Position follows a quadratic Bezier curve:: - - P(t) = (1-t)^2*E + 2t(1-t)*W + t^2*X - - Orientation is geodesic (SLERP) from entry to exit. + """Write a cubic Bézier blend zone into a pre-allocated buffer. - Args: - entry_pose: SE3 pose at blend zone entry (4x4) - waypoint_pose: SE3 pose at the corner being rounded (4x4) - exit_pose: SE3 pose at blend zone exit (4x4) - out: Output array, shape (n_samples, 4, 4). Written in-place. - skip: Number of initial samples to skip (for junction dedup). + Position follows the cubic through the control points + (entry, p1, p2, exit); orientation is the geodesic from entry to exit. """ E = entry_pose[:3, 3] - W = waypoint_pose[:3, 3] X = exit_pose[:3, 3] n_total = out.shape[0] + skip t = np.linspace(0.0, 1.0, n_total)[skip:] - # Batch SLERP for orientation (entry -> exit) batch_se3_interp(entry_pose, exit_pose, t, out) - # Override translation with quadratic Bezier position omt = 1.0 - t out[:, :3, 3] = ( - np.outer(omt * omt, E) + np.outer(2.0 * omt * t, W) + np.outer(t * t, X) + np.outer(omt * omt * omt, E) + + np.outer(3.0 * omt * omt * t, p1) + + np.outer(3.0 * omt * t * t, p2) + + np.outer(t * t * t, X) ) def build_composite_cartesian_path( waypoints: list[NDArray[np.float64]], blend_radii: list[float], - samples_per_segment: int = PATH_SAMPLES, + samples_per_segment: int | None = None, ) -> NDArray[np.float64]: - """Build a composite Cartesian path with blend zones at intermediate waypoints. - - Concatenates linear Cartesian segments connected by quadratic Bezier blend - zones. Implements ABB-style zone overlap clamping: if two adjacent blend - zones would overlap, both radii are proportionally reduced so they don't - exceed half the segment length. + """A polyline through SE3 waypoints with its interior corners rounded: + :func:`build_blended_path` over straight segments. Args: - waypoints: List of SE3 poses (4x4) defining the path corners. - Must have at least 2 waypoints. - blend_radii: Blend radius (mm) for each intermediate waypoint. - Length must equal ``len(waypoints) - 2`` (no blend at start/end). - ``r=0`` means stop at the waypoint (no blending). - samples_per_segment: Number of linear interpolation samples per segment - - Returns: - (M, 4, 4) ndarray of SE3 poses forming the complete path. - - Raises: - ValueError: If inputs are inconsistent. + waypoints: SE3 poses (4x4) defining the path corners, at least 2. + blend_radii: Blend radius (mm) for each intermediate waypoint, + ``len(waypoints) - 2`` of them; ``0`` means stop at the waypoint. + samples_per_segment: As :func:`build_blended_path` takes it. """ n = len(waypoints) if n < 2: raise ValueError("Need at least 2 waypoints") - if len(blend_radii) != max(0, n - 2): - raise ValueError( - f"Expected {max(0, n - 2)} blend radii, got {len(blend_radii)}" - ) + segments = [LineSegment(waypoints[i], waypoints[i + 1]) for i in range(n - 1)] + return build_blended_path(segments, blend_radii, samples_per_segment) - # No blending for 2-waypoint path - if n == 2: - out = np.empty((samples_per_segment, 4, 4), dtype=np.float64) - _linear_se3_segment_into(waypoints[0], waypoints[1], out) - return out - # Segment lengths in mm (FK transforms are in meters) - seg_lengths: list[float] = [0.0] * (n - 1) - for i in range(n - 1): - seg_lengths[i] = ( - float(np.linalg.norm(waypoints[i + 1][:3, 3] - waypoints[i][:3, 3])) - * 1000.0 - ) +def build_blended_path( + segments: list[LineSegment | ArcSegment], + blend_radii: list[float], + samples_per_segment: int | None = None, +) -> NDArray[np.float64]: + """Build a composite cartesian path from straight and circular segments + whose junctions are rounded by blend zones. + + Each zone trims both adjoining segments by its radius, measured along + the segment (arc length on an arc), and joins the two trim points with + a cubic Bézier whose handles lie along the segments' directions of + travel there, two thirds of the trim long: the zone is tangent to the + incoming segment where it starts and to the outgoing one where it + ends. Between two lines the cubic is exactly the degree-raised + quadratic through the corner point; an arc's zone follows its + curvature into and out of the corner. The ABB zone rule applies: a radius never eats more + than half of either adjoining segment, and two zones sharing a segment + are scaled down together until they fit. + + Args: + segments: The path's segments in order, at least one. + blend_radii: Blend radius (mm) for each junction, ``len(segments) - 1`` + of them; ``0`` means stop at the junction. + samples_per_segment: Poses per run of a segment, a blend zone + taking its share of them. ``None`` samples every run and zone + by its own length at ``PATH_STEP_MM`` on the combined metric, + ``PATH_MAX_POINTS`` poses at most, so a long straight run is + sampled as finely as a short one. + + Returns: + (M, 4, 4) ndarray of SE3 poses forming the complete path. + """ + n_seg = len(segments) + if n_seg < 1: + raise ValueError("Need at least 1 segment") + if len(blend_radii) != n_seg - 1: + raise ValueError(f"Expected {n_seg - 1} blend radii, got {len(blend_radii)}") + + seg_lengths = [seg.length_mm() for seg in segments] # Clamp blend radii (zone overlap prevention) clamped = list(blend_radii) @@ -431,8 +423,8 @@ def build_composite_cartesian_path( clamped[i + 1] *= scale # Pre-compute per-segment trim fractions - seg_exit_frac = [0.0] * (n - 1) - seg_entry_frac = [0.0] * (n - 1) + seg_exit_frac = [0.0] * n_seg + seg_entry_frac = [0.0] * n_seg for i in range(len(clamped)): if clamped[i] > 0: if seg_lengths[i] > 0: @@ -440,98 +432,155 @@ def build_composite_cartesian_path( if seg_lengths[i + 1] > 0: seg_entry_frac[i + 1] = clamped[i] / seg_lengths[i + 1] - # Interleaved precompute: count linear segments and blend zones in order - total_rows = 0 - for seg_idx in range(n - 1): - s_start = seg_entry_frac[seg_idx] - s_end = 1.0 - seg_exit_frac[seg_idx] - if s_end > s_start + 1e-9: - rows = samples_per_segment - if total_rows > 0 and seg_idx > 0: - rows -= 1 - total_rows += rows - if seg_idx < len(clamped) and clamped[seg_idx] > 0: - avg_seg_len = (seg_lengths[seg_idx] + seg_lengths[seg_idx + 1]) / 2.0 - frac = clamped[seg_idx] / avg_seg_len if avg_seg_len > 1e-6 else 0.0 - bs = _blend_sample_count(frac, samples_per_segment) - rows = bs - if total_rows > 0: - rows -= 1 - total_rows += rows - - out = np.empty((total_rows, 4, 4), dtype=np.float64) - row = 0 - # Workspace buffers for blend zone endpoints (hoisted out of loop) entry_buf = np.zeros((4, 4), dtype=np.float64) exit_buf = np.zeros((4, 4), dtype=np.float64) - for seg_idx in range(n - 1): - start = waypoints[seg_idx] - end = waypoints[seg_idx + 1] - + # Size every piece first, so a budget spreads over the whole path: the + # runs of each segment between its zones, and the zones. + pieces: list[tuple[int, bool, float, float]] = [] + counts: list[int] = [] + for seg_idx, seg in enumerate(segments): s_start = seg_entry_frac[seg_idx] s_end = 1.0 - seg_exit_frac[seg_idx] - - # Linear segment if s_end > s_start + 1e-9: - skip = 1 if (row > 0 and seg_idx > 0) else 0 - n_write = samples_per_segment - skip - _linear_se3_segment_into( - start, - end, - out[row : row + n_write], - s_start, - s_end, - skip=skip, + pieces.append((seg_idx, False, s_start, s_end)) + span = s_end - s_start + counts.append( + samples_per_segment - 1 + if samples_per_segment is not None + else _intervals(span * seg_lengths[seg_idx], span * seg.angle_rad()) ) - row += n_write - - # Blend zone at end of this segment if seg_idx < len(clamped) and clamped[seg_idx] > 0: - se3_interp(start, end, 1.0 - seg_exit_frac[seg_idx], entry_buf) - corner = end - next_end = waypoints[seg_idx + 2] - se3_interp(end, next_end, seg_entry_frac[seg_idx + 1], exit_buf) - - avg_seg_len = (seg_lengths[seg_idx] + seg_lengths[seg_idx + 1]) / 2.0 - frac = clamped[seg_idx] / avg_seg_len if avg_seg_len > 1e-6 else 0.0 - bs = _blend_sample_count(frac, samples_per_segment) - skip = 1 if row > 0 else 0 - n_write = bs - skip - blend_path_into( - entry_buf, - corner, - exit_buf, - out[row : row + n_write], - skip=skip, + t_in = 1.0 - seg_exit_frac[seg_idx] + t_out = seg_entry_frac[seg_idx + 1] + pieces.append((seg_idx, True, t_in, t_out)) + if samples_per_segment is not None: + avg_seg_len = (seg_lengths[seg_idx] + seg_lengths[seg_idx + 1]) / 2.0 + frac = clamped[seg_idx] / avg_seg_len if avg_seg_len > 1e-6 else 0.0 + counts.append(_blend_sample_count(frac, samples_per_segment) - 1) + else: + # The zone's control polygon is about 2r long; its curve is + # shorter. + seg.sample(t_in, entry_buf) + segments[seg_idx + 1].sample(t_out, exit_buf) + counts.append( + _intervals( + 2.0 * clamped[seg_idx], _rotation_angle(entry_buf, exit_buf) + ) + ) + if samples_per_segment is None: + _fit_budget(counts) + + out = np.empty((1 + sum(counts), 4, 4), dtype=np.float64) + row = 0 + for (seg_idx, zone, t_a, t_b), intervals in zip(pieces, counts, strict=True): + # Pieces share their junction pose; each after the first skips it. + skip = 1 if row > 0 else 0 + n_write = intervals + 1 - skip + if not zone: + segments[seg_idx].sample_into(out[row : row + n_write], t_a, t_b, skip) + else: + seg, nxt = segments[seg_idx], segments[seg_idx + 1] + seg.sample(t_a, entry_buf) + nxt.sample(t_b, exit_buf) + handle_m = 2.0 / 3.0 * clamped[seg_idx] / 1000.0 + p1 = entry_buf[:3, 3] + handle_m * seg.tangent(t_a) + p2 = exit_buf[:3, 3] - handle_m * nxt.tangent(t_b) + _cubic_blend_into( + entry_buf, exit_buf, p1, p2, out[row : row + n_write], skip=skip ) - row += n_write + row += n_write return out[:row] -def _linear_se3_segment_into( - start: NDArray[np.float64], - end: NDArray[np.float64], - out: NDArray[np.float64], - s_start: float = 0.0, - s_end: float = 1.0, - skip: int = 0, -) -> None: - """Write linearly interpolated SE3 poses into pre-allocated buffer. +def build_spline_path( + waypoints: Sequence[NDArray[np.float64]], +) -> NDArray[np.float64]: + """Poses along a cubic spline through SE3 ``waypoints``, the first being + where the path starts; every waypoint is one of the poses. + + Position is a natural cubic spline per axis over chord-length knots: + uniform knots overshoot between unevenly spaced waypoints, and a + natural end cannot swing wide of the first and last segments as + not-a-knot can. Orientation slerps between the waypoints' rotations. + + Both run on one schedule, the distance along the path on the combined + metric sqrt(t² + (w·θ)²), sampled at ``PATH_STEP_MM``: a waypoint that + only turns the tool takes its turn there, the tool standing at it, + rather than all at once between two poses. Position keeps its + chord-length knots and holds still while the tool turns in place; a + cubic over the combined metric would swing the tool wide of a + waypoint it only turns at. - Args: - start: Start SE3 pose (4x4) - end: End SE3 pose (4x4) - out: Output array, shape (n_samples, 4, 4). Written in-place. - s_start: Start interpolation fraction (0-1) - s_end: End interpolation fraction (0-1) - skip: Number of initial samples to skip (for junction dedup). + Returns: + (M, 4, 4) SE3 poses; one pose when every waypoint is the first. """ - n_total = out.shape[0] + skip - s_values = np.linspace(s_start, s_end, n_total)[skip:] - batch_se3_interp(start, end, s_values, out) + points = np.array([w[:3, 3] for w in waypoints], dtype=np.float64) * 1000.0 + rotations = Rotation.from_matrix(np.array([w[:3, :3] for w in waypoints])) + if len(points) > 1: + chord = np.linalg.norm(np.diff(points, axis=0), axis=1) + turn = (rotations[:-1].inv() * rotations[1:]).magnitude() + # A waypoint that repeats the one before it is nothing to pass through. + keep = np.concatenate( + ([True], np.hypot(chord, _PATH_ROT_WEIGHT_MM_PER_RAD * turn) > _REPEAT_MM) + ) + points = points[keep] + rotations = rotations[keep] + if len(points) < 2: + return np.asarray(waypoints[0], dtype=np.float64)[np.newaxis].copy() + + chord = np.linalg.norm(np.diff(points, axis=0), axis=1) + turn = (rotations[:-1].inv() * rotations[1:]).magnitude() + along_knots = np.concatenate(([0.0], np.cumsum(chord))) + knots = np.concatenate( + ([0.0], np.cumsum(np.hypot(chord, _PATH_ROT_WEIGHT_MM_PER_RAD * turn))) + ) + + # The cubic runs through the distinct positions only: a waypoint that + # turns the tool in place shares its position's knot. + moves = np.concatenate(([True], chord > _REPEAT_MM)) + position: CubicSpline | None = None + if int(moves.sum()) > 1: + position = CubicSpline( + along_knots[moves], points[moves], bc_type="natural", axis=0 + ) + + counts = [_intervals(float(chord[i]), float(turn[i])) for i in range(len(chord))] + _fit_budget(counts) + u = np.concatenate( + [knots[:1]] + + [ + np.linspace(knots[i], knots[i + 1], counts[i] + 1)[1:] + for i in range(len(counts)) + ] + ) + + out = np.zeros((len(u), 4, 4), dtype=np.float64) + out[:, 3, 3] = 1.0 + out[:, :3, :3] = Slerp(knots, rotations)(u).as_matrix() + if position is None: + out[:, :3, 3] = points[0] / 1000.0 + else: + out[:, :3, 3] = position(np.interp(u, knots, along_knots)) / 1000.0 + return out + + +def cartesian_path_knots(cart_poses: NDArray[np.float64]) -> NDArray[np.float64]: + """Normalized cumulative tool distance along an SE3 pose chain, on the + metric sqrt(translation² + (w·rotation)²): the path parameter a timing + solver should key the poses to, so that a constant ``ds/dt`` is a + constant tool speed. Repeated poses share a knot value; the caller + drops them.""" + n = len(cart_poses) + knots = np.zeros(n, dtype=np.float64) + for i in range(1, n): + knots[i] = knots[i - 1] + pose_distance_m(cart_poses[i - 1], cart_poses[i]) + total = float(knots[-1]) + if total > 1e-12: + knots /= total + return knots def _blend_sample_count(frac: float, samples_per_segment: int) -> int: diff --git a/parol6/motion/streaming_executors.py b/parol6/motion/streaming_executors.py index 7b40f29..e4e1998 100644 --- a/parol6/motion/streaming_executors.py +++ b/parol6/motion/streaming_executors.py @@ -27,7 +27,7 @@ import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.config import INTERVAL_S, LIMITS -from pinokin import se3_exp_ws, se3_inverse, se3_log_ws, se3_mul +from pinokin import arrays_equal_6, so3_exp, so3_log logger = logging.getLogger(__name__) @@ -39,57 +39,197 @@ def _pose_to_tangent_jit( ref_pose: np.ndarray, pose: np.ndarray, - ref_inv: np.ndarray, - delta: np.ndarray, + rel_rot: np.ndarray, out: np.ndarray, omega_ws: np.ndarray, - R_ws: np.ndarray, - V_inv_ws: np.ndarray, ) -> None: - """Convert SE3 pose to 6D tangent vector relative to reference. - - Uses workspace variants for zero internal allocation. + """Coordinates of ``pose`` against ``ref_pose``: its translation in the + reference's axes, then the axis-angle of its rotation relative to the + reference. The two are independent, so a straight line in these + coordinates is a straight line for the TCP with the tool turning about + one fixed axis — not the screw an SE3 twist traces when both change. Args: ref_pose: Reference pose (4x4 SE3) pose: Pose to convert (4x4 SE3) - ref_inv: Workspace buffer for reference inverse (4x4) - delta: Workspace buffer for delta transform (4x4) - out: Output tangent vector (6,) [vx, vy, vz, wx, wy, wz] - omega_ws: Workspace buffer for axis-angle (3,) - R_ws: Workspace buffer for rotation matrix (3,3) - V_inv_ws: Workspace buffer for V inverse matrix (3,3) + rel_rot: Workspace for the relative rotation (3x3) + out: Output coordinates (6,) [x, y, z, wx, wy, wz] + omega_ws: Workspace for the axis-angle (3,) """ - se3_inverse(ref_pose, ref_inv) - se3_mul(ref_inv, pose, delta) - se3_log_ws(delta, out, omega_ws, R_ws, V_inv_ws) + for i in range(3): + acc = 0.0 + for k in range(3): + acc += ref_pose[k, i] * (pose[k, 3] - ref_pose[k, 3]) + out[i] = acc + for j in range(3): + acc = 0.0 + for k in range(3): + acc += ref_pose[k, i] * pose[k, j] + rel_rot[i, j] = acc + so3_log(rel_rot, omega_ws) + out[3] = omega_ws[0] + out[4] = omega_ws[1] + out[5] = omega_ws[2] @njit(cache=True) def _tangent_to_pose_jit( ref_pose: np.ndarray, tangent: np.ndarray, - delta: np.ndarray, + rel_rot: np.ndarray, out: np.ndarray, omega_ws: np.ndarray, - R_ws: np.ndarray, - V_ws: np.ndarray, ) -> None: - """Convert 6D tangent vector back to SE3 pose. - - Uses workspace variants for zero internal allocation. + """The pose at ``tangent`` against ``ref_pose``; the inverse of + :func:`_pose_to_tangent_jit`. Args: ref_pose: Reference pose (4x4 SE3) - tangent: Tangent vector (6,) [vx, vy, vz, wx, wy, wz] - delta: Workspace buffer for delta transform (4x4) + tangent: Coordinates (6,) [x, y, z, wx, wy, wz] + rel_rot: Workspace for the relative rotation (3x3) out: Output pose (4x4 SE3) - omega_ws: Workspace buffer for axis-angle (3,) - R_ws: Workspace buffer for rotation matrix (3,3) - V_ws: Workspace buffer for V matrix (3,3) + omega_ws: Workspace for the axis-angle (3,) """ - se3_exp_ws(tangent, delta, omega_ws, R_ws, V_ws) - se3_mul(ref_pose, delta, out) + omega_ws[0] = tangent[3] + omega_ws[1] = tangent[4] + omega_ws[2] = tangent[5] + so3_exp(omega_ws, rel_rot) + for i in range(3): + acc = ref_pose[i, 3] + for k in range(3): + acc += ref_pose[i, k] * tangent[k] + out[i, 3] = acc + for j in range(3): + acc = 0.0 + for k in range(3): + acc += ref_pose[i, k] * rel_rot[k, j] + out[i, j] = acc + out[3, 0] = 0.0 + out[3, 1] = 0.0 + out[3, 2] = 0.0 + out[3, 3] = 1.0 + + +@njit(cache=True) +def _rebase_rate_jit( + tangent: np.ndarray, + rel_rot: np.ndarray, + rate: np.ndarray, + ws: np.ndarray, + turn_linear: bool, +) -> None: + """A rate of the tangent state at ``tangent`` (its velocity or + acceleration), rewritten in place in coordinates about the pose that + state reaches: the rotation part through the right jacobian of SO(3) + at ``tangent``'s rotation; the translation part turned by ``rel_rotᵀ`` + (``rel_rot`` is the rotation ``tangent`` reaches) when ``turn_linear``, + so it keeps its direction in the world, else left to turn with the + tool.""" + for i in range(3): + if turn_linear: + ws[i] = ( + rel_rot[0, i] * rate[0] + + rel_rot[1, i] * rate[1] + + rel_rot[2, i] * rate[2] + ) + else: + ws[i] = rate[i] + px = tangent[3] + py = tangent[4] + pz = tangent[5] + t2 = px * px + py * py + pz * pz + if t2 < 1e-12: + a = 0.5 - t2 / 24.0 + b = 1.0 / 6.0 - t2 / 120.0 + else: + t = math.sqrt(t2) + a = (1.0 - math.cos(t)) / t2 + b = (t - math.sin(t)) / (t2 * t) + wx = rate[3] + wy = rate[4] + wz = rate[5] + c1x = py * wz - pz * wy + c1y = pz * wx - px * wz + c1z = px * wy - py * wx + c2x = py * c1z - pz * c1y + c2y = pz * c1x - px * c1z + c2z = px * c1y - py * c1x + rate[0] = ws[0] + rate[1] = ws[1] + rate[2] = ws[2] + rate[3] = wx - a * c1x + b * c2x + rate[4] = wy - a * c1y + b * c2y + rate[5] = wz - a * c1z + b * c2z + + +@njit(cache=True) +def _same_pose_jit(a: np.ndarray, b: np.ndarray) -> bool: + """Whether two SE3 poses are the same pose.""" + for i in range(3): + for j in range(4): + if a[i, j] != b[i, j]: + return False + return True + + +@njit(cache=True) +def _hold_inside_jit( + pos: np.ndarray, + vel: np.ndarray, + prev: np.ndarray, + measured: np.ndarray, + lo: np.ndarray, + hi: np.ndarray, + held: np.ndarray, +) -> bool: + """Stop, in ``pos`` and ``vel``, each joint moving outward that this + tick carried across a limit or that the arm reports past one: at the + limit, or where it is if that is short of it. ``held`` marks them; + returns whether there were any.""" + any_held = False + for j in range(pos.shape[0]): + v = vel[j] + stop = False + if v > 0.0: + if (pos[j] > hi[j] and prev[j] <= hi[j]) or measured[j] > hi[j]: + if pos[j] > hi[j]: + pos[j] = hi[j] + stop = True + elif v < 0.0: + if (pos[j] < lo[j] and prev[j] >= lo[j]) or measured[j] < lo[j]: + if pos[j] < lo[j]: + pos[j] = lo[j] + stop = True + held[j] = stop + if stop: + vel[j] = 0.0 + any_held = True + return any_held + + +@njit(cache=True) +def below_speed(vel: np.ndarray, tol_sq: float) -> bool: + """Whether the squared norm of ``vel`` is under ``tol_sq``.""" + s = 0.0 + for i in range(vel.shape[0]): + s += vel[i] * vel[i] + return s < tol_sq + + +def cap_twist(twist: np.ndarray, linear_max: float, angular_max: float) -> None: + """Scale a twist's linear and angular parts down to their ceilings in + place, each keeping its direction. The live jog and its preview cap + alike through this.""" + lin = math.sqrt(twist[0] * twist[0] + twist[1] * twist[1] + twist[2] * twist[2]) + if lin > linear_max: + k = linear_max / lin + for i in range(3): + twist[i] *= k + ang = math.sqrt(twist[3] * twist[3] + twist[4] * twist[4] + twist[5] * twist[5]) + if ang > angular_max: + k = angular_max / ang + for i in range(3, 6): + twist[i] *= k # Module-level constant avoids tuple creation per error check. @@ -153,8 +293,12 @@ def _apply_limits(self) -> None: def set_limits(self, velocity_frac: float = 1.0, accel_frac: float = 1.0) -> None: """Set velocity/acceleration as fraction of limits (0.0-1.0).""" - self._vel_scale = max(0.01, min(1.0, velocity_frac)) - self._acc_scale = max(0.01, min(1.0, accel_frac)) + vel = max(0.01, min(1.0, velocity_frac)) + acc = max(0.01, min(1.0, accel_frac)) + if vel == self._vel_scale and acc == self._acc_scale: + return + self._vel_scale = vel + self._acc_scale = acc self._apply_limits() def _tick_ruckig(self) -> tuple[Result, np.ndarray, np.ndarray]: @@ -177,10 +321,11 @@ def _tick_ruckig(self) -> tuple[Result, np.ndarray, np.ndarray]: def stop(self) -> None: """Request graceful stop - decelerate to zero velocity.""" + # Whole-array assignment: ruckig hands out a copy of its targets, so + # writing into an element of one changes nothing. self.inp.control_interface = ControlInterface.Velocity - for i in range(self.num_dofs): - self.inp.target_velocity[i] = 0.0 - self.inp.target_acceleration[i] = 0.0 + self.inp.target_velocity = self._zeros + self.inp.target_acceleration = self._zeros # ============================================================================= @@ -227,6 +372,14 @@ def __init__(self, num_dofs: int = 6, dt: float = INTERVAL_S): self._max_acc_buf: list[float] = [0.0] * num_dofs self._max_jerk_buf: list[float] = [0.0] * num_dofs self._target_vel_buf: list[float] = [0.0] * num_dofs + self._held_acc_buf: list[float] = [0.0] * num_dofs + + # The jog velocity Ruckig was last handed, and whether it still + # holds with the jog limits: a jog repeats its target every tick, + # and handing Ruckig the same parameters again is wasted work. + self._jog_target = np.zeros(num_dofs, dtype=np.float64) + self._jog_applied = False + self._held = np.zeros(num_dofs, dtype=np.bool_) super().__init__(num_dofs, dt) @@ -250,14 +403,18 @@ def _init_state(self) -> None: def _apply_limits(self) -> None: """Apply current limits (with scaling) to Ruckig parameters.""" + self._apply_scaled_vel_limit() for i in range(self.num_dofs): - self._max_vel_buf[i] = self._hardware_v_max[i] * self._vel_scale self._max_acc_buf[i] = self._hardware_a_max[i] * self._acc_scale self._max_jerk_buf[i] = self._hardware_j_max[i] - self.inp.max_velocity = self._max_vel_buf self.inp.max_acceleration = self._max_acc_buf self.inp.max_jerk = self._max_jerk_buf + def _apply_scaled_vel_limit(self) -> None: + for i in range(self.num_dofs): + self._max_vel_buf[i] = self._hardware_v_max[i] * self._vel_scale + self.inp.max_velocity = self._max_vel_buf + def set_cart_velocity_limit(self, limit_mm_s: float | None) -> None: """ Set Cartesian velocity limit for subsequent position targets. @@ -286,6 +443,15 @@ def sync_position(self, pos: list[float] | np.ndarray) -> None: self.inp.current_velocity = self._zeros self.inp.current_acceleration = self._zeros self.inp.target_position = self._sync_pos_buf + self._jog_applied = False + + def set_limits(self, velocity_frac: float = 1.0, accel_frac: float = 1.0) -> None: + super().set_limits(velocity_frac, accel_frac) + self._jog_applied = False + + def stop(self) -> None: + super().stop() + self._jog_applied = False def set_position_target(self, q_target: list[float]) -> None: """ @@ -301,13 +467,14 @@ def set_position_target(self, q_target: list[float]) -> None: if self._cart_vel_limit is not None and self._cart_vel_limit > 0: self._apply_cart_velocity_limit(q_target) else: - self._max_vel_buf[:] = self._hardware_v_max - self.inp.max_velocity = self._max_vel_buf + self._apply_scaled_vel_limit() + self.inp.synchronization = Synchronization.Time self.inp.control_interface = ControlInterface.Position self._sync_pos_buf[:] = q_target self.inp.target_position = self._sync_pos_buf self.inp.target_velocity = self._zeros # Stop at target + self._jog_applied = False self.active = True def set_jog_velocity(self, joint_velocities: NDArray[np.float64]) -> None: @@ -320,6 +487,11 @@ def set_jog_velocity(self, joint_velocities: NDArray[np.float64]) -> None: Args: joint_velocities: Desired velocity for each joint in rad/s (signed) """ + if self._jog_applied and arrays_equal_6(joint_velocities, self._jog_target): + self.active = True + return + self._jog_target[:] = joint_velocities + self._jog_applied = True # Jog uses its own velocity limits (~80% of hardware) rather than the hardware caps. for i in range(self.num_dofs): self._max_vel_buf[i] = self._jog_v_max[i] * self._vel_scale @@ -327,6 +499,11 @@ def set_jog_velocity(self, joint_velocities: NDArray[np.float64]) -> None: self.inp.max_velocity = self._max_vel_buf self.inp.max_acceleration = self._max_acc_buf + # Each joint brakes on its own profile: synchronized, a joint whose + # limit stops it would be stretched to finish with one still + # ramping, and carried past the stopping distance its lookahead + # measured. + self.inp.synchronization = Synchronization.No self.inp.control_interface = ControlInterface.Velocity self._target_vel_buf[:] = joint_velocities self.inp.target_velocity = self._target_vel_buf @@ -361,16 +538,16 @@ def _apply_cart_velocity_limit(self, q_target: list[float]) -> None: for j in range(self.num_dofs): # Joint velocity = dq[j] * scale, so max joint vel = |dq[j]| * max_scale. q_dot_max = min( - abs(self._dq_buf[j]) * max_scale, self._hardware_v_max[j] + abs(self._dq_buf[j]) * max_scale, + self._hardware_v_max[j] * self._vel_scale, ) # Non-zero minimum avoids Ruckig issues with zero limits. self._max_vel_buf[j] = max(q_dot_max, 1e-6) self.inp.max_velocity = self._max_vel_buf else: - # Near-zero motion: fall back to hardware limits. - self._max_vel_buf[:] = self._hardware_v_max - self.inp.max_velocity = self._max_vel_buf + # Near-zero motion: fall back to the scaled hardware limits. + self._apply_scaled_vel_limit() def tick(self) -> tuple[np.ndarray, np.ndarray, bool]: """ @@ -395,11 +572,36 @@ def tick(self) -> tuple[np.ndarray, np.ndarray, bool]: return pos, vel, result == Result.Finished + def hold_inside( + self, + prev: np.ndarray, + measured: np.ndarray, + lo: np.ndarray, + hi: np.ndarray, + ) -> None: + """Never let the position :meth:`tick` just returned step across a + joint limit: a joint moving outward that this tick carried from + ``prev`` across its limit, or that ``measured`` reports past it, + stops there — at the limit, with its velocity and acceleration + zeroed, in the returned buffers and in Ruckig's state alike.""" + if not _hold_inside_jit( + self._pos_out, self._vel_out, prev, measured, lo, hi, self._held + ): + return + self._held_acc_buf[:] = self.out.new_acceleration + for j in range(self.num_dofs): + if self._held[j]: + self._held_acc_buf[j] = 0.0 + self.inp.current_position = self._pos_out + self.inp.current_velocity = self._vel_out + self.inp.current_acceleration = self._held_acc_buf + def reset_limits(self) -> None: """Reset velocity, acceleration, and jerk limits to hardware defaults.""" self._vel_scale = 1.0 self._acc_scale = 1.0 self._apply_limits() + self._jog_applied = False def reset(self) -> None: """Reset executor state.""" @@ -407,6 +609,7 @@ def reset(self) -> None: self._acc_scale = 1.0 self.active = False self._cart_vel_limit = None + self._jog_applied = False self._init_state() @property @@ -434,8 +637,15 @@ class CartesianStreamingExecutor(RuckigExecutorBase): Key features: - Jerk-limited smoothing via Ruckig in Cartesian space - Position mode for MOVECART (straight-line TCP motion) - - Velocity mode for CARTJOG (1-DOF jogging) + - Velocity mode for JOGL (6-DOF twist jogging) - WRF/TRF frame support for jogging + + The reference moves with the tool. A new pose target is taken from + where the limiter is when it arrives, so the tangent to it is the turn + still to make, never a coordinate that wraps at half a turn from where + the stream began; and while the tool turns under the velocity interface + the reference follows it every tick, so a twist is resolved against + the tool as it now stands. """ def __init__(self, dt: float = INTERVAL_S): @@ -462,32 +672,44 @@ def __init__(self, dt: float = INTERVAL_S): # direction, and super().__init__() calls it, so both exist first. self._direction = np.zeros(6, dtype=np.float64) self._cur_tangent = np.zeros(6, dtype=np.float64) - self._delta_tangent = np.zeros(6, dtype=np.float64) - self._last_target = np.zeros(6, dtype=np.float64) + # The pose target being tracked; _has_target is False once Ruckig + # has been taken off it (a jog, a brake, a sync). + self._target_pose = np.zeros((4, 4), dtype=np.float64) self._has_target = False + # Whether Ruckig runs on the velocity interface (a jog or a brake), + # where the reference follows the tool. + self._velocity_mode = False + # Whether the linear motion belongs to the tool (a tool-frame jog, + # and the brake that ends it) and turns with it, rather than + # holding its direction in the world. + self._tool_rates = False + # The jog twist last resolved, its frame, and whether Ruckig still + # holds it: a jog repeats its twist every tick. + self._jog_src = np.zeros(6, dtype=np.float64) + self._jog_wrf = False + self._jog_resolved = False + self._rate_buf = np.zeros(6, dtype=np.float64) + self._acc_buf = np.zeros(6, dtype=np.float64) + self._rate_ws = np.zeros(3, dtype=np.float64) super().__init__(num_dofs=6, dt=dt) # 6-DOF: [x, y, z, wx, wy, wz] self._tangent_buf = np.zeros(6, dtype=np.float64) self._vel_np_buf = np.zeros(6, dtype=np.float64) - self._world_vel_buf = np.zeros(6, dtype=np.float64) # Ruckig's default (Time) only makes the six components FINISH # together; each still takes its own time-optimal route there, so - # the tangent bows and the TCP leaves the straight line by - # millimetres. Phase holds them to one shared profile, which is - # what makes the interpolation the screw geodesic. Ruckig falls - # back to time synchronization by itself when the limits make a - # shared profile impossible. + # the coordinates bow and the TCP leaves the straight line by + # millimetres. Phase holds them to one shared profile, which keeps + # the TCP on its line and the tool on its axis. Ruckig falls back + # to time synchronization by itself when the limits make a shared + # profile impossible. self.inp.synchronization = Synchronization.Phase - # SE3 workspace buffers let the JIT pose conversions run with zero allocation. - self._ref_inv_buf = np.zeros((4, 4), dtype=np.float64) - self._delta_buf = np.zeros((4, 4), dtype=np.float64) + # Workspace buffers let the JIT pose conversions run with zero allocation. self._result_pose_buf = np.zeros((4, 4), dtype=np.float64) self._omega_ws = np.zeros(3, dtype=np.float64) self._R_ws = np.zeros((3, 3), dtype=np.float64) - self._V_ws = np.zeros((3, 3), dtype=np.float64) # Reused for V and V_inv def _init_limits(self) -> None: """Initialize Cartesian velocity/acceleration/jerk limits from centralized config.""" @@ -586,6 +808,9 @@ def sync_pose(self, current_pose: np.ndarray) -> None: self.reference_pose = current_pose.copy() # avoid aliasing with cached FK self._cur_tangent.fill(0.0) self._has_target = False + self._velocity_mode = False + self._tool_rates = False + self._jog_resolved = False # Reset Ruckig state to origin (relative to reference) self.inp.current_position = self._zeros self.inp.current_velocity = self._zeros @@ -593,12 +818,38 @@ def sync_pose(self, current_pose: np.ndarray) -> None: self.inp.target_position = self._zeros self.active = False + def set_limits(self, velocity_frac: float = 1.0, accel_frac: float = 1.0) -> None: + super().set_limits(velocity_frac, accel_frac) + self._jog_resolved = False + + def stop(self) -> None: + super().stop() + self._has_target = False + self._velocity_mode = True + self._jog_resolved = False + + def _rebase(self, tangent: np.ndarray, vel: np.ndarray, acc: np.ndarray) -> None: + """Move the reference to the pose the limiter is at, ``tangent``, + with ``vel`` and ``acc`` its Ruckig velocity and acceleration: the + state is rewritten about the new reference, at its origin, moving + as it was. Needs the rotation ``tangent`` reaches in ``_R_ws``, + which :meth:`_tangent_to_pose` leaves there.""" + assert self.reference_pose is not None + turn = not self._tool_rates + _rebase_rate_jit(tangent, self._R_ws, vel, self._rate_ws, turn) + _rebase_rate_jit(tangent, self._R_ws, acc, self._rate_ws, turn) + self.reference_pose[:] = self._result_pose_buf + self._cur_tangent.fill(0.0) + self.inp.current_position = self._zeros + self.inp.current_velocity = vel + self.inp.current_acceleration = acc + self._jog_resolved = False + def _pose_to_tangent(self, pose: np.ndarray) -> np.ndarray: """ - Convert SE3 pose to 6D tangent vector relative to reference. - - The tangent vector is the Lie algebra representation (twist): - [vx, vy, vz, wx, wy, wz] where v is linear and w is angular. + Coordinates of an SE3 pose relative to the reference: + [x, y, z, wx, wy, wz], the translation in the reference's axes and + the relative rotation's axis-angle (see ``_pose_to_tangent_jit``). Args: pose: 4x4 SE3 matrix to convert @@ -612,12 +863,9 @@ def _pose_to_tangent(self, pose: np.ndarray) -> np.ndarray: _pose_to_tangent_jit( self.reference_pose, pose, - self._ref_inv_buf, - self._delta_buf, + self._R_ws, self._tangent_buf, self._omega_ws, - self._R_ws, - self._V_ws, ) return self._tangent_buf @@ -637,11 +885,9 @@ def _tangent_to_pose(self, tangent: np.ndarray) -> np.ndarray: _tangent_to_pose_jit( self.reference_pose, self._tangent_buf, - self._delta_buf, + self._R_ws, self._result_pose_buf, self._omega_ws, - self._R_ws, - self._V_ws, ) return self._result_pose_buf @@ -655,8 +901,6 @@ def set_pose_target(self, target_pose: np.ndarray) -> None: Args: target_pose: Target TCP pose as SE3 """ - target_tangent = self._pose_to_tangent(target_pose) - # Re-planning a target Ruckig is already tracking costs the phase # synchronization that keeps the TCP on its line. A re-plan tests # the current velocity and acceleration against the new profile @@ -666,23 +910,38 @@ def set_pose_target(self, target_pose: np.ndarray) -> None: # target at the tick rate, so this is the common case, not an # edge one: the same move retargeted every tick left the line by # 4.7 mm, and left by none at all when set once. - if self._has_target: - same = True - for i in range(6): - if self._last_target[i] != target_tangent[i]: - same = False - break - if same: - self.active = True - return - self._last_target[:] = target_tangent + if self._has_target and _same_pose_jit(self._target_pose, target_pose): + self.active = True + return + self._target_pose[:] = target_pose self._has_target = True + self._jog_resolved = False + + # A new target is taken from where the limiter is: the tangent to + # it is then the move still to make, a straight line with the tool + # turning about one axis, and a stream that keeps turning the tool + # never nears the half turn where the coordinates wrap. + cur = self._cur_tangent + if self.reference_pose is not None and ( + cur[0] != 0.0 + or cur[1] != 0.0 + or cur[2] != 0.0 + or cur[3] != 0.0 + or cur[4] != 0.0 + or cur[5] != 0.0 + ): + self._tangent_to_pose(cur) + self._rate_buf[:] = self.inp.current_velocity + self._acc_buf[:] = self.inp.current_acceleration + self._rebase(cur, self._rate_buf, self._acc_buf) + self._velocity_mode = False + self._tool_rates = False - # The envelope is direction-dependent (see _apply_limits), and - # the direction is the one from where the limiter is to the - # target, not the target's own bearing from the reference. - np.subtract(target_tangent, self._cur_tangent, out=self._delta_tangent) - self._set_direction(self._delta_tangent) + target_tangent = self._pose_to_tangent(target_pose) + # The envelope is direction-dependent (see _apply_limits), and the + # limiter stands at the origin, so the target's tangent is the + # direction to it. + self._set_direction(target_tangent) self.inp.control_interface = ControlInterface.Position self.inp.target_position = target_tangent @@ -691,73 +950,54 @@ def set_pose_target(self, target_pose: np.ndarray) -> None: self._apply_limits() self.active = True - def set_jog_velocity_1dof( - self, axis: int, velocity: float, is_rotation: bool - ) -> None: - """ - Set 1-DOF jog velocity in body frame (TRF - Tool Reference Frame). - - The tangent space is relative to reference_pose, so velocities are - naturally in body/tool frame. Use this for TRF jogging. - - Uses velocity mode - Ruckig smoothly accelerates/decelerates - to reach target velocity. Call with velocity=0 to stop. - - Args: - axis: Axis index (0=X, 1=Y, 2=Z) - velocity: Target velocity (m/s for linear, rad/s for rotation) - is_rotation: True for rotation axes (RX, RY, RZ) - """ - self._target_velocity_arr.fill(0.0) - if is_rotation: - self._target_velocity_arr[3 + axis] = velocity - else: - self._target_velocity_arr[axis] = velocity - - self._has_target = False - self._set_direction(self._target_velocity_arr) - self.inp.control_interface = ControlInterface.Velocity - self.inp.target_velocity = self._target_velocity_arr - self._target_acceleration_arr.fill(0.0) - self.inp.target_acceleration = self._target_acceleration_arr - - self._apply_limits() - self.active = True - - def set_jog_velocity_1dof_wrf( - self, - axis: int, - velocity: float, - is_rotation: bool, - ) -> None: - """ - Set 1-DOF jog velocity in world reference frame (WRF). - - Transforms the velocity from world frame to body frame (tangent space) - before applying to Ruckig. Requires reference_pose to be set. - - Args: - axis: Axis index (0=X, 1=Y, 2=Z) - velocity: Target velocity (m/s for linear, rad/s for rotation) - is_rotation: True for rotation axes (RX, RY, RZ) + def set_jog_twist(self, twist: np.ndarray, wrf: bool) -> None: + """Drive the TCP at a 6-DOF velocity `[vx, vy, vz, wx, wy, wz]` + (m/s, rad/s), in world axes when `wrf` else in the tool's. + + Velocity mode: Ruckig ramps to the twist under the envelope, + which follows the twist's direction so a diagonal runs at the + configured TCP ceiling rather than sqrt(3) times it; a twist + asking for more than the ceiling is scaled down to it, direction + kept, since Ruckig's velocity interface does not bound the + target itself. An all-zero twist is a brake. Needs + `reference_pose`, which `sync_pose` sets. + + The twist is resolved against the reference, which follows the + tool while it turns (see :meth:`tick`): a tool-frame twist moves + the tool along its axes as they now stand, and a world-frame turn + is about the world axis whatever turn came before it. Setting the + same twist again changes nothing and costs nothing, until the + reference moves. """ if self.reference_pose is None: - logger.warning("set_jog_velocity_1dof_wrf called without reference_pose") + logger.warning("set_jog_twist called without reference_pose") return - - self._world_vel_buf.fill(0.0) - if is_rotation: - self._world_vel_buf[3 + axis] = velocity + if ( + self._jog_resolved + and self._jog_wrf == wrf + and arrays_equal_6(twist, self._jog_src) + ): + self.active = True + return + self._jog_src[:] = twist + self._jog_wrf = wrf + self._jog_resolved = True + t = self._target_velocity_arr + if wrf: + # The coordinates are in the reference's axes: Rᵀ · world. + R = self.reference_pose + for i in range(3): + t[i] = R[0, i] * twist[0] + R[1, i] * twist[1] + R[2, i] * twist[2] + t[3 + i] = R[0, i] * twist[3] + R[1, i] * twist[4] + R[2, i] * twist[5] else: - self._world_vel_buf[axis] = velocity - - # Transform world frame to body frame (tangent space): body velocity = R^T @ world velocity. - R = self.reference_pose[:3, :3] - - np.dot(R.T, self._world_vel_buf[:3], self._target_velocity_arr[:3]) - np.dot(R.T, self._world_vel_buf[3:], self._target_velocity_arr[3:]) + t[:] = twist + cap_twist( + t, self._v_lin_max * self._vel_scale, self._v_ang_max * self._vel_scale + ) self._has_target = False + self._velocity_mode = True + self._tool_rates = not wrf self._set_direction(self._target_velocity_arr) self.inp.control_interface = ControlInterface.Velocity self.inp.target_velocity = self._target_velocity_arr @@ -807,6 +1047,17 @@ def tick(self) -> tuple[np.ndarray, NDArray[np.float64], bool]: self._cur_tangent[:] = pos self._vel_np_buf[:] = vel + # Under the velocity interface the reference follows the tool while + # it turns. The tangent's rotation coordinates only turn the tool + # about a fixed axis through the reference; a twist resolved + # against a reference the tool has turned away from would turn it + # about some other axis, and move it along the axes it had. The + # interface integrates position without reading it back, so the + # shift changes nothing about the motion itself. + if self._velocity_mode and (pos[3] != 0.0 or pos[4] != 0.0 or pos[5] != 0.0): + self._acc_buf[:] = self.out.new_acceleration + self._rebase(pos, self._vel_np_buf, self._acc_buf) + # Don't auto-deactivate in velocity mode - caller controls via set_jog_velocity(0) return smoothed_pose, self._vel_np_buf, result == Result.Finished @@ -826,4 +1077,9 @@ def reset(self) -> None: self._acc_scale = 1.0 self.reference_pose = None self.active = False + self._cur_tangent.fill(0.0) + self._has_target = False + self._velocity_mode = False + self._tool_rates = False + self._jog_resolved = False self._init_state() diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 24c7c9a..fd1774b 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -14,11 +14,11 @@ from __future__ import annotations import logging +import math from dataclasses import dataclass from enum import Enum import numpy as np -from numba import njit from numpy.typing import NDArray from ruckig import InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] @@ -31,9 +31,14 @@ import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.config import INTERVAL_S, LIMITS, rad_to_steps +from parol6.motion.geometry import _rotation_angle, cartesian_path_knots +from parol6.utils.error_catalog import make_error +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import IKError, TrajectoryPlanningError +from parol6.utils.joint_limits import TRAVEL_MAX_RAD, TRAVEL_MIN_RAD -from pinokin import Damping, IKSolver, se3_from_rpy +from pinokin import Damping, IKSolver, se3_interp logger = logging.getLogger(__name__) @@ -98,6 +103,22 @@ def _quintic_samples( return q0 + (q1 - q0) * s * s * s * (10.0 - 15.0 * s + 6.0 * s * s) +# Peak ds/dt and d²s/dt² of a quintic from s=0 to s=1 over one second. +_QUINTIC_PEAK_VEL = 1.875 +_QUINTIC_PEAK_ACC = 10.0 / math.sqrt(3.0) + +# A timing solver's duration can land a hair past a whole number of ticks. +_TICK_ROUNDING = 1e-6 + + +def _tick_intervals(duration: float, dt: float) -> int: + """Control ticks a move of ``duration`` spans, rounded up to a whole + number. The player plays one row per tick, so a trajectory's rows sit + exactly a tick apart; stretched to whole ticks, it lasts as long as + it plans and accelerates no harder.""" + return max(1, math.ceil(duration / dt - _TICK_ROUNDING)) + + class _LinearPath: """Piecewise linear path wrapper for TOPPRA compatibility. @@ -147,69 +168,210 @@ def from_string(cls, name: str) -> ProfileType: return cls.TOPPRA -# Step is anomalous if its magnitude exceeds this multiple of the chain's -# median step. Relative threshold keeps the rule invariant to sample density, -# speed, and move type. Insensitive in [10, 20+]; real LM hops exceed 20×. -_IK_OUTLIER_RATIO: float = 10.0 - -# Symmetric padding around each outlier run; absorbs LM seed-bleed into the -# samples just after the hop. FK deviation scales linearly with pad. -_IK_OUTLIER_PADDING: int = 4 - - -@njit(cache=True) -def _smooth_singularity_outliers(positions: NDArray[np.float64]) -> int: - """In-place repair of LM-IK branch hops near wrist singularities. - - pinokin's LM picks IK solutions without a continuity preference, so at - J5 ≈ 0 it can jump several degrees off the natural chain in one or two - samples. Detected as steps > _IK_OUTLIER_RATIO × the chain's median - step; replaced by linear interpolation over the run plus padding. +# Largest joint change allowed between consecutive cartesian IK waypoints. +# A bigger step is solved again at poses in between; one that survives is +# the solver hopping to another IK branch, and the commanded path would +# whip the arm through the hop: the move is refused rather than the hop +# smoothed over (par6's `move_l_max_joint_step_rad`). +IK_MAX_JOINT_STEP_RAD: float = 0.35 + +# How far the tool may move while the wrist reconfigures at a singularity +# for the reconfiguration to count as one: with J5 at zero the J4/J6 pair +# is a null motion and the tool stands still; a hop that moves the tool +# more than this is a branch flip, and refused. +_WRIST_NULL_MOTION_POS_M: float = 5e-4 +_WRIST_NULL_MOTION_ROT_RAD: float = np.radians(0.5) + + +def _ik_branch_hop(positions: NDArray[np.float64]) -> int | None: + """Index of the first waypoint the chain reaches with a joint step past + ``IK_MAX_JOINT_STEP_RAD``, or None when the chain stays on one branch.""" + if len(positions) < 2: + return None + steps = np.max(np.abs(np.diff(positions, axis=0)), axis=1) + hops = np.nonzero(steps > IK_MAX_JOINT_STEP_RAD)[0] + if len(hops) == 0: + return None + return int(hops[0]) + 1 + + +# The turns of J4 tried, in order, when a chain has to leave a wrist +# singularity: the two quarter turns first, since a tilt out of the +# arm's plane is what a move from the standby pose usually asks for. +_WRIST_TURNS_RAD: tuple[float, ...] = ( + np.pi / 2, + -np.pi / 2, + np.pi / 4, + -np.pi / 4, + 3 * np.pi / 4, + -3 * np.pi / 4, + np.pi, +) + + +def _wrist_turn( + q_from: NDArray[np.float64], turn: float, pose_from: NDArray[np.float64] +) -> NDArray[np.float64] | None: + """``q_from`` with its wrist turned by ``turn``: J4 forward, J6 back. + None when the turn leaves the joint window or moves the tool, which it + does not at the singularity and does a little near it.""" + bridge = q_from.copy() + bridge[3] += turn + bridge[5] -= turn + if np.any(bridge < TRAVEL_MIN_RAD) or np.any(bridge > TRAVEL_MAX_RAD): + return None + robot = PAROL6_ROBOT.robot + for frac in (0.5, 1.0): + pose = robot.fkine(q_from + frac * (bridge - q_from)) + if np.linalg.norm(pose[:3, 3] - pose_from[:3, 3]) > _WRIST_NULL_MOTION_POS_M: + return None + if _rotation_angle(pose_from, pose) > _WRIST_NULL_MOTION_ROT_RAD: + return None + return bridge + + +# Largest joint step between the rows of a wrist turn: the collision guard +# checks rows, so the turn carries enough of them to be checked along its +# sweep and not only at its ends. +_WRIST_TURN_ROW_RAD = np.radians(5.0) + + +def _leave_wrist_singularity( + solver: IKSolver, + se3_poses: list[NDArray[np.float64]], + q_from: NDArray[np.float64], + q_hint: NDArray[np.float64] | None, +) -> tuple[NDArray[np.float64], int] | None: + """The joint chain for ``se3_poses`` from a wrist standing at its + singularity, led by the turn of the wrist the chain needs, and the + number of rows the turn adds ahead of the path; None when no turn + gives one. + + With J5 at zero, J4 and J6 share an axis: turning J4 by an angle and J6 + back by the same angle leaves the tool where it is. A pose a hair off + the singularity fixes the split between them, so the chain's first + step can ask for a quarter turn of J4 that the tool never sees, or + the solver can find no step at all from a seed whose jacobian has + lost a rank. The turn is made first, as a joint move of its own, and + the chain is solved again from the turned wrist. The turns are tried + in a fixed order, the solver's own answer for the first pose + (``q_hint``) last, and the first whose chain steps no joint further + between two rows than the turn's own rows do is taken, so the same + move always turns the wrist the same way. The split the turn settles + on decides whether the solver snaps the wrist round in one row + further along, where the tool has barely left the singularity; when + every chain snaps, the one that snaps least is taken. """ - n = positions.shape[0] - dims = positions.shape[1] - if n < 3: - return 0 - - diffs = np.empty(n - 1) - for i in range(n - 1): - s = 0.0 - for c in range(dims): - v = positions[i + 1, c] - positions[i, c] - s += v * v - diffs[i] = np.sqrt(s) - - median_step = np.median(diffs) - if median_step == 0.0: - return 0 - threshold = _IK_OUTLIER_RATIO * median_step - - n_patched = 0 - i = 1 - while i < n - 1: - if diffs[i - 1] <= threshold and diffs[i] <= threshold: - i += 1 + turns: list[float] = list(_WRIST_TURNS_RAD) + if q_hint is not None: + turns.append(float(q_hint[3] - q_from[3])) + least: tuple[NDArray[np.float64], int] | None = None + least_step = np.inf + for turn in turns: + bridge = _wrist_turn(q_from, turn, se3_poses[0]) + if bridge is None: continue - j = i - while j < n - 1 and (diffs[j - 1] > threshold or diffs[j] > threshold): - j += 1 - lo = i - _IK_OUTLIER_PADDING - if lo < 1: - lo = 1 - hi = j + _IK_OUTLIER_PADDING - if hi > n - 1: - hi = n - 1 - span = hi - (lo - 1) - inv_span = 1.0 / span - for k in range(lo, hi): - alpha = (k - (lo - 1)) * inv_span - for c in range(dims): - positions[k, c] = positions[lo - 1, c] + alpha * ( - positions[hi, c] - positions[lo - 1, c] - ) - n_patched += 1 - i = hi + 1 - return n_patched + result = solver.batch_ik(se3_poses[1:], bridge, stop_on_failure=True) + if not result.all_valid: + continue + path = np.asarray(result.joint_positions, dtype=np.float64) + chain = np.concatenate([bridge[np.newaxis], path]) + if _ik_branch_hop(chain) is not None: + continue + step = float(np.max(np.abs(np.diff(chain, axis=0)))) if len(chain) > 1 else 0.0 + if step > _WRIST_TURN_ROW_RAD and step >= least_step: + continue + rows = max(1, int(np.ceil(abs(turn) / _WRIST_TURN_ROW_RAD))) + fractions = np.arange(1, rows + 1, dtype=np.float64)[:, np.newaxis] / rows + sweep = q_from + fractions * (bridge - q_from) + least = (np.concatenate([q_from[np.newaxis], sweep, path]), rows) + least_step = step + if step <= _WRIST_TURN_ROW_RAD: + break + return least + + +# Halvings a joint step past IK_MAX_JOINT_STEP_RAD gets before it is taken +# for a hop between IK branches. A fast but continuous swing, such as the +# wrist turning past a hair off its singularity, splits into steps within +# the limit a level or two down; a jump between branches stays a jump +# however finely the path is cut. +_HOP_BISECTIONS: int = 6 +# How far the bridge's solution at a waypoint may differ from the batch +# solve's before the rest of the chain is solved again from the bridge. +_SAME_SOLUTION_RAD: float = 1e-6 + + +def _bridge( + solver: IKSolver, + pose_a: NDArray[np.float64], + q_a: NDArray[np.float64], + pose_b: NDArray[np.float64], + depth: int, + poses: list[NDArray[np.float64]], + qs: list[NDArray[np.float64]], +) -> bool: + """Solve ``pose_b`` on from ``q_a`` at ``pose_a``, halving the stretch + between them while a joint steps further than ``IK_MAX_JOINT_STEP_RAD``. + The poses solved on the way, ``pose_b`` last, and their solutions are + appended to ``poses`` and ``qs``. False when a step survives ``depth`` + halvings.""" + if solver.solve(pose_b, q0=q_a): + q_b = np.array(solver.q, dtype=np.float64) + if float(np.max(np.abs(q_b - q_a))) <= IK_MAX_JOINT_STEP_RAD: + poses.append(pose_b) + qs.append(q_b) + return True + if depth == 0: + return False + # Halfway as a straight segment runs: position lerp, rotation geodesic. + mid = np.empty((4, 4), dtype=np.float64) + se3_interp(pose_a, pose_b, 0.5, mid) + mid[:3, 3] = 0.5 * (pose_a[:3, 3] + pose_b[:3, 3]) + if not _bridge(solver, pose_a, q_a, mid, depth - 1, poses, qs): + return False + return _bridge(solver, mid, qs[-1], pose_b, depth - 1, poses, qs) + + +def _bridged_chain( + solver: IKSolver, + se3_poses: list[NDArray[np.float64]], + positions: NDArray[np.float64], + hop: int, +) -> tuple[NDArray[np.float64], NDArray[np.float64]] | None: + """The poses and joint chain with every hop from ``hop`` on bridged by + :func:`_bridge`, the poses it solved spliced in between the path's own; + None when a hop survives the bridge, a jump between IK branches. Where + the bridge lands on another solution than the batch solve did, the + rest of the chain is solved again from where it landed.""" + out_poses = list(se3_poses[:hop]) + out_q = list(positions[:hop]) + i = hop + while True: + if not _bridge( + solver, + out_poses[-1], + out_q[-1], + se3_poses[i], + _HOP_BISECTIONS, + out_poses, + out_q, + ): + return None + if float(np.max(np.abs(out_q[-1] - positions[i]))) > _SAME_SOLUTION_RAD: + result = solver.batch_ik(se3_poses[i:], out_q[-1], stop_on_failure=True) + if not result.all_valid: + return None + positions = np.concatenate( + [positions[:i], np.asarray(result.joint_positions, dtype=np.float64)] + ) + nxt = _ik_branch_hop(positions[i:]) + end = len(se3_poses) if nxt is None else i + nxt + out_poses.extend(se3_poses[i + 1 : end]) + out_q.extend(positions[i + 1 : end]) + if nxt is None: + return np.asarray(out_poses), np.asarray(out_q) + i = end @dataclass @@ -223,10 +385,19 @@ class JointPath: Attributes: positions: (N, 6) array of joint angles in radians valid: Per-row IK validity. None means all rows are valid. + prefix: Leading rows that are a wrist reconfiguration at a + singularity, run as a joint move before the path proper; row + ``prefix`` is the path's first pose. Zero for a plain path. + knots: For a path solved from cartesian poses, each pose's + normalized distance along the path (``cartesian_path_knots``), + one per row from row ``prefix`` on: the path parameter to time + it by. None for a joint-space path. """ positions: NDArray[np.float64] # (N, 6) joint angles in radians valid: NDArray[np.bool_] | None = None # (N,) per-row validity, None = all valid + prefix: int = 0 + knots: NDArray[np.float64] | None = None @property def is_partial(self) -> bool: @@ -242,7 +413,7 @@ def __getitem__(self, idx: int) -> NDArray[np.float64]: @classmethod def from_poses( cls, - poses: NDArray[np.float64] | list[np.ndarray], + poses: NDArray[np.float64], seed_q: NDArray[np.float64], stop_on_failure: bool = True, ) -> JointPath: @@ -252,41 +423,25 @@ def from_poses( Each IK solve uses the previous solution as seed, maintaining continuity. Args: - poses: Either (N, 6) array of [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg] - or list of SE3 poses + poses: (N, 4, 4) SE3 poses seed_q: Initial joint angles for IK seeding (radians) stop_on_failure: If True, stop solving after first IK failure (real controller). If False, solve all poses (diagnostic). + A joint step past ``IK_MAX_JOINT_STEP_RAD`` between two poses is + solved again at poses in between; a step that stays that large + however finely the path is cut is a hop between IK branches. + Returns: - JointPath with solved joint positions. If some poses failed, - ``valid`` is set to a per-row bool array (``is_partial`` is True). + JointPath with solved joint positions, and the poses' knots. If + some poses failed, ``valid`` is set to a per-row bool array + (``is_partial`` is True). Raises: - IKError: If fewer than 2 consecutive valid poses from the start. + IKError: If fewer than 2 consecutive valid poses from the start, + or the chain hops between IK branches. """ - from parol6.utils.error_catalog import make_error - from parol6.utils.error_codes import ErrorCode - from parol6.utils.errors import IKError - - # Convert to list of SE3 (4x4) matrices for batch_ik - if isinstance(poses, np.ndarray) and poses.ndim == 3: - se3_poses = [poses[i] for i in range(len(poses))] - elif isinstance(poses, np.ndarray): - n = len(poses) - se3_poses = [np.empty((4, 4), dtype=np.float64) for _ in range(n)] - for i, p in enumerate(poses): - se3_from_rpy( - p[0] / 1000.0, - p[1] / 1000.0, - p[2] / 1000.0, - np.radians(p[3]), - np.radians(p[4]), - np.radians(p[5]), - se3_poses[i], - ) - else: - se3_poses = poses + se3_poses = [poses[i] for i in range(len(poses))] solver = IKSolver( PAROL6_ROBOT.robot, @@ -304,11 +459,50 @@ def from_poses( if result.all_valid: positions = np.asarray(result.joint_positions, dtype=np.float64) - _smooth_singularity_outliers(positions) - return cls(positions=positions) + hop = _ik_branch_hop(positions) + if hop is None: + return cls(positions=positions, knots=cartesian_path_knots(poses)) + if hop == 1: + # A path leaving a wrist singularity turns the wrist first. + turned = _leave_wrist_singularity( + solver, se3_poses, positions[0], positions[1] + ) + if turned is not None: + return cls( + positions=turned[0], + prefix=turned[1], + knots=cartesian_path_knots(poses), + ) + bridged = _bridged_chain(solver, se3_poses, positions, hop) + if bridged is not None: + return cls(positions=bridged[1], knots=cartesian_path_knots(bridged[0])) + raise IKError( + make_error( + ErrorCode.IK_PARTIAL_PATH, + valid=str(hop), + total=str(len(se3_poses)), + ) + ) valid = np.array(result.valid, dtype=np.bool_) first_fail = int(np.argmin(valid)) # first False index + if first_fail == 1: + # A seed at a wrist singularity can leave the solver no step + # to take; the turned wrist is a seed it can solve from. The + # preview takes the same way out, or it would refuse a move + # the controller runs. + turned = _leave_wrist_singularity( + solver, + se3_poses, + np.asarray(result.joint_positions, dtype=np.float64)[0], + None, + ) + if turned is not None: + return cls( + positions=turned[0], + prefix=turned[1], + knots=cartesian_path_knots(poses), + ) if first_fail < 2: if stop_on_failure: raise IKError( @@ -420,7 +614,9 @@ class Trajectory: Attributes: steps: (M, 6) motor steps at each control tick - duration: Actual duration in seconds + duration: How long the player takes over it [s]: one control tick + per row, the first holding the pose the move starts from. Zero + for a move that goes nowhere. """ steps: NDArray[np.int32] # (M, 6) motor steps @@ -455,6 +651,8 @@ def __init__( dt: float = INTERVAL_S, cart_vel_limit: float | None = None, cart_acc_limit: float | None = None, + path_knots: NDArray[np.float64] | None = None, + constant_tool_speed: bool = False, ): """ Initialize trajectory builder. @@ -469,8 +667,18 @@ def __init__( dt: Control loop time step cart_vel_limit: Cartesian linear velocity limit in m/s (for Cartesian commands) cart_acc_limit: Cartesian linear acceleration limit in m/s² (for Cartesian commands) + path_knots: Path-parameter value of each joint waypoint, strictly + increasing from 0 to 1 — cumulative tool distance for a + cartesian path, so that a constant ``ds/dt`` is a constant + tool speed. ``None`` spaces the waypoints evenly. + constant_tool_speed: Hold the whole path to one ``ds/dt``, the + fastest the steepest stretch and the cartesian ceiling allow, + rather than running each stretch as fast as it can (what a + process move promises). """ self.joint_path = joint_path + self.path_knots = path_knots + self.constant_tool_speed = constant_tool_speed self.profile = ( ProfileType.from_string(profile) if isinstance(profile, str) else profile ) @@ -510,34 +718,145 @@ def build(self) -> Trajectory: at the control rate. No interpolation of the original joint path is needed since TOPP-RA's trajectory already provides smooth, continuous positions. - For RUCKIG profile: Uses Ruckig for point-to-point motion (ignores path waypoints) - For other profiles: Uses TOPP-RA trajectory directly + RUCKIG runs point to point, from the first row to the last; a path + that is not a straight run between them is timed by TOPP-RA + instead. QUINTIC and TRAPEZOID run each joint on its own profile + from end to end on a straight run, and the path coordinate along + any other path. Returns: - Trajectory ready for execution + Trajectory ready for execution, its rows one control tick apart """ - if len(self.joint_path) < 2: - steps = _rad_to_steps_alloc( - self.joint_path.positions[0:1] # Keep 2D shape (1, 6) - ) + positions = self.joint_path.positions + # A path that goes nowhere is done where it stands: there is no + # distance for a timing solver to spread over, and every profile + # would either refuse it or divide by it. Nowhere is within a + # motor step, the resolution the arm is driven at. + if len(self.joint_path) < 2 or self._within_a_step(positions): + steps = _rad_to_steps_alloc(positions[0:1]) # Keep 2D shape (1, 6) return Trajectory( steps=steps, duration=0.0, - positions_rad=self.joint_path.positions[0:1].copy(), + positions_rad=positions[0:1].copy(), ) - if self.profile == ProfileType.RUCKIG: - # Point-to-point jerk-limited motion; ignores intermediate waypoints + if self.joint_path.prefix > 0: + return self._build_with_prefix() + + if self.path_knots is not None and self.profile in ( + ProfileType.LINEAR, + ProfileType.QUINTIC, + ProfileType.TRAPEZOID, + ): + # These profiles time a path coordinate spaced by row; spaced + # by the tool's own distance instead, their cruise is one tool + # speed, as TOPPRA holds it through the knots. + self.joint_path = self._resampled_by_knots(self.path_knots) + self.path_knots = None + + along_path = self._is_cartesian_path() or not self._is_point_to_point() + if self.profile == ProfileType.RUCKIG and not along_path: return self._build_ruckig_trajectory() elif self.profile == ProfileType.LINEAR: return self._build_simple_trajectory() elif self.profile == ProfileType.QUINTIC: - return self._build_quintic_trajectory() + if along_path: + return self._build_quintic_trajectory_along_path() + return self._build_quintic_trajectory_joint() elif self.profile == ProfileType.TRAPEZOID: - return self._build_trapezoid_trajectory() + if along_path: + # A trapezoid of the path coordinate is what LINEAR runs. + return self._build_simple_trajectory() + return self._build_trapezoid_trajectory_joint() else: return self._build_toppra_trajectory() + def _trajectory(self, trajectory_rad: NDArray[np.float64]) -> Trajectory: + """The rows as the player takes them, one per control tick: the + trajectory lasts as many ticks as it has rows, the queue time the + controller reports for it.""" + return Trajectory( + steps=_rad_to_steps_alloc(trajectory_rad), + duration=len(trajectory_rad) * self.dt, + positions_rad=trajectory_rad, + ) + + def _resampled_by_knots(self, knots: NDArray[np.float64]) -> JointPath: + """The joint path resampled at even steps of the path parameter the + knots give each row (the tool's normalized distance), as many rows + as it had. Repeated knots keep their first row.""" + positions = self.joint_path.positions + knots = np.asarray(knots, dtype=np.float64) + keep = np.concatenate(([True], np.diff(knots) > 1e-12)) + at = np.linspace(0.0, 1.0, len(positions)) + resampled = np.empty_like(positions) + for j in range(positions.shape[1]): + resampled[:, j] = np.interp(at, knots[keep], positions[keep, j]) + return JointPath(positions=resampled) + + @staticmethod + def _within_a_step(positions: NDArray[np.float64]) -> bool: + """The path ends on the step it starts on and strays no more than a + step between: a move of even one step still goes there.""" + ends = _rad_to_steps_alloc(positions[[0, -1]]) + if not np.array_equal(ends[0], ends[1]): + return False + steps = _rad_to_steps_alloc(positions) + return bool(np.max(np.abs(steps - steps[0])) <= 1) + + def _is_point_to_point(self) -> bool: + """The rows run straight from the first to the last and never turn + back: the path per-joint profiles, which see only its two ends, + reproduce.""" + positions = self.joint_path.positions + chord = positions[-1] - positions[0] + length_sq = float(np.dot(chord, chord)) + if length_sq < 1e-18: + return False + rel = positions - positions[0] + along = rel @ chord / length_sq + off = rel - np.outer(along, chord) + return bool(np.max(np.abs(off)) < 1e-9 and np.all(np.diff(along) >= -1e-12)) + + def _build_with_prefix(self) -> Trajectory: + """A wrist reconfiguration ahead of the path is its own joint move, + timed by the joint limits alone, and the path follows it from + rest: the cartesian timing (knots, tool ceiling, constant tool + speed) applies to the path, which starts at the reconfigured pose, + and a requested duration covers the two together.""" + p = self.joint_path.prefix + turn = TrajectoryBuilder( + joint_path=JointPath(positions=self.joint_path.positions[: p + 1]), + profile=self.profile, + velocity_frac=self.velocity_frac, + accel_frac=self.accel_frac, + jerk_frac=self.jerk_frac, + dt=self.dt, + ).build() + # A requested duration is the whole move's: the path gets what the + # turn's motion leaves of it, and runs as fast as it can when the + # turn left none, as any request shorter than the move can be does. + path_duration = None + if self.duration: + remaining = self.duration - (len(turn) - 1) * self.dt + path_duration = remaining if remaining > 0 else None + path = TrajectoryBuilder( + joint_path=JointPath(positions=self.joint_path.positions[p:]), + profile=self.profile, + velocity_frac=self.velocity_frac, + accel_frac=self.accel_frac, + jerk_frac=self.jerk_frac, + duration=path_duration, + dt=self.dt, + cart_vel_limit=self.cart_vel_limit, + cart_acc_limit=self.cart_acc_limit, + path_knots=self.path_knots, + constant_tool_speed=self.constant_tool_speed, + ).build() + return self._trajectory( + np.concatenate([turn.positions_rad, path.positions_rad[1:]]) + ) + def _build_toppra_trajectory(self) -> Trajectory: """ Build trajectory using TOPP-RA's time-optimal path parameterization. @@ -547,10 +866,23 @@ def _build_toppra_trajectory(self) -> Trajectory: and optional Cartesian velocity limits. """ positions = self.joint_path.positions + if self.path_knots is not None: + # Waypoints that cover no distance would give a zero-width + # segment; the path keeps the first of any such run. + keep = np.concatenate(([True], np.diff(self.path_knots) > 1e-12)) + positions = positions[keep] + ss_waypoints = np.asarray(self.path_knots, dtype=np.float64)[keep] + if len(positions) < 2: + raise TrajectoryPlanningError( + make_error( + ErrorCode.TRAJ_NO_STEPS, + detail="the path covers no tool distance to time", + ) + ) + else: + ss_waypoints = np.linspace(0.0, 1.0, len(positions)) n_points = len(positions) - ss_waypoints = np.linspace(0.0, 1.0, n_points) - # Piecewise linear PPoly — prevents cubic spline overshoot that # amplifies orientation error near wrist singularities n_seg = n_points - 1 @@ -573,6 +905,8 @@ def _build_toppra_trajectory(self) -> Trajectory: cart_constraint = self._build_cart_vel_constraint(path, ss_waypoints) if cart_constraint is not None: constraints.append(cart_constraint) + if self.constant_tool_speed: + constraints.append(self._build_path_speed_cap(path, positions, c[0])) try: # Use evenly-spaced gridpoints - TOPPRA docs recommend "at least a few times @@ -612,12 +946,14 @@ def _build_toppra_trajectory(self) -> Trajectory: n_points, ) - # Sample at control rate, including the exact endpoint - n_output = max(2, int(np.floor(duration / self.dt)) + 1) - times = np.arange(n_output - 1) * self.dt - trajectory_rad = np.empty((n_output, 6), dtype=np.float64) + # One row per tick, the solved trajectory slowed onto a whole + # number of ticks and ending exactly at its endpoint. + intervals = _tick_intervals(duration, self.dt) + times = np.arange(intervals) * (duration / intervals) + trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) trajectory_rad[:-1] = jnt_traj(times) trajectory_rad[-1] = jnt_traj(duration) + duration = intervals * self.dt logger.debug( "TrajectoryBuilder: output_samples=%d, duration=%.3f", @@ -625,59 +961,77 @@ def _build_toppra_trajectory(self) -> Trajectory: duration, ) - steps = _rad_to_steps_alloc(trajectory_rad) - - return Trajectory( - steps=steps, duration=duration, positions_rad=trajectory_rad - ) + return self._trajectory(trajectory_rad) except Exception as e: - logger.warning("TOPPRA failed: %s. Falling back to LINEAR profile.", e) - return self._build_simple_trajectory() + # A move the solver cannot time is refused, never quietly run + # under a different profile than the one selected. + raise TrajectoryPlanningError( + make_error(ErrorCode.TRAJ_NO_STEPS, detail=f"TOPPRA failed: {e}") + ) from e def _build_simple_trajectory(self) -> Trajectory: """ - Build trajectory with simple linear interpolation. - - Uses uniform s-spacing with local slowdown where velocity limits - would be exceeded. This handles singularities and wrist flips by - stretching only the affected segments. + Build the LINEAR profile: constant velocity along the path with ramps + at the acceleration limit at either end. + + The path coordinate runs a trapezoid whose cruise is the fastest the + steepest joint allows and whose ramps are at that joint's + acceleration limit, so the profile never steps its velocity. The + limits come from the path's own length, segment by segment, so a + wrist flip or a reconfiguration mid-path costs the time it takes + rather than being averaged away by the endpoint delta. On a + cartesian path the cruise also keeps the tool under its ceiling. """ - duration = ( - self.duration - if self.duration and self.duration > 0 - else self._compute_joint_duration_linear() - ) - - n_output = max(2, int(np.ceil(duration / self.dt))) - s_values = np.linspace(0.0, 1.0, n_output) - trajectory_rad = self.joint_path.sample_many(s_values) + vmax_s, amax_s, _ = self._compute_s_profile_limits() + vmax_s = min(vmax_s, self._cruise_cap()) + if not np.isfinite(vmax_s) or not np.isfinite(amax_s): + vmax_s, amax_s = 1.0, 1.0 - trajectory_rad, duration = self._enforce_segment_limits( - trajectory_rad, duration + profile_duration = _trapezoid_duration(1.0, vmax_s, amax_s) + duration = max(profile_duration, self.duration or 0.0, self.dt * 2) + intervals = _tick_intervals(duration, self.dt) + duration = intervals * self.dt + times = np.arange(intervals + 1) * self.dt + profile_s = _trapezoid_samples( + times * (profile_duration / duration), 0.0, 1.0, vmax_s, amax_s ) + trajectory_rad = self.joint_path.sample_many(profile_s) - steps = _rad_to_steps_alloc(trajectory_rad) + trajectory_rad = self._enforce_segment_limits(trajectory_rad) - return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) + return self._trajectory(trajectory_rad) def _is_cartesian_path(self) -> bool: """Check if this is a Cartesian path (has Cartesian velocity limits set).""" return self.cart_vel_limit is not None and self.cart_vel_limit > 0 + def _cruise_cap(self) -> float: + """The ``ds/dt`` ceiling on a path timed through its path coordinate: + on a process move, one tool speed the steepest stretch and the tool + ceiling allow; on any other cartesian path, the tool ceiling alone, + the joints slowing only where they must; none off one.""" + if self.constant_tool_speed: + return self._row_speed_cap() + return self._tool_speed_cap() + def _compute_s_profile_limits(self) -> tuple[float, float, float]: """ Compute path parameter (s) limits derived from joint limits. - For a linear path in joint space: - joint_velocity = joint_delta * (ds/dt) - joint_acceleration = joint_delta * (d²s/dt²) - joint_jerk = joint_delta * (d³s/dt³) + Along a path in joint space from s=0 to s=1, joint j travels + L[j] = sum of |delta q[j]| over the rows, so on average: + joint_velocity = L[j] * (ds/dt) + joint_acceleration = L[j] * (d²s/dt²) + joint_jerk = L[j] * (d³s/dt³) So the s-profile limits are: - vmax_s = min(v_max[j] / |delta[j]|) for all joints - amax_s = min(a_max[j] / |delta[j]|) for all joints - jmax_s = min(j_max[j] / |delta[j]|) for all joints + vmax_s = min(v_max[j] / L[j]) for all joints + amax_s = min(a_max[j] / L[j]) for all joints + jmax_s = min(j_max[j] / L[j]) for all joints + + The path's length rather than its endpoint delta: a path that comes + back to where it started still has somewhere to go. Returns: (vmax_s, amax_s, jmax_s): Limits for the path parameter profile @@ -686,19 +1040,13 @@ def _compute_s_profile_limits(self) -> tuple[float, float, float]: if len(positions) < 2: return (1.0, 1.0, 1.0) - total_delta = np.abs(positions[-1] - positions[0]) + length = np.sum(np.abs(np.diff(positions, axis=0)), axis=0) # Avoid division by zero for joints that don't move with np.errstate(divide="ignore", invalid="ignore"): - vmax_s_per_joint = np.where( - total_delta > 1e-9, self.v_max / total_delta, np.inf - ) - amax_s_per_joint = np.where( - total_delta > 1e-9, self.a_max / total_delta, np.inf - ) - jmax_s_per_joint = np.where( - total_delta > 1e-9, self.j_max / total_delta, np.inf - ) + vmax_s_per_joint = np.where(length > 1e-9, self.v_max / length, np.inf) + amax_s_per_joint = np.where(length > 1e-9, self.a_max / length, np.inf) + jmax_s_per_joint = np.where(length > 1e-9, self.j_max / length, np.inf) # The limiting joint determines the s-profile limits vmax_s = float(np.min(vmax_s_per_joint)) @@ -708,10 +1056,8 @@ def _compute_s_profile_limits(self) -> tuple[float, float, float]: return (vmax_s, amax_s, jmax_s) def _enforce_segment_limits( - self, - trajectory_rad: NDArray[np.float64], - duration: float, - ) -> tuple[NDArray[np.float64], float]: + self, trajectory_rad: NDArray[np.float64] + ) -> NDArray[np.float64]: """ Enforce velocity limits by locally stretching segments that exceed limits. @@ -720,30 +1066,36 @@ def _enforce_segment_limits( and wrist flips by slowing only where necessary, not globally. Args: - trajectory_rad: Joint positions in radians, shape (N, 6) - duration: Initial trajectory duration + trajectory_rad: Joint positions in radians, shape (N, 6), one + control tick apart Returns: - (adjusted_trajectory, adjusted_duration): Resampled trajectory with - locally stretched segments and new total duration + The trajectory resampled with locally stretched segments, its + rows still one tick apart; the input itself when nothing needs + stretching. """ n_points = len(trajectory_rad) if n_points < 2: - return trajectory_rad, duration + return trajectory_rad - initial_dt = duration / (n_points - 1) + initial_dt = self.dt + duration = (n_points - 1) * initial_dt deltas = np.diff(trajectory_rad, axis=0) # (N-1, 6) # Minimum time per segment to respect velocity limits: # max(|delta[j]| / v_max[j]) over joints min_segment_times = np.max(np.abs(deltas) / self.v_max, axis=1) # (N-1,) + segment_times = np.maximum(min_segment_times, initial_dt) # Approximate acceleration check: is the velocity change between - # adjacent segments feasible? + # adjacent segments feasible at the times they take once slowed for + # velocity? Measured at the original spacing instead, a segment + # slowed for velocity counts its excess twice. if n_points > 2: - velocities = deltas / initial_dt - accel = np.diff(velocities, axis=0) / initial_dt # (N-2, 6) + velocities = deltas / segment_times[:, np.newaxis] + spans = 0.5 * (segment_times[:-1] + segment_times[1:]) + accel = np.diff(velocities, axis=0) / spans[:, np.newaxis] # (N-2, 6) accel_times = np.zeros(n_points - 1) for i in range(len(accel)): max_accel_ratio = np.max(np.abs(accel[i]) / self.a_max) @@ -754,15 +1106,11 @@ def _enforce_segment_limits( accel_times[i + 1] = max( accel_times[i + 1], min_segment_times[i + 1] * stretch ) - min_segment_times = np.maximum(min_segment_times, accel_times) - - min_segment_times = np.maximum(min_segment_times, self.dt) - - segment_times = np.maximum(min_segment_times, initial_dt) + segment_times = np.maximum(segment_times, accel_times) new_duration = float(np.sum(segment_times)) if new_duration <= duration * 1.001: # No significant change - return trajectory_rad, duration + return trajectory_rad logger.warning( "Extending duration from %.3fs to %.3fs (%.1f%% increase) to respect velocity/acceleration limits", @@ -775,16 +1123,31 @@ def _enforce_segment_limits( cumulative_times = np.zeros(n_points) cumulative_times[1:] = np.cumsum(segment_times) - n_output = max(2, int(np.ceil(new_duration / self.dt))) - output_times = np.linspace(0.0, new_duration, n_output) + intervals = _tick_intervals(new_duration, self.dt) + output_times = np.arange(intervals + 1) * (new_duration / intervals) - new_trajectory = np.empty((n_output, 6), dtype=np.float64) + new_trajectory = np.empty((intervals + 1, 6), dtype=np.float64) for j in range(6): new_trajectory[:, j] = np.interp( output_times, cumulative_times, trajectory_rad[:, j] ) - return new_trajectory, new_duration + return new_trajectory + + def _requested_or(self, fastest: float) -> float: + """The requested duration, stretched to ``fastest`` when shorter: + a duration no joint can keep is not kept by overrunning its + limits.""" + if not self.duration: + return fastest + if self.duration < fastest: + logger.warning( + "Extending duration from %.3fs to %.3fs to respect velocity/acceleration limits", + self.duration, + fastest, + ) + return fastest + return self.duration def _compute_joint_duration_trapezoid(self) -> float: """ @@ -830,49 +1193,22 @@ def _compute_joint_duration_quintic(self) -> float: total_delta = np.abs(positions[-1] - positions[0]) - time_vel = 1.875 * total_delta / self.v_max - - with np.errstate(divide="ignore", invalid="ignore"): - time_acc = np.where( - self.a_max > 0, - np.sqrt(5.77 * total_delta / self.a_max), - 0.0, - ) - - time_per_joint = np.maximum(time_vel, time_acc) - return max(float(np.max(time_per_joint)), self.dt * 2) - - def _compute_joint_duration_linear(self) -> float: - """ - Compute duration for joint paths using linear interpolation. - - Accounts for both velocity and acceleration limits: - - For velocity limit: T_vel = delta / v_max - - For acceleration limit (triangular profile): T_acc = 2 * sqrt(2 * delta / a_max) - - Returns the maximum duration across all joints. - """ - positions = self.joint_path.positions - if len(positions) < 2: - return self.dt * 2 - - total_delta = np.abs(positions[-1] - positions[0]) - - time_vel = total_delta / self.v_max + time_vel = _QUINTIC_PEAK_VEL * total_delta / self.v_max with np.errstate(divide="ignore", invalid="ignore"): time_acc = np.where( self.a_max > 0, - 2.0 * np.sqrt(2.0 * total_delta / self.a_max), + np.sqrt(_QUINTIC_PEAK_ACC * total_delta / self.a_max), 0.0, ) time_per_joint = np.maximum(time_vel, time_acc) return max(float(np.max(time_per_joint)), self.dt * 2) - def _compute_cartesian_duration_from_path(self) -> float: + def _compute_duration_along_rows(self) -> float: """ - Compute duration for Cartesian paths based on per-segment joint requirements. + Compute duration for a path followed row by row, from per-segment + joint requirements. This properly handles singularities and wrist flips by analyzing the maximum joint movement required in each path segment, not just @@ -895,18 +1231,6 @@ def _compute_cartesian_duration_from_path(self) -> float: return max(float(np.sum(segment_times)), self.dt * 2) - def _build_quintic_trajectory(self) -> Trajectory: - """ - Build trajectory with quintic polynomial velocity profile. - - For joint moves: each joint follows its own quintic profile. - For Cartesian moves: TCP follows quintic profile along path. - """ - if self._is_cartesian_path(): - return self._build_quintic_trajectory_cartesian() - else: - return self._build_quintic_trajectory_joint() - def _build_quintic_trajectory_joint(self) -> Trajectory: """ Build per-joint quintic trajectory. @@ -917,14 +1241,12 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] - if self.duration: - duration = self.duration - else: - duration = self._compute_joint_duration_quintic() + duration = self._requested_or(self._compute_joint_duration_quintic()) - n_output = max(2, int(np.ceil(duration / self.dt))) - times = np.linspace(0.0, duration, n_output) - trajectory_rad = np.empty((n_output, 6), dtype=np.float64) + intervals = _tick_intervals(duration, self.dt) + duration = intervals * self.dt + times = np.arange(intervals + 1) * self.dt + trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) for j in range(6): delta = end_pos[j] - start_pos[j] @@ -936,54 +1258,36 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: times, start_pos[j], end_pos[j], duration ) - trajectory_rad, duration = self._enforce_segment_limits( - trajectory_rad, duration - ) - - steps = _rad_to_steps_alloc(trajectory_rad) + trajectory_rad = self._enforce_segment_limits(trajectory_rad) - return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) + return self._trajectory(trajectory_rad) - def _build_quintic_trajectory_cartesian(self) -> Trajectory: + def _build_quintic_trajectory_along_path(self) -> Trajectory: """ - Build Cartesian quintic trajectory. + Build a quintic trajectory of the path coordinate. - TCP follows quintic polynomial profile along the path, with local - slowdown where velocity limits would be exceeded. + The path is followed row by row along a quintic profile of s, with + local slowdown where velocity limits would be exceeded. On a + cartesian path the profile's peak keeps the tool under its ceiling. """ if self.duration: duration = self.duration else: # Use per-segment analysis to handle singularities and wrist flips - duration = self._compute_cartesian_duration_from_path() + duration = self._compute_duration_along_rows() + duration = max(duration, _QUINTIC_PEAK_VEL / self._cruise_cap()) - # Quintic profile for the path parameter s, from s=0 to s=1 - n_output = max(2, int(np.ceil(duration / self.dt))) - times = np.linspace(0.0, duration, n_output) + intervals = _tick_intervals(duration, self.dt) + duration = intervals * self.dt + times = np.arange(intervals + 1) * self.dt profile_s = _quintic_samples(times, 0.0, 1.0, duration) trajectory_rad = self.joint_path.sample_many(profile_s) - trajectory_rad, duration = self._enforce_segment_limits( - trajectory_rad, duration - ) - - steps = _rad_to_steps_alloc(trajectory_rad) + trajectory_rad = self._enforce_segment_limits(trajectory_rad) - return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) - - def _build_trapezoid_trajectory(self) -> Trajectory: - """ - Build trajectory with trapezoidal velocity profile. - - For joint moves: each joint follows its own trapezoidal profile. - For Cartesian moves: TCP follows trapezoidal profile along path. - """ - if self._is_cartesian_path(): - return self._build_trapezoid_trajectory_cartesian() - else: - return self._build_trapezoid_trajectory_joint() + return self._trajectory(trajectory_rad) def _build_trapezoid_trajectory_joint(self) -> Trajectory: """ @@ -995,14 +1299,12 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] - if self.duration: - duration = self.duration - else: - duration = self._compute_joint_duration_trapezoid() + duration = self._requested_or(self._compute_joint_duration_trapezoid()) - n_output = max(2, int(np.ceil(duration / self.dt))) - times = np.linspace(0.0, duration, n_output) - trajectory_rad = np.empty((n_output, 6), dtype=np.float64) + intervals = _tick_intervals(duration, self.dt) + duration = intervals * self.dt + times = np.arange(intervals + 1) * self.dt + trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) for j in range(6): delta = end_pos[j] - start_pos[j] @@ -1013,7 +1315,7 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: profile_duration = _trapezoid_duration(delta, self.v_max[j], self.a_max[j]) # Scale this joint's own profile time onto the synchronized duration - time_scale = profile_duration / duration if duration > 0 else 1.0 + time_scale = profile_duration / duration trajectory_rad[:, j] = _trapezoid_samples( times * time_scale, @@ -1023,54 +1325,9 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: self.a_max[j], ) - trajectory_rad, duration = self._enforce_segment_limits( - trajectory_rad, duration - ) - - steps = _rad_to_steps_alloc(trajectory_rad) - - return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) - - def _build_trapezoid_trajectory_cartesian(self) -> Trajectory: - """ - Build Cartesian trapezoidal trajectory. - - TCP follows trapezoidal velocity profile along the path, with local - slowdown where velocity limits would be exceeded. - """ - if self.duration: - duration = self.duration - else: - # Use per-segment analysis to handle singularities and wrist flips - duration = self._compute_cartesian_duration_from_path() - - vmax_s, amax_s, _ = self._compute_s_profile_limits() - - # Trapezoidal profile for the path parameter s, from s=0 to s=1 - profile_duration = _trapezoid_duration(1.0, vmax_s, amax_s) - - # If user specified longer duration, scale to match - if self.duration and self.duration > profile_duration: - time_scale = profile_duration / self.duration - duration = self.duration - else: - time_scale = 1.0 - duration = profile_duration - - n_output = max(2, int(np.ceil(duration / self.dt))) - times = np.linspace(0.0, duration, n_output) + trajectory_rad = self._enforce_segment_limits(trajectory_rad) - profile_s = _trapezoid_samples(times * time_scale, 0.0, 1.0, vmax_s, amax_s) - - trajectory_rad = self.joint_path.sample_many(profile_s) - - trajectory_rad, duration = self._enforce_segment_limits( - trajectory_rad, duration - ) - - steps = _rad_to_steps_alloc(trajectory_rad) - - return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) + return self._trajectory(trajectory_rad) def _build_cart_vel_constraint( self, path: ta.SplineInterpolator | _LinearPath, ss_waypoints: NDArray @@ -1164,6 +1421,83 @@ def vlim_func(s: float) -> NDArray: logger.warning("Failed to build Cartesian velocity constraint: %s", e) return None + def _build_path_speed_cap( + self, + path: _LinearPath, + positions: NDArray[np.float64], + slopes: NDArray[np.float64], + ) -> constraint.Constraint: + """:meth:`_path_speed_cap` as a TOPP-RA constraint. Holding every + stretch to it is what a constant tool speed costs; on a process + move that is the point rather than the price.""" + cap = self._path_speed_cap(positions, slopes) + vlim_buffer = np.empty((6, 2), dtype=np.float64) + + def vlim_func(s: float) -> NDArray: + dq_ds = np.abs(path(s, 1)) + q_dot_max = np.maximum(dq_ds * cap, 1e-6) + vlim_buffer[:, 0] = -q_dot_max + vlim_buffer[:, 1] = q_dot_max + return vlim_buffer.copy() + + return constraint.JointVelocityConstraintVarying(vlim_func) + + def _row_slopes(self) -> tuple[NDArray[np.float64], NDArray[np.float64]]: + """The rows each segment starts at, and each segment's ``dq/ds``, + for a path whose rows are evenly spaced in its parameter, as the + profiles that time ``s`` directly take it.""" + positions = self.joint_path.positions + return positions[:-1], np.diff(positions, axis=0) * (len(positions) - 1) + + def _row_speed_cap(self) -> float: + """:meth:`_path_speed_cap` over the rows (:meth:`_row_slopes`).""" + return self._path_speed_cap(*self._row_slopes()) + + def _tool_speed_cap(self) -> float: + """:meth:`_tool_speed_limit` over the rows (:meth:`_row_slopes`).""" + return self._tool_speed_limit(*self._row_slopes()) + + def _tool_speed_limit( + self, positions: NDArray[np.float64], slopes: NDArray[np.float64] + ) -> float: + """The fastest ``ds/dt`` that keeps the tool under the cartesian + ceiling wherever it moves fastest per unit of path; unbounded off + a cartesian path, or on one where the tool only turns. + ``positions`` are the rows the path runs through, one per segment + start in ``slopes``.""" + if not self._is_cartesian_path(): + return np.inf + assert self.cart_vel_limit is not None + robot = PAROL6_ROBOT.robot + jac = np.zeros((6, 6), dtype=np.float64, order="F") + fastest = 0.0 + for i in range(len(slopes)): + robot.jacob0_into(positions[i], jac) + fastest = max(fastest, float(np.linalg.norm(jac[:3, :] @ slopes[i]))) + return self.cart_vel_limit / fastest if fastest > 1e-9 else np.inf + + def _path_speed_cap( + self, positions: NDArray[np.float64], slopes: NDArray[np.float64] + ) -> float: + """One ``ds/dt`` ceiling for the whole path: the fastest constant the + steepest stretch allows under the joint limits, and under the + cartesian ceiling (:meth:`_tool_speed_limit`). ``positions`` are + the rows the path runs through, one per segment start in + ``slopes``.""" + with np.errstate(divide="ignore", invalid="ignore"): + per_joint = np.where( + np.abs(slopes) > 1e-9, self.v_max / np.abs(slopes), np.inf + ) + cap = min(float(np.min(per_joint)), self._tool_speed_limit(positions, slopes)) + if not np.isfinite(cap) or cap <= 0.0: + raise TrajectoryPlanningError( + make_error( + ErrorCode.TRAJ_NO_STEPS, + detail="the path covers no tool distance to hold a speed along", + ) + ) + return cap + def _build_ruckig_trajectory(self) -> Trajectory: """ Build trajectory using Ruckig for jerk-limited point-to-point motion. @@ -1196,7 +1530,10 @@ def _build_ruckig_trajectory(self) -> Trajectory: max_iters = int(est_duration / self.dt) + 500 # generous margin trajectory_rad = np.empty((max_iters, n_dofs), dtype=np.float64) - count = 0 + # Row 0 is the start, played on the first tick like every profile's: + # Ruckig's first output is already a tick along. + trajectory_rad[0] = start_pos + count = 1 result = Result.Working while result == Result.Working: @@ -1212,15 +1549,7 @@ def _build_ruckig_trajectory(self) -> Trajectory: if result == Result.Error: raise RuntimeError("Ruckig failed to compute trajectory") - actual_duration = out.trajectory.duration - - trajectory_rad = trajectory_rad[:count] - - steps = _rad_to_steps_alloc(trajectory_rad) - - return Trajectory( - steps=steps, duration=actual_duration, positions_rad=trajectory_rad - ) + return self._trajectory(trajectory_rad[:count]) def _estimate_simple_duration(self) -> float: """Estimate minimum duration based on joint velocity limits. diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 690dbce..6523c72 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -22,6 +22,7 @@ import logging import math +import re as _re from dataclasses import dataclass, field from collections.abc import Sequence from enum import IntEnum, auto @@ -33,6 +34,11 @@ from numba import njit from parol6.config import LIMITS +from parol6.utils.joint_limits import ( + TRAVEL_MAX_RAD, + TRAVEL_MIN_RAD, + joint_outside_travel_deg, +) from waldoctl import ActionState, ToolStatus from waldoctl.execution import ExecutionSpeed, validate_execution_scale from waldoctl.shapes import Attachment @@ -71,7 +77,7 @@ def _enc_hook(obj: object) -> object: #: Bumped on any change to the command envelope, a reply or the STATUS layout. -PROTO_VERSION = 1 +PROTO_VERSION = 2 _REQ_ID_BYTES = 4 MAX_REQ_ID = 2**32 - 1 @@ -206,6 +212,14 @@ class CmdType(IntEnum): # ============================================================================= +def _check_finite(name: str, values: Sequence[float]) -> None: + """A pose or joint target with a NaN or an infinity in it plans nothing + sensible; refuse it at the wire.""" + for v in values: + if isinstance(v, bool) or not math.isfinite(v): + raise ValueError(f"{name} must be finite numbers, got {list(values)!r}") + + def _check_speed_accel(speed: float, accel: float, *, signed: bool = False) -> None: """Validate speed/accel are in the expected fractional range.""" lo = -1.0 if signed else 0.0 @@ -221,6 +235,31 @@ def _check_speed_accel(speed: float, accel: float, *, signed: bool = False) -> N ) +def planned_move_timing( + duration: float, speed: float, accel: float +) -> tuple[float, float]: + """The ``(duration, speed)`` a planned move puts on the wire: a positive + *duration* times the move and leaves speed 0, otherwise *speed* does and + duration is 0. Raises ``ValueError`` for a negative or non-finite + duration, or for a speed (when it times the move) or accel outside + ``(0, 1]``.""" + if not (math.isfinite(duration) and duration >= 0.0): + raise ValueError(f"duration={duration} must be finite seconds >= 0") + if not (0.0 < accel <= 1.0): + raise ValueError( + f"accel={accel} is out of range (0.0, 1.0]. " + "Accel is a fraction of max acceleration, not a percentage." + ) + if duration > 0.0: + return duration, 0.0 + if not (0.0 < speed <= 1.0): + raise ValueError( + f"speed={speed} is out of range (0.0, 1.0]. " + "Speed is a fraction of max velocity, not a percentage." + ) + return 0.0, speed + + class MotionParamsMixin: """Mixin providing resolved motion parameters for wire structs. @@ -244,7 +283,8 @@ def resolved_duration(self) -> float | None: @property def resolved_speed(self) -> float: - """Velocity fraction 0-1, defaults to 1.0 (full speed).""" + """Velocity fraction 0-1; 1.0 on a duration-timed move, which + leaves speed unset.""" s = cast("float | None", getattr(self, "speed")) return s if s is not None and s > 0.0 else 1.0 @@ -277,16 +317,13 @@ def __post_init__(self) -> None: if has_duration and has_speed: raise ValueError("MOVEJ requires only one of duration or speed") _check_speed_accel(self.speed, self.accel) + _check_finite("MOVEJ angles", self.angles) if not self.rel: - for i in range(6): - if not ( - LIMITS.joint.position.deg[i, 0] - <= self.angles[i] - <= LIMITS.joint.position.deg[i, 1] - ): - raise ValueError( - f"Joint {i + 1} target ({self.angles[i]:.1f} deg) is out of range" - ) + i = joint_outside_travel_deg(self.angles) + if i >= 0: + raise ValueError( + f"Joint {i + 1} target ({self.angles[i]:.1f} deg) is out of range" + ) class MoveJPoseCmd( @@ -313,6 +350,7 @@ def __post_init__(self) -> None: if has_duration and has_speed: raise ValueError("MOVEJ_POSE requires only one of duration or speed") _check_speed_accel(self.speed, self.accel) + _check_finite("MOVEJ_POSE pose", self.pose) class MoveLCmd( @@ -341,6 +379,7 @@ def __post_init__(self) -> None: if has_duration and has_speed: raise ValueError("MOVEL requires only one of duration or speed") _check_speed_accel(self.speed, self.accel) + _check_finite("MOVEL pose", self.pose) class MoveCCmd( @@ -368,6 +407,8 @@ def __post_init__(self) -> None: raise ValueError("MOVEC requires either duration > 0 or speed > 0") if has_duration and has_speed: raise ValueError("MOVEC requires only one of duration or speed") + _check_finite("MOVEC via", self.via) + _check_finite("MOVEC end", self.end) if self.speed is not None: _check_speed_accel(self.speed, self.accel) @@ -401,6 +442,7 @@ def __post_init__(self) -> None: for i in range(len(waypoints)): if len(waypoints[i]) != 6: raise ValueError(f"Waypoint {i} must have 6 values (x,y,z,rx,ry,rz)") + _check_finite(f"MOVES waypoint {i}", waypoints[i]) class MovePCmd( @@ -432,6 +474,7 @@ def __post_init__(self) -> None: for i in range(len(waypoints)): if len(waypoints[i]) != 6: raise ValueError(f"Waypoint {i} must have 6 values (x,y,z,rx,ry,rz)") + _check_finite(f"MOVEP waypoint {i}", waypoints[i]) class CheckpointCmd( @@ -448,6 +491,10 @@ class CheckpointCmd( # -- Streaming commands: servo (position) -- +# Joint travel [deg] as plain floats, for the servo_j sweep. +_SERVO_MIN_DEG: tuple[float, ...] = tuple(float(v) for v in np.degrees(TRAVEL_MIN_RAD)) +_SERVO_MAX_DEG: tuple[float, ...] = tuple(float(v) for v in np.degrees(TRAVEL_MAX_RAD)) + class ServoJCmd( msgspec.Struct, @@ -462,6 +509,26 @@ class ServoJCmd( speed: Annotated[float, msgspec.Meta(gt=0.0, le=1.0)] = 1.0 accel: Annotated[float, msgspec.Meta(gt=0.0, le=1.0)] = 1.0 + def __post_init__(self) -> None: + # A stream sends one of these every few ticks: six plain floats + # inside travel, the case that matters, pass in one sweep; anything + # else goes through the full checks for its refusal. + angles = self.angles + for i in range(6): + v = angles[i] + if v.__class__ is not float or not ( + _SERVO_MIN_DEG[i] <= v <= _SERVO_MAX_DEG[i] + ): + break + else: + return + _check_finite("SERVOJ angles", angles) + i = joint_outside_travel_deg(angles) + if i >= 0: + raise ValueError( + f"Joint {i + 1} target ({angles[i]:.1f} deg) is out of range" + ) + class ServoJPoseCmd( msgspec.Struct, @@ -476,6 +543,9 @@ class ServoJPoseCmd( speed: Annotated[float, msgspec.Meta(gt=0.0, le=1.0)] = 1.0 accel: Annotated[float, msgspec.Meta(gt=0.0, le=1.0)] = 1.0 + def __post_init__(self) -> None: + _check_finite("SERVOJ_POSE pose", self.pose) + class ServoLCmd( msgspec.Struct, @@ -490,6 +560,9 @@ class ServoLCmd( speed: Annotated[float, msgspec.Meta(gt=0.0, le=1.0)] = 1.0 accel: Annotated[float, msgspec.Meta(gt=0.0, le=1.0)] = 1.0 + def __post_init__(self) -> None: + _check_finite("SERVOL pose", self.pose) + # -- Streaming commands: jog (velocity) -- @@ -601,11 +674,32 @@ class ResetLoopStatsCmd( class TeleportCmd( msgspec.Struct, tag=int(CmdType.TELEPORT), array_like=True, frozen=True, gc=False ): - """TELEPORT: instantly set joint angles in degrees (simulator only).""" + """TELEPORT: instantly set joint angles in degrees (simulator only). + + A system command: the controller answers OK once the pose is applied, + or an error when it is refused. The angles must be finite and inside + the hard joint limits, to half a motor step; each tool position must be + finite and within ``[0, 1]``. + """ angles: Annotated[list[float], msgspec.Meta(min_length=6, max_length=6)] tool_positions: list[float] | None = None + def __post_init__(self) -> None: + _check_finite("angles", self.angles) + i = joint_outside_travel_deg(self.angles) + if i >= 0: + limits = LIMITS.joint.position.deg + raise ValueError( + f"angles[{i}]={self.angles[i]} is outside the hard limits " + f"[{limits[i, 0]}, {limits[i, 1]}] deg" + ) + if self.tool_positions is not None: + _check_finite("tool_positions", self.tool_positions) + for i, p in enumerate(self.tool_positions): + if not (0.0 <= p <= 1.0): + raise ValueError(f"tool_positions[{i}]={p} is outside [0, 1]") + class WriteIOCmd( msgspec.Struct, tag=int(CmdType.WRITE_IO), array_like=True, frozen=True, gc=False @@ -756,8 +850,10 @@ class ToolActionCmd( ): """TOOL_ACTION: [CmdType.TOOL_ACTION, tool_key, action, params] - Generic tool action command. The controller validates *tool_key* - against the registry and delegates to the appropriate 100 Hz command. + Generic tool action command. *tool_key*, *action* and *params* are + validated against the tool's config on decode, so a malformed action is + refused before it is acknowledged; the controller then delegates to the + tool's 100 Hz command. """ tool_key: Annotated[str, msgspec.Meta(min_length=1, max_length=64)] @@ -767,8 +863,10 @@ class ToolActionCmd( def __post_init__(self) -> None: key = self.tool_key.strip().upper() registry = get_registry() - if key not in registry: + cfg = registry.get(key) + if cfg is None: raise ValueError(f"Unknown tool '{key}'. Available: {list(registry)}") + cfg.validate_action(self.action.strip().lower(), self.params) class ToolStatusCmd( @@ -1097,10 +1195,38 @@ def _build_struct_to_cmdtype(structs: list[type]) -> dict[type, CmdType]: return mapping +def pascal_to_snake(name: str) -> str: + """``MoveJPose`` → ``move_j_pose``, ``IsSimulator`` → ``is_simulator``.""" + s = _re.sub(r"([A-Z]+)([A-Z][a-z])", r"\1_\2", name) + s = _re.sub(r"([a-z0-9])([A-Z])", r"\1_\2", s) + return s.lower() + + # Build at import time _COMMAND_STRUCTS = _collect_command_structs() STRUCT_TO_CMDTYPE: dict[type, CmdType] = _build_struct_to_cmdtype(_COMMAND_STRUCTS) +#: Pose targets are the ``move_j`` / ``servo_j`` calls that sent them: +#: waldoctl has no ``move_j_pose`` method for the name to point at. +_REPORTED_AS: dict[str, str] = {"MoveJPoseCmd": "move_j", "ServoJPoseCmd": "servo_j"} + +#: Wire struct → the snake_case name of the waldoctl method that sends it +#: (``MoveJCmd`` → ``"move_j"``, ``HomeCmd`` → ``"home"``). What ``queue()`` +#: and ``activity()`` report, so the same name reads across backends. +WIRE_COMMAND_NAMES: dict[type, str] = { + struct_cls: _REPORTED_AS.get( + struct_cls.__name__, pascal_to_snake(struct_cls.__name__.removesuffix("Cmd")) + ) + for struct_cls in _COMMAND_STRUCTS +} + + +def wire_command_name(struct_cls: type) -> str: + """The reported name of a command, from its wire struct type.""" + name = WIRE_COMMAND_NAMES.get(struct_cls) + return name if name is not None else pascal_to_snake(struct_cls.__name__) + + # Build Command union dynamically from collected structs Command: TypeAlias = Union[tuple(_COMMAND_STRUCTS)] @@ -1435,6 +1561,9 @@ class CommandCompletionResultStruct( command_index: int session_id: int completed: bool + # The command finished as a FAILURE — cancelled by a stop, or the + # pipeline gave up on it — as RobotError wire form; None otherwise. + error: list | None = None def __post_init__(self) -> None: if type(self.command_index) is not int or not 0 <= self.command_index < 2**63: @@ -2195,6 +2324,7 @@ def unpack_rx_frame_into( "HomingJointState", "HomingPhase", "CheckpointCmd", + "planned_move_timing", # Command structs — streaming (servo/jog) "ServoJCmd", "ServoJPoseCmd", diff --git a/parol6/robot.py b/parol6/robot.py index 1bc2385..5f3aa09 100644 --- a/parol6/robot.py +++ b/parol6/robot.py @@ -350,28 +350,49 @@ def __init__( **kwargs, ) - async def set_position(self, position: float, **kwargs: float | int) -> int: - speed = float(kwargs.get("speed", 0.5)) - current = int(kwargs.get("current", self.current_range[0])) - return await self._cmd("move", [position, speed, current]) + async def set_position( + self, + position: float, + *, + speed: float = 0.5, + current: float = 0.5, + **wait_kwargs: Any, + ) -> int: + return await self._cmd("move", [position, speed, current], **wait_kwargs) async def calibrate(self, **kwargs: object) -> int: - return await self._cmd("calibrate") + return await self._cmd("calibrate", **kwargs) + + async def stop(self, **kwargs: object) -> int: + """Halt the jaws where they are, keeping the grip, ahead of anything + still queued: the action running fails as cancelled, and the queue + behind it is kept. On an uncalibrated gripper, which has no position + to hold, this releases instead.""" + return await self._cmd("stop", **kwargs) + + async def release(self, **kwargs: object) -> int: + """Drop the grip, freeing the jaws for manual handling, once the + commands queued ahead of it have run. The jaws stay where they + are.""" + return await self._cmd("idle", **kwargs) async def action_r(self, engaged: bool) -> None: await self.calibrate() - async def open(self, **kwargs: float | int) -> int: - return await self.set_position(0.0, **kwargs) + async def open( + self, *, speed: float = 0.5, current: float = 0.5, **wait_kwargs: Any + ) -> int: + return await self.set_position(0.0, speed=speed, current=current, **wait_kwargs) - async def close(self, **kwargs: float | int) -> int: - return await self.set_position(1.0, **kwargs) + async def close( + self, *, speed: float = 0.5, current: float = 0.5, **wait_kwargs: Any + ) -> int: + return await self.set_position(1.0, speed=speed, current=current, **wait_kwargs) @property def adjust_step(self) -> int: - """Default current step: ~10% of range, rounded to nearest 10 mA.""" - lo, hi = self.current_range - return max(10, round((hi - lo) / 10 / 10) * 10) + """Current step in percent points of the current range.""" + return 10 @property def adjust_labels(self) -> tuple[str, str]: @@ -991,5 +1012,8 @@ def create_dry_run_client(self, **kwargs: Any) -> DryRunClient | None: return DryRunRobotClient( initial_joints_deg=initial_joints_deg, initial_homed=initial_homed, + initial_gripper_calibrated=bool( + kwargs.get("initial_gripper_calibrated", False) + ), robot=self, ) diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py index b36186b..f1dd067 100644 --- a/parol6/server/command_executor.py +++ b/parol6/server/command_executor.py @@ -4,6 +4,7 @@ import re import time from collections import deque +from collections.abc import Callable from dataclasses import dataclass, field from typing import TYPE_CHECKING @@ -13,7 +14,14 @@ MotionCommand, ) from parol6.config import MAX_COMMAND_QUEUE_SIZE, TRACE -from parol6.protocol.wire import Command, decode_command +from parol6.protocol.wire import Command, decode_command, wire_command_name +from parol6.utils.error_catalog import ( + RobotError, + attributed, + extract_robot_error, + make_error, +) +from parol6.utils.error_codes import ErrorCode from waldoctl import ActionState if TYPE_CHECKING: @@ -68,11 +76,19 @@ class CommandExecutor: Immediate commands (system, query) are handled directly by the controller. """ - def __init__(self, state_manager: "StateManager"): + def __init__( + self, state_manager: "StateManager", others_in_flight: Callable[[], bool] + ): self._state_manager = state_manager + # Whether work beside the streams (planned motion, a tool action) is + # under way, which a refusal left standing would be read against. + self._others_in_flight = others_in_flight self.command_queue: deque[QueuedCommand] = deque(maxlen=MAX_COMMAND_QUEUE_SIZE) self.active_command: QueuedCommand | None = None + # The last refusal logged and when, so a stream refused at 50 Hz + # logs once a second rather than every datagram. + self._refusal_logged: tuple[int, float] = (-1, 0.0) def _update_queue_state(self, state: "ControllerState") -> None: """Update queue snapshot and next action in state.""" @@ -80,7 +96,7 @@ def _update_queue_state(self, state: "ControllerState") -> None: state.queue_nonstreamable.clear() for qc in self.command_queue: if not (isinstance(qc.command, MotionCommand) and qc.command.streamable): - state.queue_nonstreamable.append(type(qc.command).__name__) + state.queue_nonstreamable.append(wire_command_name(type(qc.command.p))) state.action_next = ( state.queue_nonstreamable[0] if state.queue_nonstreamable else "" ) @@ -209,7 +225,7 @@ def execute_active_command(self) -> None: # One-time setup on first activation if not ac.activated: self._setup_active(ac, state) - state.action_current = type(ac.command).__name__ + state.action_current = wire_command_name(type(ac.command.p)) state.action_params = _format_cmd_params(ac.command.p) state.action_state = ActionState.EXECUTING state.executing_command_index = ac.command_index @@ -230,11 +246,22 @@ def execute_active_command(self) -> None: self._process_tick_result(ac, code, state) except Exception as e: - logger.error("Command execution error: %s", e) + # A stream refused in setup (unhomed, off the simulator, a + # bad parameter) answers no datagram: the standing error is + # how its client learns of it. + error = extract_robot_error(e, ErrorCode.MOTN_SETUP_FAILED, detail=str(e)) + now = time.monotonic() + if ( + error.code != self._refusal_logged[0] + or now - self._refusal_logged[1] >= 1.0 + ): + logger.error("Command execution error: %s", e) + self._refusal_logged = (error.code, now) + self._latch_failure(ac, error, state) + self._release_streams(state) state.action_current = "" state.executing_command_index = -1 state.action_params = "" - state.action_state = ActionState.IDLE self._update_queue_state(state) self.active_command = None @@ -289,9 +316,18 @@ def _process_tick_result( time.time(), ) + self._latch_failure( + ac, + ac.command.robot_error + or make_error( + ErrorCode.MOTN_TICK_FAILED, detail=type(ac.command).__name__ + ), + state, + ) + self._release_streams(state) state.action_current = "" + state.executing_command_index = -1 state.action_params = "" - state.action_state = ActionState.IDLE # Drop queued streamable commands so they don't pile up after a failure. if isinstance(ac.command, MotionCommand) and ac.command.streamable: @@ -307,6 +343,32 @@ def _process_tick_result( self._update_queue_state(state) self.active_command = None + @staticmethod + def _release_streams(state: "ControllerState") -> None: + """Drop whatever motion the streaming executors carry: a stream cut + off (a stop, an E-stop, a teleport, a planned move, a stream of + another kind) or failed leaves nothing for the next one, which + starts from the arm at rest instead of running on the way this one + was going, or back to where it was.""" + state.streaming_executor.reset() + state.cartesian_streaming_executor.reset() + + def _latch_failure( + self, ac: QueuedCommand, error: RobotError, state: "ControllerState" + ) -> None: + """A command that ends in error fails its own index, and leaves the + error standing as a planned move's does: a stream answers no + datagram, so ``error()`` and STATUS are how its client hears of a + brake-out or a refusal. It stands only while nothing else is under + way: beside a running program it would misdescribe the program.""" + error = attributed(error, ac.command_index) + state.record_failure(ac.command_index, error) + if self._others_in_flight(): + state.action_state = ActionState.IDLE + else: + state.error = error + state.action_state = ActionState.ERROR + # ---- Cancellation and queue management ---- def cancel_active_command(self, reason: str = "Cancelled by user") -> None: @@ -321,6 +383,7 @@ def cancel_active_command(self, reason: str = "Cancelled by user") -> None: ) state = self._state_manager.get_state() + self._release_streams(state) state.action_current = "" state.executing_command_index = -1 state.action_params = "" @@ -337,6 +400,7 @@ def cancel_active_streamable(self) -> bool: ac = self.active_command if ac and isinstance(ac.command, MotionCommand) and ac.command.streamable: state = self._state_manager.get_state() + self._release_streams(state) state.action_current = "" state.executing_command_index = -1 state.action_params = "" diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 2b00c5a..c00cb56 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -16,13 +16,15 @@ from parol6.ack_policy import ARM_MOTION_CMD_TYPES, AckPolicy from parol6.commands.base import ( - CommandBase, ExecutionStatusCode, MotionCommand, QueryCommand, SystemCommand, + arm_homed, ) from parol6.commands.shape_commands import SetShapesCommand +from parol6.commands.tool_action_command import ToolActionCommand +from parol6.commands.basic_commands import TeleportCommand from parol6.commands.system_commands import ( EstopCommand, SelectProfileCommand, @@ -33,7 +35,10 @@ from parol6.server.motion_planner import MotionPlanner, PlanCommand from parol6.server.segment_player import SegmentPlayer from parol6.protocol.wire import ( + wire_command_name, + CmdType, CommandCode, + SelectToolCmd, ToolActionCmd, pack_error, pack_ok, @@ -42,7 +47,11 @@ split_request, unpack_rx_frame_into, ) -from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error +from parol6.utils.error_catalog import ( + RobotError, + extract_robot_error, + make_error, +) from parol6.utils.error_codes import ErrorCode from parol6.server.command_registry import ( CommandCategory, @@ -50,8 +59,8 @@ create_command_from_struct, discover_commands, ) -from parol6.server.state import ControllerState, StateManager -from waldoctl import ActionState +from parol6.server.state import ATTACHMENT_CHANGED, ControllerState, StateManager +from waldoctl import ActionState, ToolStatus from parol6.server.status_broadcast import StatusBroadcaster from parol6.server.async_logging import AsyncLogHandler from parol6.server.loop_timer import ( @@ -63,6 +72,8 @@ ) from parol6.server.status_cache import close_cache, get_cache from parol6.server.transport_manager import TransportManager +from parol6.tools import get_registry, unselected_tool_refusal +from parol6.server.transports.transport_factory import is_simulation_mode from parol6.server.transports.mock_serial_transport import MockSerialTransport from parol6.server.transports.udp_transport import UDPTransport from parol6.config import ( @@ -82,6 +93,12 @@ logger = logging.getLogger("parol6.server.controller") +# Streams that work from the reported pose: on an unreferenced arm there is +# none, and they are refused before they take the arm from anything. +_REFERENCED_STREAMS = frozenset( + (CmdType.JOGL, CmdType.SERVOJ, CmdType.SERVOJ_POSE, CmdType.SERVOL) +) + @dataclass class ControllerConfig: @@ -144,6 +161,9 @@ def __init__(self, config: ControllerConfig): "sim", # tick_simulation ] ) + self._tick_count = 0 + self._broadcast_rate_hz = 0.0 + self._broadcast_interval = 1 self._cmd_rate = EventRateMetrics() self._gc_tracker = GCTracker() self._ack_policy = AckPolicy() @@ -156,19 +176,15 @@ def __init__(self, config: ControllerConfig): ) self._executor = CommandExecutor( state_manager=self.state_manager, + others_in_flight=self._others_in_flight, ) - # Motion pipeline: planner subprocess computes trajectories, - # segment player consumes them in the control loop + # Motion pipeline: planner subprocess computes trajectories and + # forwards the rest (tool actions included) in order; the segment + # player consumes them in the control loop. self._planner = MotionPlanner() self._segment_player = SegmentPlayer(self._planner) - # Tool action side channel — runs concurrently with both streaming - # and trajectory execution (writes to gripper_hw, not Position_out) - self._tool_cmd: CommandBase | None = None - self._tool_cmd_activated: bool = False - self._tool_cmd_index: int = -1 - self._initialize_components() def _initialize_components(self) -> None: @@ -331,11 +347,38 @@ 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") + # Flush stale commands so the robot doesn't replay old moves. + # First, while the gripper still counts as calibrated: the tool + # is halted holding its grip, not released as an uncalibrated + # one would be. + self._cancel_pipeline(state, "Serial reconnect", "a serial reconnect") + state.gripper_calibrated = False + + def _cancel_pipeline(self, state: ControllerState, reason: str, scope: str) -> None: + """Discard all motion — planned, queued (tool actions included) and + streaming — and fail every command it owed with ``MOTN_CANCELLED``, + so a wait on any of them raises instead of running out its timeout. + The tool is halted in place, keeping its grip: a stop that let the + jaws carry on would report the action cancelled while the gripper + went on closing.""" + owed = self._segment_player.owed_indices(state) + active = self._executor.active_command + if active is not None: + owed.append(active.command_index) + owed.extend(q.command_index for q in self._executor.command_queue) + self._segment_player.cancel(state) + self._executor.cancel_active_command(reason) + self._executor.clear_queue(reason) + self._fail_cancelled(state, owed, scope) + + @staticmethod + def _fail_cancelled(state: ControllerState, owed: list[int], scope: str) -> None: + """Fail every index in *owed* not already finished with + ``MOTN_CANCELLED``. Cancel paths only — it allocates, though not per + index: a stop runs it for its whole queue in the tick it stops.""" + state.fail_unfinished( + sorted(set(owed)), make_error(ErrorCode.MOTN_CANCELLED, scope=scope) + ) def _check_attachments(self, state: ControllerState) -> None: if not state.has_attachments: @@ -352,14 +395,13 @@ def _check_attachments(self, state: ControllerState) -> None: 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") + self._cancel_pipeline( + state, "Attachment context changed", "an attachment change" + ) state.Speed_out.fill(0) state.error = make_error( ErrorCode.COMM_VALIDATION_ERROR, - detail="attachment context changed; reconcile the physical scene and reapply", + detail=ATTACHMENT_CHANGED, ) state.attachment_motion_stopped = True @@ -375,11 +417,7 @@ def _handle_estop(self, state: ControllerState) -> None: if not self.estop_active: logger.warning("E-STOP activated") self.estop_active = True - self._segment_player.cancel(state) - self._resync_planner(state) - if self._executor.active_command: - self._executor.cancel_active_command("E-Stop activated") - self._executor.clear_queue("E-Stop activated") + self._cancel_pipeline(state, "E-Stop activated", "the e-stop") state.Command_out = CommandCode.DISABLE state.Speed_out.fill(0) state.enabled = False @@ -401,11 +439,19 @@ def _handle_estop(self, state: ControllerState) -> None: def _execute_commands(self, state: ControllerState) -> None: """Phase 3: Execute active command.""" - # Tool action side channel — ticks concurrently with everything - self._tick_tool_cmd(state) + # A tool stop drives only the gripper, beside everything else. + self._segment_player.tick_tool_stop(state) + + if state.command_out_locked and state.Command_out == CommandCode.TELEPORT: + # The simulator lands the arm on this tick's frame. Motion read + # after the teleport starts on the next tick, from the landing, + # and cannot overwrite the frame that lands it. + return # Segment player handles trajectory + inline commands from planner - if self._segment_player.tick(state): + playing = self._segment_player.tick(state) + self._planner.set_playing(self._segment_player.active) + if playing: return # Streaming command executor (jog/servo) @@ -420,42 +466,17 @@ def _execute_commands(self, state: ControllerState) -> None: state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) - def _tick_tool_cmd(self, state: ControllerState) -> None: - """Tick tool action side channel (concurrent with motion).""" - if self._tool_cmd is None: - return - - if not self._tool_cmd_activated: - self._tool_cmd.setup(state) - self._tool_cmd_activated = True - - code = self._tool_cmd.tick(state) - - if code == ExecutionStatusCode.COMPLETED: - state.record_completion(self._tool_cmd_index) - self._tool_cmd = None - self._tool_cmd_activated = False - elif code == ExecutionStatusCode.FAILED: - logger.error( - "Tool action failed: %s - %s", - type(self._tool_cmd).__name__, - self._tool_cmd.robot_error, - ) - raw_error = self._tool_cmd.robot_error or make_error( - ErrorCode.MOTN_TICK_FAILED, detail=type(self._tool_cmd).__name__ - ) - # Rebuilt from the wire, not `replace`d: the refusal is an - # exception now, and a dataclass replace on one does not - # survive the copy the state makes of it. - attributed = raw_error.to_wire() - attributed[0] = self._tool_cmd_index - state.error = RobotError.from_wire(attributed) - state.action_state = ActionState.ERROR - state.completed_command_index = max( - state.completed_command_index, self._tool_cmd_index - ) - self._tool_cmd = None - self._tool_cmd_activated = False + def _others_in_flight(self) -> bool: + """Whether queued work — planned motion or a tool action — or a tool + stop is under way beside the streams: what a refused stream leaves + standing as the error would be read as their failure.""" + state = self.state_manager.get_state() + return ( + self._segment_player.active + or bool(state.pending_planned) + or state.plan_in_flight + or self._segment_player.tool_stopping + ) def _write_to_firmware(self, state: ControllerState) -> None: """Phase 4: Write state to serial transport.""" @@ -559,64 +580,16 @@ def _main_control_loop(self): """Main control loop with phase-based structure and precise timing.""" self._timer.start() pt = self._phase_timer - tick_count = 0 - # Re-derived from the state rather than captured once: SET_STATUS_RATE - # moves the rate mid-session, and a snapshot taken here would keep - # broadcasting at whatever the rate was at boot. The sentinel rate - # never matches, so the first tick derives the real interval. - broadcast_rate_hz = 0.0 - broadcast_interval = 1 while self.running: try: state = self.state_manager.get_state() - tick_count += 1 - - 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"): - self._execute_commands(state) - - if state.status_rate_hz != broadcast_rate_hz: - broadcast_rate_hz = state.status_rate_hz - broadcast_interval = status_broadcast_interval(broadcast_rate_hz) - - if tick_count % broadcast_interval == 0: - with pt.phase("status"): - if self._status_broadcaster: - self._status_broadcaster.tick() - - with pt.phase("write"): - self._write_to_firmware(state) - - with pt.phase("sim"): - # Pass tool teleport position if set by TeleportCommand - tool_tp = state.tool_teleport_pos - if tool_tp >= 0: - state.tool_teleport_pos = -1.0 # consume - # Cancel in-flight tool action so it doesn't re-arm the ramp - self._tool_cmd = None - self._tool_cmd_activated = False - self._transport_mgr.tick_simulation( - state.current_tool, - tool_teleport_pos=tool_tp, - ) - + self._run_tick(state) pt.tick() self._sync_timer_metrics(state) self._log_periodic_status(state) self._gc_tracker.collect_deferred( - self._timer.time_to_next_deadline(), tick_count + self._timer.time_to_next_deadline(), self._tick_count ) self._timer.wait_for_next_tick() @@ -634,6 +607,54 @@ def _main_control_loop(self): state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) + def _run_tick(self, state: ControllerState) -> None: + """One control tick's phases, in loop order.""" + pt = self._phase_timer + self._tick_count += 1 + + 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"): + self._execute_commands(state) + + # Re-derived from the state rather than captured once: SET_STATUS_RATE + # moves the rate mid-session, and a snapshot taken at boot would keep + # broadcasting at whatever the rate was then. The sentinel rate never + # matches, so the first tick derives the real interval. + if state.status_rate_hz != self._broadcast_rate_hz: + self._broadcast_rate_hz = state.status_rate_hz + self._broadcast_interval = status_broadcast_interval( + self._broadcast_rate_hz + ) + + if self._tick_count % self._broadcast_interval == 0: + with pt.phase("status"): + if self._status_broadcaster: + self._status_broadcaster.tick() + + with pt.phase("write"): + self._write_to_firmware(state) + + with pt.phase("sim"): + # Pass tool teleport position if set by TeleportCommand + tool_tp = state.tool_teleport_pos + if tool_tp >= 0: + state.tool_teleport_pos = -1.0 # consume + self._transport_mgr.tick_simulation( + state.current_tool, + tool_teleport_pos=tool_tp, + ) + def _poll_commands(self, state: ControllerState) -> None: """Poll and process UDP commands (non-blocking). @@ -744,7 +765,7 @@ def _handle_motion_command( req_id: int, ) -> None: """Queue motion command for execution.""" - cmd_name = type(command).__name__ + cmd_name = wire_command_name(type(command.p)) cmd_type = command._cmd_type if not state.attachments_valid and cmd_type in ARM_MOTION_CMD_TYPES: @@ -754,7 +775,7 @@ def _handle_motion_command( addr, make_error( ErrorCode.COMM_VALIDATION_ERROR, - detail="attachment context changed; reconcile the physical scene and reapply", + detail=ATTACHMENT_CHANGED, ), ) elif self._stale_attachment_logged_epoch != state.attachment_epoch: @@ -762,8 +783,9 @@ def _handle_motion_command( # 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", + "Dropping streamed %s: %s", cmd_name, + ATTACHMENT_CHANGED, ) return if not state.enabled: @@ -778,10 +800,27 @@ def _handle_motion_command( "Motion command rejected - controller disabled: %s", cmd_name ) return + if cmd_type in _REFERENCED_STREAMS and not arm_homed(state): + # Refused before the stream takes the arm from anything: a home + # in flight is establishing the reference it lacks. The refusal + # stands against the next index, past every earlier command, so + # no earlier wait reads it as its failure; it takes no index of + # its own, which a wait for the queue would wait on forever. + if state.error is None or state.error.code != ErrorCode.MOTN_NOT_HOMED: + logger.warning("Streamed %s refused: robot not homed", cmd_name) + state.error = make_error(ErrorCode.MOTN_NOT_HOMED, state.next_command_index) + if self._ack_policy.requires_ack(cmd_type): + self._reply_error(req_id, addr, state.error) + return # Streaming commands: cancel segment playback + existing streamable handling if getattr(command, "streamable", False): + # The queue yields to the stream — its tool actions with it, the + # one playing halted where the jaws are — and every command it + # owed fails as cancelled. + owed = self._segment_player.owed_indices(state) self._segment_player.cancel(state) + self._fail_cancelled(state, owed, "a streamed command") # Unconditional: a jog self-collision sets the viz but no state.error. state.clear_collision() # Coalesce decoded motion only: unread UDP packets can contain @@ -811,37 +850,22 @@ def _handle_motion_command( ) return - # Tool actions bypass planner — execute directly via side channel - # (writes to gripper_hw, not Position_out, so concurrent with everything) if isinstance(command.p, ToolActionCmd): - # Clear error state from previous failure (same as non-streaming path) - if state.error is not None: - state.error = None - state.action_state = ActionState.IDLE - # Unconditional: a jog self-collision sets the viz but no state.error. - state.clear_collision() - - cmd_obj, _, error_msg = create_command_from_struct(command.p) - if cmd_obj is None: - logger.error("Failed to create tool command: %s", error_msg) + if command.p.action.strip().lower() == "stop": + self._stop_tool(command.p, state, addr, req_id) + return + # Queued, so judged against the newest selection rather than the + # fitted tool: a select_tool still queued fits it first. + refusal = unselected_tool_refusal(command.p.tool_key, state.accepted_tool) + if refusal is not None: + logger.warning("Tool action refused: %s", refusal) if cmd_type and self._ack_policy.requires_ack(cmd_type): self._reply_error( req_id, addr, - make_error(ErrorCode.COMM_DECODE_ERROR, detail=error_msg or ""), + make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=refusal), ) return - # New tool action replaces any in-flight one - self._tool_cmd = cmd_obj - self._tool_cmd_activated = False - cmd_index = self._assign_command_index(state) - self._tool_cmd_index = cmd_index - logger.log( - TRACE, "Command %s → tool side channel (index=%d)", cmd_name, cmd_index - ) - if cmd_type and self._ack_policy.requires_ack(cmd_type): - self._reply_ok_index(req_id, addr, cmd_index) - return # Non-streaming commands → planner # Cancel active streaming command to avoid Position_in race @@ -878,9 +902,53 @@ def _handle_motion_command( ) state.pending_planned.append((cmd_index, cmd_name)) state.plan_submitted_index = cmd_index + if isinstance(command.p, SelectToolCmd): + state.accepted_tool = command.p.tool_name.strip().upper() if cmd_type and self._ack_policy.requires_ack(cmd_type): self._reply_ok_index(req_id, addr, cmd_index) + def _stop_tool( + self, + params: ToolActionCmd, + state: ControllerState, + addr: tuple[str, int], + req_id: int, + ) -> None: + """Run a tool stop now rather than in its turn: it halts the tool + action playing and settles the jaws beside the queue, which keeps + everything it holds. It acts on the tool fitted now.""" + stop, _, error_msg = create_command_from_struct(params) + if stop is None: + logger.error("Failed to create tool stop: %s", error_msg) + self._reply_error( + req_id, + addr, + make_error(ErrorCode.COMM_DECODE_ERROR, detail=error_msg or ""), + ) + return + assert isinstance(stop, ToolActionCommand) + try: + stop.setup(state) + except Exception as e: + self._reply_error( + req_id, + addr, + extract_robot_error(e, ErrorCode.MOTN_SETUP_FAILED, detail=str(e)), + ) + return + if stop.robot_error is not None: + logger.warning("Tool stop refused: %s", stop.robot_error.cause) + self._reply_error(req_id, addr, stop.robot_error) + return + if state.error is not None: + state.error = None + state.action_state = ActionState.IDLE + # Unconditional: a jog self-collision sets the viz but no state.error. + state.clear_collision() + cmd_index = self._assign_command_index(state) + self._segment_player.stop_tool(stop, cmd_index, state) + self._reply_ok_index(req_id, addr, cmd_index) + def _handle_query( self, command: QueryCommand, @@ -900,22 +968,6 @@ def _handle_query( req_id, addr, make_error(ErrorCode.COMM_DECODE_ERROR, detail=str(e)) ) - def _resync_planner(self, state: ControllerState) -> None: - """Bring the planner subprocess back to the controller's tool and world. - - The planner applies SET_TCP_TRANSFORM / SELECT_TOOL / SET_SHAPES at - plan time, when the command is still queued. Cancelling that queue - leaves the planner holding a change the controller never applied, - and every later plan would be solved against it. - """ - self._planner.sync_tool( - state.current_tool, - variant_key=state.current_tool_variant, - tcp_offset_m=state.tcp_offset_m, - tcp_rotation_rad=state.tcp_rotation_rad, - ) - self._planner.sync_shapes(state.shapes) - def _handle_system_command( self, command: SystemCommand, @@ -925,7 +977,27 @@ def _handle_system_command( ) -> None: """Execute system command, apply side effects, and send reply.""" try: + # Reset-state discards the pipeline BEFORE the state reset runs: + # the reset forgets what was pending, and every command the + # pipeline owed has to be failed first. + if isinstance(command, ResetStateCommand): + self._cancel_pipeline(state, "Reset", "reset_state") + if isinstance(command, TeleportCommand): + refusal = self._teleport_refusal(state, command) + if refusal is not None: + self._reply_error(req_id, addr, refusal) + return command.setup(state) + if isinstance(command, TeleportCommand): + # The pose jumps: whatever was driving the arm is void, a + # pause held a queue that is gone, and the failure the arm + # was left in stays where it happened. + self._cancel_pipeline(state, "Teleport", "a teleport") + state.execution_paused = False + state.error = None + if state.action_state == ActionState.ERROR: + state.action_state = ActionState.IDLE + state.clear_collision() code = command.tick(state) # This SystemCommand set a real signal (e.g. RESET's ENABLE) for @@ -944,31 +1016,31 @@ def _handle_system_command( if isinstance(command, EstopCommand) else "User requested stop" ) - self._segment_player.cancel(state) - self._executor.cancel_active_command(reason) - self._executor.clear_queue(reason) - self._resync_planner(state) + self._cancel_pipeline( + state, + reason, + "estop" if isinstance(command, EstopCommand) else "stop", + ) # A pause holds the queue it interrupted; that queue is gone. state.execution_paused = False - # Reset-state: cancel motion pipeline so stale segments don't play. - # Also sync the (now-cleared) tool state to the planner subprocess - # so its PAROL6_ROBOT singleton matches the controller's. + # Reset-state: sync the (now-cleared) tool state and the restored + # profile to the planner subprocess, so its PAROL6_ROBOT singleton + # and the profile it plans with match the controller's. if isinstance(command, ResetStateCommand): - self._segment_player.cancel(state) - self._executor.cancel_active_command("Reset") - self._executor.clear_queue("Reset") - self._resync_planner(state) - state.execution_paused = False + self._planner.resync(state) # 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) - self._executor.cancel_active_command("Simulator mode toggle") - self._executor.clear_queue("Simulator mode toggle") + # Cancelled while the gripper still counts as calibrated, so + # the tool is halted holding its grip rather than released. + self._cancel_pipeline( + state, "Simulator mode toggle", "a simulator toggle" + ) + state.gripper_calibrated = False success, error = self._transport_mgr.switch_simulator_mode( command._switch_simulator, sync_state=state ) @@ -976,9 +1048,8 @@ def _handle_system_command( raise RuntimeError(error or "Simulator toggle failed") if command._switch_port is not None: state.invalidate_attachments() + state.gripper_calibrated = False self._transport_mgr.switch_to_port(command._switch_port) - if command._sync_mock: - self._transport_mgr.sync_mock_from_state(state) # Sync motion profile to planner (SelectProfile is a SystemCommand) if isinstance(command, SelectProfileCommand): @@ -1004,6 +1075,44 @@ def _handle_system_command( extract_robot_error(e, ErrorCode.MOTN_SETUP_FAILED, detail=str(e)), ) + def _teleport_refusal( + self, state: ControllerState, command: TeleportCommand + ) -> RobotError | None: + """Why a teleport cannot be applied, or None. It moves the arm, so it + is gated the way arm motion is — never on a stale attachment context + or a disabled controller — and it is the simulator's alone, with + tool positions for the degrees of freedom the fitted tool has.""" + if not is_simulation_mode(): + return make_error(ErrorCode.SYS_NOT_SIMULATOR, detail="teleport") + tool_positions = command.p.tool_positions + if tool_positions is not None: + # As many as status reports for it: a recorded keyframe replays + # the positions status gave, whichever tool was fitted. + reported = ToolStatus() + cfg = get_registry().get(state.current_tool) + if cfg is not None: + cfg.populate_status(state, reported) + dof = len(reported.positions) + if len(tool_positions) != dof: + return make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail=( + f"tool_positions has {len(tool_positions)} entries; the fitted " + f"tool {state.current_tool} reports {dof} positions" + ), + ) + if not state.attachments_valid: + return make_error( + ErrorCode.COMM_VALIDATION_ERROR, + detail=ATTACHMENT_CHANGED, + ) + if not state.enabled: + return make_error( + ErrorCode.SYS_CONTROLLER_DISABLED, + detail=state.disabled_reason or "Controller disabled", + ) + return None + def _assign_command_index(self, state: ControllerState) -> int: """Assign a monotonically increasing command index.""" idx = state.next_command_index diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 9e641ed..8e12f9a 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -4,8 +4,8 @@ the 100Hz control loop to a separate process. Commands flow in via ``command_queue`` and computed segments flow back via ``segment_queue``. -Non-trajectory motion commands (Home, SelectTool, Gripper, Checkpoint, Delay) -are forwarded as ``InlineSegment`` tokens so that the SegmentPlayer can +Non-trajectory motion commands (Home, SelectTool, tool actions, Checkpoint, +Delay) are forwarded as ``InlineSegment`` tokens so that the SegmentPlayer can execute them in the control loop while preserving command ordering. TrajectoryPlanner holds the shared planning logic used by both the real-time @@ -18,13 +18,14 @@ import multiprocessing import queue import signal +import time from dataclasses import dataclass, field from typing import TYPE_CHECKING, Union, cast from math import radians import numpy as np -from parol6.config import INTERVAL_S +from parol6.config import BLEND_HOLD_S, INTERVAL_S from parol6.protocol.wire import ( HomeCmd, MoveJCmd, @@ -33,8 +34,10 @@ SetTcpOffsetCmd, SetTcpTransformCmd, ToolActionCmd, + wire_command_name, ) from parol6.server.command_executor import _format_cmd_params +from parol6.tools import get_registry from parol6.utils.error_catalog import RobotError, extract_robot_error from parol6.utils.error_codes import ErrorCode @@ -46,6 +49,10 @@ logger = logging.getLogger(__name__) +# How soon a held blend chain notices the motion ahead of it has finished, +# which is when its hold starts. +_PLAYBACK_POLL_S = 0.02 + # --------------------------------------------------------------------------- # Segment types (planner → player via segment_queue) # --------------------------------------------------------------------------- @@ -84,6 +91,9 @@ class InlineSegment: command_index: int params: object # wire struct (msgspec.Struct — picklable) generation: int = 0 + # Seconds it holds the arm, as far as the plan can tell: a tool action's + # estimated travel, which the queued duration counts. Zero otherwise. + duration: float = 0.0 @dataclass @@ -245,6 +255,9 @@ def __init__(self, diagnostic: bool = False) -> None: self._max_blend_lookahead = MAX_BLEND_LOOKAHEAD self._robot_module = PAROL6_ROBOT self._blend_buffer: list[tuple[int, TrajectoryMoveCommandBase]] = [] + # The name each in-flight command is reported under: the wire + # command's, so a HOME planned as a joint return still reads "home". + self._names: dict[int, str] = {} self._output: list[Segment] = [] # Pre-compute home position in steps @@ -268,13 +281,16 @@ def process(self, params: object, command_index: int = 0) -> list[Segment]: and bool(self.state.Homed_in[:6].all()) ): params = MoveJCmd(angles=self._home_deg, speed=self._home_return_speed) + # Reported as the home it answers, not the move it plans. + self._names[command_index] = "home" cmd_class = self._registry.get_command_for_struct(type(params)) if cmd_class is not None and issubclass(cmd_class, self._trajectory_base): self._handle_trajectory(command_index, params, cmd_class) # type: ignore[invalid-argument-type] else: - # Tool actions run concurrently with motion — don't flush blend - if not isinstance(params, ToolActionCmd) and self._blend_buffer: + # Every inline command, a tool action included, takes its turn + # with the arm at rest: the chain ahead of it ends there. + if self._blend_buffer: self._flush_blend() self._handle_inline(command_index, params) @@ -291,6 +307,11 @@ def cancel(self) -> None: """Clear blend buffer.""" self._blend_buffer.clear() + @property + def holding(self) -> bool: + """A blend chain is waiting for the move its last corner rounds into.""" + return bool(self._blend_buffer) + def sync_tool( self, tool_name: str, @@ -399,7 +420,7 @@ def _flush_blend(self) -> None: trajectory_steps=head_cmd.trajectory_steps.copy(), trajectory_rad=head_cmd.trajectory_rad.copy(), duration=head_cmd._duration, - command_name=type(head_cmd).__name__, + command_name=self._reported_name(head_idx, head_cmd), action_params=_format_cmd_params(head_cmd.p), blend_consumed_indices=consumed_indices, ) @@ -435,16 +456,21 @@ def _emit_trajectory( trajectory_steps=cmd.trajectory_steps.copy(), trajectory_rad=cmd.trajectory_rad.copy(), duration=cmd._duration, - command_name=type(cmd).__name__, + command_name=self._reported_name(command_index, cmd), action_params=_format_cmd_params(params) if params is not None else "", ) ) self.state.Position_in[:] = cmd.trajectory_steps[-1] + def _reported_name(self, command_index: int, cmd: TrajectoryMoveCommandBase) -> str: + name = self._names.pop(command_index, None) + return name if name is not None else wire_command_name(type(cmd.p)) + def _emit_error( self, command_index: int, cmd: TrajectoryMoveCommandBase, exc: Exception ) -> None: """Append an ErrorSegment to output, with diagnostic data if available.""" + self._names.pop(command_index, None) cartesian_path = None ik_valid = None if self._diagnostic: @@ -513,10 +539,18 @@ def _try_advance_past_error(self, cmd: TrajectoryMoveCommandBase) -> None: def _handle_inline(self, command_index: int, params: object) -> None: """Emit an InlineSegment and predict state changes.""" + duration = 0.0 + if isinstance(params, ToolActionCmd): + cfg = get_registry().get(params.tool_key.strip().upper()) + if cfg is not None: + duration = cfg.estimate_duration( + params.action.strip().lower(), params.params + ) self._output.append( InlineSegment( command_index=command_index, params=params, + duration=duration, ) ) @@ -608,6 +642,10 @@ def cancel(self) -> None: """Clear blend buffer on CancelAll.""" self._planner.cancel() + @property + def holding(self) -> bool: + return self._planner.holding + def apply_tool( self, tool_name: str, @@ -638,9 +676,15 @@ def motion_planner_main( segment_queue: multiprocessing.Queue, shutdown_event: EventType, ready_event: EventType, + playing_event: EventType, avoid_core: int | None = None, ) -> None: - """Worker process main loop — compute trajectories and forward inline commands.""" + """Worker process main loop — compute trajectories and forward inline commands. + + A held blend chain is planned once ``BLEND_HOLD_S`` passes with neither + a command arriving nor anything playing ahead of it (``playing_event``): + a script sending its chain move by move while earlier motion plays is + still sending it.""" signal.signal(signal.SIGINT, signal.SIG_IGN) from parol6.server import set_pdeathsig from parol6.tools import register_plugin_tools @@ -683,13 +727,35 @@ def motion_planner_main( multiprocessing.current_process().pid, ) + # The generation a plan raised in. The controller fails every plan + # submitted before its cancel for that failure, so they are skipped; the + # syncs among them are not, since one of them is that cancel's resync. + failed_generation = -1 + # What a held chain's hold runs from: the last command to arrive, or the + # last moment something played ahead of it. + held_from = time.monotonic() try: while not shutdown_event.is_set(): + if worker.holding: + if playing_event.is_set(): + held_from = time.monotonic() + timeout = min( + _PLAYBACK_POLL_S, + max(0.0, held_from + BLEND_HOLD_S - time.monotonic()), + ) + else: + timeout = BLEND_HOLD_S try: - msg = command_queue.get(timeout=0.1) + msg = command_queue.get(timeout=timeout) except queue.Empty: - worker.flush_stale_blend() + if ( + worker.holding + and not playing_event.is_set() + and time.monotonic() - held_from >= BLEND_HOLD_S + ): + worker.flush_stale_blend() continue + held_from = time.monotonic() if isinstance(msg, CancelAll): worker.cancel() @@ -717,6 +783,8 @@ def motion_planner_main( continue if isinstance(msg, PlanCommand): + if msg.generation <= failed_generation: + continue try: worker.process_command(msg) except Exception as e: @@ -739,7 +807,7 @@ def motion_planner_main( ) ) worker.cancel() - _drain_queue(command_queue) + failed_generation = msg.generation except (EOFError, OSError, BrokenPipeError, KeyboardInterrupt): # Expected when the parent process is shutting down: the queue's @@ -769,6 +837,10 @@ def __init__(self) -> None: self._segment_queue: multiprocessing.Queue = multiprocessing.Queue() self._shutdown_event: EventType = multiprocessing.Event() self._ready_event: EventType = multiprocessing.Event() + # Set while segments play or wait to: a blend chain arriving behind + # them is held until they are done. + self._playing_event: EventType = multiprocessing.Event() + self._playing = False self._process: multiprocessing.Process | None = None # CancelAll travels the command FIFO behind plans already queued, so # the worker still emits them after a cancel; the generation is what @@ -789,6 +861,8 @@ def start(self, avoid_core: int | None = None) -> None: return self._shutdown_event.clear() self._ready_event.clear() + self._playing_event.clear() + self._playing = False self._process = multiprocessing.Process( target=motion_planner_main, args=( @@ -796,6 +870,7 @@ def start(self, avoid_core: int | None = None) -> None: self._segment_queue, self._shutdown_event, self._ready_event, + self._playing_event, avoid_core, ), daemon=True, @@ -878,11 +953,38 @@ def sync_shapes(self, shapes: list) -> None: """Replace the planner checker's workspace keep-out shapes.""" self.submit(SyncShapes(shapes=list(shapes))) + def resync(self, state: ControllerState) -> None: + """Bring the planner back to the controller's tool, world and profile. + + The planner applies SET_TCP_TRANSFORM / SELECT_TOOL / SET_SHAPES at + plan time, when the command is still queued. Dropping that queue + leaves the planner holding a change the controller never applied, + and every later plan would be solved against it. + """ + self.sync_tool( + state.current_tool, + variant_key=state.current_tool_variant, + tcp_offset_m=state.tcp_offset_m, + tcp_rotation_rad=state.tcp_rotation_rad, + ) + self.sync_shapes(state.shapes) + self.sync_profile(state.motion_profile) + def cancel(self) -> None: """Cancel all pending work in the planner.""" self._generation += 1 self.submit(CancelAll()) + def set_playing(self, playing: bool) -> None: + """Tell the planner whether segments play or wait to. Called every + tick; the event is touched only when the answer changes.""" + if playing != self._playing: + self._playing = playing + if playing: + self._playing_event.set() + else: + self._playing_event.clear() + # -- planner → main -- def poll_segment(self) -> Segment | None: diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 0ea94f5..c444620 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -7,7 +7,11 @@ - **TrajectorySegment**: index into pre-computed waypoints at 100Hz (zero-allocation hot path, identical to the old execute_step()). - **InlineSegment**: create the command object from its wire params and - tick it in the control loop until completion (Home, Gripper, etc.). + tick it in the control loop until completion (Home, tool actions, etc.). + The arm holds still while a tool action runs. + +A tool stop is the one tool action that does not wait its turn: it halts +the tool action playing and runs beside the queue until the jaws are still. """ from __future__ import annotations @@ -20,6 +24,7 @@ from parol6.commands._collision_guard import guard_joint_path from parol6.commands.base import CommandBase, ExecutionStatusCode +from parol6.commands.tool_action_command import ToolActionCommand from parol6.config import ( COLLISION_PATH_SAMPLES, EXECUTION_OVERRIDE_TRANSITION_S, @@ -29,7 +34,7 @@ rad_to_steps, steps_to_rad, ) -from parol6.protocol.wire import CommandCode, DelayCmd +from parol6.protocol.wire import CommandCode, DelayCmd, ToolActionCmd, wire_command_name from parol6.server.command_executor import _format_cmd_params from parol6.server.command_registry import create_command_from_struct from parol6.server.motion_planner import ( @@ -39,7 +44,12 @@ Segment, TrajectorySegment, ) -from parol6.utils.error_catalog import RobotError, make_error +from parol6.utils.error_catalog import ( + RobotError, + attributed, + extract_robot_error, + make_error, +) from parol6.utils.error_codes import ErrorCode from parol6.utils.errors import TrajectoryPlanningError from waldoctl import ActionState @@ -73,6 +83,8 @@ class SegmentPlayer: "_settle_ticks", "_settle_err", "_last_shapes_version", + "_tool_stop", + "_tool_stop_index", ) def __init__(self, planner: MotionPlanner) -> None: @@ -91,12 +103,20 @@ def __init__(self, planner: MotionPlanner) -> None: self._settle_ticks: int = 0 self._settle_err: int = -1 self._last_shapes_version: int = 0 + # The tool stop settling the jaws, and its command index. + self._tool_stop: ToolActionCommand | None = None + self._tool_stop_index: int = -1 @property def active(self) -> bool: """True if playing a segment or has buffered segments.""" return self._active is not None or bool(self._buffer) + @property + def tool_stopping(self) -> bool: + """A tool stop is waiting for the jaws to come still.""" + return self._tool_stop is not None + def tick(self, state: ControllerState) -> bool: """Execute one tick. Returns True if actively playing/executing. @@ -115,6 +135,8 @@ def tick(self, state: ControllerState) -> bool: for idx in seg.blend_consumed_indices: if idx > state.plan_received_index: state.plan_received_index = idx + elif isinstance(seg, InlineSegment): + state.queued_duration += seg.duration seg = self._planner.poll_segment() # MoveIt-style invalidation: a world change (SET_SHAPES bumps @@ -139,12 +161,21 @@ def tick(self, state: ControllerState) -> bool: 0.0 if state.execution_paused else state.execution_speed ) return False - if state.execution_paused and not isinstance( - self._buffer[0], ErrorSegment - ): + head = self._buffer[0] + if state.execution_paused and not isinstance(head, ErrorSegment): state.execution_applied_speed = 0.0 state.Speed_out.fill(0) return True + if ( + self._tool_stop is not None + and isinstance(head, InlineSegment) + and isinstance(head.params, ToolActionCmd) + ): + # The next tool action waits for the jaws a tool stop + # is settling; the arm holds with it. + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) + return True self._activate_next(state) if self._active is None: continue # activation-time world guard rejected the segment @@ -289,6 +320,11 @@ def tick(self, state: ControllerState) -> bool: if held: state.Speed_out.fill(0) return True + if isinstance(active.params, ToolActionCmd): + # The arm holds still while the tool acts, as it does + # through a delay. + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) result = self._tick_inline(active, state) if result is None: # Instant completion — try next immediately @@ -300,7 +336,9 @@ def tick(self, state: ControllerState) -> bool: logger.error( "Command %d failed: %s", active.command_index, active.error ) - state.error = active.error + error = attributed(active.error, active.command_index) + state.error = error + state.record_failure(active.command_index, error) pairs = active.colliding_pairs state.collision_active = bool(pairs) state.collision_pairs = tuple(pairs) if pairs else () @@ -309,9 +347,10 @@ def tick(self, state: ControllerState) -> bool: state.action_params = "" self._active = None # Halt: cancel all remaining planned work + self._fail_dropped(state) self._buffer.clear() self._planner.cancel() - self._drain_planner_queue(state) + self._drain_planner_queue(state, failed=True) return False # Unknown segment type @@ -404,8 +443,20 @@ def _tick_inline(self, seg: InlineSegment, state: ControllerState) -> bool | Non cmd = self._inline_cmd if not self._inline_activated: - cmd.setup(state) - state.action_current = type(cmd).__name__ + try: + cmd.setup(state) + except Exception as e: + # Raised out of the tick, the segment would stay unactivated + # and raise again on every tick after. + self._on_failure( + seg, + extract_robot_error( + e, ErrorCode.MOTN_SETUP_FAILED, seg.command_index, detail=str(e) + ), + state, + ) + return False + state.action_current = wire_command_name(type(cmd.p)) state.action_params = _format_cmd_params(seg.params) self._inline_activated = True @@ -431,16 +482,25 @@ 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 != seg.command_index: state.record_completion(idx) + state.record_completion(seg.command_index) + self._retire(seg, state) + + def _retire(self, seg: Segment, state: ControllerState) -> None: + """Take *seg*, whose outcome is recorded, out of play; the segments + queued behind it play on.""" + final_idx = seg.command_index + if isinstance(seg, TrajectorySegment): + for idx in seg.blend_consumed_indices: if idx > final_idx: final_idx = idx state.queued_duration -= seg.duration + elif isinstance(seg, InlineSegment): + state.queued_duration -= seg.duration state.queued_segments -= 1 - state.record_completion(seg.command_index) while state.pending_planned and state.pending_planned[0][0] <= final_idx: state.pending_planned.popleft() state.action_current = "" @@ -453,17 +513,20 @@ def _on_failure( self, seg: Segment, error: RobotError, state: ControllerState ) -> None: """Handle inline command failure: set error state, clear buffer, cancel planner.""" + error = attributed(error, seg.command_index) state.error = error + state.record_failure(seg.command_index, error) state.action_current = "" state.action_params = "" state.action_state = ActionState.ERROR self._active = None + self._fail_dropped(state) self._buffer.clear() self._planner.cancel() - self._drain_planner_queue(state) + self._drain_planner_queue(state, failed=True) def _world_guard( - self, seg: Segment, steps: np.ndarray, state: ControllerState + self, seg: TrajectorySegment, steps: np.ndarray, state: ControllerState ) -> bool: """Validate trajectory waypoints (motor steps) against the current collision world; on violation, halt playback like an ErrorSegment. @@ -493,7 +556,8 @@ def _world_guard( seg.command_index, exc.robot_error, ) - state.error = exc.robot_error + error = attributed(exc.robot_error, seg.command_index) + state.error = error pairs = exc.colliding_pairs state.collision_active = bool(pairs) state.collision_pairs = tuple(pairs) if pairs else () @@ -501,14 +565,109 @@ def _world_guard( state.action_current = "" state.action_params = "" self._active = None + state.record_failure(seg.command_index, error) + # The moves its blend absorbed were the same path, and fail + # with it. + for index in seg.blend_consumed_indices: + state.record_failure(index, error) + self._fail_dropped(state) self._buffer.clear() self._planner.cancel() - self._drain_planner_queue(state) + self._drain_planner_queue(state, failed=True) + return False + return True + + @staticmethod + def _fail_dropped(state: ControllerState) -> None: + """The commands queued behind a failed one are dropped with it: each + fails as cancelled, so a wait on it raises instead of running out its + timeout. One whose failure is already recorded (the failed command, + the moves its blend absorbed) keeps it. Failure path only — it + allocates.""" + state.fail_unfinished( + [index for index, _ in state.pending_planned], + make_error(ErrorCode.MOTN_CANCELLED, scope="the failure ahead of it"), + ) + + def owed_indices(self, state: ControllerState) -> list[int]: + """Every command index this pipeline still owes an outcome: the + active segment and the commands its blend consumed, each one + submitted but not yet started, and a tool stop still settling. Read + BEFORE :meth:`cancel`, which forgets them. Stop path only — it + allocates.""" + owed = [idx for idx, _ in state.pending_planned] + active = self._active + if active is not None: + owed.append(active.command_index) + if isinstance(active, TrajectorySegment): + owed.extend(active.blend_consumed_indices) + if self._tool_stop is not None: + owed.append(self._tool_stop_index) + return owed + + def _halt_tool_action(self, state: ControllerState) -> bool: + """Halt the tool action playing, where the jaws are and keeping its + grip. Whether one was playing.""" + cmd = self._inline_cmd + if ( + self._active is None + or not self._inline_activated + or not isinstance(cmd, ToolActionCommand) + ): return False + cmd.halt(state) return True + def stop_tool( + self, stop: ToolActionCommand, index: int, state: ControllerState + ) -> None: + """Run the tool stop *stop*, set up and numbered *index*, ahead of + the queue: the tool action playing — or a tool stop still settling — + is halted and fails as cancelled by it, and the next queued tool + action waits until the jaws are still. The rest of the queue plays + on.""" + cancelled = make_error(ErrorCode.MOTN_CANCELLED, scope="a tool stop") + active = self._active + if active is not None and self._halt_tool_action(state): + state.record_failure(active.command_index, cancelled) + self._retire(active, state) + if self._tool_stop is not None: + state.record_failure(self._tool_stop_index, cancelled) + self._tool_stop = stop + self._tool_stop_index = index + + def tick_tool_stop(self, state: ControllerState) -> None: + """Tick the tool stop until the jaws are still. It drives only the + gripper, beside whatever the arm is doing.""" + stop = self._tool_stop + if stop is None: + return + code = stop.tick(state) + if code == ExecutionStatusCode.EXECUTING: + return + if code == ExecutionStatusCode.COMPLETED: + state.record_completion(self._tool_stop_index) + else: + error = stop.robot_error or make_error( + ErrorCode.MOTN_TICK_FAILED, detail=type(stop).__name__ + ) + logger.error("Tool stop %d failed: %s", self._tool_stop_index, error) + state.record_failure( + self._tool_stop_index, attributed(error, self._tool_stop_index) + ) + self._tool_stop = None + self._tool_stop_index = -1 + def cancel(self, state: ControllerState) -> None: - """Clear buffer, drain stale segments, and stop playback.""" + """Clear buffer, drain stale segments, and stop playback. A tool + action playing, or a tool stop settling, is halted where the jaws + are: one let run on would close on after it was reported + cancelled.""" + self._halt_tool_action(state) + if self._tool_stop is not None: + self._tool_stop.halt(state) + self._tool_stop = None + self._tool_stop_index = -1 if self._active is not None: # Planned trajectories live here rather than in CommandExecutor. # Cancelling its command cannot clear this player's activity. @@ -521,15 +680,31 @@ def cancel(self, state: ControllerState) -> None: self._phase = -1.0 self._inline_cmd = None self._inline_activated = False + # A seek cut short leaves no step running for the next "home" to + # report as its own. + state.homing_step = 0 self._buffer.clear() self._planner.cancel() # Drain stale segments from planner output queue self._drain_planner_queue(state) - def _drain_planner_queue(self, state: ControllerState) -> None: - """Drain any remaining segments from the planner's output queue.""" + def _drain_planner_queue( + self, state: ControllerState, failed: bool = False + ) -> None: + """Drain any remaining segments from the planner's output queue. + + Every path that drops planned work ends here. The planner applies a + tool selection or a TCP change when it plans it, so when dropped + work — or a *failed* command, which it may have applied in part — + can have left it holding one the controller never applied, it is + brought back to the controller's tool. A tool action accepted from + here on is judged against the fitted tool: no selection is queued. + """ while self._planner.poll_segment() is not None: pass + if failed or state.pending_planned: + self._planner.resync(state) + state.accepted_tool = state.current_tool state.pending_planned.clear() state.queued_segments = 0 state.queued_duration = 0.0 diff --git a/parol6/server/state.py b/parol6/server/state.py index 41bc1f8..4278dd5 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -4,6 +4,7 @@ import logging import secrets from collections import deque +from collections.abc import Iterable from dataclasses import dataclass, field from typing import Any @@ -15,9 +16,16 @@ from parol6.config import CONTROL_RATE_HZ, steps_to_rad from parol6.motion import CartesianStreamingExecutor, StreamingExecutor from parol6.protocol.wire import CommandCode -from parol6.utils.error_catalog import RobotError +from parol6.utils.error_catalog import RobotError, attributed from waldoctl import ActionState +# How many exact outcomes (successes, failures) the completion query retains. +_OUTCOME_RING = 1024 + +ATTACHMENT_CHANGED = ( + "attachment context changed; reconcile the physical scene and reapply" +) + class GripperHWState: """Named wrapper over the raw gripper numpy arrays. @@ -181,6 +189,9 @@ class ControllerState: # Tool configuration (affects kinematics and visualization) _current_tool: str = "NONE" + # The tool the newest accepted select_tool names: the fitted tool once + # the queue reaches it, and what a tool action sent behind it acts on. + accepted_tool: str = "NONE" _current_tool_variant: str = "" _tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) _tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0) @@ -247,7 +258,7 @@ class ControllerState: action_params: str = "" # HomeState value of the live HomeCommand (1 signalling, 2 waiting for the # firmware to clear the homed bits, 3 waiting for every joint); meaningful - # only while action_current is "HomeCommand". + # only while action_current is "home". homing_step: int = 0 # Status broadcast rate for this session. Mutable so SET_STATUS_RATE can # raise it for a capture or a tuning run and drop it back without a @@ -265,8 +276,20 @@ class ControllerState: 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) + _recent_completions: list[int] = field(default_factory=lambda: [-1] * _OUTCOME_RING) _completion_cursor: int = 0 + # Where each index sits in its ring: a stop owes an outcome for every + # queued command, and a ring scan per index would hold the stop's tick. + _completion_slots: dict[int, int] = field(default_factory=dict) + # Commands that ended as failures (a stop discarded them), with why — + # preallocated rings beside the success ring, so the completion query + # can answer "failed" instead of leaving a wait to run out its timeout. + _recent_failures: list[int] = field(default_factory=lambda: [-1] * _OUTCOME_RING) + _failure_errors: list[RobotError | None] = field( + default_factory=lambda: [None] * _OUTCOME_RING + ) + _failure_cursor: int = 0 + _failure_slots: dict[int, int] = field(default_factory=dict) last_checkpoint: str = "" # Planning behavior (stop on first IK failure vs solve all for diagnostic) @@ -347,6 +370,13 @@ class ControllerState: # Named wrapper over raw gripper arrays (initialized in __post_init__) gripper_hw: GripperHWState = field(init=False, repr=False) + # Set when a calibrate action completes; cleared when the transport + # (re)connects, since the gripper may have lost power. Tracked here, one + # flag for whichever gripper is fitted: the SSG-48 firmware packs a + # calibrated bit into its status byte, but that bit is unverified on the + # other supported grippers, so it is not read. reset() leaves it alone: + # resetting software state does not uncalibrate the gripper. + gripper_calibrated: bool = False def __post_init__(self) -> None: """Initialize E-stop to released state and named gripper wrapper.""" @@ -361,55 +391,103 @@ def clear_collision(self) -> None: 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 - ) + cursor = self._completion_cursor + evicted = self._recent_completions[cursor] + if self._completion_slots.get(evicted) == cursor: + del self._completion_slots[evicted] + self._recent_completions[cursor] = index + self._completion_slots[index] = cursor + self._completion_cursor = (cursor + 1) % _OUTCOME_RING def command_completed(self, index: int) -> bool: - return index >= 0 and index in self._recent_completions + return index >= 0 and index in self._completion_slots + + def record_failure(self, index: int, error: RobotError) -> None: + """Retain a command that ended without completing, and why. It is + past the completion watermark all the same: nothing more will run + for it.""" + self.fail_unfinished((index,), error) + + def fail_unfinished(self, indices: Iterable[int], error: RobotError) -> None: + """Record *error* as the failure of each of *indices* that has no + outcome yet: a command ends once, so its first outcome is the one + kept. *error* need not be attributed to them (see + :meth:`command_failure`). A stop fails its whole queue in the tick + that stops the arm, hence one pass over locals.""" + done = self._completion_slots + slots = self._failure_slots + ring = self._recent_failures + errors = self._failure_errors + cursor = self._failure_cursor + top = self.completed_command_index + for index in indices: + if index < 0 or index in done or index in slots: + continue + if index > top: + top = index + evicted = ring[cursor] + if slots.get(evicted) == cursor: + del slots[evicted] + ring[cursor] = index + errors[cursor] = error + slots[index] = cursor + cursor = (cursor + 1) % _OUTCOME_RING + self._failure_cursor = cursor + self.completed_command_index = top + + def command_failure(self, index: int) -> RobotError | None: + """Why *index* failed, attributed to it, or None. A stop records one + error for every command it drops, attributed here, on the read.""" + if index < 0: + return None + cursor = self._failure_slots.get(index) + if cursor is None: + return None + error = self._failure_errors[cursor] + return None if error is None else attributed(error, index) def reset(self) -> None: """ - Reset robot state to initial values without losing connection state. - - Preserves: ser, ip, port, start_time, next_command_index - Resets: positions, speeds, I/O, queues, tool, errors, etc. + Reset the program-level state a script builds up, as ``reset_state`` + promises: world shapes, tool selection, errors, pause, motion profile + and execution speed. + + Preserves what the contract says it does not touch — the protective + stop latch (``enabled`` / ``disabled_reason``; only ``reset()`` clears + it), homed state, the digital outputs and the gripper's output frame — + and everything the firmware reports (positions, I/O inputs). + Also preserves ``next_command_index`` and the completion history, so a + wait on a command from before the reset — including one this reset + cancelled — still resolves. """ self.invalidate_attachments() - # Safety and control flags - self.enabled = True + # Program flags (the protective-stop latch is deliberately left alone) self.execution_paused = False + self.execution_speed = 1.0 self.soft_error = False - self.disabled_reason = "" - self.e_stop_active = False self.motion_profile = "TOPPRA" # Tool back to none self._current_tool = "NONE" + self.accepted_tool = "NONE" self._current_tool_variant = "" self._tcp_offset_m = (0.0, 0.0, 0.0) self._tcp_rotation_rad = (0.0, 0.0, 0.0) PAROL6_ROBOT.apply_tool("NONE") - # Command and telemetry buffers - zero out + # Program-layer world shapes; the installation layer is config and + # stays. + PAROL6_ROBOT.apply_shapes([]) + self.shapes = [] + self.has_attachments = False + self.attachments_valid = True + self.attachment_motion_stopped = False + self.shapes_version += 1 + + # Stop commanding motion; the arm holds where it is. self.Command_out = CommandCode.IDLE - self.Position_out.fill(0) self.Speed_out.fill(0) - self.Gripper_data_out.fill(0) - self.Position_in.fill(0) - self.Speed_in.fill(0) - self.Timing_data_in.fill(0) - self.Gripper_data_in.fill(0) self.Affected_joint_out.fill(0) - self.InOut_out.fill(0) - self.InOut_in.fill(0) - self.InOut_in[4] = 1 # E-STOP released (0=pressed, 1=released) - self.Homed_in.fill(0) - self.Temperature_error_in.fill(0) - self.Position_error_in.fill(0) - self.Timeout_out = 0 - self.XTR_data = 0 # Action tracking self.action_current = "" @@ -420,15 +498,11 @@ def reset(self) -> None: self.queue_nonstreamable.clear() self.pending_planned.clear() - # Queue progress tracking. next_command_index is deliberately NOT - # reset: indices must stay monotonic across reset so a stale - # pre-reset status frame (its completed_index is a high-water mark) - # can never satisfy a wait on a post-reset command. + # Queue progress tracking. next_command_index, completed_command_index + # and the completion rings are deliberately NOT reset: indices stay + # monotonic, and the outcome of every command issued so far stays + # readable. 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 @@ -488,9 +562,7 @@ def set_shapes(self, shapes: list) -> None: """ 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" - ) + raise ValueError(ATTACHMENT_CHANGED) if (attached or self.has_attachments) and ( self.action_state == ActionState.EXECUTING or self.queued_segments diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index a778884..96418b2 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -17,7 +17,7 @@ from pinokin import arrays_equal_6 from waldoctl import ActionState, ToolState, ToolStatus -from parol6.config import speed_steps_to_rad, steps_to_deg, steps_to_rad +from parol6.config import INTERVAL_S, speed_steps_to_deg, steps_to_deg, steps_to_rad from parol6.protocol.wire import pack_status from parol6.tools import get_registry from parol6.utils.error_catalog import RobotError @@ -37,7 +37,6 @@ ) from parol6.server.state import ControllerState, get_fkine_flat_mm, get_fkine_se3 from parol6.tools import compose_tcp_transform, get_tool_transform -from parol6 import config as _cfg # Drive-fault labels indexed by (overtemperature | following-error << 1). # Built once: the bits are read every control tick, and the repo's hot path @@ -51,6 +50,15 @@ logger = logging.getLogger(__name__) +# TCP samples the speed is differentiated across: at the control rate a +# window of 50 ms, which holds the loop's millisecond of jitter to a few +# percent of the estimate. +_TCP_SPEED_WINDOW: int = 6 +# A TCP whose newest moving sample is this old is at rest: a window of +# control ticks, so a slow move that changes the step count only every few +# frames still reads as moving, whatever the status rate. +_TCP_STILL_S: float = _TCP_SPEED_WINDOW * INTERVAL_S + def _cleanup_shm(shm: SharedMemory | None) -> None: """Safely close and unlink a shared memory segment.""" @@ -140,14 +148,14 @@ class StatusCache: - io: 5 ints [in1,in2,out1,out2,estop] - tool_status: pre-allocated ToolStatus (mutated in-place) - pose: 16 floats (flattened transform) - - last_serial_s: wall clock time of last cache update + - last_serial_s: perf_counter time of the last fresh serial frame """ def __init__(self) -> None: # Public snapshots (materialized only when they change) self.angles_deg: np.ndarray = np.zeros((6,), dtype=np.float64) self.speeds: np.ndarray = np.zeros((6,), dtype=np.int32) - self.speeds_rad_s: np.ndarray = np.zeros((6,), dtype=np.float64) + self.speeds_deg_s: np.ndarray = np.zeros((6,), dtype=np.float64) self.io: np.ndarray = np.zeros((5,), dtype=np.uint8) self.pose: np.ndarray = np.zeros((16,), dtype=np.float64) self.tcp_speed: float = 0.0 # TCP linear velocity in mm/s @@ -155,7 +163,10 @@ def __init__(self) -> None: # Pre-allocated ToolStatus — mutated in-place by populate_status() self.tool_status: ToolStatus = ToolStatus() - self.last_serial_s: float = 0.0 # last time a fresh serial frame was observed + # When the last fresh serial frame was observed, on perf_counter: + # monotonic ticks every 15.6 ms on Windows before Python 3.13, too + # coarse to differentiate the TCP speed over. + self.last_serial_s: float = 0.0 self._last_tool_name: str = "NONE" # Track tool changes self._last_tool_variant: str = "" # Track variant changes self._last_tcp_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) @@ -201,18 +212,18 @@ def __init__(self) -> None: # Dirty-check: last q_rad submitted to the IK worker self._ik_last_q_rad: np.ndarray = np.full(6, np.nan, dtype=np.float64) - # TCP speed computation state - self._prev_tcp_pos: np.ndarray = np.zeros(3, dtype=np.float64) - self._tcp_pos_buf: np.ndarray = np.zeros(3, dtype=np.float64) - self._tcp_pos_initialized: bool = False - - # Broadcast period the last TCP sample was taken at. The rate is a - # session knob, and the gap being differentiated was governed by the - # period in force when the earlier sample was taken, not by the one - # that has just replaced it. - self._tcp_sample_period_s: float = ( - _cfg.INTERVAL_S * _cfg.status_broadcast_interval(_cfg.STATUS_RATE_HZ) + # TCP speed computation state: a ring of the last TCP samples, each + # stamped with the time its serial frame was observed. The cache + # refreshes on every status broadcast and on every query that reads + # it, so the gap being differentiated is measured, not assumed from + # the broadcast period; differentiating across the ring rather than + # to the sample before keeps the loop's jitter out of the estimate. + self._tcp_hist_pos: np.ndarray = np.zeros( + (_TCP_SPEED_WINDOW, 3), dtype=np.float64 ) + self._tcp_hist_t: np.ndarray = np.zeros(_TCP_SPEED_WINDOW, dtype=np.float64) + self._tcp_hist_n: int = 0 + self._tcp_hist_i: int = 0 # Per-joint drive faults, one bit per condition. One entry per joint # always — an all-clear list of empty tuples is how a consumer tells @@ -425,18 +436,15 @@ def update_from_state(self, state: ControllerState) -> None: or state.tcp_rotation_rad != self._last_tcp_rotation ) - # Convert speeds from steps/s to rad/s when they change if spd_changed: - speed_steps_to_rad(self.speeds, self.speeds_rad_s) + speed_steps_to_deg(self.speeds, self.speeds_deg_s) if tool_changed: self._last_tool_name = state.current_tool self._last_tool_variant = state.current_tool_variant self._last_tcp_offset = state.tcp_offset_m self._last_tcp_rotation = state.tcp_rotation_rad - self._tcp_pos_initialized = ( - False # avoid speed spike from TCP offset change - ) + self._tcp_hist_n = 0 # avoid speed spike from TCP offset change # Sync tool transform to IK worker T_tool = get_tool_transform( state.current_tool, variant_key=state.current_tool_variant @@ -460,26 +468,33 @@ def update_from_state(self, state: ControllerState) -> None: if pos_changed or tool_changed: self.pose[:] = get_fkine_flat_mm(state) - # Compute TCP speed from consecutive FK positions (mm/s) - # pose is row-major 4x4: translation at indices 3,7,11 - self._tcp_pos_buf[0] = self.pose[3] - self._tcp_pos_buf[1] = self.pose[7] - self._tcp_pos_buf[2] = self.pose[11] - if self._tcp_pos_initialized: - dt = self._tcp_sample_period_s - dx = self._tcp_pos_buf[0] - self._prev_tcp_pos[0] - dy = self._tcp_pos_buf[1] - self._prev_tcp_pos[1] - dz = self._tcp_pos_buf[2] - self._prev_tcp_pos[2] - self.tcp_speed = (dx * dx + dy * dy + dz * dz) ** 0.5 / dt - else: - self._tcp_pos_initialized = True - self._prev_tcp_pos[:] = self._tcp_pos_buf - self._tcp_sample_period_s = ( - _cfg.INTERVAL_S * _cfg.status_broadcast_interval(state.status_rate_hz) - ) + # TCP speed (mm/s) across the sample ring; pose is row-major + # 4x4: translation at indices 3,7,11 + i = self._tcp_hist_i + self._tcp_hist_pos[i, 0] = self.pose[3] + self._tcp_hist_pos[i, 1] = self.pose[7] + self._tcp_hist_pos[i, 2] = self.pose[11] + self._tcp_hist_t[i] = self.last_serial_s + self._tcp_hist_i = (i + 1) % _TCP_SPEED_WINDOW + self._tcp_hist_n = min(self._tcp_hist_n + 1, _TCP_SPEED_WINDOW) + if self._tcp_hist_n >= 2: + oldest = (self._tcp_hist_i - self._tcp_hist_n) % _TCP_SPEED_WINDOW + dt = self.last_serial_s - self._tcp_hist_t[oldest] + if dt > 0.0: + dx = self.pose[3] - self._tcp_hist_pos[oldest, 0] + dy = self.pose[7] - self._tcp_hist_pos[oldest, 1] + dz = self.pose[11] - self._tcp_hist_pos[oldest, 2] + self.tcp_speed = (dx * dx + dy * dy + dz * dz) ** 0.5 / dt else: - # Robot not moving — reset TCP speed to zero - self.tcp_speed = 0.0 + # No movement seen for a window of ticks is a speed of zero: + # the arm has come to rest, or its frames have stopped and the + # speed they gave is no longer known. The ring is dropped so a + # restart is not differentiated against the hold. + if self._tcp_hist_n: + newest = (self._tcp_hist_i - 1) % _TCP_SPEED_WINDOW + if time.perf_counter() - self._tcp_hist_t[newest] >= _TCP_STILL_S: + self.tcp_speed = 0.0 + self._tcp_hist_n = 0 # Submit IK request asynchronously try: @@ -566,9 +581,7 @@ def update_from_state(self, state: ControllerState) -> None: # Only a live HomeCommand owns homing_step; any cancel path that drops # the command clears action_current, so derive "idle" from that. - self._homing_step = ( - state.homing_step if state.action_current == "HomeCommand" else 0 - ) + self._homing_step = state.homing_step if state.action_current == "home" else 0 collision_changed = ( self._collision_active != state.collision_active @@ -606,7 +619,7 @@ def to_binary( return pack_status( self.pose, self.angles_deg, - self.speeds_rad_s, + self.speeds_deg_s, self.io, self._action_current, self._action_state, @@ -641,13 +654,13 @@ def to_binary( def mark_serial_observed(self) -> None: """Mark that a fresh serial frame was observed just now.""" - self.last_serial_s = time.monotonic() + self.last_serial_s = time.perf_counter() def age_s(self) -> float: """Seconds since last fresh serial observation (used to gate broadcasting).""" if self.last_serial_s <= 0: return 1e9 - return time.monotonic() - self.last_serial_s + return time.perf_counter() - self.last_serial_s @property def joint_en(self) -> np.ndarray: diff --git a/parol6/server/transport_manager.py b/parol6/server/transport_manager.py index 496f74e..343cf15 100644 --- a/parol6/server/transport_manager.py +++ b/parol6/server/transport_manager.py @@ -271,18 +271,6 @@ def disconnect(self) -> None: logger.debug("Error disconnecting transport: %s", e) self.transport = None - def sync_mock_from_state(self, state: Any) -> None: - """Sync mock transport from controller state after RESET. - - Args: - state: ControllerState to sync from. - """ - if isinstance(self.transport, MockSerialTransport): - self.transport.sync_from_controller_state(state) - # Skip stale frames - _, ver, _ = self.transport.get_latest_frame_view() - self._last_version = ver - def tick_simulation( self, tool_name: str = "NONE", diff --git a/parol6/server/transports/mock_serial_transport.py b/parol6/server/transports/mock_serial_transport.py index 07f24be..92628df 100644 --- a/parol6/server/transports/mock_serial_transport.py +++ b/parol6/server/transports/mock_serial_transport.py @@ -24,6 +24,7 @@ _pack_positions, ) from parol6.server.state import ControllerState +from parol6.tools import PneumaticToolSimulator, get_registry if TYPE_CHECKING: from parol6.tools import ToolSimulator @@ -151,11 +152,13 @@ def _simulate_motion_jit( speed_in.fill(0) elif command_out == CommandCode.TELEPORT: - # Instant position set — no ramping + # Instant position set — no ramping; the pose is exact, so the arm + # is referenced there. for i in range(6): position_in[i] = position_out[i] position_f[i] = float(position_out[i]) speed_in[i] = 0 + homed_in.fill(1) command_out = CommandCode.IDLE else: @@ -547,16 +550,25 @@ def tick_simulation( dt = cfg.INTERVAL_S self._state.last_update = now - # Snap gripper position if teleport requested - if tool_teleport_pos >= 0: - self._state.gripper_pos_f = tool_teleport_pos - self._state.gripper_data_in[1] = int(tool_teleport_pos + 0.5) - self._state.gripper_ramp[_RAMP_TARGET] = tool_teleport_pos + # Resolved before a teleport snaps the tool: the snap is in the + # fitted tool's convention. + if tool_name != self._simulator_tool_name: + self._simulator_tool_name = tool_name + # Clear stale ramp state from the previous tool self._state.gripper_ramp[_RAMP_ACTIVE] = _RAMP_OFF - # Also snap the pneumatic/generic ramp so it doesn't overwrite - frac = tool_teleport_pos / 255.0 - self._state.tool_ramp_current = frac - self._state.tool_ramp_target = frac + self._state.tool_ramp_current = 0.0 + self._state.tool_ramp_target = 0.0 + + tool_cfg = get_registry().get(tool_name) + if tool_cfg is not None: + self._simulator = tool_cfg.create_simulator() + if self._simulator is not None: + self._simulator.resolve_params(tool_cfg) + else: + self._simulator = None + + if tool_teleport_pos >= 0: + self._teleport_tool(tool_teleport_pos / 255.0) if dt > 0: state = self._state @@ -580,24 +592,6 @@ def tick_simulation( state.homing_countdown, ) - # Tool simulation: resolve params on change, then tick - if tool_name != self._simulator_tool_name: - self._simulator_tool_name = tool_name - # Clear stale ramp state from the previous tool - state.gripper_ramp[_RAMP_ACTIVE] = _RAMP_OFF - state.tool_ramp_current = 0.0 - state.tool_ramp_target = 0.0 - - from parol6.tools import get_registry - - tool_cfg = get_registry().get(tool_name) - if tool_cfg is not None: - self._simulator = tool_cfg.create_simulator() - if self._simulator is not None: - self._simulator.resolve_params(tool_cfg) - else: - self._simulator = None - if self._simulator is not None: self._simulator.tick(state, dt) @@ -610,6 +604,22 @@ def tick_simulation( self._frame_version += 1 self._frame_ts = time.time() + def _teleport_tool(self, position: float) -> None: + """Snap the fitted tool to *position*, read as status reads it.""" + st = self._state + if isinstance(self._simulator, PneumaticToolSimulator): + # Its ramp runs to 1.0 with the valve open, which status reads + # as 0.0: open. + position = 1.0 - position + byte = position * 255.0 + st.gripper_pos_f = byte + st.gripper_data_in[1] = int(byte + 0.5) + st.gripper_ramp[_RAMP_TARGET] = byte + st.gripper_ramp[_RAMP_ACTIVE] = _RAMP_OFF + # The binary-tool ramp snaps too, or it writes its old position back. + st.tool_ramp_current = position + st.tool_ramp_target = position + # ================================ # Latest-frame API (reduced-copy) # ================================ diff --git a/parol6/tools.py b/parol6/tools.py index 2cc4e8f..f8c056e 100644 --- a/parol6/tools.py +++ b/parol6/tools.py @@ -63,6 +63,21 @@ def tick(self, state: MockRobotState, dt: float) -> None: ... +def _require_numbers(action: str, params: list, names: tuple[str, ...]) -> None: + if len(params) != len(names): + raise ValueError(f"{action} takes [{', '.join(names)}], got {params!r}") + for name, v in zip(names, params): + if isinstance(v, bool) or not isinstance(v, (int, float)): + raise ValueError(f"{name} must be a number, got {v!r}") + if not math.isfinite(v): + raise ValueError(f"{name} must be finite, got {v!r}") + + +def _require_within(name: str, v: float, lo: float, hi: float) -> None: + if not lo <= v <= hi: + raise ValueError(f"{name} = {v} is outside [{lo}, {hi}]") + + # --------------------------------------------------------------------------- # Base config # --------------------------------------------------------------------------- @@ -87,8 +102,14 @@ class ToolConfig: def populate_status(self, hw: ControllerState, out: ToolStatus) -> None: """Fill *out* from hardware state. Override in subclasses.""" + def validate_action(self, action: str, params: list) -> None: + """Raise ``ValueError`` unless *action* with *params* is one this tool + can run. Checked when the command is decoded, so a bad action is + refused before it is acknowledged.""" + raise ValueError(f"Tool '{self.name}' takes no actions") + def create_command(self, action: str, params: list) -> MotionCommand | None: - """Create a command engine for this tool action. Returns None if not supported.""" + """Create a command engine for a validated tool action. Returns None if not supported.""" return None def create_simulator(self) -> ToolSimulator | None: @@ -130,16 +151,26 @@ def populate_status(self, hw: ControllerState, out: ToolStatus) -> None: out.engaged = bool(hw.InOut_out[port_idx]) out.state = ToolState.IDLE + def validate_action(self, action: str, params: list) -> None: + if action not in self.valid_actions: + raise ValueError( + f"Pneumatic gripper has no action '{action}' " + f"({' | '.join(self.valid_actions)})" + ) + if action in ("move", "set_position"): + _require_numbers(action, params, ("position",)) + _require_within("position", params[0], 0.0, 1.0) + elif params: + raise ValueError(f"{action} takes no parameters") + def create_command(self, action: str, params: list) -> PneumaticGripperCommand: from parol6.commands.gripper_commands import PneumaticGripperCommand - if action not in self.valid_actions: - raise ValueError(f"Invalid action '{action}' for pneumatic gripper") + dwell_s = self.estimate_duration(action, params) if action in ("move", "set_position"): - position = float(params[0]) if params and len(params) > 0 else 0.0 - action = "open" if position < 0.5 else "close" + action = "open" if float(params[0]) < 0.5 else "close" return PneumaticGripperCommand.from_tool_action( - action=action, port=self.io_port + action=action, port=self.io_port, dwell_s=dwell_s ) def estimate_duration(self, action: str, params: list) -> float: @@ -159,13 +190,7 @@ class ElectricGripperConfig(ToolConfig): current_range: tuple[int, int] = (0, 0) position_range: tuple[float, float] = (0.0, 1.0) speed_range: tuple[float, float] = (0.0, 1.0) - valid_actions: tuple[str, ...] = ( - "move", - "open", - "close", - "set_position", - "calibrate", - ) + valid_actions: tuple[str, ...] = ("move", "calibrate", "stop", "idle") # Motor controller / mechanical properties encoder_cpr: int = 16_384 # encoder counts per revolution @@ -184,37 +209,44 @@ def populate_status(self, hw: ControllerState, out: ToolStatus) -> None: out.engaged = bool(hw.Gripper_data_in[2]) # speed > 0 out.state = ToolState.IDLE + def validate_action(self, action: str, params: list) -> None: + if action not in self.valid_actions: + raise ValueError( + f"Electric gripper has no action '{action}' " + f"({' | '.join(self.valid_actions)})" + ) + if action != "move": + if params: + raise ValueError(f"{action} takes no parameters") + return + _require_numbers(action, params, ("position", "speed", "current")) + _require_within("position", params[0], *self.position_range) + _require_within("speed", params[1], *self.speed_range) + _require_within("current", params[2], 0.0, 1.0) + def create_command(self, action: str, params: list) -> ElectricGripperCommand: from parol6.commands.gripper_commands import ElectricGripperCommand - if action not in self.valid_actions: - raise ValueError(f"Invalid action '{action}' for electric gripper") - # Translate Python-level method names to wire-level "move" action - if action == "open": - params = [0.0] + params[1:] - action = "move" - elif action == "close": - params = [1.0] + params[1:] - action = "move" - elif action == "set_position": - action = "move" - position = float(params[0]) if len(params) > 0 else 0.0 - speed = float(params[1]) if len(params) > 1 else 0.5 - current = int(params[2]) if len(params) > 2 else 500 + if action != "move": + return ElectricGripperCommand.from_tool_action(action=action) + lo, hi = self.current_range return ElectricGripperCommand.from_tool_action( - action=action, position=position, speed=speed, current=current + action=action, + position=float(params[0]), + speed=float(params[1]), + current=round(lo + float(params[2]) * (hi - lo)), ) def estimate_duration(self, action: str, params: list) -> float: - # Resolve position delta from action + params (same logic as create_command) - if action in ("open", "close"): - target = 0.0 if action == "open" else 1.0 - speed = float(params[0]) if len(params) > 0 else 0.5 - elif action in ("move", "set_position"): - target = float(params[0]) if len(params) > 0 else 0.0 - speed = float(params[1]) if len(params) > 1 else 0.5 - else: + if action == "calibrate": + from parol6.commands.gripper_commands import CALIBRATE_TICKS + from parol6.config import INTERVAL_S + + return CALIBRATE_TICKS * INTERVAL_S + if action != "move": return 0.0 + target = float(params[0]) + speed = float(params[1]) # Assume worst-case full travel (0→target or 1→target) pos_delta = max(target, 1.0 - target) @@ -359,6 +391,35 @@ def tick(self, state: MockRobotState, dt: float) -> None: ) +def unselected_tool_refusal(tool_key: str, current_tool: str) -> str | None: + """Why a tool action naming *tool_key* is refused on acceptance, or + None: only the selected tool takes actions.""" + key = tool_key.strip().upper() + if key != current_tool: + return f"tool '{key}' is not the selected tool ('{current_tool}')" + return None + + +def tool_action_refusal( + tool_key: str, action: str, *, current_tool: str, gripper_calibrated: bool +) -> str | None: + """Why a decoded tool action cannot run on the arm as it stands, or + None. The action and its parameters were validated on decode; these + checks need the controller's state at the moment the action's turn + comes in the queue, and the dry run applies them to its own so a + script previews the refusal it would get live.""" + refusal = unselected_tool_refusal(tool_key, current_tool) + if refusal is not None: + return refusal + if ( + isinstance(get_registry().get(tool_key.strip().upper()), ElectricGripperConfig) + and action.strip().lower() == "move" + and not gripper_calibrated + ): + return "the gripper is not calibrated: run the calibrate action first" + return None + + # --------------------------------------------------------------------------- # Registry # --------------------------------------------------------------------------- diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 4a75378..1980eb4 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -95,6 +95,12 @@ class _ErrorTemplate: effect="Motion command rejected before dispatch.", remedy="Run home() first. Jogging remains available.", ), + ErrorCode.MOTN_CANCELLED: _ErrorTemplate( + title="Command cancelled", + cause="The command was cancelled by {scope} before it finished.", + effect="The motion did not run to completion.", + remedy="Re-issue the command if the motion is still wanted.", + ), # -- Communication -- ErrorCode.COMM_QUEUE_FULL: _ErrorTemplate( title="Command queue full", @@ -152,6 +158,12 @@ class _ErrorTemplate: effect="Profile not changed.", remedy="Use one of: TOPPRA, RUCKIG, QUINTIC, TRAPEZOID, LINEAR.", ), + ErrorCode.SYS_NOT_SIMULATOR: _ErrorTemplate( + title="Simulator-only command", + cause="{detail} is only available on the simulator.", + effect="Command rejected; the arm is unchanged.", + remedy="Switch to the simulator with simulator(True), or drive the arm with a planned move.", + ), ErrorCode.SYS_SELF_COLLISION: _ErrorTemplate( title="Self-collision predicted", cause="Planned configuration would self-collide at sample {sample} of {total}: {pairs}", @@ -182,6 +194,17 @@ def make_error( ) +def attributed(error: RobotError, command_index: int) -> RobotError: + """*error* as the failure of *command_index*. Rebuilt from the wire, not + ``replace``d: a RobotError is an exception, and a dataclass replace does + not survive the copy a state snapshot makes of it.""" + if error.command_index == command_index: + return error + wire = error.to_wire() + wire[0] = command_index + return RobotError.from_wire(wire) + + def extract_robot_error( exc: Exception, fallback_code: ErrorCode, command_index: int = -1, **params: object ) -> RobotError: diff --git a/parol6/utils/error_codes.py b/parol6/utils/error_codes.py index c75a241..d2ddb8c 100644 --- a/parol6/utils/error_codes.py +++ b/parol6/utils/error_codes.py @@ -10,6 +10,8 @@ from enum import IntEnum +from waldoctl.errors import MOTN_CANCELLED as _WALDOCTL_MOTN_CANCELLED + class ErrorCode(IntEnum): # IK subsystem @@ -27,6 +29,9 @@ class ErrorCode(IntEnum): MOTN_SETUP_FAILED = 33 MOTN_TICK_FAILED = 34 MOTN_NOT_HOMED = 35 + # waldoctl's shared number, so a client reading either backend sees one + # code for a command a stop discarded. + MOTN_CANCELLED = _WALDOCTL_MOTN_CANCELLED # Communication / protocol COMM_QUEUE_FULL = 40 @@ -41,3 +46,4 @@ class ErrorCode(IntEnum): SYS_PROFILE_INVALID = 53 SYS_SELF_COLLISION = 54 SYS_STATUS_RATE_INVALID = 55 + SYS_NOT_SIMULATOR = 56 diff --git a/parol6/utils/errors.py b/parol6/utils/errors.py index a47d793..a16872b 100644 --- a/parol6/utils/errors.py +++ b/parol6/utils/errors.py @@ -5,10 +5,7 @@ from __future__ import annotations -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from .error_catalog import RobotError +from waldoctl.errors import RobotError class IKError(RuntimeError): @@ -29,13 +26,21 @@ def __init__(self, robot_error: RobotError): super().__init__(str(robot_error)) -class MotionError(RuntimeError): - """Pipeline planning/execution error detected via status broadcast.""" +class MotionError(RobotError): + """Pipeline planning/execution error detected via status broadcast or a + completion: the runtime's :class:`RobotError`, raised as this client's + own type so ``except RobotError`` reads it on every backend.""" def __init__(self, robot_error: RobotError): self.robot_error = robot_error - super().__init__(str(robot_error)) - - @property - def command_index(self) -> int: - return self.robot_error.command_index + super().__init__( + robot_error.command_index, + robot_error.code, + robot_error.title, + robot_error.cause, + robot_error.effect, + robot_error.remedy, + ) + + def __reduce__(self) -> tuple: + return (type(self), (self.robot_error,)) diff --git a/parol6/utils/ik.py b/parol6/utils/ik.py index 8fdb90f..4fcd52b 100644 --- a/parol6/utils/ik.py +++ b/parol6/utils/ik.py @@ -13,8 +13,8 @@ from numpy.typing import ArrayLike, NDArray from pinokin import Damping as _Damping, IKSolver as _IKSolver, Robot -import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.config import IK_SAFETY_MARGINS_RAD +from parol6.utils.joint_limits import TRAVEL_MAX_RAD, TRAVEL_MIN_RAD logger = logging.getLogger(__name__) @@ -227,8 +227,9 @@ def solve_ik( _cl_t_below = np.zeros(6, dtype=np.bool_) _cl_t_above = np.zeros(6, dtype=np.bool_) _cl_dummy_target = np.zeros(6, dtype=np.float64) -_cl_mn = np.ascontiguousarray(PAROL6_ROBOT._joint_limits_radian[:, 0]) -_cl_mx = np.ascontiguousarray(PAROL6_ROBOT._joint_limits_radian[:, 1]) +# A joint parked on its limit reads back up to half a motor step past it. +_cl_mn = np.ascontiguousarray(TRAVEL_MIN_RAD) +_cl_mx = np.ascontiguousarray(TRAVEL_MAX_RAD) _last_violation_mask = np.zeros(6, dtype=np.bool_) _last_any_violation = False diff --git a/parol6/utils/joint_limits.py b/parol6/utils/joint_limits.py new file mode 100644 index 0000000..2419e0c --- /dev/null +++ b/parol6/utils/joint_limits.py @@ -0,0 +1,48 @@ +"""Joint travel as the arm reports it. + +A joint parked on its limit reads back a hair past the limit's decimal: +the motor step it stands on rounds to the nearest step, outward as often +as not, and a float carrying the angle rounds again. Every check of a +joint against its travel allows half a motor step either side, so what +the arm reports can be commanded again; anything further out is refused. +""" + +import numpy as np +from numpy.typing import NDArray + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.config import LIMITS + +#: Half a motor step of each joint [rad]. +HALF_STEP_RAD: NDArray[np.float64] = ( + 0.5 * PAROL6_ROBOT.radian_per_step_constant / PAROL6_ROBOT.joint.ratio +) + +#: Each joint's travel widened by half a motor step [rad]. +TRAVEL_MIN_RAD: NDArray[np.float64] = LIMITS.joint.position.rad[:, 0] - HALF_STEP_RAD +TRAVEL_MAX_RAD: NDArray[np.float64] = LIMITS.joint.position.rad[:, 1] + HALF_STEP_RAD + +# Plain floats: the degree check runs on every streamed servo target, where +# indexing a numpy table would box a scalar per comparison. +_MIN_RAD: tuple[float, ...] = tuple(float(v) for v in TRAVEL_MIN_RAD) +_MAX_RAD: tuple[float, ...] = tuple(float(v) for v in TRAVEL_MAX_RAD) +_MIN_DEG: tuple[float, ...] = tuple(float(v) for v in np.degrees(TRAVEL_MIN_RAD)) +_MAX_DEG: tuple[float, ...] = tuple(float(v) for v in np.degrees(TRAVEL_MAX_RAD)) + + +def joint_outside_travel_rad(q: "NDArray[np.float64] | list[float]") -> int: + """Index of the first joint of ``q`` [rad] more than half a motor step + past its travel, or -1 when every joint is inside it.""" + for i in range(6): + if not (_MIN_RAD[i] <= q[i] <= _MAX_RAD[i]): + return i + return -1 + + +def joint_outside_travel_deg(angles: "NDArray[np.float64] | list[float]") -> int: + """Index of the first joint of ``angles`` [deg] more than half a motor + step past its travel, or -1 when every joint is inside it.""" + for i in range(6): + if not (_MIN_DEG[i] <= angles[i] <= _MAX_DEG[i]): + return i + return -1 diff --git a/parol6/utils/warmup.py b/parol6/utils/warmup.py index 6717ff3..e43ffc2 100644 --- a/parol6/utils/warmup.py +++ b/parol6/utils/warmup.py @@ -11,7 +11,9 @@ import numpy as np -from parol6.commands.servo_commands import _max_vel_ratio_jit +from parol6.commands._collision_guard import _lookahead_jit +from parol6.commands.basic_commands import _jog_lookahead_jit, _track_rates_jit +from parol6.commands.servo_commands import _max_vel_ratio_jit, _step_toward_jit from parol6.config import ( deg_to_steps, deg_to_steps_scalar, @@ -31,10 +33,13 @@ steps_to_rad_scalar, ) from parol6.motion.streaming_executors import ( + _hold_inside_jit, _pose_to_tangent_jit, + _rebase_rate_jit, + _same_pose_jit, _tangent_to_pose_jit, + below_speed, ) -from parol6.motion.trajectory import _smooth_singularity_outliers from parol6.protocol.wire import ( _pack_bitfield, _pack_positions, @@ -110,6 +115,9 @@ def _progress(label: str) -> None: steps_to_deg(dummy_6i, out_6f) steps_to_deg_scalar(0, 0) rad_to_steps(dummy_6f, out_6i) + # A row of the IK solver's (N, 6) joint path: the solver returns it + # Fortran-ordered, so each row is a strided array. + rad_to_steps(np.zeros((2, 6), dtype=np.float64, order="F")[0], out_6i) rad_to_steps_scalar(0.0, 0) steps_to_rad(dummy_6i, out_6f) steps_to_rad_scalar(0, 0) @@ -298,40 +306,51 @@ def _progress(label: str) -> None: ) _progress("simulator & I/O") - # Workspace arrays for jit functions below (SE3 funcs already warmed by pinokin) + # parol6/motion/streaming_executors.py dummy_twist = np.zeros(6, dtype=np.float64) omega_ws = np.zeros(3, dtype=np.float64) - R_ws = np.zeros((3, 3), dtype=np.float64) - V_ws = np.zeros((3, 3), dtype=np.float64) - V_inv_ws = np.zeros((3, 3), dtype=np.float64) - - # parol6/motion/streaming_executors.py - ref_inv = np.zeros((4, 4), dtype=np.float64) - delta_4x4 = np.zeros((4, 4), dtype=np.float64) - _pose_to_tangent_jit( - dummy_4x4, - dummy_4x4_b, - ref_inv, - delta_4x4, - dummy_twist, - omega_ws, - R_ws, - V_inv_ws, - ) - _tangent_to_pose_jit( - dummy_4x4, dummy_twist, delta_4x4, dummy_4x4_out, omega_ws, R_ws, V_ws + rel_rot = np.zeros((3, 3), dtype=np.float64) + _pose_to_tangent_jit(dummy_4x4, dummy_4x4_b, rel_rot, dummy_twist, omega_ws) + _tangent_to_pose_jit(dummy_4x4, dummy_twist, rel_rot, dummy_4x4_out, omega_ws) + _rebase_rate_jit(dummy_twist, rel_rot, np.zeros(6), omega_ws, True) + _same_pose_jit(dummy_4x4, dummy_4x4_b) + _hold_inside_jit( + np.zeros(6), + np.zeros(6), + dummy_6f, + dummy_6f, + dummy_6f, + dummy_6f, + np.zeros(6, dtype=np.bool_), ) + below_speed(dummy_6f, 1e-8) # parol6/commands/servo_commands.py _max_vel_ratio_jit(dummy_6f, dummy_6f) + _step_toward_jit(np.zeros(6), dummy_6f) + + # parol6/commands/_collision_guard.py + _lookahead_jit( + dummy_6f, dummy_6f, 0.15, dummy_6f, dummy_6f, dummy_6f, True, np.zeros(6) + ) - # parol6/motion/trajectory.py — non-trivial array exercises every branch - # (diff loop, median, bad-detection, interp loop). - dummy_chain = np.zeros((10, 6), dtype=np.float64) - for i in range(10): - dummy_chain[i] = i * 0.01 - dummy_chain[5, 3] += 1.0 # synthetic outlier so the interp loop compiles - _smooth_singularity_outliers(dummy_chain) + # parol6/commands/basic_commands.py + _jog_lookahead_jit( + dummy_6f, + False, + dummy_6f, + dummy_6f, + dummy_6f, + dummy_6f, + dummy_6f, + dummy_6f, + dummy_6f, + 1.0, + 0.01, + np.zeros(6, dtype=np.int8), + np.zeros(6), + ) + _track_rates_jit(dummy_6f, np.zeros(6), np.zeros(6), 0.01) elapsed = time.perf_counter() - start logger.info("JIT warmup complete (%.1fs).", elapsed) diff --git a/pyproject.toml b/pyproject.toml index 92fe83a..c0d2d0d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -45,7 +45,7 @@ dependencies = [ "psutil>=5.9", "msgspec>=0.18", "ormsgpack>=1.4.0", - "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.14.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.15.0", ] [tool.setuptools.packages.find] diff --git a/tests/conftest.py b/tests/conftest.py index feaee5c..d03ad74 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -227,6 +227,10 @@ def server_proc(request, ports: TestPorts, robot_api_env): "PAROL6_CONTROLLER_IP": ports.server_ip, "PAROL6_CONTROLLER_PORT": str(ports.server_port), "PAROL6_MCAST_PORT": str(ports.mcast_port), + # A test sends a blend chain one acknowledged command at a + # time, and a degraded CI loop (macOS runs ~40 Hz) forwards + # them more than the default hold apart. + "PAROL6_BLEND_HOLD_S": "0.5", }, ) @@ -372,6 +376,7 @@ def client(ports: TestPorts): 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.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.server.controller import Controller, ControllerConfig monkeypatch.setenv("PAROL6_FAKE_SERIAL", "1") @@ -386,6 +391,10 @@ def controller(monkeypatch): ctl._status_broadcaster.close() ctl._transport_mgr.disconnect() ctl.state_manager.reset_state() + # The robot model is per process: a tool or shape this controller + # applied would otherwise shape every later in-process plan. + PAROL6_ROBOT.apply_tool("NONE") + PAROL6_ROBOT.apply_shapes([]) def pytest_sessionfinish(session, exitstatus): diff --git a/tests/integration/conftest.py b/tests/integration/conftest.py index c7beb51..c7e91e7 100644 --- a/tests/integration/conftest.py +++ b/tests/integration/conftest.py @@ -8,10 +8,12 @@ def clean_state(server_proc, client): """ Reset controller state before each integration test for isolation. - Uses RESET command to instantly reset positions, queues, tool, errors. + Clears a protective stop a failed test may have latched (``reset_state`` + leaves it), then resets the program state and homes. Sets LINEAR motion profile for faster test execution. Depends on server_proc to ensure server is ready before resetting. """ + client.reset() client.reset_state() client.select_profile("LINEAR") idx = client.home() diff --git a/tests/integration/controller_loop.py b/tests/integration/controller_loop.py new file mode 100644 index 0000000..62848e3 --- /dev/null +++ b/tests/integration/controller_loop.py @@ -0,0 +1,159 @@ +"""Drive an in-process Controller (the ``controller`` fixture) one pass of +its own loop body at a time against the fake serial, and talk to it over +its real UDP socket.""" + +import socket +import time +from types import SimpleNamespace + +import numpy as np +import pytest + +import parol6.commands.base as command_base +from parol6.config import INTERVAL_S, deg_to_steps +from parol6.protocol.wire import ( + ErrorMsg, + OkMsg, + PingCmd, + decode_message, + encode_command, +) +from parol6.server.controller import Controller + + +# The status an in-process controller broadcasts lands on a loopback port +# held here, not on the multicast group a running controller is heard on. +_status_sink: socket.socket | None = None + + +def _keep_status_local(controller: Controller) -> None: + global _status_sink + broadcaster = controller._status_broadcaster + if broadcaster is None: + return + if _status_sink is None: + _status_sink = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + _status_sink.bind(("127.0.0.1", 0)) + port = _status_sink.getsockname()[1] + if broadcaster._use_unicast and broadcaster.port == port: + return + broadcaster.port = port + broadcaster._switch_to_unicast() + + +def tick(controller: Controller, state) -> None: + """One control tick: the controller's own loop body, every phase.""" + _keep_status_local(controller) + controller._run_tick(state) + + +def tick_until(controller: Controller, state, condition, message: str, ticks=50): + for _ in range(ticks): + tick(controller, state) + if condition(): + return + pytest.fail(message) + + +def tick_for(controller: Controller, state, condition, message: str, seconds: float): + """Tick at the control rate until *condition* holds, for up to *seconds* + of wall time (a cold planner JITs its motion pipeline).""" + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + tick(controller, state) + if condition(): + return + time.sleep(INTERVAL_S) + pytest.fail(message) + + +class VirtualClock: + """The clock the commands' timers read (a jog's duration, a stream's + grace), advanced one control interval per :meth:`tick`. The timers then + count ticks, as they do on a loop that holds its rate, whatever the + machine running the test does to its sleeps.""" + + def __init__(self, monkeypatch: pytest.MonkeyPatch) -> None: + self.now = 0.0 + monkeypatch.setattr( + command_base, "time", SimpleNamespace(perf_counter=lambda: self.now) + ) + + def tick(self, controller: Controller, state) -> None: + tick(controller, state) + self.now += INTERVAL_S + + +def _reply(controller: Controller, state, sock: socket.socket, req_id: int): + """Tick until the reply to request *req_id* arrives and return it + decoded, or None after two seconds of wall time: loopback delivers a + datagram some ticks late on some hosts (macOS), so no tick count + bounds the wait.""" + deadline = time.monotonic() + 2.0 + while time.monotonic() < deadline: + tick(controller, state) + try: + data, _ = sock.recvfrom(4096) + except BlockingIOError: + continue + reply = decode_message(data) + if getattr(reply, "req_id", None) == req_id: + return reply + return None + + +def drain(controller: Controller, state, sock: socket.socket, req_id: int) -> None: + """Tick until the controller answers a ping sent now. Datagrams from one + socket to one address are read in the order they were sent, so every + one sent before the ping has been read by then, however late it was + delivered.""" + sock.sendto(encode_command(PingCmd(), req_id), address(controller)) + if _reply(controller, state, sock, req_id) is None: + pytest.fail("the controller never answered the ping") + + +def address(controller: Controller) -> tuple[str, int]: + assert controller.udp_transport is not None + return ("127.0.0.1", controller.udp_transport.socket.getsockname()[1]) + + +def push(controller: Controller, sock: socket.socket, cmd, req_id: int = 0) -> None: + """Send a fire-and-forget datagram (jog, servo): nothing answers it.""" + sock.sendto(encode_command(cmd, req_id), address(controller)) + + +def send(controller: Controller, state, sock: socket.socket, cmd, req_id: int): + """Send *cmd* to the controller, tick until it answers, and return the + decoded reply.""" + sock.sendto(encode_command(cmd, req_id), address(controller)) + reply = _reply(controller, state, sock, req_id) + if not isinstance(reply, (OkMsg, ErrorMsg)): + pytest.fail(f"no reply to {type(cmd).__name__}: {reply}") + return reply + + +def ready( + controller: Controller, state, *, homed: bool, at_deg: list[float] | None = None +) -> None: + """Bring the fake serial up enabled, referenced or in the boot state, + at ``at_deg`` when given.""" + from parol6.server.transports.mock_serial_transport import MockSerialTransport + + robot = controller._transport_mgr.transport + assert isinstance(robot, MockSerialTransport) + state.Homed_in[:] = 1 if homed else 0 + if at_deg is not None: + deg_to_steps(np.asarray(at_deg, dtype=np.float64), state.Position_in) + elif not homed: + state.Position_in[:] = 0 + robot.sync_from_controller_state(state) + tick_until( + controller, + state, + lambda: state.enabled and (all(state.Homed_in[:6]) == homed), + "the fake serial never reported an enabled robot", + ) + # The frame read on that tick predates the sync; the next tick + # produces one from the synced state and the one after reads it. + tick(controller, state) + tick(controller, state) diff --git a/tests/integration/test_attachment_estop.py b/tests/integration/test_attachment_estop.py index fe2e9c7..a44cce1 100644 --- a/tests/integration/test_attachment_estop.py +++ b/tests/integration/test_attachment_estop.py @@ -10,41 +10,21 @@ 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 tests.integration.controller_loop import tick, tick_until 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( + tick_until( controller, state, lambda: state.enabled and all(state.Homed_in[:6]), @@ -60,7 +40,7 @@ def test_estop_owns_the_error_until_release_then_the_attachment_latches(controll sender.sendto( encode_command(SetShapesCmd(shapes=[ShapeWire(*part.to_wire())])), address ) - _tick_until( + tick_until( controller, state, lambda: state.has_attachments, @@ -68,20 +48,20 @@ def test_estop_owns_the_error_until_release_then_the_attachment_latches(controll ) robot.press_estop(True) - _tick_until( + tick_until( controller, state, lambda: controller.estop_active, "the E-stop press was never seen", ) for _ in range(5): - _tick(controller, state) + 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( + tick_until( controller, state, lambda: state.error is not None diff --git a/tests/integration/test_blend_lookahead.py b/tests/integration/test_blend_lookahead.py index b9c0d9d..f900fbb 100644 --- a/tests/integration/test_blend_lookahead.py +++ b/tests/integration/test_blend_lookahead.py @@ -11,6 +11,108 @@ class TestJointBlendLookahead: """Joint-space blending with N-command lookahead.""" + def test_a_blended_relative_chain_past_a_joint_limit_is_refused( + self, client, server_proc + ): + """Relative moves blended into one path are held to the joint limits + at every target, as a single move is: two +25° steps of J1 from + standby end past its limit, so the chain is refused and the arm + stays inside it.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + from parol6 import MotionError + from parol6.config import LIMITS + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + hi = float(LIMITS.joint.position.deg[0, 1]) + assert standby[0] + 50.0 > hi > standby[0] + 25.0 + step = [25.0, 0.0, 0.0, 0.0, 0.0, 0.0] + first = client.move_j(step, speed=0.5, rel=True, r=5.0, wait=False) + second = client.move_j(step, speed=0.5, rel=True, wait=False) + assert min(first, second) >= 0 + with pytest.raises(MotionError): + client.wait_command(second, timeout=10.0) + angles = client.angles() + assert angles is not None + assert angles[0] < hi, f"J1 ran to {angles[0]:.1f}°, past its {hi:.1f}° limit" + + @pytest.mark.parametrize( + ("joint", "side"), + [(1, 0), (2, 1), (4, 0), (4, 1), (5, 1)], + ids=["J2-min", "J3-max", "J5-min", "J5-max", "J6-max"], + ) + def test_relative_moves_from_a_joint_parked_on_its_limit_run( + self, client, server_proc, joint, side + ): + """A joint parked on its limit reads back a hair past the limit's + decimal, by a motor step's rounding or a float's. A relative move + of another joint leaves it where it is parked, which is no move + past the limit, alone or blended into a chain.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + from parol6.config import LIMITS + + parked = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + parked[joint] = float(LIMITS.joint.position.deg[joint, side]) + assert client.teleport(parked) == 1 + step = [10.0, 0.0, 0.0, 0.0, 0.0, 0.0] + assert client.move_j(step, speed=0.5, rel=True, timeout=10.0) >= 0 + + out = client.move_j(step, speed=0.5, rel=True, r=5.0, wait=False) + back = client.move_j( + [-10.0, 0.0, 0.0, 0.0, 0.0, 0.0], speed=0.5, rel=True, wait=False + ) + assert min(out, back) >= 0 + assert client.wait_command(back, timeout=10.0) + angles = client.angles() + assert angles is not None + assert abs(angles[0] - (parked[0] + 10.0)) < 0.5 + assert abs(angles[joint] - parked[joint]) < 0.01 + + @pytest.mark.parametrize( + "profile", ["TOPPRA", "LINEAR", "QUINTIC", "TRAPEZOID", "RUCKIG"] + ) + def test_a_blended_chain_runs_its_path_under_every_profile( + self, client, server_proc, profile + ): + """A blended joint chain goes where its moves go, whichever profile + times it: out along J2 and back swings the arm out before it comes + home, and standby → A → B passes by A rather than cutting straight + across to B.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + assert client.select_profile(profile) > 0 + + out = client.move_j( + [0.0, 20.0, 0.0, 0.0, 0.0, 0.0], speed=0.5, rel=True, r=10.0, wait=False + ) + back = client.move_j( + [0.0, -20.0, 0.0, 0.0, 0.0, 0.0], speed=0.5, rel=True, wait=False + ) + assert min(out, back) >= 0 + # The blend cuts the turn-round by its radius: the arm swings out + # most of the 20°, never all of it. + assert client.wait_status( + lambda s: s.angles[1] > standby[1] + 15.0, timeout=10.0 + ), f"{profile}: the chain never swung J2 out" + assert client.wait_command(back, timeout=10.0) + angles = client.angles() + assert angles is not None + assert abs(angles[1] - standby[1]) < 1.0 + + a = [80.0, -80.0, 170.0, 5.0, 5.0, 170.0] + b = [70.0, -90.0, 160.0, 10.0, 10.0, 160.0] + assert client.move_j(a, speed=0.5, r=10.0, wait=False) >= 0 + last = client.move_j(b, speed=0.5, wait=False) + assert last >= 0 + # Straight from standby to B passes A 10° off in J2. + assert client.wait_status( + lambda s: max(abs(q - t) for q, t in zip(s.angles, a)) < 3.0, timeout=10.0 + ), f"{profile}: the chain cut straight past A" + assert client.wait_command(last, timeout=10.0) + angles = client.angles() + assert angles is not None + assert max(abs(q - t) for q, t in zip(angles, b)) < 1.0 + def test_three_move_j_blended_reaches_final_target(self, client, server_proc): """Three move_j with blend zones should reach the last target.""" targets = [ @@ -41,10 +143,11 @@ def test_move_j_r0_stops_blend_chain(self, client, server_proc): ([60, -60, 150, 15, 15, 150], 30.0), # separate motion ] - for t, r in targets: - assert client.move_j(t, speed=0.5, r=r, wait=False) >= 0 - - assert client.wait_motion(timeout=15.0) + indices = [client.move_j(t, speed=0.5, r=r, wait=False) for t, r in targets] + assert all(index >= 0 for index in indices) + # The last move's own completion: its r>0 holds it for a partner, so + # the arm rests between the stopped chain and it. + assert client.wait_command(indices[-1], timeout=15.0) angles = client.angles() assert angles is not None @@ -166,10 +269,12 @@ def test_move_l_r0_stops_blend_chain(self, client, server_proc): ([start[0], start[1] + 45, start[2], start[3], start[4], start[5]], 20.0), ] - for t, r in targets: - assert client.move_l(t, speed=0.5, r=r, wait=False) >= 0 - - assert client.wait_motion(timeout=15.0) + indices = [client.move_l(t, speed=0.5, r=r, wait=False) for t, r in targets] + assert all(index >= 0 for index in indices) + # The last move's own completion, not the arm keeping still: a + # pause between the stopped chain and the move after it is not + # the end of the program. + assert client.wait_command(indices[-1], timeout=15.0) final = client.pose() assert final is not None @@ -184,18 +289,18 @@ class TestMixedTypeBlendTermination: def test_move_j_then_move_l_executes_separately(self, client, server_proc): """move_j(r>0) followed by move_l should not blend across types.""" # Small joint move with blend radius - assert ( - client.move_j( - [85, -85, 175, 2, 2, 175], - speed=0.5, - r=20.0, - wait=False, - ) - >= 0 + index = client.move_j( + [85, -85, 175, 2, 2, 175], + speed=0.5, + r=20.0, + wait=False, ) + assert index >= 0 - # Wait for joint move, then get the pose for a reachable Cartesian target - assert client.wait_motion(timeout=10.0) + # The move's own completion, not the arm keeping still: the planner + # holds an r>0 move for a blend partner, so the arm can still be at + # rest when wait_motion stops waiting for it to start. + assert client.wait_command(index, timeout=10.0) mid_pose = client.pose() assert mid_pose is not None diff --git a/tests/integration/test_client_waits.py b/tests/integration/test_client_waits.py new file mode 100644 index 0000000..229d442 --- /dev/null +++ b/tests/integration/test_client_waits.py @@ -0,0 +1,102 @@ +"""What a client reads while the arm moves: ``wait_motion()`` returns only +once everything queued before it has run and the arm has come to rest, and +the TCP speed reads zero once the serial frames it is measured from stop.""" + +import itertools +import socket +import time + +import numpy as np +import pytest + +from parol6 import RobotClient +from parol6.config import HOME_ANGLES_DEG, INTERVAL_S +from parol6.protocol.wire import ( + JogJCmd, + ResponseMsg, + TcpSpeedCmd, + TcpSpeedResultStruct, + decode_message, + encode_command, +) +from tests.integration.controller_loop import address, push, ready, tick + +pytestmark = pytest.mark.integration + + +def test_wait_motion_returns_once_the_queue_ahead_of_it_has_run( + client: RobotClient, server_proc +): + """A move queued behind a delay has not started while the arm stands + still through the delay: the wait covers the move, not the stillness. + A queue that a stop discarded ends the wait instead of holding it.""" + start = client.angles() + assert start is not None + there = [start[0] - 15.0, *start[1:]] + + assert client.delay(2.0) >= 0 + assert client.move_j(there, duration=1.0, wait=False) >= 0 + assert client.wait_motion(timeout=10.0) + here = client.angles() + assert here is not None + assert np.allclose(here, there, atol=0.5), ( + f"wait_motion returned with the arm at {here}, before the move queued " + f"behind the delay reached {there}" + ) + + assert client.delay(30.0) >= 0 + assert client.stop() == 1 + assert client.wait_motion(timeout=5.0), "a discarded delay held the wait" + + +def _tcp_speed(controller, state, sock: socket.socket, req_id: int) -> float: + """The controller's answer to a TCP_SPEED query, ticking until it comes + (for up to two seconds: loopback can deliver it some ticks late).""" + sock.sendto(encode_command(TcpSpeedCmd(), req_id), address(controller)) + deadline = time.monotonic() + 2.0 + while time.monotonic() < deadline: + tick(controller, state) + try: + data, _ = sock.recvfrom(4096) + except BlockingIOError: + continue + reply = decode_message(data) + if isinstance(reply, ResponseMsg) and reply.req_id == req_id: + assert isinstance(reply.result, TcpSpeedResultStruct), reply + return reply.result.speed + pytest.fail("no reply to the TCP speed query") + + +def test_tcp_speed_reads_zero_once_the_serial_frames_stop(controller, monkeypatch): + """A serial dropout mid-move leaves nothing to measure the TCP by: its + speed reads zero, not the speed it had when the frames stopped.""" + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=[float(v) for v in HOME_ANGLES_DEG]) + req_ids = itertools.count(1) + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + push( + controller, + sock, + JogJCmd(speeds=[-0.5, 0.0, 0.0, 0.0, 0.0, 0.0], duration=5.0), + ) + moving = 0.0 + for _ in range(50): + moving = _tcp_speed(controller, state, sock, next(req_ids)) + if moving > 1.0: + break + assert moving > 1.0, f"the jog never moved the TCP ({moving:.2f} mm/s)" + + monkeypatch.setattr( + controller._transport_mgr, "get_latest_frame", lambda: (None, 0, 0.0) + ) + deadline = time.monotonic() + 1.0 + speed = _tcp_speed(controller, state, sock, next(req_ids)) + while speed != 0.0 and time.monotonic() < deadline: + time.sleep(INTERVAL_S) + speed = _tcp_speed(controller, state, sock, next(req_ids)) + assert speed == 0.0, ( + f"a second into a serial dropout the TCP still read {speed:.1f} mm/s, " + f"the speed it had when the frames stopped ({moving:.1f} mm/s)" + ) diff --git a/tests/integration/test_curved_commands_e2e.py b/tests/integration/test_curved_commands_e2e.py index 0604906..628eba7 100644 --- a/tests/integration/test_curved_commands_e2e.py +++ b/tests/integration/test_curved_commands_e2e.py @@ -49,7 +49,7 @@ def test_move_c_basic(self, client, server_proc, robot_api_env, home_pose): ) assert result >= 0 assert client.wait_motion(timeout=9.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() def test_move_c_with_orientation( self, client, server_proc, robot_api_env, home_pose @@ -63,7 +63,7 @@ def test_move_c_with_orientation( ) assert result >= 0 assert client.wait_motion(timeout=15.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() def test_move_c_trf_accepted(self, client, server_proc, robot_api_env, homed_robot): """Test that move_c with frame=TRF is accepted and completes.""" @@ -75,7 +75,42 @@ def test_move_c_trf_accepted(self, client, server_proc, robot_api_env, homed_rob ) assert result >= 0 assert client.wait_motion(timeout=15.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() + + def test_a_collinear_via_fails_the_move_c_alone_or_after_a_blend( + self, client, server_proc, robot_api_env, home_pose + ): + """Three points on a line name no circle. The move_c that gives + them fails on its own index, with the same error whether it runs + alone or a move_l blends into it: the move_l ahead of it asked for + nothing wrong.""" + from parol6 import MotionError + + alone = client.move_c( + via=self._offset(home_pose, dy=10), + end=self._offset(home_pose, dy=20), + speed=0.5, + wait=False, + ) + assert alone >= 0 + with pytest.raises(MotionError) as lone: + client.wait_command(alone, timeout=10.0) + assert lone.value.command_index == alone, lone.value + + head = client.move_l( + self._offset(home_pose, dx=20), speed=0.5, r=5.0, wait=False + ) + culprit = client.move_c( + via=self._offset(home_pose, dx=20, dy=10), + end=self._offset(home_pose, dx=20, dy=20), + speed=0.5, + wait=False, + ) + assert min(head, culprit) >= 0 + with pytest.raises(MotionError) as chained: + client.wait_command(culprit, timeout=10.0) + assert chained.value.command_index == culprit, chained.value + assert chained.value.code == lone.value.code, chained.value def test_move_s_basic(self, client, server_proc, robot_api_env, home_pose): """Test spline motion through waypoints.""" @@ -87,7 +122,7 @@ def test_move_s_basic(self, client, server_proc, robot_api_env, home_pose): result = client.move_s(waypoints=waypoints, duration=3.0, frame="WRF") assert result >= 0 assert client.wait_motion(timeout=15.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() def test_move_s_trf_accepted(self, client, server_proc, robot_api_env, homed_robot): """Test that move_s with frame=TRF is accepted and completes.""" @@ -99,19 +134,25 @@ def test_move_s_trf_accepted(self, client, server_proc, robot_api_env, homed_rob result = client.move_s(waypoints=waypoints, duration=3.0, frame="TRF") assert result >= 0 assert client.wait_motion(timeout=15.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() - def test_move_p_basic(self, client, server_proc, robot_api_env, home_pose): + def test_move_p_basic(self, client, server_proc, robot_api_env, homed_robot): """Test process move through waypoints with constant TCP speed.""" + # A process move runs at the one speed its slowest row allows, and + # at the home pose's wrist singularity that row is the wrist's: the + # move starts clear of it. + assert client.teleport([90.0, -80.0, 190.0, 0.0, 30.0, 180.0]) == 1 + start = client.pose() + assert start is not None waypoints = [ - self._offset(home_pose, dz=-5), - self._offset(home_pose, dx=10, dy=5, dz=-5), - self._offset(home_pose, dz=-5), + self._offset(start, dz=-5), + self._offset(start, dx=10, dy=5, dz=-5), + self._offset(start, dz=-5), ] result = client.move_p(waypoints=waypoints, speed=0.3, frame="WRF") assert result >= 0 assert client.wait_motion(timeout=15.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() def test_move_p_trf_accepted(self, client, server_proc, robot_api_env, homed_robot): """Test that move_p with frame=TRF is accepted and completes.""" @@ -123,7 +164,7 @@ def test_move_p_trf_accepted(self, client, server_proc, robot_api_env, homed_rob result = client.move_p(waypoints=waypoints, speed=0.3, frame="TRF") assert result >= 0 assert client.wait_motion(timeout=15.0) - assert client.is_robot_stopped(threshold_speed=5.0) + assert client.is_robot_stopped() class TestComputeCircleFrom3Points: diff --git a/tests/integration/test_gripper_calibration_gate.py b/tests/integration/test_gripper_calibration_gate.py new file mode 100644 index 0000000..da4ef92 --- /dev/null +++ b/tests/integration/test_gripper_calibration_gate.py @@ -0,0 +1,69 @@ +"""A jaw move needs a calibration the gripper may only get from the action +queued ahead of it, so the gate is judged when the move's turn comes: +``calibrate(); move()`` runs in order, and a move on a gripper that was +never calibrated fails — as a completion, since the ack came before its +turn — instead of driving uncalibrated jaws.""" + +import socket + +import pytest + +from parol6.protocol.wire import OkMsg, SelectToolCmd, ToolActionCmd +from parol6.utils.error_codes import ErrorCode +from tests.integration.controller_loop import ready, send, tick_for + +pytestmark = pytest.mark.integration + + +def test_a_jaw_move_waits_for_the_calibrate_ahead_of_it(controller): + state = controller.state_manager.get_state() + controller._planner.start() + ready(controller, state, homed=True) + move = ToolActionCmd(tool_key="SSG-48", action="move", params=[0.5, 0.5, 0.4]) + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + selected = send(controller, state, sock, SelectToolCmd(tool_name="SSG-48"), 9) + assert isinstance(selected, OkMsg), selected + tick_for( + controller, + state, + lambda: state.current_tool == "SSG-48", + "the select_tool never ran", + seconds=30.0, + ) + never = send(controller, state, sock, move, 1) + assert isinstance(never, OkMsg) and never.index is not None, never + tick_for( + controller, + state, + lambda: state.command_failure(never.index) is not None, + "the move on a never-calibrated gripper was not failed", + seconds=30.0, + ) + failure = state.command_failure(never.index) + assert failure is not None + assert failure.code == int(ErrorCode.COMM_VALIDATION_ERROR) + assert "not calibrated" in failure.cause + assert state.gripper_hw.feedback_position == 0, "the jaws must not have moved" + + calibrate = send( + controller, + state, + sock, + ToolActionCmd(tool_key="SSG-48", action="calibrate", params=[]), + 2, + ) + moved = send(controller, state, sock, move, 3) + assert isinstance(calibrate, OkMsg) and calibrate.index is not None + assert isinstance(moved, OkMsg) and moved.index is not None + tick_for( + controller, + state, + lambda: state.command_completed(moved.index), + "the move queued behind the calibrate never completed", + seconds=30.0, + ) + assert state.command_completed(calibrate.index) + assert state.command_failure(moved.index) is None + assert abs(state.gripper_hw.feedback_position / 255.0 - 0.5) < 0.05 diff --git a/tests/integration/test_home_fastpath.py b/tests/integration/test_home_fastpath.py index b195f76..43a17c6 100644 --- a/tests/integration/test_home_fastpath.py +++ b/tests/integration/test_home_fastpath.py @@ -48,3 +48,33 @@ def test_home_returns_via_planned_move_when_referenced( # Degenerate re-home (already at standby) completes as a no-op move. assert client.home(wait=True, timeout=10.0) >= 0 + + +@pytest.mark.asyncio +async def test_a_planned_home_reports_no_seek_in_progress(server_proc, ports): + """Referencing progress belongs to the switch seek: a home that runs as + a planned return move after a seek reports none, rather than the step + the last seek ended on.""" + client = server_proc.create_async_client( + host=ports.server_ip, port=ports.server_port + ) + try: + assert await client.wait_ready(timeout=5.0) + assert await client.home(calibrate=True, wait=True, timeout=30.0) >= 0 + away = [45.0, -60.0, 150.0, 0.0, 30.0, 90.0] + assert await client.move_j(away, duration=1.0, wait=True) >= 0 + + index = await client.home() + assert index >= 0 + homing_frames = 0 + async for status in client.stream_status(): + if status.action_current == "home": + homing_frames += 1 + assert not status.homing, ( + f"a planned home reported seek progress: {status.homing}" + ) + if status.completed_index >= index: + break + assert homing_frames > 0, "the planned home was never reported" + finally: + await client.close() diff --git a/tests/integration/test_move_timing.py b/tests/integration/test_move_timing.py new file mode 100644 index 0000000..30554d0 --- /dev/null +++ b/tests/integration/test_move_timing.py @@ -0,0 +1,86 @@ +"""How a planned move is timed, through the client and the simulated +controller: with no timing given it runs at half speed and half +acceleration, a positive duration sets its length whatever speed says, and +a timing value out of range is refused before anything is sent.""" + +import math + +import pytest + +from parol6 import RobotClient +from parol6.client.dry_run_client import DryRunRobotClient + +pytestmark = pytest.mark.integration + + +def _queued_seconds(client: RobotClient, moves: int) -> float: + """Planned seconds the paused queue holds once it holds *moves* moves.""" + seconds: list[float] = [] + + def holds(status) -> bool: + if status.queued_segments != moves: + return False + seconds.append(status.queued_duration) + return True + + assert client.wait_status(holds, timeout=5.0), f"the queue never held {moves}" + return seconds[-1] + + +def test_an_untimed_move_runs_at_half_speed_and_a_duration_overrides_speed( + client: RobotClient, +): + start = client.angles() + assert start is not None + there = list(start) + # Long enough to cruise at half speed under LINEAR: a shorter move that + # the acceleration ramps alone fill plans the same at any faster speed. + there[0] -= 90.0 + try: + # Paused, so each move is planned and held rather than run. + assert client.pause() == 1 + assert client.move_j(there, wait=False) >= 0 + untimed = _queued_seconds(client, 1) + assert client.move_j(start, speed=0.5, accel=0.5, wait=False) >= 0 + at_half = _queued_seconds(client, 2) - untimed + assert client.move_j(there, duration=4.0, speed=0.1, wait=False) >= 0 + timed = _queued_seconds(client, 3) - untimed - at_half + finally: + assert client.stop() == 1 + assert untimed == pytest.approx(at_half, abs=0.02), ( + f"a move given no timing planned {untimed:.3f}s, the same move at " + f"speed and accel 0.5 {at_half:.3f}s" + ) + assert timed == pytest.approx(4.0, abs=0.02), ( + f"a 4 s move planned {timed:.3f}s: its speed overrode its duration" + ) + + +def test_a_timing_value_out_of_range_is_refused_before_anything_is_sent( + client: RobotClient, +): + """``speed`` and ``accel`` are fractions in (0, 1] and ``duration`` is a + finite number of seconds >= 0: the live client and the dry run refuse + anything else with ValueError, not a controller rejection.""" + start = client.angles() + pose = client.pose() + assert start is not None and pose is not None + there = list(start) + there[0] -= 5.0 + preview = DryRunRobotClient(initial_joints_deg=start) + for rbt in (client, preview): + for bad in (0.0, -0.5, math.nan, math.inf, 1.5): + with pytest.raises(ValueError): + rbt.move_j(there, speed=bad) + with pytest.raises(ValueError): + rbt.move_l(pose, speed=0.5, accel=bad) + with pytest.raises(ValueError): + rbt.move_c(pose, pose, speed=bad) + with pytest.raises(ValueError): + rbt.move_p([pose, pose], duration=2.0, accel=bad) + for bad in (-1.0, math.nan, math.inf): + with pytest.raises(ValueError): + rbt.move_j(there, duration=bad) + with pytest.raises(ValueError): + rbt.move_s([pose, pose], duration=bad) + assert preview.program_length == 0, "the dry run recorded a refused move" diff --git a/tests/integration/test_pipeline_failures.py b/tests/integration/test_pipeline_failures.py new file mode 100644 index 0000000..e59cd8c --- /dev/null +++ b/tests/integration/test_pipeline_failures.py @@ -0,0 +1,345 @@ +"""What the motion pipeline owes when it drops, fails, refuses or holds +work: commands a failed move or a jog drops take their effects with them — +the TCP the planner already applied, the tool actions sent for a dropped +tool selection — and fail as cancelled; a command that failed to plan +reports that failure to any later wait; a stream refused on an unhomed arm +fails nothing but itself; a blend chain sent move by move while earlier +motion plays is held until it can be rounded; and a blend hold that is not +a positive duration is refused.""" + +import os +import socket +import subprocess +import sys +from pathlib import Path + +import numpy as np +import pytest + +from parol6 import MotionError, RobotClient +from parol6.protocol.wire import ( + HomeCmd, + JogLCmd, + OkMsg, + SelectToolCmd, + ServoLCmd, + ToolActionCmd, +) +from parol6.utils.error_codes import ErrorCode +from tests.conftest import wait_until +from tests.integration.controller_loop import push, ready, send, tick_for, tick_until + +pytestmark = pytest.mark.integration + +#: Fraction of the planned-move linear ceiling (0.2 m/s) the blend chain runs at. +SPEED = 0.25 + + +def _offset(pose: list[float], dx: float, dy: float, dz: float) -> list[float]: + return [pose[0] + dx, pose[1] + dy, pose[2] + dz, *pose[3:]] + + +def _out_of_reach(pose: list[float]) -> list[float]: + return _offset(pose, 1000.0, 0.0, 0.0) + + +def _assert_a_move_to_where_it_is_stays(client: RobotClient) -> None: + """A move_l to the pose the arm reads goes nowhere only when the planner + solves it against the TCP the controller reports the pose for.""" + start = client.angles() + pose = client.pose() + assert start is not None and pose is not None + index = client.move_l(pose, duration=1.0, wait=False) + assert index >= 0 and client.wait_command(index, timeout=10.0) + after = client.angles() + assert after is not None + assert np.allclose(after, start, atol=0.5), ( + f"the planner kept a TCP the controller dropped: {start} -> {after}" + ) + + +def test_a_tcp_change_a_jog_or_a_failed_move_drops_is_dropped_by_the_planner_too( + client: RobotClient, server_proc +): + """The planner applies a TCP change when it plans it, while the command + is still queued. A jog that takes the arm from the queue, or a move that + fails ahead of the change, drops it on the controller; the planner must + let it go as well, or every later plan is solved against a TCP the + controller never applied.""" + assert client.delay(3.0) >= 0 + assert client.set_tcp_transform(0, 0, 40, 0, 90, 0) >= 0 + assert client.jog_j(0, 0.2, duration=0.2) == 1 + assert client.wait_motion(timeout=5.0) + assert client.tcp_transform() == pytest.approx([0] * 6) + _assert_a_move_to_where_it_is_stays(client) + + pose = client.pose() + assert pose is not None + # The delay holds the failure back until the change is queued behind it. + assert client.delay(1.0) >= 0 + assert client.move_l(_out_of_reach(pose), wait=False) >= 0 + dropped = client.set_tcp_offset(0, 0, 40) + assert dropped >= 0 + with pytest.raises(MotionError) as cancelled: + client.wait_command(dropped, timeout=5.0) + assert cancelled.value.robot_error.code == ErrorCode.MOTN_CANCELLED + assert client.tcp_offset() == pytest.approx([0, 0, 0]) + _assert_a_move_to_where_it_is_stays(client) + + +def test_a_tool_action_sent_for_a_selection_a_failed_move_drops_is_cancelled_with_it( + client: RobotClient, server_proc +): + """A tool action sent right behind the select_tool that fits its tool + belongs to that selection: when a move failing ahead of both drops the + selection, the action fails as cancelled with it, and the tool still + fitted takes its own actions again.""" + fitted = client.select_tool("PNEUMATIC") + assert fitted >= 0 and client.wait_command(fitted, timeout=10.0) + pose = client.pose() + assert pose is not None + # The delay holds the failure back until both are queued behind it. + assert client.delay(1.5) >= 0 + assert client.move_l(_out_of_reach(pose), wait=False) >= 0 + selecting = client.select_tool("SSG-48") + calibrating = client.tool_action("SSG-48", "calibrate", wait=False) + assert selecting >= 0 and calibrating >= 0 + + for index in (selecting, calibrating): + with pytest.raises(MotionError) as cancelled: + client.wait_command(index, timeout=5.0) + assert cancelled.value.robot_error.code == ErrorCode.MOTN_CANCELLED, ( + cancelled.value + ) + assert cancelled.value.command_index == index + tools = client.tools() + assert tools is not None and tools.tool == "PNEUMATIC" + opening = client.tool_action("PNEUMATIC", "open", wait=False) + assert opening >= 0 and client.wait_command(opening, timeout=5.0) + + +def test_a_wait_on_a_move_that_failed_to_plan_raises_its_failure_after_the_error_clears( + client: RobotClient, server_proc +): + """The next accepted command clears the standing error, not the failed + command's outcome: a wait on it asked afterwards still raises the error + it failed with, attributed to it.""" + pose = client.pose() + start = client.angles() + assert pose is not None and start is not None + failing = client.move_l(_out_of_reach(pose), wait=False) + assert failing >= 0 + wait_until( + lambda: client.error() is not None, 10.0, "the unreachable move never failed" + ) + failure = client.error() + assert failure is not None + + target = [start[0] + 5.0, *start[1:]] + assert client.move_j(target, duration=0.5, timeout=10.0) >= 0 + assert client.error() is None + with pytest.raises(MotionError) as failed: + client.wait_command(failing, timeout=2.0) + assert failed.value.robot_error.code == failure.code + assert failed.value.command_index == failing + + +def test_a_stream_refused_unhomed_is_not_the_failure_of_the_tool_action_beside_it( + controller, +): + """A cartesian jog refused on an unhomed arm leaves its refusal standing + as its own, not against the calibration queued before it.""" + state = controller.state_manager.get_state() + controller._planner.start() + ready(controller, state, homed=False) + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + selected = send(controller, state, sock, SelectToolCmd(tool_name="SSG-48"), 1) + assert isinstance(selected, OkMsg), selected + tick_for( + controller, + state, + lambda: state.current_tool == "SSG-48", + "the select_tool never ran", + seconds=30.0, + ) + calibrate = send( + controller, + state, + sock, + ToolActionCmd(tool_key="SSG-48", action="calibrate", params=[]), + 2, + ) + assert isinstance(calibrate, OkMsg) and calibrate.index is not None, calibrate + calibrating = calibrate.index + + push( + controller, + sock, + JogLCmd(velocities=[0.3, 0.0, 0.0, 0.0, 0.0, 0.0], duration=1.0), + ) + tick_until( + controller, + state, + lambda: state.error is not None, + "the unhomed jog_l was not refused", + ) + assert state.error is not None + assert state.error.code == int(ErrorCode.MOTN_NOT_HOMED), state.error + assert not state.command_completed(calibrating), ( + "the calibration finished before the refusal" + ) + # wait_command reads a standing error at or below its own index as + # that command's failure. + assert state.error.command_index > calibrating, ( + f"the jog's refusal stands against index {state.error.command_index}, " + f"failing a wait on the calibration ({calibrating})" + ) + # Wall time, not a tick budget: the calibration reaches the loop + # through the planner process, like all queued work. + tick_for( + controller, + state, + lambda: state.command_completed(calibrating), + "the calibration never completed", + seconds=30.0, + ) + assert state.command_failure(calibrating) is None + + +def test_a_cartesian_stream_refused_unhomed_leaves_the_home_running(controller): + """A jog_l or servo_l that reaches an arm while it homes is refused for + want of the reference the home is establishing; it does not take the + arm from the home and cancel it first.""" + state = controller.state_manager.get_state() + controller._planner.start() + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + for req_id, stream in enumerate( + ( + JogLCmd(velocities=[0.3, 0.0, 0.0, 0.0, 0.0, 0.0], duration=1.0), + ServoLCmd(pose=[200.0, 0.0, 200.0, 180.0, 0.0, 180.0]), + ), + start=1, + ): + name = type(stream).__name__ + ready(controller, state, homed=False) + reply = send(controller, state, sock, HomeCmd(), req_id) + assert isinstance(reply, OkMsg) and reply.index is not None, reply + homing = reply.index + tick_for( + controller, + state, + lambda: state.executing_command_index == homing, + "the home never started", + seconds=30.0, + ) + + push(controller, sock, stream) + tick_until( + controller, + state, + lambda: state.error is not None, + f"{name} was not refused unhomed", + ) + assert state.error is not None + assert state.error.code == int(ErrorCode.MOTN_NOT_HOMED), state.error + assert state.command_failure(homing) is None, ( + f"{name} cancelled the home: {state.command_failure(homing)}" + ) + tick_until( + controller, + state, + lambda: state.command_completed(homing) and all(state.Homed_in[:6]), + f"the home never completed after {name} was refused", + ticks=500, + ) + + +def test_a_blend_hold_that_is_not_a_positive_duration_is_refused_at_import(): + """PAROL6_BLEND_HOLD_S is how long the planner waits for the rest of a + blend chain: NaN or infinity kills the planner, zero or less spins it + and plans every move alone. Such a value is refused where it is read.""" + root = Path(__file__).resolve().parents[2] + for value in ("nan", "inf", "-0.5", "0"): + result = subprocess.run( + [sys.executable, "-c", "import parol6.config"], + cwd=root, + env={**os.environ, "PAROL6_BLEND_HOLD_S": value}, + capture_output=True, + text=True, + timeout=120, + ) + assert result.returncode != 0, f"PAROL6_BLEND_HOLD_S={value} was accepted" + assert "ValueError" in result.stderr, result.stderr[-2000:] + assert "PAROL6_BLEND_HOLD_S" in result.stderr, result.stderr[-2000:] + + +def test_a_blend_chain_sent_move_by_move_while_a_move_plays_is_one_motion( + client: RobotClient, server_proc +): + """A script sends a blended chain one move at a time, each further apart + than the blend hold, while the move before the chain still plays: the + chain is held until its last move and rounds its corners, instead of + each move being planned alone once the queue falls quiet and stopping + at every corner.""" + assert client.select_profile("TOPPRA") > 0 + pose = client.pose() + assert pose is not None + top = pose[2] + lowered = _offset(pose, 0.0, 0.0, -40.0) + corners = [_offset(pose, 40.0, 0.0, -40.0), _offset(pose, 40.0, 40.0, -40.0)] + end = _offset(pose, 0.0, 40.0, -40.0) + radius = 12.0 + path: list[np.ndarray] = [] + + def tracing(done): + def record(s) -> bool: + path.append(s.pose[[3, 7, 11]]) + return done(s) + + return record + + assert client.move_l(lowered, duration=4.0, wait=False) >= 0 + # Each move goes out once the long one is 12 mm further down, about a + # second after the one before: twice the hold the test server runs. + last = -1 + for depth, target, r in ( + (4.0, corners[0], radius), + (16.0, corners[1], radius), + (28.0, end, 0.0), + ): + assert client.wait_status( + tracing(lambda s, d=depth: s.pose[11] < top - d), timeout=10.0 + ), f"the long move never came {depth} mm down" + last = client.move_l(target, speed=SPEED, r=r, wait=False) + assert last >= 0 + assert client.wait_status( + tracing(lambda s: s.completed_index >= last), timeout=20.0 + ), "the chain never completed" + + kept = [path[0]] + for p in path[1:]: + if np.linalg.norm(p - kept[-1]) > 1e-6: + kept.append(p) + pts = np.asarray(kept) + for corner in corners: + miss = float(np.min(np.linalg.norm(pts - np.array(corner[:3]), axis=1))) + assert 1.0 < miss <= radius + 0.5, ( + f"the chain passed {miss:.2f} mm from its corner at {corner[:3]}: " + "a move planned alone stops at its end" + ) + + bottom = np.array(lowered[:3]) + finish = np.array(end[:3]) + arrived = np.flatnonzero(np.linalg.norm(pts - bottom, axis=1) < 0.5) + assert arrived.size, "the long move never arrived" + chain = pts[arrived[0] :] + body = chain[ + (np.linalg.norm(chain - bottom, axis=1) > 8.0) + & (np.linalg.norm(chain - finish, axis=1) > 8.0) + ] + steps = np.linalg.norm(np.diff(body, axis=0), axis=1) + assert steps.min() > 0.3, "the chain comes to rest between its moves" diff --git a/tests/integration/test_planned_paths.py b/tests/integration/test_planned_paths.py new file mode 100644 index 0000000..b8d6f69 --- /dev/null +++ b/tests/integration/test_planned_paths.py @@ -0,0 +1,723 @@ +"""Planned cartesian paths, sampled from the controller's status stream +while the simulator drives them: a process move rounds its corner and +holds one tool speed, a spline never reverses along unevenly spaced +waypoints, a TRF move runs along the tool axis, a relative WRF rotation +turns about the TCP, and a ``move_l`` with a blend radius rounds into the +``move_c`` after it and out into the ``move_l`` after that. At the edges: +a full circle under every profile, a tool-frame turn a hair off the wrist +singularity, a line the wrist can follow only by flipping, a waypoint that +only turns the tool, a long straight process run past the base, a leg that +turns the tool, and a spline timed shorter than any arm could run it.""" + +import math +import threading +import time + +import numpy as np +import pytest + +pytestmark = pytest.mark.integration + +#: Fraction of the planned-move linear ceiling (0.2 m/s) the moves run at. +SPEED = 0.25 +CRUISE_MM_S = 0.2 * 1000.0 * SPEED + + +def _rotz(deg: float) -> np.ndarray: + c, s = math.cos(math.radians(deg)), math.sin(math.radians(deg)) + return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) + + +def _rotx(deg: float) -> np.ndarray: + c, s = math.cos(math.radians(deg)), math.sin(math.radians(deg)) + return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]) + + +def _rotation_angle_deg(a: np.ndarray, b: np.ndarray) -> float: + """The angle of the rotation taking ``a`` to ``b``.""" + tr = float(np.trace(a.T @ b)) + return math.degrees(math.acos(max(-1.0, min(1.0, (tr - 1.0) / 2.0)))) + + +def _point_to_segment_mm(p: np.ndarray, a: np.ndarray, b: np.ndarray) -> float: + ab = b - a + t = float(np.clip(np.dot(p - a, ab) / max(np.dot(ab, ab), 1e-12), 0.0, 1.0)) + return float(np.linalg.norm(p - (a + t * ab))) + + +def _polyline_miss_mm(p: np.ndarray, pts: np.ndarray) -> float: + """How far the polyline through ``pts`` passes from ``p``.""" + if len(pts) < 2: + return float(np.linalg.norm(pts[0] - p)) + return min( + _point_to_segment_mm(p, a, b) for a, b in zip(pts[:-1], pts[1:], strict=True) + ) + + +def _wire_rotation(pose: list[float]) -> np.ndarray: + """The rotation a wire pose names (intrinsic XYZ, degrees).""" + from pinokin import se3_from_rpy + + se3 = np.zeros((4, 4)) + rx, ry, rz = np.radians(pose[3:]) + se3_from_rpy(0.0, 0.0, 0.0, rx, ry, rz, se3) + return se3[:3, :3].copy() + + +class _TcpSampler: + """Samples the TCP transform from ``status()`` on a background thread; + ``positions`` drops the repeats the status cache serves between its + updates.""" + + def __init__(self, client): + self._client = client + self._done = threading.Event() + self._thread = threading.Thread(target=self._run, daemon=True) + self.frames: list[np.ndarray] = [] + + def __enter__(self): + self._thread.start() + return self + + def __exit__(self, *exc): + self._done.set() + self._thread.join(timeout=2.0) + + def _run(self): + while not self._done.is_set(): + status = self._client.status() + if status is not None: + self.frames.append( + np.asarray(status.pose, dtype=np.float64).reshape(4, 4) + ) + time.sleep(0.02) + + def positions(self) -> np.ndarray: + pts = [f[:3, 3] for f in self.frames] + kept = [pts[0]] + for p in pts[1:]: + if np.linalg.norm(p - kept[-1]) > 1e-6: + kept.append(p) + return np.asarray(kept) + + +def _start(client) -> tuple[list[float], np.ndarray]: + """The current wire pose and its transform (mm).""" + pose = client.pose() + status = client.status() + assert pose is not None and status is not None + return list(pose), np.asarray(status.pose, dtype=np.float64).reshape(4, 4) + + +def _offset(pose: list[float], dx: float, dy: float, dz: float) -> list[float]: + return [pose[0] + dx, pose[1] + dy, pose[2] + dz, pose[3], pose[4], pose[5]] + + +def _max_turn_deg(pts: np.ndarray, min_step_mm: float) -> float: + steps = [d for d in np.diff(pts, axis=0) if np.linalg.norm(d) > min_step_mm] + worst = 0.0 + for a, b in zip(steps[:-1], steps[1:], strict=True): + cosang = float(np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))) + worst = max(worst, math.degrees(math.acos(max(-1.0, min(1.0, cosang))))) + return worst + + +@pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "TRAPEZOID"]) +def test_move_p_rounds_its_corner_and_holds_one_tool_speed( + client, server_proc, profile +): + """An L-shaped process move cuts its corner by a quarter of the shorter + leg, never stops in it, and cruises at one tool speed, under whichever + profile times it. The geometry is read off the arm; the speed off the + plan the arm plays row by row, which a status sample rate cannot + scatter.""" + from parol6.client.dry_run_client import DryRunRobotClient + + assert client.select_profile(profile) > 0 + pose, start = _start(client) + s = start[:3, 3] + corner = _offset(pose, 50.0, 0.0, 0.0) + end = _offset(pose, 50.0, 50.0, 0.0) + corner_xyz, end_xyz = np.array(corner[:3]), np.array(end[:3]) + radius = 0.25 * 50.0 + + preview = DryRunRobotClient(initial_joints_deg=client.angles()) + assert preview.select_profile(profile) == 1 + index = preview.move_p([corner, end], speed=SPEED) + record = preview.plan() + block = record.blocks[index] + assert block.error is None, block.error + planned = ( + np.asarray(record.tcp[block.start_row : block.start_row + block.rows, :3]) + * 1000.0 + ) + speeds = np.linalg.norm(np.diff(planned, axis=0), axis=1) / record.row_dt_s + + with _TcpSampler(client) as sampler: + assert client.move_p([corner, end], speed=SPEED, timeout=20.0) >= 0 + assert client.wait_motion(timeout=20.0) + pts = sampler.positions() + assert len(pts) > 10 + + assert np.linalg.norm(pts[-1] - end_xyz) < 0.5 + miss = float(np.min(np.linalg.norm(pts - corner_xyz, axis=1))) + print(f"\nmove_p corner miss {miss:.2f} mm (radius {radius:.1f})") + assert 1.0 < miss <= radius + 0.5, "the corner is rounded, within its radius" + + on_legs = [ + min( + _point_to_segment_mm(p, s, corner_xyz), + _point_to_segment_mm(p, corner_xyz, end_xyz), + ) + for p in pts + if np.linalg.norm(p - corner_xyz) > radius + 0.5 + ] + assert max(on_legs) < 0.5, "outside the corner zone the path is the polyline" + + away = (np.linalg.norm(planned[1:] - s, axis=1) > 12.0) & ( + np.linalg.norm(planned[1:] - end_xyz, axis=1) > 12.0 + ) + cruise = speeds[away] + print( + f"{profile} cruise {cruise.min():.1f}..{cruise.max():.1f} mm/s " + f"of {CRUISE_MM_S:.0f}" + ) + slow, fast = np.percentile(cruise, [10, 90]) + assert slow > 0.8 * fast, "one tool speed through the corner" + assert cruise.max() < 1.1 * CRUISE_MM_S + + +def test_move_s_never_reverses_along_unevenly_spaced_waypoints(client, server_proc): + """A spline through collinear, unevenly spaced waypoints runs the line + monotonically: no dip behind the start, no overshoot past the end.""" + assert client.select_profile("TOPPRA") > 0 + pose, start = _start(client) + s = start[:3, 3] + waypoints = [_offset(pose, 0.0, d, 0.0) for d in (10.0, 45.0, 60.0)] + end_xyz = np.array(waypoints[-1][:3]) + axis = (end_xyz - s) / np.linalg.norm(end_xyz - s) + + with _TcpSampler(client) as sampler: + assert client.move_s(waypoints, speed=SPEED, timeout=20.0) >= 0 + assert client.wait_motion(timeout=20.0) + pts = sampler.positions() + assert len(pts) > 10 + + along = (pts - s) @ axis + lateral = np.linalg.norm((pts - s) - np.outer(along, axis), axis=1) + print( + f"\nmove_s along {along.min():.2f}..{along.max():.2f} mm, lateral {lateral.max():.2f} mm" + ) + assert along.min() > -0.3, "the spline never dips behind its start" + assert along.max() < 60.0 + 0.3, "the spline never overshoots its end" + assert np.all(np.diff(along) > -0.3), "the tool never turns back along the line" + assert lateral.max() < 0.5 + # Between two status samples the tool covers a few millimetres: a + # waypoint is passed when the sampled polyline runs within 1 mm of it. + for wp in waypoints: + w = np.array(wp[:3]) + assert ( + min( + _point_to_segment_mm(w, a, b) + for a, b in zip(pts[:-1], pts[1:], strict=True) + ) + < 1.0 + ) + assert np.linalg.norm(pts[-1] - end_xyz) < 0.5 + + +@pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "QUINTIC", "TRAPEZOID"]) +def test_a_move_c_that_ends_where_it_starts_runs_the_whole_circle( + client, server_proc, profile +): + """An end equal to the start asks for a full circle, through the via + point opposite the start, under whichever profile times it. The arm + settles a hair off the pose it was sent to, so the start the arc is + planned from is never exactly the end the script wrote; an end read + back from the arm is exactly that start. Either way the circle is + whole, and takes about as long as its length at the speed asked.""" + from parol6.client.dry_run_client import DryRunRobotClient + + radius = 30.0 + centre = np.array([0.0, 340.0, 210.0]) + + def on_circle(angle_deg: float) -> list[float]: + a = math.radians(angle_deg) + return [radius * math.cos(a), 340.0, 210.0 + radius * math.sin(a), 90, 0, 90] + + assert client.select_profile(profile) > 0 + start, via = on_circle(0.0), on_circle(180.0) + assert client.move_j(pose=start, speed=0.5, wait=True, timeout=20.0) >= 0 + + for read_back in (False, True): + end = client.pose() if read_back else start + here = client.angles() + assert end is not None and here is not None + which = "read-back" if read_back else "written" + + preview = DryRunRobotClient(initial_joints_deg=here) + assert preview.select_profile(profile) == 1 + index = preview.move_c(via=via, end=end, speed=SPEED) + record = preview.plan() + planned = record.blocks[index].rows * record.row_dt_s + assert record.blocks[index].error is None, record.blocks[index].error + + with _TcpSampler(client) as sampler: + assert ( + client.move_c(via=via, end=end, speed=SPEED, wait=True, timeout=20.0) + >= 0 + ) + assert client.wait_motion(timeout=20.0) + pts = sampler.positions() + assert len(pts) > 10, f"{which} end: the arm did not run the circle" + + # Measured against the circle, not the chords between samples: a slow + # CI loop spaces the samples out, and a chord cuts inside the arc. + off = pts - centre + drift = np.abs(np.hypot(off[:, 0], off[:, 2]) - radius).max() + assert drift < 1.0, f"{which} end: the arm left the circle by {drift:.1f} mm" + assert np.abs(off[:, 1]).max() < 1.0 + # A whole turn about the centre passes the via point opposite the start. + angle = np.degrees(np.unwrap(np.arctan2(off[:, 2], off[:, 0]))) + turn = abs(angle[-1] - angle[0]) + assert abs(turn - 360.0) < 2.0, ( + f"{which} end: the arm turned {turn:.0f}° about the centre" + ) + assert np.linalg.norm(pts[-1] - np.asarray(start[:3])) < 0.5 + circle_s = 2.0 * math.pi * radius / CRUISE_MM_S + assert planned < 2.0 * circle_s, ( + f"{which} end: a {circle_s:.1f} s circle planned as {planned:.1f} s" + ) + + +def test_a_trf_move_l_runs_along_the_tool_axis(client, server_proc): + """A TRF pose is an offset in the tool frame at the start of the move.""" + pose, start = _start(client) + s = start[:3, 3] + expected = s + start[:3, :3] @ np.array([0.0, 0.0, -20.0]) + + with _TcpSampler(client) as sampler: + assert ( + client.move_l( + [0.0, 0.0, -20.0, 0.0, 0.0, 0.0], frame="TRF", speed=SPEED, timeout=20.0 + ) + >= 0 + ) + assert client.wait_motion(timeout=20.0) + pts = sampler.positions() + assert len(pts) > 3 + + _, final = _start(client) + print(f"\nTRF landed {final[:3, 3]} for {expected}") + assert np.linalg.norm(final[:3, 3] - expected) < 0.5 + assert _rotation_angle_deg(final[:3, :3], start[:3, :3]) < 0.5 + assert max(_point_to_segment_mm(p, s, expected) for p in pts) < 0.3 + + +def test_a_relative_wrf_rotation_turns_about_the_tcp(client, server_proc): + """``rel`` in WRF applies the rotation about the TCP: the tool turns + in place, and its position never leaves where it stood.""" + pose, start = _start(client) + s = start[:3, 3] + expected_r = _rotz(15.0) @ start[:3, :3] + + with _TcpSampler(client) as sampler: + assert ( + client.move_l( + [0.0, 0.0, 0.0, 0.0, 0.0, 15.0], + frame="WRF", + rel=True, + speed=SPEED, + timeout=20.0, + ) + >= 0 + ) + assert client.wait_motion(timeout=20.0) + + _, final = _start(client) + drift = max(float(np.linalg.norm(f[:3, 3] - s)) for f in sampler.frames) + print( + f"\nWRF rel rotation: position drift {drift:.3f} mm, orientation error {_rotation_angle_deg(final[:3, :3], expected_r):.3f} deg" + ) + assert drift < 0.5, "a pure rotation keeps the TCP where it is" + assert _rotation_angle_deg(final[:3, :3], expected_r) < 0.5 + assert _rotation_angle_deg(final[:3, :3], start[:3, :3]) > 14.0 + + +def test_a_move_l_with_a_radius_rounds_into_the_move_c_after_it(client, server_proc): + """``move_l(r)`` → ``move_c(r)`` → ``move_l``: one continuous path whose + corners are cut within ``r`` of their junctions, and whose arc lies on + its circle outside the blend zones.""" + assert client.select_profile("TOPPRA") > 0 + pose, start = _start(client) + s = start[:3, 3] + r = 12.0 + radius = 30.0 + a = _offset(pose, 40.0, 0.0, 0.0) + a_xyz = np.array(a[:3]) + centre = a_xyz + np.array([radius, 0.0, 0.0]) + via = _offset( + pose, 40.0 + radius * (1.0 - math.sqrt(0.5)), radius * math.sqrt(0.5), 0.0 + ) + b = _offset(pose, 40.0 + radius, radius, 0.0) + b_xyz = np.array(b[:3]) + end = _offset(pose, 40.0 + radius, radius + 30.0, 0.0) + end_xyz = np.array(end[:3]) + + with _TcpSampler(client) as sampler: + assert client.move_l(a, speed=SPEED, r=r, wait=False) >= 0 + assert client.move_c(via, b, speed=SPEED, r=r, wait=False) >= 0 + assert client.move_l(end, speed=SPEED, timeout=30.0) >= 0 + assert client.wait_motion(timeout=30.0) + pts = sampler.positions() + assert len(pts) > 20 + assert np.linalg.norm(pts[-1] - end_xyz) < 0.5 + + for junction in (a_xyz, b_xyz): + miss = float(np.min(np.linalg.norm(pts - junction, axis=1))) + print(f"\ncorner miss {miss:.2f} mm (r {r})") + assert 1.0 < miss <= r + 0.5 + + # The arc's quadrant, with a degree kept clear of the lines before + # and after it (they lie exactly on its bounding rays, and sampling + # noise puts them on either side). + rel = pts - centre + angle = np.degrees(np.arctan2(rel[:, 1], rel[:, 0])) + on_arc = ( + (np.linalg.norm(pts - a_xyz, axis=1) > r + 1.0) + & (np.linalg.norm(pts - b_xyz, axis=1) > r + 1.0) + & (angle > 91.0) + & (angle < 179.0) + ) + assert on_arc.sum() >= 3 + radial = np.abs(np.linalg.norm(rel[on_arc][:, :2], axis=1) - radius) + print(f"arc radial error {radial.max():.2f} mm over {on_arc.sum()} samples") + assert radial.max() < 0.5 + + body = (np.linalg.norm(pts - s, axis=1) > 8.0) & ( + np.linalg.norm(pts - end_xyz, axis=1) > 8.0 + ) + steps = np.linalg.norm(np.diff(pts[body], axis=0), axis=1) + assert steps.min() > 0.3, "the chain never comes to rest between its moves" + assert _max_turn_deg(pts, 0.5) < 30.0, "the path turns gradually, never at a corner" + + +def _off_geodesic_deg(r: np.ndarray, keys: list[np.ndarray]) -> float: + """How far ``r`` lies from the piecewise geodesic through ``keys``.""" + from scipy.spatial.transform import Rotation, Slerp + + t = np.linspace(0.0, 1.0, 401) + worst = math.inf + for a, b in zip(keys[:-1], keys[1:], strict=True): + arc = Slerp([0.0, 1.0], Rotation.from_matrix(np.stack([a, b])))(t) + rel = arc.inv() * Rotation.from_matrix(r) + worst = min(worst, float(np.degrees(rel.magnitude()).min())) + return worst + + +def test_move_s_turns_the_tool_along_the_geodesic_between_waypoints( + client, server_proc +): + """A spline's orientation turns from each waypoint's rotation to the + next along the shortest arc between them, the rotations read as the + wire names them (intrinsic XYZ).""" + from pinokin import se3_from_rpy + + assert client.select_profile("TOPPRA") > 0 + # Clear of the wrist singularity at standby, where any reorientation + # needs a wrist turn first. + assert client.teleport([90.0, -80.0, 190.0, 0.0, 30.0, 180.0]) == 1 + pose, start = _start(client) + waypoints = [ + [pose[0] + 20.0, pose[1], pose[2], pose[3] + 25.0, pose[4] + 20.0, pose[5]], + [ + pose[0] + 40.0, + pose[1] + 15.0, + pose[2], + pose[3] + 10.0, + pose[4] + 35.0, + pose[5] + 30.0, + ], + ] + keys = [start[:3, :3]] + for wp in waypoints: + se3 = np.zeros((4, 4)) + rx, ry, rz = np.radians(wp[3:]) + se3_from_rpy(0.0, 0.0, 0.0, rx, ry, rz, se3) + keys.append(se3[:3, :3].copy()) + + with _TcpSampler(client) as sampler: + assert client.move_s(waypoints, speed=SPEED, timeout=20.0) >= 0 + assert client.wait_motion(timeout=20.0) + assert len(sampler.frames) > 10 + worst = max(_off_geodesic_deg(f[:3, :3], keys) for f in sampler.frames) + print(f"\nmove_s orientation off the geodesic by up to {worst:.3f} deg") + assert worst < 0.5 + assert _rotation_angle_deg(sampler.frames[-1][:3, :3], keys[-1]) < 0.5 + + +def test_a_wrist_turn_is_collision_checked_all_the_way_round(client, server_proc): + """From standby a tool-frame reorientation first turns J4 a quarter turn + out of the wrist singularity. A keep-out the wrist sweeps through only + partway round that turn — clear of where it starts, where it ends and + of the path after it — refuses the move when it is planned: the arm + never stirs, and a dry run previews the same refusal.""" + from waldoctl import Sphere + + from parol6 import MotionError + from parol6.client.dry_run_client import DryRunRobotClient + + before = client.angles() + assert before is not None + keep_out = Sphere( + name="wrist-arc", radius=0.01, pose=(-0.0581, 0.2168, 0.2869, 0.0, 0.0, 0.0) + ) + try: + assert client.set_shapes([keep_out]) == 1 + with pytest.raises(MotionError, match="wrist-arc"): + client.move_l( + [0.0, 0.0, 0.0, -15.0, 0.0, 0.0], frame="TRF", speed=SPEED, timeout=20.0 + ) + finally: + assert client.set_shapes([]) == 1 + after = client.angles() + assert after is not None + assert np.allclose(after, before, atol=0.05), "the refused move moved the arm" + + preview = DryRunRobotClient(initial_joints_deg=before) + # A preview's shapes are the process's robot model's: cleared, or every + # later preview in this process plans around the keep-out too. + try: + assert preview.set_shapes([keep_out]) == 1 + index = preview.move_l( + [0.0, 0.0, 0.0, -15.0, 0.0, 0.0], frame="TRF", speed=SPEED + ) + refusal = preview.plan().blocks[index].error + finally: + assert preview.set_shapes([]) == 1 + assert refusal is not None and "wrist-arc" in str(refusal), ( + "the preview ran the turn the arm refuses" + ) + + +def test_a_tool_frame_turn_a_hair_off_the_wrist_singularity_runs_with_a_long_tool( + client, server_proc +): + """With the SSG-48 fitted the TCP stands 14 cm out from the wrist. A + hair off the singularity (J5 at 0.3°) a tool-frame turn about the + tool's x axis leaves it through a turn of the wrist, which swings that + long tool a fraction of a millimetre on the way: the move still plans, + runs and turns the tool in place, and a dry run previews it.""" + from parol6.client.dry_run_client import DryRunRobotClient + + near_singular = [90.0, -90.0, 180.0, 0.0, 0.3, 180.0] + turn = [0.0, 0.0, 0.0, 30.0, 0.0, 0.0] + fitted = client.select_tool("SSG-48") + assert fitted >= 0 and client.wait_command(fitted, timeout=10.0) + try: + assert client.teleport(near_singular) == 1 + _, start = _start(client) + assert client.move_l(turn, frame="TRF", speed=SPEED, timeout=20.0) >= 0 + _, final = _start(client) + finally: + bare = client.select_tool("NONE") + assert bare >= 0 and client.wait_command(bare, timeout=10.0) + assert np.linalg.norm(final[:3, 3] - start[:3, 3]) < 0.5 + assert _rotation_angle_deg(final[:3, :3], start[:3, :3] @ _rotx(30.0)) < 0.5 + + preview = DryRunRobotClient(initial_joints_deg=near_singular) + # The preview fits the tool to the process's robot model: taken off + # again, or every later preview in this process plans with it. + try: + assert preview.select_tool("SSG-48") >= 0 + index = preview.move_l(turn, frame="TRF", speed=SPEED) + error = preview.plan().blocks[index].error + finally: + preview.select_tool("NONE") + assert error is None, error + + +def test_a_line_the_wrist_follows_only_by_flipping_is_refused(client, server_proc): + """From J5 = +20° to the pose with J5 = -20° and J4 a turn of 20° round, + the straight line passes beside the wrist singularity: following it + runs J4 into its stop, and the only way on is to flip the whole wrist + halfway along. That move is refused when it is planned, the arm never + stirs, and a dry run previews the same refusal.""" + from parol6 import MotionError + from parol6.client.dry_run_client import DryRunRobotClient + from parol6.utils.error_codes import ErrorCode + + assert client.teleport([90.0, -90.0, 180.0, 20.0, -20.0, 180.0]) == 1 + target = client.pose() + assert target is not None + start = [90.0, -90.0, 180.0, 0.0, 20.0, 180.0] + assert client.teleport(start) == 1 + before = client.angles() + assert before is not None + + with pytest.raises(MotionError) as refused: + client.move_l(target, speed=SPEED, timeout=20.0) + assert refused.value.code == ErrorCode.IK_PARTIAL_PATH, refused.value + after = client.angles() + assert after is not None + assert np.allclose(after, before, atol=0.05), "the refused move moved the arm" + + preview = DryRunRobotClient(initial_joints_deg=start) + index = preview.move_l(target, speed=SPEED) + error = preview.plan().blocks[index].error + assert error is not None and error.code == ErrorCode.IK_PARTIAL_PATH, error + + +@pytest.mark.parametrize( + "turns", + [ + pytest.param([(0.0, 30.0), (20.0, 30.0)], id="first"), + pytest.param([(20.0, 0.0), (20.0, 30.0), (40.0, 30.0)], id="inside"), + ], +) +def test_move_s_turns_the_tool_at_a_waypoint_that_only_turns_it( + client, server_proc, turns +): + """A waypoint at the position of the one before it, with the tool + turned about x, is a waypoint like any other, whether it opens the + list or sits inside it: the spline runs through it with the tool + turned there, rather than refusing the move or turning the tool on + the way to the next waypoint.""" + assert client.select_profile("TOPPRA") > 0 + assert client.teleport([90.0, -80.0, 190.0, 0.0, 30.0, 180.0]) == 1 + pose, start = _start(client) + waypoints = [ + [pose[0] + dx, pose[1], pose[2], pose[3] + drx, pose[4], pose[5]] + for dx, drx in turns + ] + keys = [_wire_rotation(wp) for wp in waypoints] + + with _TcpSampler(client) as sampler: + assert client.move_s(waypoints, speed=SPEED, timeout=20.0) >= 0 + assert client.wait_motion(timeout=20.0) + frames = sampler.frames + assert len(frames) > 10 + + # Within 3 mm and 3° of each waypoint: a status sample lands a few + # milliseconds either side of the instant the tool passes it. + for wp, key in zip(waypoints, keys, strict=True): + miss = min( + max( + float(np.linalg.norm(f[:3, 3] - wp[:3])) / 3.0, + _rotation_angle_deg(f[:3, :3], key) / 3.0, + ) + for f in frames + ) + assert miss < 1.0, f"the tool never stood at {np.round(wp, 1).tolist()}" + worst = max(_off_geodesic_deg(f[:3, :3], [start[:3, :3], *keys]) for f in frames) + assert worst < 0.5, f"the tool left the turn between the waypoints by {worst:.1f}°" + lateral = max(float(np.linalg.norm(f[1:3, 3] - start[1:3, 3])) for f in frames) + assert lateral < 0.5 + assert np.linalg.norm(frames[-1][:3, 3] - waypoints[-1][:3]) < 0.5 + assert _rotation_angle_deg(frames[-1][:3, :3], keys[-1]) < 0.5 + + +def test_move_p_runs_a_long_straight_run_past_the_base(client, server_proc): + """A process move along a 300 mm line that passes 40 mm from the J1 + axis swings J1 through 150° as the line goes by; the move runs, and + stays on the line, as a move_l along the same line does.""" + assert client.teleport([-74.98, -97.76, 189.08, -1.51, 73.55, 105.45]) == 1 + pose, start = _start(client) + s = start[:3, 3] + end = _offset(pose, 0.0, 300.0, 0.0) + end_xyz = np.array(end[:3]) + + with _TcpSampler(client) as sampler: + assert client.move_p([pose, end], speed=0.5, timeout=30.0) >= 0 + assert client.wait_motion(timeout=30.0) + pts = sampler.positions() + assert len(pts) > 10 + + off = max(_point_to_segment_mm(p, s, end_xyz) for p in pts) + assert off < 0.5, f"the tool left the line by {off:.1f} mm" + assert np.linalg.norm(pts[-1] - end_xyz) < 0.5 + + +@pytest.mark.parametrize("chain", ["move_p", "move_l"]) +def test_a_leg_that_turns_the_tool_still_runs_straight(client, server_proc, chain): + """A straight leg is a straight line whatever the tool does along it: + a leg that turns the tool a quarter turn about its own axis runs the + line between its ends, as a process move and as a blended move_l, and + rounds the corner into the next leg no more sharply than the same legs + do with the tool held still.""" + from parol6.client.dry_run_client import DryRunRobotClient + + s = [-50.0, 250.0, 200.0, 90.0, 0.0, 90.0] + corner = [50.0, 250.0, 200.0, 90.0, 0.0, 180.0] + end = [50.0, 250.0, 300.0, 90.0, 0.0, 180.0] + # A process move rounds its corner by a quarter of the shorter leg. + zone = 0.25 * 100.0 if chain == "move_p" else 5.0 + + def run(robot, corner: list[float], end: list[float], **wait) -> int: + if chain == "move_p": + return robot.move_p([corner, end], speed=SPEED, **wait) + assert robot.move_l(corner, speed=SPEED, r=zone, wait=False) >= 0 + return robot.move_l(end, speed=SPEED, **wait) + + assert client.select_profile("TOPPRA") > 0 + assert client.move_j(pose=s, speed=0.5, timeout=20.0) >= 0 + q0 = client.angles() + assert q0 is not None + + with _TcpSampler(client) as sampler: + assert run(client, corner, end, timeout=30.0) >= 0 + assert client.wait_motion(timeout=30.0) + pts = sampler.positions() + assert len(pts) > 20 + + a, c, e = (np.array(p[:3]) for p in (s, corner, end)) + off = max( + min(_point_to_segment_mm(p, a, c), _point_to_segment_mm(p, c, e)) + for p in pts + if np.linalg.norm(p - c) > zone + 0.5 + ) + assert off < 0.5, f"{chain}: the turning leg bowed {off:.1f} mm off its line" + assert np.linalg.norm(pts[-1] - e) < 0.5 + + def sharpest_turn(turned: bool) -> float: + """The largest change of direction between two planned rows.""" + preview = DryRunRobotClient(initial_joints_deg=q0) + assert preview.select_profile("TOPPRA") == 1 + if turned: + run(preview, corner, end) + else: + run(preview, [*corner[:3], *s[3:]], [*end[:3], *s[3:]]) + record = preview.plan() + assert all(block.error is None for block in record.blocks) + rows = np.asarray(record.tcp[:, :3], dtype=np.float64) * 1000.0 + return _max_turn_deg(rows, 0.05) + + turned, held = sharpest_turn(True), sharpest_turn(False) + assert turned < held + 5.0, ( + f"{chain}: the path kinks {turned:.0f}° between two rows turning the " + f"tool, {held:.0f}° holding it" + ) + + +def test_move_s_timed_too_short_still_runs_through_its_waypoints(client, server_proc): + """A duration no arm could keep is stretched to one it can, never met by + cutting the path: a spline up, across and back down, timed to 20 ms, + still passes every waypoint.""" + assert client.select_profile("TOPPRA") > 0 + tool = [180.0, -80.0, 180.0] + waypoints = [ + [250.0, 0.0, 150.0, *tool], + [250.0, 0.0, 230.0, *tool], + [290.0, 0.0, 230.0, *tool], + [290.0, 0.0, 150.0, *tool], + ] + assert client.move_j(pose=waypoints[0], speed=0.5, timeout=20.0) >= 0 + + with _TcpSampler(client) as sampler: + assert client.move_s(waypoints, duration=0.02, timeout=20.0) >= 0 + assert client.wait_motion(timeout=20.0) + pts = sampler.positions() + + for wp in waypoints: + miss = _polyline_miss_mm(np.array(wp[:3]), pts) + assert miss < 2.0, f"the spline passed {miss:.0f} mm from {wp[:3]}" diff --git a/tests/integration/test_planning_regressions.py b/tests/integration/test_planning_regressions.py new file mode 100644 index 0000000..b691c32 --- /dev/null +++ b/tests/integration/test_planning_regressions.py @@ -0,0 +1,84 @@ +"""Plans at the edges of what a host sends: a pose read back from an arm +parked on a joint limit, commanded again, and a dry run of a blended chain +whose last move cannot be reached.""" + +import numpy as np +import pytest + +pytestmark = pytest.mark.integration + + +def test_a_pose_read_back_on_a_joint_limit_can_be_commanded_again(client, server_proc): + """A host replays what it read: the arm's own angles, a motor step's + rounding of where it was sent, or a preview's rows, carried as + float32. Parked on a joint limit, either lands a hair past the limit's + decimal; the arm is there, so teleporting or servoing to it is + accepted.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + from parol6.client.dry_run_client import DryRunRobotClient + from parol6.config import LIMITS + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + limits = LIMITS.joint.position.deg + for joint, side in ((1, 0), (2, 1), (4, 0), (4, 1), (5, 1)): + parked = list(standby) + parked[joint] = float(limits[joint, side]) + assert client.teleport(parked) == 1 + read_back = client.angles() + assert read_back is not None + assert client.teleport(read_back) == 1, f"J{joint + 1} at {read_back[joint]}" + assert client.servo_j(read_back) > 0, f"J{joint + 1} at {read_back[joint]}" + + on_limit = list(standby) + on_limit[4] = float(limits[4, 1]) + preview = DryRunRobotClient(initial_joints_deg=standby) + index = preview.move_j(on_limit, speed=0.5) + # The hold after the move carries the pose the arm stands at, whichever + # tick the move's last row falls on. + preview.delay(0.1) + record = preview.plan() + assert record.blocks[index].error is None + row = np.degrees(np.asarray(record.joints_rad[-1], dtype=np.float64)).tolist() + assert client.teleport(row) == 1, f"J5 at {row[4]}" + assert client.servo_j(row) > 0, f"J5 at {row[4]}" + angles = client.angles() + assert angles is not None + assert abs(angles[4] - on_limit[4]) < 0.01 + + +def test_a_failing_blended_chain_previews_its_moves_from_where_they_start(): + """A dry run of a blended chain whose last move is out of reach still + draws the moves ahead of it where they run: the first before the + second, and the second, a relative move, from where the first ends + rather than from where the chain began.""" + from parol6.client.dry_run_client import DryRunRobotClient + + r = 10.0 + preview = DryRunRobotClient( + initial_joints_deg=[90.0, -80.0, 190.0, 0.0, 30.0, 180.0] + ) + s = np.asarray(preview.pose()[:3]) + first = preview.move_l([0.0, 0.0, -20.0, 0.0, 0.0, 0.0], rel=True, r=r, speed=0.5) + second = preview.move_l([0.0, 30.0, 0.0, 0.0, 0.0, 0.0], rel=True, r=r, speed=0.5) + unreachable = preview.move_l([0.0, 0.0, 2000.0, 0.0, 0.0, 0.0], rel=True, speed=0.5) + record = preview.plan() + assert record.blocks[unreachable].error is not None + + a, b = record.blocks[first], record.blocks[second] + drawn = ( + np.asarray(record.tcp[a.start_row : b.start_row + b.rows, :3], dtype=np.float64) + * 1000.0 + ) + assert len(drawn) > 0, "the preview drew neither move ahead of the failure" + first_end = s + np.array([0.0, 0.0, -20.0]) + second_end = first_end + np.array([0.0, 30.0, 0.0]) + to_first = np.linalg.norm(drawn - first_end, axis=1) + to_second = np.linalg.norm(drawn - second_end, axis=1) + # A blend zone rounds a junction by up to its radius. + assert to_first.min() < r + 0.5, ( + f"the preview passed {to_first.min():.0f} mm from the first move's end" + ) + assert to_second.min() < r + 0.5, ( + f"the preview passed {to_second.min():.0f} mm from the second move's end" + ) + assert np.argmin(to_first) < np.argmin(to_second) diff --git a/tests/integration/test_profile_commands.py b/tests/integration/test_profile_commands.py index f2f1c0e..9b1a8af 100644 --- a/tests/integration/test_profile_commands.py +++ b/tests/integration/test_profile_commands.py @@ -115,6 +115,87 @@ def test_cartesian_move_reaches_target_all_profiles(self, client, server_proc): f"(expected {target_pose[0]:.1f}, got {pose[0]:.1f})" ) + def test_a_dry_run_times_a_move_as_the_selected_profile_runs_it( + self, client, server_proc + ): + """A preview is only worth its rows if it is timed as the arm will run + the move: after ``select_profile`` the dry run plans with that profile, + as the controller does.""" + from parol6.client.dry_run_client import DryRunRobotClient + + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + target = [40.0, -60.0, 200.0, 30.0, 20.0, 150.0] + + def previewed(profile: str) -> float: + preview = DryRunRobotClient(initial_joints_deg=standby) + assert preview.select_profile(profile) == 1 + index = preview.move_j(target, speed=0.5, accel=1.0) + record = preview.plan() + assert record.blocks[index].error is None + return record.blocks[index].rows * record.row_dt_s + + quintic = previewed("QUINTIC") + assert quintic > previewed("TOPPRA") + 0.3, ( + "the preview timed the move under QUINTIC as it does under TOPPRA" + ) + + assert client.select_profile("QUINTIC") > 0 + assert client.teleport(standby) == 1 + planned: list[float] = [] + + def holds_the_move(status) -> bool: + if status.queued_segments != 1: + return False + planned.append(float(status.queued_duration)) + return True + + # Paused, the controller plans the move and holds it, and the status + # stream carries the duration it planned, whatever rate the loop + # then plays it at. + try: + assert client.pause() == 1 + assert client.move_j(target, speed=0.5, accel=1.0, wait=False) >= 0 + assert client.wait_status(holds_the_move, timeout=10.0) + finally: + assert client.stop() == 1 + assert abs(planned[-1] - quintic) < 0.02, ( + f"previewed {quintic:.2f} s under QUINTIC, the controller planned " + f"{planned[-1]:.2f} s" + ) + + @pytest.mark.parametrize("profile", ["LINEAR", "QUINTIC", "TRAPEZOID"]) + def test_a_short_timed_move_takes_its_whole_duration(self, profile): + """A move timed to 50 ms reaches its target 50 ms in, not a control + tick early. The preview plays one row per control tick, as the + controller does, so a plan that spreads the time it names over one + tick too few shows there: on a move this short, a tick early is + half again the acceleration planned.""" + from parol6.client.dry_run_client import DryRunRobotClient + + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + target = [standby[0] + 0.2, *standby[1:]] + duration = 0.05 + preview = DryRunRobotClient(initial_joints_deg=standby) + assert preview.select_profile(profile) == 1 + # Small and fast enough that every profile keeps the duration asked. + index = preview.move_j(target, duration=duration, accel=1.0) + preview.delay(0.1) + record = preview.plan() + block = record.blocks[index] + assert block.error is None and block.start_row == 0 + j1 = np.degrees(np.asarray(record.joints_rad[:, 0], dtype=np.float64)) + # The arm holds the target to half a motor step of J1 (0.0044°); a + # tick before it ends, the move is still over 0.01° short of it. + arrived = int(np.argmax(np.abs(j1 - target[0]) < 0.005)) * record.row_dt_s + assert arrived >= duration - 1e-9, ( + f"{profile}: a {duration * 1000:.0f} ms move arrived " + f"{arrived * 1000:.0f} ms in" + ) + @pytest.mark.integration class TestServoCartesian: diff --git a/tests/integration/test_queue_readback.py b/tests/integration/test_queue_readback.py index d44b776..183aaa0 100644 --- a/tests/integration/test_queue_readback.py +++ b/tests/integration/test_queue_readback.py @@ -43,7 +43,7 @@ def test_the_queue_lists_what_is_owed_and_a_stop_clears_it(client: RobotClient): ) listed = client.queue() assert listed and all(name for name in listed), listed - assert any("MoveJ" in name for name in listed) + assert all(name == "move_j" for name in listed), listed assert np.allclose(client.angles(), start, atol=0.05) # Resuming drains it: what the queue reports is what is still owed. @@ -91,14 +91,20 @@ def test_the_queue_lists_what_is_owed_and_a_stop_clears_it(client: RobotClient): ) assert client.wait_command(trailing, timeout=25) - # Stop clears what was owed, and the readback says so immediately. + # Stop clears what was owed, and the readback says so immediately. A + # joint move to a pose is listed by the method that sent it, as + # every command is. assert client.pause() == 1 + here = client.pose() + assert here is not None client.move_j(first, duration=2, wait=False) client.move_j(second, duration=2, wait=False) + client.move_j(pose=here, duration=2, wait=False) _wait( - lambda: len(client.queue() or []) >= 2, + lambda: len(client.queue() or []) >= 3, "the paused queue never listed the commands a Stop must clear", ) + assert client.queue() == ["move_j", "move_j", "move_j"], client.queue() assert client.stop() == 1 _wait(lambda: client.queue() == [], "Stop left work in the queue") assert not client.execution_speed().paused, ( diff --git a/tests/integration/test_reset_semantics.py b/tests/integration/test_reset_semantics.py index f1356cf..930db1e 100644 --- a/tests/integration/test_reset_semantics.py +++ b/tests/integration/test_reset_semantics.py @@ -12,7 +12,8 @@ import pytest -from parol6 import RobotClient +from parol6 import MotionError, RobotClient +from parol6.utils.error_codes import ErrorCode from waldoctl import StatusBuffer pytestmark = pytest.mark.integration @@ -42,3 +43,44 @@ def _capture(s: StatusBuffer) -> bool: "wait_command satisfied by a stale pre-reset status frame" ) assert client.wait_command(idx, timeout=5.0), "delay never completed" + + +def test_reset_state_keeps_the_protective_stop_outputs_and_homed( + client: RobotClient, server_proc +): + """reset_state restores the program-level state — profile, speed, pause, + queues, tool, shapes — and nothing physical: it does not clear a + protective stop (only reset() does), un-home the arm, or change the + digital outputs.""" + start = client.angles() + assert start is not None + away = list(start) + away[0] += 10.0 + + output = client.write_io(0, 1) + assert output >= 0 and client.wait_command(output, timeout=5.0) + assert client.select_profile("RUCKIG") == 1 + assert client.set_execution_speed(0.5) == 1 + try: + assert client.estop() == 1 + assert client.wait_status(lambda s: not s.enabled, timeout=2.0) + assert client.reset_state() == 1 + + with pytest.raises(MotionError) as refused: + client.move_j(away, speed=0.5) + assert refused.value.robot_error.code == ErrorCode.SYS_CONTROLLER_DISABLED + assert client.wait_status(lambda s: s.homed and not s.enabled, timeout=2.0) + io = client.io() + assert io is not None and io[2] == 1, f"reset_state changed the outputs: {io}" + assert client.profile() == "TOPPRA" + assert client.execution_speed().target_scale == 1.0 + + assert client.reset() == 1 + assert client.wait_status(lambda s: s.enabled and s.homed, timeout=2.0) + moved = client.move_j(away, speed=0.5) + assert moved >= 0 and client.wait_command(moved, timeout=10.0) + finally: + client.reset() + # Waited on: the next test's reset_state would discard it queued, + # and a reset leaves the output where it is. + client.wait_command(client.write_io(0, 0), timeout=5.0) diff --git a/tests/integration/test_status_rate.py b/tests/integration/test_status_rate.py index 39abce5..fbcfa04 100644 --- a/tests/integration/test_status_rate.py +++ b/tests/integration/test_status_rate.py @@ -9,12 +9,14 @@ import asyncio import time +from types import SimpleNamespace import pytest from parol6 import AsyncRobotClient +from parol6.server import status_cache from parol6.server.state import ControllerState -from parol6.server.status_cache import StatusCache +from parol6.server.status_cache import _TCP_SPEED_WINDOW, StatusCache from parol6.utils.error_codes import ErrorCode from parol6.utils.errors import MotionError @@ -144,44 +146,107 @@ async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports) @pytest.mark.integration -def test_the_speed_derivative_follows_the_rate_it_was_sampled_at(): - """TCP speed is a difference over the broadcast period, so the period the - cache divides by has to be the one the controller is actually broadcasting - at — and the sample that straddles a rate change spans the period it was - taken at, not the one that has just replaced it. Get either wrong and a - steady arm appears to change speed the moment somebody changes the rate. +def test_the_speed_derivative_follows_the_frames_it_was_sampled_from(monkeypatch): + """TCP speed is displacement over the time between the serial frames the + samples came from: not over the broadcast period, which a query + refreshing the cache between broadcasts would halve, and not over the + refresh interval, which the loop's jitter would scatter. Only J1 moves, so equal step increments are equal chords of one circle - about the base axis: the displacement is the same every sample, and any - change in the reported speed is the period alone. + about the base axis: the displacement is the same every frame, and any + change in the reported speed is timing alone. """ + clock = [100.0] + monkeypatch.setattr( + status_cache, + "time", + SimpleNamespace( + monotonic=lambda: clock[0], + perf_counter=lambda: clock[0], + time=time.time, + sleep=time.sleep, + ), + ) cache = StatusCache() try: state = ControllerState() - def advance() -> float: - state.Position_in[0] += 200 + def frame(dt: float, steps: int = 200) -> float: + clock[0] += dt + state.Position_in[0] += steps + cache.mark_serial_observed() cache.update_from_state(state) return cache.tcp_speed - advance() # first difference has nothing to difference against - started = advance() + for _ in range(2 * _TCP_SPEED_WINDOW): + started = frame(0.01) assert started > 0.0, "a moving arm has to report a speed" - # Halve whatever rate this environment configured, rather than - # assuming the 50 Hz default: the cache reads its period from the - # state, so a shell with PAROL6_STATUS_RATE_HZ set would otherwise - # fail the ratio for reasons that have nothing to do with the code. - state.status_rate_hz = state.status_rate_hz / 2 - straddling = advance() - settled = advance() + # A refresh between frames (a query) sees the frame the broadcast + # saw: no new information, and the speed stands. + cache.update_from_state(state) + assert cache.tcp_speed == started - assert straddling == pytest.approx(started, rel=1e-3), ( - "the sample taken before the rate changed spans the old period" - ) + # Frames at half the rate carry the same chord over twice the + # time: half the speed, once the window has turned over. + for _ in range(2 * _TCP_SPEED_WINDOW): + settled = frame(0.02) assert settled == pytest.approx(started / 2, rel=1e-3), ( - "half the broadcast rate is twice the period, so the same " - f"movement per frame is half the speed: {settled} vs {started}" + f"the same movement per frame over twice the time is half the speed: " + f"{settled} vs {started}" + ) + + # A window of frames that leave the arm where it is: at rest. + for _ in range(_TCP_SPEED_WINDOW): + frame(0.02, steps=0) + assert cache.tcp_speed == 0.0 + + # At a 5 Hz broadcast the cache is refreshed every twentieth frame. + # The first refresh after the arm stops finds its last movement a + # fifth of a second old: at rest, not a window of refreshes later. + for _ in range(3): + moving = frame(0.2, steps=4000) + assert moving > 0.0, "a moving arm has to report a speed at 5 Hz too" + assert frame(0.2, steps=0) == 0.0, ( + "the speed outlived the motion by a window of 5 Hz refreshes" + ) + finally: + monkeypatch.undo() + cache.close() + + +def test_tcp_speed_is_steady_on_a_clock_that_ticks_every_15_ms(monkeypatch): + """Windows before Python 3.13 advances ``time.monotonic`` in 15.6 ms + ticks. Frames 10 ms apart carrying equal chords read one speed, not + one scattered by a fifth as the window's ends fall on either side of + a tick: the samples are stamped with the fine clock.""" + clock = [100.0] + tick_s = 0.0156 + monkeypatch.setattr( + status_cache, + "time", + SimpleNamespace( + monotonic=lambda: (clock[0] // tick_s) * tick_s, + perf_counter=lambda: clock[0], + time=time.time, + sleep=time.sleep, + ), + ) + cache = StatusCache() + try: + state = ControllerState() + readings: list[float] = [] + for frame_no in range(4 * _TCP_SPEED_WINDOW): + clock[0] += 0.01 + state.Position_in[0] += 200 + cache.mark_serial_observed() + cache.update_from_state(state) + if frame_no > _TCP_SPEED_WINDOW: + readings.append(cache.tcp_speed) + assert readings[0] > 0.0 + assert max(readings) == pytest.approx(min(readings), rel=1e-3), ( + f"the same chord per frame read {min(readings):.1f}..{max(readings):.1f} mm/s" ) finally: + monkeypatch.undo() cache.close() diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py index b73bece..33cba24 100644 --- a/tests/integration/test_stop_semantics.py +++ b/tests/integration/test_stop_semantics.py @@ -16,6 +16,7 @@ from parol6 import MotionError, RobotClient from parol6.protocol.wire import SelectToolCmd, encode_command +from parol6.utils.error_codes import ErrorCode pytestmark = pytest.mark.integration @@ -172,3 +173,56 @@ def test_stop_discards_a_queued_tcp_transform_from_the_planner_too( assert np.allclose(after, start, atol=0.5), ( f"the planner kept the cancelled TCP: {start} -> {after}" ) + + +def test_a_stop_fails_every_discarded_command_with_motn_cancelled( + client: RobotClient, server_proc +): + """Every command a stop discards — the one playing, the ones queued behind + it — completes as a failure with MOTN_CANCELLED, so a wait on any of + them raises at once instead of running out its timeout.""" + away = [45.0, -60.0, 150.0, 0.0, 30.0, 90.0] + queued = [90.0, -45.0, 120.0, 10.0, 20.0, 90.0] + start = client.angles() + assert start is not None + + first = client.move_j(away, duration=4.0, wait=False) + second = client.move_j(queued, duration=2.0, wait=False) + third = client.delay(0.5) + assert min(first, second, third) >= 0 + _wait_until_moving(client, start) + + assert client.stop() == 1 + for index in (first, second, third): + with pytest.raises(MotionError) as cancelled: + client.wait_command(index, timeout=0.5) + assert cancelled.value.robot_error.code == ErrorCode.MOTN_CANCELLED + assert cancelled.value.command_index == index + _assert_frozen(client, away) + assert client.home(wait=True, timeout=30.0) >= 0 + + +def test_a_stream_that_preempts_planned_motion_fails_what_it_discarded( + client: RobotClient, server_proc +): + """A jog sent while planned moves play takes the arm: the move playing + and the ones queued behind it are discarded, each failed as cancelled, + so a wait on any of them raises at once instead of running out its + timeout.""" + away = [45.0, -60.0, 150.0, 0.0, 30.0, 90.0] + queued = [90.0, -45.0, 120.0, 10.0, 20.0, 90.0] + start = client.angles() + assert start is not None + + first = client.move_j(away, duration=4.0, wait=False) + second = client.move_j(queued, duration=2.0, wait=False) + assert min(first, second) >= 0 + _wait_until_moving(client, start) + + assert client.jog_j(0, 0.2, duration=0.2) == 1 + for index in (first, second): + with pytest.raises(MotionError) as cancelled: + client.wait_command(index, timeout=0.5) + assert cancelled.value.robot_error.code == ErrorCode.MOTN_CANCELLED + assert cancelled.value.command_index == index + assert client.home(wait=True, timeout=30.0) >= 0 diff --git a/tests/integration/test_stream_gates.py b/tests/integration/test_stream_gates.py new file mode 100644 index 0000000..c0dcd37 --- /dev/null +++ b/tests/integration/test_stream_gates.py @@ -0,0 +1,346 @@ +"""What a jog or servo stream may do, driven through the controller's UDP +socket and its loop against the fake serial: an unhomed arm takes only +``jog_j``; a ``jog_j`` into a joint limit ramps that joint to rest short of +it while the others carry on; a servo stream runs at the speed it asked for +and brakes to a hold when its client goes silent.""" + +import math +import socket +import time + +import numpy as np +import pytest + +from parol6.config import HOME_ANGLES_DEG, INTERVAL_S, LIMITS, steps_to_rad +from parol6.protocol.wire import ( + JogJCmd, + JogLCmd, + OkMsg, + ServoJCmd, + ServoLCmd, + SetShapesCmd, + ShapeWire, +) +from parol6.server.state import get_fkine_se3 +from parol6.utils.error_codes import ErrorCode +from pinokin import se3_rpy +from tests.integration.controller_loop import ( + VirtualClock, + drain, + push, + ready, + send, + tick, + tick_until, +) +from waldoctl import Box + +pytestmark = pytest.mark.integration + + +def _q_rad(state) -> np.ndarray: + out = np.zeros(6, dtype=np.float64) + steps_to_rad(state.Position_in, out) + return out + + +def test_an_unhomed_arm_takes_a_joint_jog_and_nothing_else(controller): + state = controller.state_manager.get_state() + ready(controller, state, homed=False) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for cmd in ( + JogLCmd(velocities=[0.3, 0.0, 0.0, 0.0, 0.0, 0.0], duration=1.0), + ServoJCmd(angles=[10.0, -90.0, 180.0, 0.0, 0.0, 180.0]), + ServoLCmd(pose=[200.0, 0.0, 200.0, 180.0, 0.0, 180.0]), + ): + before = state.Position_in.copy() + push(controller, sock, cmd) + tick_until( + controller, + state, + lambda: state.error is not None, + f"{type(cmd).__name__} was not refused unhomed", + ) + assert state.error is not None + assert state.error.code == int(ErrorCode.MOTN_NOT_HOMED), state.error + for _ in range(20): + tick(controller, state) + assert np.array_equal(state.Position_in, before), ( + "the refused stream moved the arm" + ) + state.error = None + + +def test_a_joint_jog_stops_short_of_the_limit_one_joint_at_a_time(controller): + state = controller.state_manager.get_state() + ready(controller, state, homed=True) + lo, hi = LIMITS.joint.position.rad[:, 0], LIMITS.joint.position.rad[:, 1] + start = _q_rad(state) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + # J1 and J6 together, J1 towards its limit; the jog outlives the + # travel to it. (The timer is wall-clock; the loop runs faster.) + push( + controller, + sock, + JogJCmd(speeds=[1.0, 0.0, 0.0, 0.0, 0.0, 0.3], duration=30.0), + ) + deadline = time.monotonic() + 20.0 + still = 0 + last = start.copy() + while time.monotonic() < deadline: + tick(controller, state) + q = _q_rad(state) + if np.allclose(q, last, atol=1e-6): + still += 1 + if still >= 20 and q[0] > start[0] + 0.1: + break + else: + still = 0 + last = q + else: + pytest.fail("J1 never came to rest against its limit") + q = _q_rad(state) + assert q[0] < hi[0], f"J1 ran into its limit: {q[0]:.4f} >= {hi[0]:.4f}" + assert q[0] > hi[0] - 0.1, ( + f"J1 stopped {math.degrees(hi[0] - q[0]):.1f}° short of its limit" + ) + # J6 was still being driven while J1 ramped down, and came to rest + # only against ITS limit: a jog that stopped whole with J1 would + # leave it far short. + assert hi[5] - 0.1 < q[5] < hi[5], ( + f"J6 stopped {math.degrees(hi[5] - q[5]):.1f}° short of its own " + "limit: it stopped with J1 instead of carrying on" + ) + assert q[5] > lo[5] + + +def test_a_joint_joining_a_streamed_jog_does_not_carry_another_past_its_limit( + controller, +): + """A jog streamed as the UI streams it — one datagram every other tick — + with J6 joining just as J1 nears its limit: J1's brake is its own, not + stretched to finish with J6's ramp, and each datagram continues the + motion the lookahead has measured instead of restarting it from rest.""" + state = controller.state_manager.get_state() + ready(controller, state, homed=True) + hi = LIMITS.joint.position.rad[:, 1] + peak = _q_rad(state)[0] + joined = False + still = 0 + last = _q_rad(state) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for i in range(3000): + if i % 2 == 0: + speeds = [0.3, 0.0, 0.0, 0.0, 0.0, 1.0 if joined else 0.0] + push(controller, sock, JogJCmd(speeds=speeds, duration=0.5)) + tick(controller, state) + q = _q_rad(state) + peak = max(peak, q[0]) + if not joined and q[0] >= hi[0] - 0.09: + joined = True + if joined and abs(q[0] - last[0]) < 1e-7: + still += 1 + if still >= 50: + break + else: + still = 0 + last = q + else: + pytest.fail("J1 never came to rest against its limit") + assert joined, "J1 never approached its limit" + assert peak < hi[0], ( + f"J1 ran into its limit when J6 joined: peak {peak:.4f} >= {hi[0]:.4f}" + ) + + +def test_a_servo_stream_runs_at_its_speed_and_holds_when_its_client_goes_silent( + controller, monkeypatch +): + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=list(HOME_ANGLES_DEG)) + # The grace is timed on the clock the commands read: ticks, here. + clock = VirtualClock(monkeypatch) + start = _q_rad(state) + target = np.degrees(start).tolist() + target[0] += 20.0 + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + # One datagram, then silence: the stream runs at 30% and brakes + # after the grace instead of running on to a 20° target. + push(controller, sock, ServoJCmd(angles=target, speed=0.3)) + peak = 0.0 + prev = start.copy() + for _ in range(round(3.0 / INTERVAL_S)): + clock.tick(controller, state) + q = _q_rad(state) + peak = max(peak, abs(q[0] - prev[0]) / INTERVAL_S) + prev = q + assert peak <= LIMITS.joint.hard.velocity[0] * 0.3 * 1.05, ( + f"J1 ran at {peak:.3f} rad/s against a 30% ceiling of " + f"{LIMITS.joint.hard.velocity[0] * 0.3:.3f} rad/s" + ) + assert peak > 0.01, "the servo stream never moved the arm" + end = _q_rad(state) + assert start[0] + 0.02 < end[0] < math.radians(target[0]) - 0.02, ( + f"the silent stream did not brake short of its target: {math.degrees(end[0]):.2f}° " + f"of {target[0]:.2f}°" + ) + assert controller._executor.active_command is None, ( + "the stream did not end at the hold" + ) + + +def test_a_jog_l_braked_short_of_a_keep_out_ends_in_error(controller): + """A cartesian jog heading into a keep-out brakes to rest short of it and + ends FAILED, with the collision latched as the error ``error()`` reads, + like a refused planned move: it neither runs on nor ends silently.""" + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=[0.0, -90.0, 180.0, 0.0, 0.0, 180.0]) + # A slab 10 cm above the wrist, straight up the jog's path. + slab = Box(name="slab", x=0.10, y=0.10, z=0.04, pose=(0.237, 0.0, 0.43, 0, 0, 0)) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + reply = send( + controller, + state, + sock, + SetShapesCmd(shapes=[ShapeWire(*slab.to_wire())]), + 1, + ) + assert isinstance(reply, OkMsg), reply + up = JogLCmd(velocities=[0.0, 0.0, 0.5, 0.0, 0.0, 0.0], duration=0.5) + for i in range(1500): + if i % 2 == 0: + push(controller, sock, up) + tick(controller, state) + if state.error is not None: + break + else: + pytest.fail("the jog never stopped at the keep-out") + assert state.error.code == int(ErrorCode.SYS_SELF_COLLISION), state.error + assert "slab" in state.error.cause, state.error.cause + assert controller._executor.active_command is None + # A jog pushed just before the stop can still be on its way: read, + # it clears the error and the jog brakes again at the keep-out. + # Past a ping answered after it, nothing is left in flight. + drain(controller, state, sock, 2) + tick_until( + controller, + state, + lambda: ( + state.error is not None and controller._executor.active_command is None + ), + "the jog read after the stop did not end at the keep-out", + ticks=500, + ) + assert state.error.code == int(ErrorCode.SYS_SELF_COLLISION), state.error + held = state.Position_in.copy() + for _ in range(50): + tick(controller, state) + assert state.error is not None, "the collision error did not latch" + assert np.abs(state.Position_in - held).max() <= 1, ( + "the arm moved on after the jog ended" + ) + + +def test_a_jog_l_moves_the_tcp_straight_and_ends_where_the_preview_does( + controller, monkeypatch +): + """A jog_l is a straight TCP line: with an angular part as well, the tool + turns about the TCP while the TCP holds its line. The dry run previews + the pose a full-scale diagonal ends at, held to the same speed ceiling + as on the arm. (Near the wrist singularity at standby the joint speed + ceilings slow the tool on the arm, which a preview does not model, so + the diagonal starts clear of it.) The jog's duration is timed on the + clock the commands read, one control interval per tick, as the preview + counts it.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + from parol6.client.dry_run_client import DryRunRobotClient + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + linear = float(LIMITS.cart.jog.velocity.linear) + angular = float(LIMITS.cart.jog.velocity.angular) + clear_of_the_wrist = [90.0, -80.0, 190.0, 0.0, 30.0, 180.0] + axes = ("X", "Y", "Z", "RX", "RY", "RZ") + state = controller.state_manager.get_state() + clock = VirtualClock(monkeypatch) + rpy = np.zeros(3) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for begin, velocities, duration, previewed in ( + (standby, [0.05 / linear, 0.0, 0.0, 0.0, 0.0, 0.5 / angular], 2.0, False), + (clear_of_the_wrist, [1.0, -1.0, -1.0, 0.0, 0.0, 0.0], 0.6, True), + ): + preview = DryRunRobotClient(initial_joints_deg=begin) + assert preview.jog_l( + "WRF", + axes=[a for a, v in zip(axes, velocities, strict=True) if v], + speeds_list=[v for v in velocities if v], + duration=duration, + accel=1.0, + ) + assert preview.plan().blocks[0].error is None + expected = preview.pose() + + ready(controller, state, homed=True, at_deg=begin) + start = get_fkine_se3(state)[:3, 3].copy() + direction = np.asarray(velocities[:3], dtype=np.float64) + direction /= np.linalg.norm(direction) + # Full acceleration reaches the jog's speed well inside its + # duration, where the preview, which does not model the ramps, + # holds it all along. + push( + controller, + sock, + JogLCmd(velocities=velocities, duration=duration, accel=1.0), + ) + worst = 0.0 + # The jog runs its duration, then brakes: tick through the whole of it. + for _ in range(round((duration + 1.0) / INTERVAL_S)): + clock.tick(controller, state) + offset = get_fkine_se3(state)[:3, 3] - start + worst = max( + worst, + float( + np.linalg.norm(offset - np.dot(offset, direction) * direction) + ), + ) + assert worst * 1000.0 < 1.0, ( + f"{velocities}: the TCP left its line by {worst * 1000.0:.1f} mm" + ) + if previewed: + final = get_fkine_se3(state) + se3_rpy(final, rpy) + miss = np.linalg.norm(final[:3, 3] * 1000.0 - np.asarray(expected[:3])) + turned = np.abs( + (np.degrees(rpy) - np.asarray(expected[3:]) + 180.0) % 360.0 - 180.0 + ).max() + assert miss < 3.0 and turned < 1.0, ( + f"previewed {expected}, the arm ended {miss:.1f} mm and " + f"{turned:.2f}° away" + ) + + +def test_a_servo_l_stream_through_an_unreachable_pose_resumes(controller): + """A servo_l stream that asks for a pose the solver cannot reach brakes + and holds; once the stream moves on to a pose it can reach, it tracks + that one — the stream is not over, and nothing is reported failed.""" + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=[0.0, -90.0, 180.0, 0.0, 0.0, 180.0]) + tcp = get_fkine_se3(state) + rpy = np.zeros(3) + se3_rpy(tcp, rpy) + start = [*(tcp[:3, 3] * 1000.0).tolist(), *np.degrees(rpy).tolist()] + out_of_reach = list(start) + out_of_reach[0] += 600.0 + target = list(start) + target[0] += 40.0 + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for pose, ticks in ((out_of_reach, 200), (target, 400)): + for i in range(ticks): + if i % 2 == 0: + push(controller, sock, ServoLCmd(pose=pose)) + tick(controller, state) + assert state.error is None, f"the stream failed: {state.error}" + reached = get_fkine_se3(state)[:3, 3] * 1000.0 + assert np.linalg.norm(reached - np.asarray(target[:3])) < 1.0, ( + f"the stream did not resume to {target[:3]}: it holds at {reached}" + ) diff --git a/tests/integration/test_stream_regressions.py b/tests/integration/test_stream_regressions.py new file mode 100644 index 0000000..300dc35 --- /dev/null +++ b/tests/integration/test_stream_regressions.py @@ -0,0 +1,571 @@ +"""Streams driven through the controller's UDP socket and its loop against +the fake serial, past the points where one stream hands over to the next: +a stream something else ended leaves nothing behind for the next one; a +joint jog stays inside its limits and leaves them again; a servo stream +holds each joint to its hardware speed, keeps out of keep-outs, turns the +way its targets turn and resumes when its client does; a cartesian jog +moves in the frame it is given as the tool now stands; and a timed jog +lasts its duration however many periods the loop drops.""" + +import math +import socket + +import numpy as np +import pytest + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.commands.servo_commands import SERVO_GRACE_S +from parol6.config import HOME_ANGLES_DEG, INTERVAL_S, LIMITS, steps_to_rad +from parol6.protocol.wire import ( + CommandCode, + JogJCmd, + JogLCmd, + OkMsg, + ServoJCmd, + ServoLCmd, + SetShapesCmd, + ShapeWire, + StopCmd, + TeleportCmd, +) +from parol6.server.state import get_fkine_se3 +from parol6.utils.error_codes import ErrorCode +from pinokin import se3_rpy +from tests.integration.controller_loop import ( + VirtualClock, + drain, + push, + ready, + send, + tick, +) +from waldoctl import Box + +pytestmark = pytest.mark.integration + +STANDBY = [float(v) for v in HOME_ANGLES_DEG] +CLEAR_OF_THE_WRIST = [90.0, -80.0, 190.0, 0.0, 30.0, 180.0] + + +def _measured(state) -> np.ndarray: + out = np.zeros(6, dtype=np.float64) + steps_to_rad(state.Position_in, out) + return out + + +def _commanded(state) -> np.ndarray: + out = np.zeros(6, dtype=np.float64) + steps_to_rad(state.Position_out, out) + return out + + +def _wire_pose(pose: np.ndarray) -> list[float]: + rpy = np.zeros(3) + se3_rpy(pose, rpy) + return [*(pose[:3, 3] * 1000.0).tolist(), *np.degrees(rpy).tolist()] + + +def _turned_deg(before: np.ndarray, after: np.ndarray) -> float: + cos = (np.trace(before.T @ after) - 1.0) / 2.0 + return math.degrees(math.acos(min(1.0, max(-1.0, cos)))) + + +def _stream(controller, state, clock, sock, cmd, ticks: int, until=None) -> bool: + """Send *cmd* every other tick, as a UI streams it, for *ticks* ticks or + until *until* holds; whether it did.""" + for i in range(ticks): + if i % 2 == 0: + push(controller, sock, cmd) + clock.tick(controller, state) + if until is not None and until(): + return True + return False + + +def _settle(controller, state, clock) -> None: + """Tick until the stream in flight has run out and ended.""" + for _ in range(round(5.0 / INTERVAL_S)): + clock.tick(controller, state) + if controller._executor.active_command is None: + return + pytest.fail("the stream never ended") + + +def test_a_joint_stream_cut_off_by_a_stop_or_a_teleport_restarts_from_the_arm( + controller, monkeypatch +): + """Whatever ends a joint stream, the next one starts from the arm at + rest: a jog turned round after a stop does not first run on the way the + stopped one was going, and a jog or servo stream after a teleport does + not drag the arm back towards where the teleport took it from.""" + state = controller.state_manager.get_state() + clock = VirtualClock(monkeypatch) + away = JogJCmd(speeds=[-1.0, 0.0, 0.0, 0.0, 0.0, 0.0], duration=0.5) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + + def jog_away() -> None: + ready(controller, state, homed=True, at_deg=STANDBY) + assert _stream( + controller, + state, + clock, + sock, + away, + 300, + until=lambda: math.degrees(_measured(state)[0]) <= STANDBY[0] - 20.0, + ), "the jog never moved J1" + + jog_away() + reply = send(controller, state, sock, StopCmd(), 1) + assert isinstance(reply, OkMsg), reply + stopped = math.degrees(_measured(state)[0]) + lowest = stopped + back = JogJCmd(speeds=[1.0, 0.0, 0.0, 0.0, 0.0, 0.0], duration=0.5) + for i in range(40): + if i % 2 == 0: + push(controller, sock, back) + clock.tick(controller, state) + if state.Command_out == CommandCode.MOVE: + lowest = min(lowest, math.degrees(_commanded(state)[0])) + assert lowest > stopped - 0.2, ( + f"the jog turned round after the stop first ran J1 " + f"{stopped - lowest:.1f}° further the way the stopped jog was going" + ) + _settle(controller, state, clock) + + for req_id, follow in enumerate( + ( + JogJCmd(speeds=[0.5, 0.0, 0.0, 0.0, 0.0, 0.0], duration=0.5), + ServoJCmd(angles=[STANDBY[0] + 5.0, *STANDBY[1:]]), + ), + start=2, + ): + jog_away() + reply = send(controller, state, sock, TeleportCmd(angles=STANDBY), req_id) + assert isinstance(reply, OkMsg), reply + lowest = STANDBY[0] + for i in range(30): + if i % 2 == 0: + push(controller, sock, follow) + clock.tick(controller, state) + if state.Command_out == CommandCode.MOVE: + lowest = min(lowest, math.degrees(_commanded(state)[0])) + assert lowest > STANDBY[0] - 0.2, ( + f"the {type(follow).__name__} after the teleport commanded J1 back " + f"to {lowest:.1f}°, towards where the teleport took the arm from" + ) + _settle(controller, state, clock) + + +def test_a_joint_backed_off_its_limit_jogs_towards_it_again(controller, monkeypatch): + """A joint a jog stopped at its limit is held there only while the jog + pushes into it: backed off and jogged towards the limit again within + the same stream, which J6 keeps going, it follows the jog again rather + than staying frozen until the stream ends.""" + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=STANDBY) + clock = VirtualClock(monkeypatch) + hi = LIMITS.joint.position.rad[0, 1] + + def jog(j1: float) -> JogJCmd: + return JogJCmd(speeds=[j1, 0.0, 0.0, 0.0, 0.0, 0.3], duration=0.5) + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + last = _measured(state)[0] + still = 0 + for i in range(600): + if i % 2 == 0: + push(controller, sock, jog(1.0)) + clock.tick(controller, state) + q = _measured(state)[0] + still = still + 1 if abs(q - last) < 1e-7 else 0 + last = q + if still >= 10 and q > hi - 0.1: + break + else: + pytest.fail("J1 never came to rest at its limit") + assert _stream( + controller, + state, + clock, + sock, + jog(-1.0), + 300, + until=lambda: _measured(state)[0] < hi - 0.5, + ), "J1 never backed off its limit" + low = _measured(state)[0] + for i in range(100): + if i % 2 == 0: + push(controller, sock, jog(1.0)) + clock.tick(controller, state) + low = min(low, _measured(state)[0]) + rise = _measured(state)[0] - low + assert rise > 0.1, ( + f"jogged towards its limit again, J1 rose {math.degrees(rise):.2f}° and " + f"stayed {math.degrees(hi - low - rise):.1f}° short of it" + ) + + +def test_a_joint_jog_braking_for_its_limit_stays_inside_it_when_its_accel_drops( + controller, monkeypatch +): + """The next datagram of a jog braking for a limit may carry a lower + accel (a jog-accel slider moved, a script's next jog_j at its default); + the brake still ends inside the limit. The commanded position is what + is checked: the fake serial clamps the measured one at the limit.""" + state = controller.state_manager.get_state() + clock = VirtualClock(monkeypatch) + hi = LIMITS.joint.position.rad[0, 1] + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for accel in (0.5, 0.3): + ready(controller, state, homed=True, at_deg=[-60.0, *STANDBY[1:]]) + peak = -math.inf + top = 0.0 + braking = False + prev = None + for i in range(round(3.5 / INTERVAL_S)): + if i % 2 == 0: + push( + controller, + sock, + JogJCmd( + speeds=[1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + duration=0.5, + accel=accel if braking else 1.0, + ), + ) + clock.tick(controller, state) + if state.Command_out != CommandCode.MOVE: + prev = None + continue + q = _commanded(state)[0] + peak = max(peak, q) + if prev is not None: + speed = (q - prev) / INTERVAL_S + top = max(top, speed) + braking = braking or (top > 1.0 and speed < top - 0.05) + prev = q + assert braking, "J1 never braked for its limit" + assert peak <= hi + 1e-3, ( + f"with its accel dropped to {accel} as it braked, J1 was commanded " + f"{math.degrees(peak - hi):.2f}° past its limit" + ) + _settle(controller, state, clock) + + +def test_a_servo_l_braking_as_its_client_goes_silent_keeps_each_joint_to_its_speed( + controller, monkeypatch +): + """Close to the wrist singularity a tilt out of the arm's plane asks J4 + and J6 for a large, fast swing; the stream moves each joint no further + per tick than its hardware allows, so the commanded joints lag the + solution. When the client goes silent the stream brakes, and the brake + is held to the same limit: it does not close that lag in one tick.""" + state = controller.state_manager.get_state() + tilted = np.zeros((4, 4), dtype=np.float64, order="F") + PAROL6_ROBOT.robot.fkine_into( + np.radians([90.0, -90.0, 180.0, 60.0, 20.0, 120.0]), tilted + ) + ready(controller, state, homed=True, at_deg=[90.0, -90.0, 180.0, 0.0, 2.0, 180.0]) + clock = VirtualClock(monkeypatch) + limit = LIMITS.joint.hard.velocity_steps * INTERVAL_S + worst = np.zeros(6, dtype=np.int64) + prev = state.Position_in.astype(np.int64) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for i in range(round(1.5 / INTERVAL_S)): + if i < 6 and i % 2 == 0: + push(controller, sock, ServoLCmd(pose=_wire_pose(tilted))) + clock.tick(controller, state) + if state.Command_out == CommandCode.MOVE: + out = state.Position_out.astype(np.int64) + np.maximum(worst, np.abs(out - prev), out=worst) + prev = out + assert controller._executor.active_command is None, ( + "the silent stream never braked to its end" + ) + assert worst[3] > 0.5 * limit[3], ( + "the tilt never swung J4 at its speed limit: the stream never lagged" + ) + for j in range(6): + assert worst[j] <= limit[j] + 1, ( + f"J{j + 1} was commanded {worst[j]} steps in one tick; its hardware " + f"allows {limit[j]:.0f}" + ) + + +def test_a_servo_l_stream_resending_its_target_after_going_silent_carries_on_to_it( + controller, monkeypatch +): + """A client that goes silent past the grace and comes back resending the + target it was on resumes its stream: the brake gives way and the tool + goes on to the target, rather than stopping short and ending there.""" + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=STANDBY) + clock = VirtualClock(monkeypatch) + goal = get_fkine_se3(state).copy() + goal[2, 3] -= 0.1 + target = ServoLCmd(pose=_wire_pose(goal)) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + _stream(controller, state, clock, sock, target, 20) + for _ in range(round(SERVO_GRACE_S / INTERVAL_S) + 3): + clock.tick(controller, state) + for i in range(600): + if i % 5 == 0: + push(controller, sock, target) + clock.tick(controller, state) + short = np.linalg.norm(get_fkine_se3(state)[:3, 3] - goal[:3, 3]) * 1000.0 + if short < 1.0: + break + assert controller._executor.active_command is not None, ( + f"the stream ended {short:.1f} mm short of the target its client " + "kept resending" + ) + else: + pytest.fail("the resumed stream never reached its target") + + +def test_a_servo_stream_stops_short_of_a_keep_out(controller, monkeypatch): + """A servo stream, joint or cartesian, heading into a keep-out stops the + arm short of it with the collision standing as the error, as a jog + does: it does not drive the arm in.""" + state = controller.state_manager.get_state() + checker = PAROL6_ROBOT.collision + assert checker is not None + # A slab under the wrist at standby, where both streams head. + slab = Box(name="slab", x=0.10, y=0.10, z=0.04, pose=(0.0, 0.237, 0.23, 0, 0, 0)) + + def in_slab(q: np.ndarray) -> bool: + return any( + "slab" in name for pair in checker.colliding_pairs(q) for name in pair + ) + + clock = VirtualClock(monkeypatch) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + reply = send( + controller, + state, + sock, + SetShapesCmd(shapes=[ShapeWire(*slab.to_wire())]), + 1, + ) + assert isinstance(reply, OkMsg), reply + ready(controller, state, homed=True, at_deg=STANDBY) + assert not in_slab(_measured(state)), "the arm starts inside the slab" + below = get_fkine_se3(state).copy() + below[2, 3] -= 0.104 + for cmd in ( + ServoJCmd(angles=[90.0, -90.0, 150.0, 0.0, 0.0, 180.0]), + ServoLCmd(pose=_wire_pose(below)), + ): + name = type(cmd).__name__ + ready(controller, state, homed=True, at_deg=STANDBY) + refused = None + for i in range(300): + if i % 2 == 0: + push(controller, sock, cmd) + clock.tick(controller, state) + assert not in_slab(_measured(state)), ( + f"the {name} stream drove the arm into the slab" + ) + if state.error is not None and state.error.code == int( + ErrorCode.SYS_SELF_COLLISION + ): + refused = state.error + assert refused is not None, f"the {name} stream was never stopped" + assert "slab" in refused.cause, refused.cause + _settle(controller, state, clock) + + +def test_a_jog_l_moves_in_the_frame_it_is_given_as_the_tool_now_stands( + controller, monkeypatch +): + """The axis switches within one stream, as a UI streams them: after the + tool has turned about world X, a world-Y jog turns it about world Y; + after it has turned about its own Z, a tool-X jog moves it along its X + as it now stands, not as it stood when the stream began.""" + # The tool's Z lies along world X, so turning about either is J6 alone. + begin = [0.0, -60.0, 190.0, 0.0, 20.0, 180.0] + state = controller.state_manager.get_state() + clock = VirtualClock(monkeypatch) + rest = [0.0] * 6 + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for frame, turn, then in ( + ("WRF", [0.0, 0.0, 0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.5, 0.0]), + ("TRF", [0.0, 0.0, 0.0, 0.0, 0.0, 1.0], [0.5, 0.0, 0.0, 0.0, 0.0, 0.0]), + ): + ready(controller, state, homed=True, at_deg=begin) + start = get_fkine_se3(state)[:3, :3].copy() + assert _stream( + controller, + state, + clock, + sock, + JogLCmd(velocities=turn, duration=0.5, frame=frame), + 400, + until=lambda: _turned_deg(start, get_fkine_se3(state)[:3, :3]) >= 75.0, + ), f"the {frame} jog never turned the tool" + # Brought to rest and set off again, all one stream. + for velocities, ticks in ((rest, 50), (then, 30)): + _stream( + controller, + state, + clock, + sock, + JogLCmd(velocities=velocities, duration=0.5, frame=frame), + ticks, + ) + before = get_fkine_se3(state).copy() + _stream( + controller, + state, + clock, + sock, + JogLCmd(velocities=then, duration=0.5, frame=frame), + 30, + ) + after = get_fkine_se3(state).copy() + if frame == "WRF": + turned = _turned_deg(before[:3, :3], after[:3, :3]) + assert turned > 5.0, f"the world-Y jog turned the tool {turned:.1f}°" + d = after[:3, :3] @ before[:3, :3].T + axis = np.array( + [d[2, 1] - d[1, 2], d[0, 2] - d[2, 0], d[1, 0] - d[0, 1]] + ) + axis /= np.linalg.norm(axis) + off = math.degrees(math.acos(min(1.0, abs(axis[1])))) + assert off < 5.0, ( + f"the world-Y jog turned the tool about an axis {off:.1f}° off " + f"world Y: {np.round(axis, 3)}" + ) + else: + moved = after[:3, 3] - before[:3, 3] + length = float(np.linalg.norm(moved)) + assert length > 0.005, ( + f"the tool-X jog moved the tool {length * 1000.0:.1f} mm" + ) + cos = float(np.dot(moved, after[:3, 0])) / length + off = math.degrees(math.acos(min(1.0, max(-1.0, cos)))) + assert off < 5.0, ( + f"the tool-X jog moved the tool {off:.1f}° off its X axis" + ) + _settle(controller, state, clock) + + +def test_a_servo_l_stream_turning_the_tool_past_half_a_turn_keeps_turning( + controller, monkeypatch +): + """A servo stream whose targets turn the tool steadily about its own Z + keeps turning it the same way once it is more than half a turn from + where the stream began, as J6's travel allows, rather than swinging it + back the other way through the start.""" + begin = [90.0, -60.0, 190.0, 0.0, 20.0, 10.0] + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=begin) + clock = VirtualClock(monkeypatch) + start = get_fkine_se3(state).copy() + turn = np.eye(4) + j6 = peak = begin[5] + back = 0.0 + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + # 1° a datagram, one every other tick; the last one held until the + # tool is there. + for k in range(280): + a = math.radians(min(k, 220)) + turn[:2, :2] = [[math.cos(a), -math.sin(a)], [math.sin(a), math.cos(a)]] + push(controller, sock, ServoLCmd(pose=_wire_pose(start @ turn))) + for _ in range(2): + clock.tick(controller, state) + j6 = math.degrees(_measured(state)[5]) + peak = max(peak, j6) + back = max(back, peak - j6) + assert back < 0.5, ( + f"the tool turned back {back:.1f}° after J6 reached {peak:.1f}°, " + f"{peak - begin[5]:.1f}° from where the stream began" + ) + assert abs(j6 - (begin[5] + 220.0)) < 1.0, ( + f"J6 ended at {j6:.1f}°, not the {begin[5] + 220.0:.1f}° the stream " + "turned it to" + ) + + +def test_a_jog_l_after_a_cancelled_cartesian_stream_moves_the_arm( + controller, monkeypatch +): + """A cartesian jog following a cartesian stream something else ended, + a stop, a teleport, or the jog itself cutting a servo stream off, starts + from the arm and moves it; it does not end at once where it began.""" + state = controller.state_manager.get_state() + clock = VirtualClock(monkeypatch) + down = [0.0, 0.0, -0.5, 0.0, 0.0, 0.0] + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + for req_id, cut_off_by in enumerate(("a stop", "a teleport", "the jog"), 1): + ready(controller, state, homed=True, at_deg=CLEAR_OF_THE_WRIST) + if cut_off_by == "the jog": + goal = get_fkine_se3(state).copy() + goal[2, 3] -= 0.06 + servo = ServoLCmd(pose=_wire_pose(goal)) + _stream(controller, state, clock, sock, servo, 20) + else: + jog = JogLCmd(velocities=down, duration=0.5) + _stream(controller, state, clock, sock, jog, 30) + cancel = ( + StopCmd() + if cut_off_by == "a stop" + else TeleportCmd(angles=CLEAR_OF_THE_WRIST) + ) + reply = send(controller, state, sock, cancel, req_id) + assert isinstance(reply, OkMsg), reply + tick(controller, state) + before = get_fkine_se3(state)[2, 3] + push(controller, sock, JogLCmd(velocities=down, duration=0.5)) + # Loopback can deliver the jog a tick or two late (macOS): read it + # before waiting for the stream to end. + drain(controller, state, sock, 100 + req_id) + _settle(controller, state, clock) + dropped = (before - get_fkine_se3(state)[2, 3]) * 1000.0 + assert dropped > 10.0, ( + f"after {cut_off_by} cut the stream off, a 0.5 s jog down moved the " + f"tool {dropped:.1f} mm" + ) + + +def test_a_timed_jog_l_lasts_its_duration_on_a_loop_that_drops_periods( + controller, monkeypatch +): + """A loop that overruns skips the periods it missed, so wall time runs on + while a jog advances one interval per tick. A timed jog still ends + where its preview does: its duration is counted in the ticks that move + the arm, not in the wall time the loop fell behind by.""" + from parol6.client.dry_run_client import DryRunRobotClient + + velocities = [1.0, -1.0, -1.0, 0.0, 0.0, 0.0] + duration = 0.6 + preview = DryRunRobotClient(initial_joints_deg=CLEAR_OF_THE_WRIST) + assert preview.jog_l( + "WRF", + axes=["X", "Y", "Z"], + speeds_list=velocities[:3], + duration=duration, + accel=1.0, + ) + assert preview.plan().blocks[0].error is None + expected = np.asarray(preview.pose()[:3]) + + state = controller.state_manager.get_state() + ready(controller, state, homed=True, at_deg=CLEAR_OF_THE_WRIST) + clock = VirtualClock(monkeypatch) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + push( + controller, + sock, + JogLCmd(velocities=velocities, duration=duration, accel=1.0), + ) + for _ in range(round((duration + 1.0) / INTERVAL_S)): + clock.tick(controller, state) + # The period after each tick is the one the loop overran. + clock.now += INTERVAL_S + miss = float(np.linalg.norm(get_fkine_se3(state)[:3, 3] * 1000.0 - expected)) + assert miss < 3.0, f"previewed {expected}, the jog ended {miss:.1f} mm away" diff --git a/tests/integration/test_streaming_cartesian_accuracy.py b/tests/integration/test_streaming_cartesian_accuracy.py index abd3448..1ddfecd 100644 --- a/tests/integration/test_streaming_cartesian_accuracy.py +++ b/tests/integration/test_streaming_cartesian_accuracy.py @@ -5,6 +5,8 @@ Catches bugs where reference pose gets corrupted (e.g., aliasing with FK cache). """ +import time + import numpy as np import pytest @@ -15,6 +17,24 @@ def angle_diff(a: float, b: float) -> float: return abs(diff) +def stream_servo_l(client, target: list[float], timeout: float = 10.0) -> None: + """Drive ``servo_l`` the way a dragging client does: refresh the target + every 50 ms until the tool is on it, then let the stream run out. A + stream whose client goes silent brakes and holds, so a single datagram + is never a move.""" + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + assert client.servo_l(target, speed=1.0) > 0 + time.sleep(0.05) + pose = client.pose() + on_target = np.linalg.norm( + np.array(pose[:3]) - np.array(target[:3]) + ) < 0.5 and all(angle_diff(pose[3 + i], target[3 + i]) < 0.5 for i in range(3)) + if on_target: + break + assert client.wait_motion(timeout=5.0) + + def assert_pose_accuracy( final_pose: list[float], target: list[float], @@ -62,12 +82,7 @@ def test_servo_l_reaches_target(self, client, server_proc): print(f"Target pose: {target}") - # Send servo cartesian move (fire-and-forget, no stream mode toggle needed) - result = client.servo_l(target, speed=1.0) - assert result > 0 - - # Wait for motion to settle - assert client.wait_motion(timeout=10.0) + stream_servo_l(client, target) # Verify final pose final_pose = client.pose() @@ -107,11 +122,7 @@ def test_servo_l_sequential_targets(self, client, server_proc): print(f"\n--- Move {i + 1}/{len(offsets)} ---") print(f"Target: {target[:3]}") - result = client.servo_l(target, speed=1.0) - assert result > 0 - - # Wait for this move to complete before next - assert client.wait_motion(timeout=10.0, settle_window=2.0) + stream_servo_l(client, target) final_pose = client.pose() start_pose = final_pose diff --git a/tests/integration/test_teleport.py b/tests/integration/test_teleport.py new file mode 100644 index 0000000..e3ace1b --- /dev/null +++ b/tests/integration/test_teleport.py @@ -0,0 +1,295 @@ +"""A teleport is a system command on the simulator: the controller answers +it once the pose is applied, the pose is exact so an unreferenced arm reads +referenced afterwards, whatever was driving the arm stops there, and what +cannot be applied is refused before anything moves. The arm lands before +any motion read with the teleport, which starts from there; the failure the +arm was left in stays behind; and the tool positions it takes are the ones +status reports, in the convention status reports them in.""" + +import select +import socket +import time + +import numpy as np +import pytest + +from parol6 import MotionError, RobotClient +from parol6.config import INTERVAL_S, steps_to_deg +from parol6.protocol.wire import ( + ErrorMsg, + JogJCmd, + MoveJCmd, + OkMsg, + TeleportCmd, + decode_message, +) +from parol6.utils.error_catalog import RobotError +from parol6.utils.error_codes import ErrorCode +from tests.integration.controller_loop import ( + VirtualClock, + push, + ready, + send, + tick, + tick_until, +) +from waldoctl import ActionState, Box + +pytestmark = pytest.mark.integration + + +def _angles_deg(state) -> np.ndarray: + out = np.zeros(6, dtype=np.float64) + steps_to_deg(state.Position_in, out) + return out + + +def _reply_index(controller, state, sock: socket.socket, req_id: int) -> int: + """Run the controller until it acknowledges the request: loopback may + delay either the inbound command or the outbound reply.""" + deadline = time.monotonic() + 2.0 + while True: + remaining = deadline - time.monotonic() + if remaining <= 0: + pytest.fail(f"no acknowledgement of request {req_id}") + tick(controller, state) + if not select.select([sock], [], [], min(INTERVAL_S, remaining))[0]: + continue + data, _ = sock.recvfrom(4096) + reply = decode_message(data) + if isinstance(reply, OkMsg) and reply.req_id == req_id: + assert reply.index is not None, reply + return reply.index + + +def test_a_teleport_is_acked_once_applied_and_references_the_arm(controller): + state = controller.state_manager.get_state() + ready(controller, state, homed=False) + target = [10.0, -80.0, 170.0, 5.0, -10.0, 175.0] + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + reply = send(controller, state, sock, TeleportCmd(angles=target), 1) + assert isinstance(reply, OkMsg), reply + tick_until( + controller, + state, + lambda: bool(state.Homed_in[:6].all()), + "the teleported arm never read referenced", + ) + assert np.allclose(_angles_deg(state), target, atol=0.05) + + # A jog is driving the arm; a teleport ends it where it lands. + push( + controller, + sock, + JogJCmd(speeds=[0.0, 0.0, 0.0, 0.0, 0.0, -0.8], duration=5.0), + ) + tick_until( + controller, + state, + lambda: _angles_deg(state)[5] < 174.0, + "the jog never moved the wrist", + ) + reply = send(controller, state, sock, TeleportCmd(angles=target), 2) + assert isinstance(reply, OkMsg), reply + for _ in range(30): + tick(controller, state) + assert np.allclose(_angles_deg(state), target, atol=0.05), ( + "the jog carried on after the teleport" + ) + + # Tool positions for a tool that is not fitted are refused, and the + # arm stays where it is. + reply = send( + controller, + state, + sock, + TeleportCmd( + angles=[0.0, -90.0, 180.0, 0.0, 0.0, 180.0], tool_positions=[0.5] + ), + 3, + ) + assert isinstance(reply, ErrorMsg), reply + assert RobotError.from_wire(reply.message).code == int( + ErrorCode.COMM_VALIDATION_ERROR + ) + for _ in range(5): + tick(controller, state) + assert np.allclose(_angles_deg(state), target, atol=0.05) + + # What the hard limits and [0, 1] exclude never reaches the wire. + with pytest.raises(ValueError): + TeleportCmd(angles=[10.0, -80.0, 170.0, 5.0, -10.0, 1000.0]) + with pytest.raises(ValueError): + TeleportCmd(angles=[float("nan"), -80.0, 170.0, 5.0, -10.0, 175.0]) + with pytest.raises(ValueError): + TeleportCmd(angles=target, tool_positions=[1.5]) + + +def test_a_teleport_read_with_motion_lands_before_the_motion_starts( + controller, monkeypatch +): + """A teleport and the command sent straight after it, read in one batch: + the arm lands where the teleport puts it, and the jog or the planned + move starts from there, not from where the arm stood before.""" + state = controller.state_manager.get_state() + controller._planner.start() + # The jog's duration is timed in ticks. + clock = VirtualClock(monkeypatch) + start = [0.0, -90.0, 180.0, 0.0, 0.0, 180.0] + landing = [30.0, -80.0, 170.0, 5.0, -10.0, 150.0] + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + + ready(controller, state, homed=True, at_deg=start) + push(controller, sock, TeleportCmd(angles=landing), 1) + push( + controller, + sock, + JogJCmd(speeds=[0.0, 0.0, 0.0, 0.0, 0.0, -0.5], duration=0.5), + ) + for _ in range(round(1.5 / INTERVAL_S)): + clock.tick(controller, state) + q = _angles_deg(state) + assert np.allclose(q[:5], landing[:5], atol=0.05), ( + f"the arm never landed at {landing}: it reads {q}" + ) + assert landing[5] - 60.0 < q[5] < landing[5] - 1.0, ( + f"the jog did not run from the landing: J6 reads {q[5]:.1f}°" + ) + + ready(controller, state, homed=True, at_deg=start) + goal = [40.0, *landing[1:]] + push(controller, sock, TeleportCmd(angles=landing), 2) + push(controller, sock, MoveJCmd(angles=goal, duration=1.0), 3) + moving = _reply_index(controller, state, sock, 3) + tick_until( + controller, + state, + lambda: abs(_angles_deg(state)[0] - landing[0]) < 0.05, + "the arm never landed before the move", + ) + lowest = landing[0] + deadline = time.monotonic() + 30.0 + while not state.command_completed(moving): + assert time.monotonic() < deadline, "the move never completed" + tick(controller, state) + lowest = min(lowest, float(_angles_deg(state)[0])) + time.sleep(INTERVAL_S) + assert lowest > landing[0] - 0.5, ( + f"the move started from the pose before the teleport: J1 went back " + f"to {lowest:.1f}° on its way from {landing[0]}° to {goal[0]}°" + ) + assert np.allclose(_angles_deg(state), goal, atol=0.05) + + +def test_a_teleport_leaves_the_failure_the_arm_was_in_behind( + client: RobotClient, server_proc +): + """A scrub teleports the arm out of the state a failed program left it + in: the collision a move was refused for — the error, the ERROR state, + the colliding links — does not follow the arm to where it lands.""" + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + start = client.angles() + assert start is not None + target = [0.0, -90.0, 180.0, 0.0, 0.0, 180.0] + wrist = PAROL6_ROBOT.robot.fkine(np.radians(target))[:3, 3] + blocker = Box( + name="blocker", + x=0.25, + y=0.25, + z=0.25, + pose=(float(wrist[0]), float(wrist[1]), float(wrist[2]), 0, 0, 0), + ) + assert client.set_shapes([blocker]) == 1 + try: + with pytest.raises(MotionError) as refused: + client.move_j(target, duration=1.5, wait=True) + assert refused.value.robot_error.code == ErrorCode.SYS_SELF_COLLISION + assert client.wait_status( + lambda s: ( + s.error is not None + and s.action_state == ActionState.ERROR + and s.collision_active + ), + timeout=2.0, + ), "the refusal never reached status" + finally: + assert client.set_shapes([]) == 1 + + assert client.teleport(start) == 1 + assert client.error() is None, "the refusal still stands after the teleport" + assert client.wait_status( + lambda s: ( + s.error is None + and s.action_state != ActionState.ERROR + and not s.collision_active + and not s.collision_pairs + ), + timeout=2.0, + ), "status still reports the failure after the teleport" + + +def test_a_teleport_takes_the_tool_positions_status_reports_for_every_tool( + client: RobotClient, server_proc +): + """A scrub replays a recorded keyframe — the angles, and the tool + positions status reported then — whichever tool was fitted: the + teleport takes them back and the arm lands.""" + for i, tool in enumerate(("NONE", "PNEUMATIC", "SSG-48", "MSG", "VACUUM")): + selected = client.select_tool(tool) + assert selected >= 0 and client.wait_command(selected, timeout=10.0) + assert client.wait_status( + lambda s, t=tool: s.tool_status.key == t, timeout=2.0 + ), f"status never reported {tool} fitted" + status = client.status() + assert status is not None and status.tool_status.key == tool + landing = [10.0 + 5.0 * i, -80.0, 170.0, 5.0, -10.0, 175.0] + assert ( + client.teleport(landing, tool_positions=list(status.tool_status.positions)) + == 1 + ), tool + assert client.wait_status( + lambda s, at=landing: np.allclose(s.angles, at, atol=0.05), timeout=2.0 + ), f"the arm never landed with {tool} fitted" + + +def test_a_teleported_pneumatic_jaw_reads_back_where_it_was_put( + client: RobotClient, server_proc +): + """Tool positions go in as status reads them out — 1.0 closed, 0.0 + open — so a pneumatic jaw teleported to where its valve holds it stays + there, rather than snapping to the far end and stroking back.""" + selected = client.select_tool("PNEUMATIC") + assert selected >= 0 and client.wait_command(selected, timeout=10.0) + angles = client.angles() + assert angles is not None + + # Open first: only a stroke overwrites the jaw reading an earlier tool + # left in the simulator. + for action, position in (("open", 0.0), ("close", 1.0)): + acting = client.tool_action("PNEUMATIC", action, wait=False) + assert acting >= 0 and client.wait_command(acting, timeout=5.0) + assert client.wait_status( + lambda s, p=position: abs(s.tool_status.positions[0] - p) < 0.01, + timeout=2.0, + ), f"the jaw never reached {position} on {action}" + assert client.teleport(angles, tool_positions=[position]) == 1 + + readings: list[float] = [] + + def record(s) -> bool: + readings.append(float(s.tool_status.positions[0])) + return False + + # Fixed observation window: a wrong snap sets off a 0.15 s stroke + # back, and "stays there" has no condition to poll for. + client.wait_status(record, timeout=0.5) + assert readings, "no status arrived after the teleport" + assert max(abs(r - position) for r in readings) < 0.05, ( + f"teleported to {position} with the valve on {action}, the jaw read " + f"{min(readings):.2f}..{max(readings):.2f}" + ) diff --git a/tests/integration/test_tool_operations.py b/tests/integration/test_tool_operations.py index a8543f5..2a1526e 100644 --- a/tests/integration/test_tool_operations.py +++ b/tests/integration/test_tool_operations.py @@ -11,6 +11,8 @@ import pytest_asyncio from parol6.protocol.wire import CommandCompletionCmd, encode_command +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import MotionError from waldoctl import ( ElectricGripperTool, @@ -113,8 +115,8 @@ async def test_pneumatic_open_close(self, async_client, monkeypatch): 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. + # A tool action waits its turn behind the delay the pause holds, the + # jaws still shut, and runs once the queue resumes. earlier = await client.delay(0.5) assert await client.wait_status( lambda s: s.executing_index == earlier, timeout=5.0 @@ -122,14 +124,16 @@ async def test_pneumatic_open_close(self, async_client, monkeypatch): 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" + assert not await client.wait_command(opened, timeout=0.5), ( + "the open ran past the paused delay ahead of it" + ) + assert (await tool.status()).positions[0] > 0.99, ( + "the jaws opened under the pause" ) finally: assert await client.resume() == 1 - assert await client.wait_motion(timeout=5.0) - assert await client.wait_command(opened, timeout=1.0) + assert await client.wait_command(opened, timeout=5.0) + assert await client.wait_command(earlier, timeout=1.0) cancelled = await client.delay(1.0) assert await client.wait_status( @@ -138,7 +142,9 @@ async def test_pneumatic_open_close(self, async_client, monkeypatch): 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) + with pytest.raises(MotionError) as discarded: + await client.wait_command(cancelled, timeout=1.0) + assert discarded.value.robot_error.code == ErrorCode.MOTN_CANCELLED # Deliver a real completion reply late, ahead of a different query. assert client._transport is not None @@ -185,74 +191,161 @@ async def test_pneumatic_set_position_threshold(self, async_client): # =========================================================================== -# SSG-48 Electric Gripper Methods (async, via client.tool) +# Electric Gripper Methods (async, via client.tool) # =========================================================================== +@pytest.mark.integration +@pytest.mark.asyncio +@pytest.mark.parametrize("key", ["SSG-48", "MSG"]) +async def test_an_electric_gripper_calibrates_and_moves(async_client, key): + """From an uncalibrated gripper, which refuses a jaw move, a calibration + sent right behind the selection takes effect for the jaw moves queued + behind it: they wait for it, reach their targets, and draw the + commanded fraction of the tool's current range while the jaws travel.""" + robot, client = async_client + spec = robot.tools[key] + assert isinstance(spec, ElectricGripperTool) + lo, hi = spec.current_range + + # A simulator toggle leaves the gripper uncalibrated, whatever an + # earlier test calibrated. + assert await client.simulator(True) == 1 + assert await client.select_tool(key) >= 0 + tool = client.tool + assert await tool.calibrate() >= 0 + assert await tool.set_position(0.0, speed=1.0, wait=True, timeout=15.0) >= 0 + assert (await tool.status()).positions[0] == pytest.approx(0.0, abs=0.02) + + fraction = 0.25 + moving = await tool.set_position(0.6, speed=0.1, current=fraction) + assert moving >= 0 + assert await client.wait_status( + lambda s: s.executing_index == moving and s.tool_status.positions[0] > 0.1, + timeout=5.0, + ), f"the {key} jaws never got under way" + drawn = (await tool.status()).channels[0] + assert drawn == round(lo + fraction * (hi - lo)), ( + f"a {key} move at current {fraction} drew {drawn} mA across {lo}..{hi}" + ) + assert await client.wait_command(moving, timeout=10.0) + assert (await tool.status()).positions[0] == pytest.approx(0.6, abs=0.02) + + @pytest.mark.integration class TestSSG48GripperMethods: """Test SSG-48 electric gripper via client.tool().""" @pytest.mark.asyncio - async def test_ssg48_calibrate_and_move(self, async_client): - """Calibrate and move SSG-48 gripper through tool methods.""" + async def test_a_bad_tool_action_is_refused_before_it_is_acknowledged( + self, async_client + ): + """A malformed action never leaves the client; one the arm cannot + run is refused by the controller instead of acknowledged and left + to fail — and the control loop keeps ticking through all of it.""" robot, client = async_client - spec = robot.tools["SSG-48"] - assert isinstance(spec, ElectricGripperTool) - assert spec.gripper_type == GripperType.ELECTRIC - - # Verify parameter ranges - assert spec.position_range == (0.0, 1.0) - assert spec.speed_range == (0.0, 1.0) - assert spec.current_range == (100, 1300) - await client.select_tool("SSG-48") await client.wait_motion(timeout=5.0) - tool = client.tool - # Calibrate - idx = await tool.calibrate() - assert idx >= 0 - await client.wait_motion(timeout=10.0) - - # Move to half position - idx = await tool.set_position(0.5, speed=0.7, current=600) - assert idx >= 0 - await client.wait_motion(timeout=10.0) - - -# =========================================================================== -# MSG AI Stepper Gripper Methods (async, via client.tool) -# =========================================================================== - - -@pytest.mark.integration -class TestMSGGripperMethods: - """Test MSG compliant AI stepper gripper via client.tool().""" + with pytest.raises(MotionError, match="not the selected tool"): + await client.tool_action("PNEUMATIC", "open") + + for params in ( + [], + [0.5], + [0.5, 0.5], + [0.5, 0.5, 0.5, 1], + [float("nan"), 0.5, 0.5], + [0.5, float("inf"), 0.5], + [0.5, 0.5, float("-inf")], + [-0.1, 0.5, 0.5], + [1.5, 0.5, 0.5], + [0.5, -0.5, 0.5], + [0.5, 1.5, 0.5], + [0.5, 0.5, -0.1], + [0.5, 0.5, 1.5], + [0.5, 0.5, 600], + ["0.5", 0.5, 0.5], + [True, 0.5, 0.5], + ): + with pytest.raises(ValueError): + await client.tool_action("SSG-48", "move", params) + for action, params in ( + ("bogus", []), + ("open", []), + ("set_position", [0.5]), + ("calibrate", [1]), + ("stop", [0]), + ("idle", [0]), + ): + with pytest.raises(ValueError): + await client.tool_action("SSG-48", action, params) + # The jaw methods take current as a fraction of the current range. + for current in (-0.1, 1.5, 600, float("nan"), float("inf")): + with pytest.raises(ValueError): + await tool.set_position(0.5, current=current) + assert await client.status() is not None @pytest.mark.asyncio - async def test_msg_calibrate_and_move(self, async_client): - """Calibrate and move MSG gripper through tool methods.""" + async def test_a_script_drives_the_tool_it_just_selected(self, async_client): + """A tool action sent right behind the ``select_tool`` that fits the + tool is judged against that selection, not the tool still fitted, + and queues behind it; a jaw move's current fraction grips at that + fraction of the tool's current range.""" robot, client = async_client - spec = robot.tools["MSG"] + spec = robot.tools["SSG-48"] assert isinstance(spec, ElectricGripperTool) - assert spec.gripper_type == GripperType.ELECTRIC - - await client.select_tool("MSG") - await client.wait_motion(timeout=5.0) + lo, hi = spec.current_range + assert await client.select_tool("SSG-48") >= 0 tool = client.tool + calibrating = await tool.calibrate() + assert calibrating >= 0 + assert await client.wait_command(calibrating, timeout=10.0) + + fraction = 0.3 + closing = await tool.set_position(1.0, speed=0.05, current=fraction) + assert closing >= 0 + # The close itself, not any current: the frame an earlier move left + # reports that move's current until the queue reaches the close. + assert await client.wait_status( + lambda s: s.executing_index == closing, timeout=5.0 + ), "the close never started" + commanded = (await tool.status()).channels[0] + assert commanded == round(lo + fraction * (hi - lo)), ( + f"a move at current {fraction} sent {commanded} mA across " + f"{spec.current_range}" + ) + assert await client.stop() == 1 - # Calibrate - idx = await tool.calibrate() - assert idx >= 0 - await client.wait_motion(timeout=10.0) + @pytest.mark.asyncio + async def test_a_stop_halts_the_jaws_where_they_are(self, async_client): + """A stop mid-travel fails the move with MOTN_CANCELLED and leaves + the jaws where they were, gripping, rather than letting them run on + to the target or releasing.""" + robot, client = async_client + await client.select_tool("SSG-48") + await client.wait_motion(timeout=5.0) + tool = client.tool + assert await tool.calibrate(wait=True) >= 0 + assert await tool.set_position(0.0, wait=True) >= 0 - # Move to position - idx = await tool.set_position(0.3, speed=0.5, current=500) - assert idx >= 0 - await client.wait_motion(timeout=10.0) + closing = await tool.set_position(1.0, speed=0.05) + assert await client.wait_status( + lambda s: 0.15 < s.tool_status.positions[0] < 0.6, timeout=10.0 + ), "the jaws never got under way" + assert await client.stop() == 1 + with pytest.raises(MotionError) as stopped: + await client.wait_command(closing, timeout=1.0) + assert stopped.value.robot_error.code == ErrorCode.MOTN_CANCELLED + + held = (await tool.status()).positions[0] + await asyncio.sleep(0.3) + later = await tool.status() + assert 0.1 < held < 0.9, f"the jaws ran on to {held}" + assert abs(later.positions[0] - held) < 0.02, "the jaws kept moving" + assert later.engaged, "the stop released the grip" # =========================================================================== diff --git a/tests/integration/test_tool_queue.py b/tests/integration/test_tool_queue.py new file mode 100644 index 0000000..fc49ada --- /dev/null +++ b/tests/integration/test_tool_queue.py @@ -0,0 +1,428 @@ +"""A tool action is queued work: it takes its turn in the command queue +behind the motion sent ahead of it, the arm holds still while it runs, and +the motion sent after it waits for it. What discards the queue — a stop, an +e-stop, a reset, a teleport, a stream taking the arm — discards the tool +actions in it and halts the one running where the jaws are; a pause holds +them; a tool action refused in its turn drops what is queued behind it. +Only ``tool.stop()`` acts at once, ahead of the queue.""" + +import socket +import time + +import numpy as np +import pytest + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6 import MotionError, RobotClient +from parol6.config import INTERVAL_S, steps_to_deg +from parol6.protocol.wire import ( + DelayCmd, + MoveJCmd, + OkMsg, + SelectToolCmd, + ToolActionCmd, +) +from parol6.utils.error_codes import ErrorCode +from tests.conftest import wait_until +from tests.integration.controller_loop import ready, send, tick, tick_for + +pytestmark = pytest.mark.integration + + +def _offset(pose: list[float], dx: float, dy: float, dz: float) -> list[float]: + return [pose[0] + dx, pose[1] + dy, pose[2] + dz, *pose[3:]] + + +def _open_at(client: RobotClient, angles: list[float]) -> None: + """Put the arm at *angles* with the jaws fully open. The gripper opens + them first, so the snap lands where it is already driving them.""" + opening = client.tool.open() + assert opening >= 0 and client.wait_command(opening, timeout=15.0) + assert client.teleport(angles, tool_positions=[0.0]) == 1 + assert client.wait_status( + lambda s: ( + s.tool_status.positions[0] == 0.0 + and np.allclose(s.angles, angles, atol=0.05) + ), + timeout=2.0, + ), "the arm never landed with the jaws open" + + +def _fit_ssg48_open(client: RobotClient): + """Select and calibrate the SSG-48 and open it, sent back to back: each + waits for the one ahead of it.""" + selecting = client.select_tool("SSG-48") + calibrating = client.tool.calibrate() + assert min(selecting, calibrating) >= 0 + angles = client.angles() + assert angles is not None + _open_at(client, angles) + assert client.wait_command(calibrating, timeout=1.0) + return client.tool + + +def _trace(client: RobotClient, until, timeout: float): + """TCP positions (mm) and jaw positions, frame by frame, until *until* + holds or *timeout* runs out, and whether it held.""" + tcp: list[np.ndarray] = [] + jaws: list[float] = [] + + def record(s) -> bool: + jaw = float(s.tool_status.positions[0]) + tcp.append(s.pose[[3, 7, 11]]) + jaws.append(jaw) + return until(s) + + held = client.wait_status(record, timeout=timeout) + return np.asarray(tcp), np.asarray(jaws), held + + +def _assert_cancelled(client: RobotClient, index: int, by: str) -> None: + with pytest.raises(MotionError) as cancelled: + client.wait_command(index, timeout=2.0) + assert cancelled.value.robot_error.code == ErrorCode.MOTN_CANCELLED, ( + f"{by}: {cancelled.value}" + ) + assert cancelled.value.command_index == index, by + + +def _angles_deg(state) -> np.ndarray: + out = np.zeros(6, dtype=np.float64) + steps_to_deg(state.Position_in, out) + return out + + +def test_a_tool_action_takes_its_turn_between_the_moves_around_it( + client: RobotClient, server_proc +): + """move_l(A, r) → close → move_l(B), sent back to back: the jaws stay + open while the arm travels to A, the arm comes to rest at A — a tool + action ends a blend — and holds there while the jaws close, and leaves + for B only once they have.""" + tool = _fit_ssg48_open(client) + pose = client.pose() + assert pose is not None + a = _offset(pose, 0.0, 0.0, -40.0) + b = _offset(pose, 40.0, 0.0, -40.0) + + reaching = client.move_l(a, duration=3.0, r=12.0, wait=False) + closing = tool.close(speed=0.1) + leaving = client.move_l(b, duration=1.5, wait=False) + assert min(reaching, closing, leaving) >= 0 + tcp, jaws, done = _trace( + client, lambda s: s.completed_index >= leaving, timeout=20.0 + ) + assert done, "the move sent after the close never finished" + + from_a = np.linalg.norm(tcp - np.array(a[:3]), axis=1) + at_a = from_a < 0.5 + arrived = int(np.argmax(at_a)) if at_a.any() else len(at_a) + early = jaws[:arrived].max(initial=0.0) + assert early < 0.01, ( + f"the jaws closed to {early:.2f} while the arm was still on its way to A" + ) + assert at_a.any(), ( + f"the arm blended past A, {from_a.min():.2f} mm from it at the closest" + ) + under_way = (jaws > 0.02) & (jaws < 0.97) + assert under_way.any(), "the close was never seen under way" + assert at_a[under_way].all(), ( + f"the arm moved {from_a[under_way].max():.2f} mm off A while the jaws " + "were closing" + ) + left = ~at_a & (np.arange(len(at_a)) > arrived) + assert left.any(), "the arm never left A for B" + assert jaws[left].min() > 0.97, ( + f"the arm left A for B with the jaws at {jaws[left].min():.2f}" + ) + assert np.linalg.norm(tcp[-1] - np.array(b[:3])) < 0.5 + for index in (reaching, closing, leaving): + assert client.wait_command(index, timeout=1.0) + + +def test_a_wait_on_a_tool_action_covers_the_motion_queued_ahead_of_it( + client: RobotClient, server_proc +): + """The close runs only once the move sent ahead of it has, so a wait on + the close returns with the arm already there.""" + tool = _fit_ssg48_open(client) + pose = client.pose() + assert pose is not None + a = _offset(pose, 0.0, 0.0, -40.0) + + reaching = client.move_l(a, duration=3.0, wait=False) + closing = tool.close(speed=0.1) + assert min(reaching, closing) >= 0 + assert client.wait_command(closing, timeout=15.0) + here = client.pose() + assert here is not None + off = float(np.linalg.norm(np.array(here[:3]) - np.array(a[:3]))) + assert off < 0.5, ( + f"the wait on the close returned with the arm {off:.1f} mm short of " + "the move queued ahead of it" + ) + assert client.wait_command(reaching, timeout=1.0) + assert tool.status().positions[0] > 0.97 + + +def test_a_paused_queue_holds_and_lists_the_tool_actions_in_it( + client: RobotClient, server_proc +): + """Under a pause the queue lists the tool actions among the moves, and + they wait with them — the jaws do not move — until it resumes, when + all of it runs in order.""" + tool = _fit_ssg48_open(client) + pose = client.pose() + assert pose is not None + a = _offset(pose, 0.0, 0.0, -20.0) + try: + assert client.pause() == 1 + closing = tool.close() + reaching = client.move_l(a, duration=1.0, wait=False) + opening = tool.open() + assert min(closing, reaching, opening) >= 0 + wait_until( + lambda: len(client.queue() or []) >= 3, + 5.0, + "the paused queue never listed the tool actions around the move", + ) + listed = client.queue() + assert listed == ["tool_action", "move_l", "tool_action"], listed + + # Fixed observation window: "held" has no condition to poll for. + tcp, jaws, _ = _trace(client, lambda s: False, timeout=0.5) + assert jaws.size, "no status arrived under the pause" + assert jaws.max() < 0.01, f"the jaws closed to {jaws.max():.2f} under the pause" + assert np.linalg.norm(tcp - np.array(pose[:3]), axis=1).max() < 0.5 + assert not client.wait_command(closing, timeout=0.2) + + assert client.resume() == 1 + assert client.wait_command(opening, timeout=15.0) + for index in (closing, reaching): + assert client.wait_command(index, timeout=1.0) + here = client.pose() + assert here is not None + assert np.linalg.norm(np.array(here[:3]) - np.array(a[:3])) < 0.5 + finally: + client.stop() + client.resume() + + +def test_a_stop_estop_reset_or_teleport_drops_queued_tool_actions_and_halts_the_running_one( + client: RobotClient, server_proc +): + """Each one fails a close still queued behind the move under way as + MOTN_CANCELLED before the jaws move at all, and halts a close under way + where the jaws are, failing it MOTN_CANCELLED too.""" + tool = _fit_ssg48_open(client) + start = client.angles() + pose = client.pose() + assert start is not None and pose is not None + a = _offset(pose, 0.0, 0.0, -40.0) + + def teleport_here() -> int: + here = client.angles() + assert here is not None + return client.teleport(here) + + def reenable() -> None: + assert client.reset() == 1 + + def refit() -> None: + selected = client.select_tool("SSG-48") + assert selected >= 0 and client.wait_command(selected, timeout=10.0) + + for name, discard, recover in ( + ("stop", client.stop, lambda: None), + ("estop", client.estop, reenable), + ("teleport", teleport_here, lambda: None), + # Last: it also drops the tool and the test motion profile. + ("reset_state", client.reset_state, refit), + ): + _open_at(client, start) + reaching = client.move_l(a, duration=3.0, wait=False) + closing = tool.close(speed=0.1) + assert min(reaching, closing) >= 0 + assert client.wait_status( + lambda s: np.linalg.norm(s.pose[[3, 7, 11]] - np.array(pose[:3])) > 2.0, + timeout=5.0, + ), f"{name}: the move never got under way" + assert discard() == 1 + recover() + jaw = tool.status().positions[0] + assert jaw < 0.01, ( + f"{name}: the close queued behind the move ran, the jaws at {jaw:.2f}" + ) + for index in (reaching, closing): + _assert_cancelled(client, index, name) + + _open_at(client, start) + closing = tool.close(speed=0.05) + assert closing >= 0 + assert client.wait_status( + lambda s: 0.15 < s.tool_status.positions[0] < 0.6, timeout=10.0 + ), f"{name}: the jaws never got under way" + assert discard() == 1 + recover() + _assert_cancelled(client, closing, name) + # Fixed observation window: "held" has no condition to poll for. + _, jaws, _ = _trace(client, lambda s: False, timeout=0.5) + assert jaws.size, f"{name}: no status arrived after the halt" + assert 0.1 < jaws[0] < 0.9, f"{name}: the jaws ran on to {jaws[0]:.2f}" + assert jaws.max() - jaws.min() < 0.02, ( + f"{name}: the jaws kept moving, {jaws.min():.2f}..{jaws.max():.2f}" + ) + + +def test_a_jog_takes_the_queue_from_the_tool_actions_in_it( + client: RobotClient, server_proc +): + """A jog preempts the queue, tool actions included: the close under way + and the open queued behind it fail MOTN_CANCELLED, and the jaws stay + where the jog found them.""" + tool = _fit_ssg48_open(client) + closing = tool.close(speed=0.05) + opening = tool.open(speed=0.05) + assert min(closing, opening) >= 0 + assert client.wait_status( + lambda s: 0.15 < s.tool_status.positions[0] < 0.6, timeout=10.0 + ), "the jaws never got under way" + + assert client.jog_j(0, 0.2, duration=0.2) == 1 + for index in (closing, opening): + _assert_cancelled(client, index, "a jog") + # Fixed observation window: "held" has no condition to poll for. + _, jaws, _ = _trace(client, lambda s: False, timeout=0.5) + assert jaws.size, "no status arrived after the jog" + assert 0.1 < jaws[0] < 0.9, f"the jaws ran on to {jaws[0]:.2f}" + assert jaws.max() - jaws.min() < 0.02, ( + f"the jaws kept moving under the jog, {jaws.min():.2f}..{jaws.max():.2f}" + ) + + +def test_a_tool_stop_halts_the_close_at_once_and_keeps_the_move_queued_behind_it( + client: RobotClient, server_proc +): + """``tool.stop()`` is not queued: it halts the close under way where the + jaws are, failing it MOTN_CANCELLED by a tool stop, while the move + queued behind the close is kept and runs once the close has ended.""" + tool = _fit_ssg48_open(client) + pose = client.pose() + assert pose is not None + a = _offset(pose, 0.0, 0.0, -20.0) + + closing = tool.close(speed=0.05) + reaching = client.move_l(a, duration=1.0, wait=False) + assert min(closing, reaching) >= 0 + tcp, _, under_way = _trace( + client, lambda s: s.tool_status.positions[0] > 0.2, timeout=10.0 + ) + assert under_way, "the jaws never got under way" + drift = float(np.linalg.norm(tcp - np.array(pose[:3]), axis=1).max()) + assert drift < 0.5, ( + f"the move queued behind the close moved the arm {drift:.1f} mm while " + "the jaws were closing" + ) + + stopping = tool.stop() + assert stopping >= 0 + with pytest.raises(MotionError) as stopped: + client.wait_command(closing, timeout=2.0) + assert stopped.value.robot_error.code == ErrorCode.MOTN_CANCELLED + assert stopped.value.command_index == closing + assert "a tool stop" in stopped.value.robot_error.cause + held = tool.status().positions[0] + + assert client.wait_command(reaching, timeout=10.0), ( + "the move queued behind the close never ran" + ) + assert client.wait_command(stopping, timeout=2.0) + here = client.pose() + assert here is not None + assert np.linalg.norm(np.array(here[:3]) - np.array(a[:3])) < 0.5 + later = tool.status().positions[0] + assert 0.1 < held < 0.9, f"the jaws ran on to {held:.2f}" + assert abs(later - held) < 0.02, ( + f"the jaws moved from {held:.2f} to {later:.2f} after the tool stop" + ) + + +def test_a_tool_action_refused_in_its_turn_fails_alone_and_drops_what_is_queued_behind_it( + controller, +): + """A jaw move on a gripper never calibrated is refused when its turn + comes — once the move ahead of it has run — under its own index; the + commands queued behind it fail MOTN_CANCELLED and the arm stays where + the move ahead left it.""" + state = controller.state_manager.get_state() + controller._planner.start() + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + ready(controller, state, homed=True, at_deg=standby) + a = [standby[0] + 10.0, *standby[1:]] + b = [standby[0] + 20.0, *standby[1:]] + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + selected = send(controller, state, sock, SelectToolCmd(tool_name="SSG-48"), 1) + assert isinstance(selected, OkMsg), selected + tick_for( + controller, + state, + lambda: state.current_tool == "SSG-48", + "the select_tool never ran", + seconds=30.0, + ) + indices = [] + for req_id, cmd in enumerate( + ( + MoveJCmd(angles=a, duration=1.0), + ToolActionCmd(tool_key="SSG-48", action="move", params=[1.0, 0.5, 0.5]), + MoveJCmd(angles=b, duration=1.0), + DelayCmd(seconds=0.2), + ), + start=2, + ): + reply = send(controller, state, sock, cmd, req_id) + assert isinstance(reply, OkMsg) and reply.index is not None, reply + indices.append(reply.index) + reaching, closing, leaving, dwelling = indices + + tick_for( + controller, + state, + lambda: state.command_failure(closing) is not None, + "the close on a never-calibrated gripper was not refused", + seconds=30.0, + ) + assert state.command_completed(reaching), ( + "the close was refused before the move queued ahead of it had run" + ) + failure = state.command_failure(closing) + assert failure is not None + assert failure.code == int(ErrorCode.COMM_VALIDATION_ERROR), failure + assert "not calibrated" in failure.cause + assert failure.command_index == closing + + tick_for( + controller, + state, + lambda: all( + state.command_failure(i) is not None for i in (leaving, dwelling) + ), + "the commands queued behind the refused close were not dropped", + seconds=5.0, + ) + for index in (leaving, dwelling): + dropped = state.command_failure(index) + assert dropped is not None + assert dropped.code == int(ErrorCode.MOTN_CANCELLED), dropped + assert dropped.command_index == index + + # Fixed observation window: "stays put" has no condition to poll for. + for _ in range(round(1.0 / INTERVAL_S)): + tick(controller, state) + time.sleep(INTERVAL_S) + assert np.allclose(_angles_deg(state), a, atol=0.05), ( + f"the arm left A after the refusal: {_angles_deg(state)}" + ) + assert state.gripper_hw.feedback_position == 0, "the jaws must not have moved" diff --git a/tests/integration/test_udp_smoke.py b/tests/integration/test_udp_smoke.py index 774b4ca..9dd88b6 100644 --- a/tests/integration/test_udp_smoke.py +++ b/tests/integration/test_udp_smoke.py @@ -3,11 +3,15 @@ Covers PING/PONG, GET_* endpoints, STOP semantics, and basic functionality. """ +import math import socket +import time import pytest from parol6 import RobotClient +from parol6.config import LIMITS +from waldoctl import ActionState @pytest.mark.integration @@ -53,30 +57,63 @@ def test_io(self, client, server_proc): # Test helper method too assert not client.is_estop_pressed() # Should be False in FAKE_SERIAL - def test_joint_speeds(self, client, server_proc): - """Test JOINT_SPEEDS command.""" - speeds = client.joint_speeds() - assert speeds is not None - assert isinstance(speeds, list) - assert len(speeds) == 6 # 6 joint speeds - - # Test helper method too - stopped = client.is_robot_stopped() - assert isinstance(stopped, bool) + def test_joint_speeds_read_deg_per_second(self, client, server_proc): + """While J1 jogs at full speed, the status stream and + ``joint_speeds()`` read its jog velocity limit in deg/s, the other + joints read still, and once the jog ends the arm reads stopped.""" + rate = math.degrees(LIMITS.joint.jog.velocity[0]) + assert client.is_robot_stopped() + assert client.jog_j(0, -1.0, duration=1.5, accel=1.0) == 1 + assert client.wait_status( + lambda s: abs(s.speeds[0]) > 0.9 * rate, timeout=2.0 + ), f"the status stream never read J1 near its {rate:.1f} deg/s jog" + peak = 0.0 + others = 0.0 + deadline = time.monotonic() + 1.5 + while time.monotonic() < deadline: + speeds = client.joint_speeds() + assert speeds is not None + peak = max(peak, abs(speeds[0])) + others = max(others, *(abs(v) for v in speeds[1:])) + time.sleep(0.02) + assert peak == pytest.approx(rate, rel=0.05), ( + f"J1 read {peak:.2f} at a jog of {rate:.2f} deg/s" + ) + assert others < 0.5, f"a still joint read {others:.2f} deg/s" + assert client.wait_motion(timeout=5.0) + assert client.is_robot_stopped() def test_status_aggregate(self, client, server_proc): - """Test STATUS aggregate command.""" - from parol6.protocol.wire import StatusResultStruct - + """STATUS reads what the single queries read of the arm at rest, and + the tool the reset left fitted.""" status = client.status() - assert status is not None - assert isinstance(status, StatusResultStruct) - - # Should contain all status components (as struct attributes) - assert hasattr(status, "pose") - assert hasattr(status, "angles") - assert hasattr(status, "io") - assert hasattr(status, "tool_status") + angles = client.angles() + pose = client.pose() + io = client.io() + assert status is not None and angles and pose and io + assert status.angles == pytest.approx(angles, abs=1e-6) + assert [status.pose[3], status.pose[7], status.pose[11]] == pytest.approx( + pose[:3], abs=1e-3 + ) + assert status.speeds == pytest.approx([0.0] * 6, abs=0.5) + assert status.io == io + assert status.tool_status.key == "NONE" + + def test_activity_names_the_move_under_way(self, client, server_proc): + """ACTIVITY reports the move while it plays and idle once it ends.""" + start = client.angles() + assert start is not None + there = [start[0] - 10.0, *start[1:]] + moving = client.move_j(there, duration=2.0, wait=False) + assert moving >= 0 + assert client.wait_status(lambda s: s.executing_index == moving, timeout=5.0) + playing = client.activity() + assert playing is not None + assert (playing.state, playing.command) == (ActionState.EXECUTING, "move_j") + assert client.wait_command(moving, timeout=10.0) + done = client.activity() + assert done is not None + assert (done.state, done.command) == (ActionState.IDLE, "") @pytest.mark.integration @@ -154,10 +191,6 @@ def test_cartesian_move_validation(self, client, server_proc): """Test cartesian movement with proper validation.""" from parol6.utils.errors import MotionError - # Test that move requires either duration or speed (struct validates) - with pytest.raises(ValueError): - client.move_l([50, 50, 50, 0, 0, 0]) # No duration or speed - # Unreachable pose — planner surfaces IK failure via MotionError with pytest.raises(MotionError): client.move_l( diff --git a/tests/integration/test_unhomed_motion_gate.py b/tests/integration/test_unhomed_motion_gate.py index 2c5f739..27e66ad 100644 --- a/tests/integration/test_unhomed_motion_gate.py +++ b/tests/integration/test_unhomed_motion_gate.py @@ -10,38 +10,90 @@ itself is the way out. """ +import socket + import pytest -from parol6 import MotionError, RobotClient +from parol6 import RobotClient +from parol6.protocol.wire import ( + HomeCmd, + JogJCmd, + MoveJCmd, + OkMsg, + encode_command, +) from parol6.utils.error_codes import ErrorCode +from tests.integration.controller_loop import address, ready, send, tick_for, tick_until pytestmark = pytest.mark.integration -def test_planned_motion_refused_until_homed(client: RobotClient, server_proc): - """move_j from the unhomed boot state raises MOTN_NOT_HOMED (not a - garbage collision prediction); after homing the same move is accepted. +def test_planned_motion_refused_until_homed(controller): + """move_j from the unhomed boot state is refused with MOTN_NOT_HOMED (not + a garbage collision prediction); after homing the same move is accepted. Jog remains available while unhomed.""" + state = controller.state_manager.get_state() + # The boot state: nothing referenced, all-zero steps — exactly how a + # controller starts. Nothing a client sends can un-home a simulator. + ready(controller, state, homed=False) + controller._planner.start() target = [90.0, -90.0, 180.0, 0.0, 0.0, 170.0] - # The autouse fixture homes; reset back to the unhomed boot state - # (Homed_in and Position_in zeroed — exactly how a controller starts). - client.reset_state() - - # The STATUS stream reports the unhomed state (WC feeds it to dry runs). - assert client.wait_status(lambda s: not s.homed, timeout=2.0) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setblocking(False) + queued = send(controller, state, sock, MoveJCmd(angles=target, duration=1.5), 1) + assert isinstance(queued, OkMsg), queued + tick_for( + controller, + state, + lambda: state.error is not None, + "the unhomed move was never refused", + seconds=60.0, + ) + assert state.error is not None + assert state.error.code == int(ErrorCode.MOTN_NOT_HOMED), state.error - with pytest.raises(MotionError, match="not homed") as exc_info: - client.move_j(target, duration=1.5, wait=True) - assert exc_info.value.robot_error.code == int(ErrorCode.MOTN_NOT_HOMED) + # Jogging an unhomed robot stays allowed — no planning involved. A + # jog datagram is not acknowledged; the arm moving is the answer. + sock.sendto( + encode_command( + JogJCmd(speeds=[0.2, 0.0, 0.0, 0.0, 0.0, 0.0], duration=1.0), 2 + ), + address(controller), + ) + tick_until( + controller, + state, + lambda: state.Position_in[0] != 0, + "the unhomed jog never moved the arm", + ticks=300, + ) - # Jogging an unhomed robot stays allowed — no planning involved. - assert client.jog_j(0, 0.2, 0.1) >= 0 - - # Homing establishes references; the identical move now proceeds. - assert client.home(wait=True, timeout=30.0) >= 0 - assert client.wait_status(lambda s: s.homed, timeout=2.0) - assert client.move_j(target, duration=1.5, wait=True) >= 0 + # Homing establishes references; the identical move now proceeds. + homing = send(controller, state, sock, HomeCmd(), 3) + assert isinstance(homing, OkMsg) and homing.index is not None, homing + homed = homing.index + tick_for( + controller, + state, + lambda: all(state.Homed_in[:6]) and state.command_completed(homed), + "homing never referenced the robot", + seconds=60.0, + ) + accepted = send( + controller, state, sock, MoveJCmd(angles=target, duration=1.5), 4 + ) + assert isinstance(accepted, OkMsg), accepted + assert accepted.index is not None + moved = accepted.index + tick_for( + controller, + state, + lambda: state.command_completed(moved) or state.error is not None, + "the move after homing never completed", + seconds=60.0, + ) + assert state.error is None, state.error def test_home_calibrate_rereferences_homed_robot(client: RobotClient, server_proc): diff --git a/tests/test_examples.py b/tests/test_examples.py index b36fd11..bab98c5 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -4,42 +4,69 @@ conflict with the shared integration test server. """ +import contextlib import os +import re import subprocess import sys from pathlib import Path import pytest +from parol6 import Robot + EXAMPLES_DIR = Path(__file__).resolve().parents[1] / "examples" EXAMPLES = sorted( p.name for p in EXAMPLES_DIR.glob("*.py") if not p.name.startswith("_") ) +# Written for a controller that is already running, as a Waldo Commander +# session runs one; the other examples start their own. +ATTACHED = {"demo_showcase.py", "precision.py"} + ENV = { **os.environ, "PAROL6_FAKE_SERIAL": "1", "PAROL6_STATUS_RATE_HZ": "20", } +# What the controller logs for a command it failed or refused in its turn, +# and a client raising it: an example that exits 0 without having waited on +# the failure still reports it here. +FAILURE = re.compile(r"Command \d+ failed|Inline command failed|MotionError") + @pytest.mark.examples @pytest.mark.timeout(300) @pytest.mark.parametrize("script", EXAMPLES) -def test_example_runs(script, ports): - """Run each example as a subprocess and check it exits cleanly.""" - result = subprocess.run( - [sys.executable, str(EXAMPLES_DIR / script)], - # Windows can reserve the default status port even with no listener. - # The subprocess and its controller share the OS-probed test port. - env={**ENV, "PAROL6_MCAST_PORT": str(ports.mcast_port)}, - capture_output=True, - text=True, - timeout=240, - ) +def test_example_runs(script, ports, monkeypatch, caplog): + """Run each example as a subprocess and check it exits cleanly, with + every command it sent carried out.""" + # Windows can reserve the default status port even with no listener. + # The subprocess and its controller share the OS-probed test port. + env = {**ENV, "PAROL6_MCAST_PORT": str(ports.mcast_port)} + with contextlib.ExitStack() as stack: + if script in ATTACHED: + for key, value in env.items(): + monkeypatch.setenv(key, value) + # Its log reaches this process's logging, where caplog reads it. + stack.enter_context(Robot(host="127.0.0.1", port=5001, normalize_logs=True)) + result = subprocess.run( + [sys.executable, str(EXAMPLES_DIR / script)], + env=env, + capture_output=True, + text=True, + timeout=240, + ) assert result.returncode == 0, ( f"{script} failed (exit {result.returncode}):\n" f"--- stdout ---\n{result.stdout[-2000:]}\n" f"--- stderr ---\n{result.stderr[-2000:]}" ) + # An example that starts its own controller relays its log to stderr. + output = "\n".join((result.stdout, result.stderr, caplog.text)) + failures = [line for line in output.splitlines() if FAILURE.search(line)] + assert not failures, ( + f"{script} exited 0, but commands it sent failed:\n" + "\n".join(failures[:20]) + ) diff --git a/tests/test_jit_warmup.py b/tests/test_jit_warmup.py index 2ba1ca4..05bbc43 100644 --- a/tests/test_jit_warmup.py +++ b/tests/test_jit_warmup.py @@ -87,6 +87,12 @@ def test_no_recompile_during_motion() -> None: client.move_j(_W1, speed=0.5, r=5.0) # blended client.move_j(_W2, speed=0.5, r=5.0) # blended client.move_j(_HOME, speed=0.5) + # A cartesian move's joint path is the IK solver's own array, in the + # solver's layout, and from the standby pose it starts with a wrist + # turn: both go through the planner's rad-to-steps conversion. + pose = client.pose() + pose[1] += 15.0 + client.move_l(pose, speed=0.5) client.wait_motion(timeout=10.0) recompiled = sorted( diff --git a/tests/unit/test_blend.py b/tests/unit/test_blend.py index 184e0b3..f0f11ad 100644 --- a/tests/unit/test_blend.py +++ b/tests/unit/test_blend.py @@ -4,8 +4,12 @@ import pytest from parol6.motion.geometry import ( + ArcSegment, + LineSegment, _blend_joint_path_into, _linear_joint_segment_into, + build_blended_path, + build_composite_cartesian_path, build_composite_joint_path, ) @@ -251,3 +255,78 @@ def test_config_exists(self): assert isinstance(MAX_BLEND_LOOKAHEAD, int) assert MAX_BLEND_LOOKAHEAD >= 1 + + +def _se3(xyz_m): + m = np.eye(4) + m[:3, 3] = xyz_m + return m + + +class TestBlendedCartesianPath: + """The corner zone between segments: what it is between two lines, and + that an arc joins it like a line does.""" + + def test_a_line_line_corner_is_the_quadratic_through_the_corner(self): + """A chain of straight moves rounds on the quadratic through the + corner: the cubic zone is that quadratic, degree-raised.""" + a, corner, b = ( + _se3([0.0, 0.0, 0.0]), + _se3([0.1, 0.0, 0.0]), + _se3([0.1, 0.1, 0.0]), + ) + r_mm = 20.0 + path = build_composite_cartesian_path( + [a, corner, b], [r_mm], samples_per_segment=40 + ) + pc, pa, pb = corner[:3, 3], a[:3, 3], b[:3, 3] + entry = pc + (pa - pc) / np.linalg.norm(pa - pc) * r_mm / 1000.0 + exit_ = pc + (pb - pc) / np.linalg.norm(pb - pc) * r_mm / 1000.0 + # Dense enough that the distance to the nearest sample is the + # distance to the curve, well under the tolerance. + t = np.linspace(0.0, 1.0, 200_001)[:, None] + quadratic = (1 - t) ** 2 * entry + 2 * (1 - t) * t * pc + t**2 * exit_ + in_zone = 0 + for pose in path: + q = pose[:3, 3] + if np.linalg.norm(q - pc) > r_mm / 1000.0 + 1e-12: + continue + in_zone += 1 + miss = np.min(np.linalg.norm(quadratic - q, axis=1)) + assert miss < 1e-6, f"a zone sample left the quadratic by {miss:e} m" + assert in_zone > 10 + # The corner is cut, by less than the zone's radius. + closest = min(np.linalg.norm(pose[:3, 3] - pc) for pose in path) + assert 0.001 < closest < r_mm / 1000.0 + + def test_an_arc_rounds_into_the_line_after_it(self): + """line → arc → line with zones at both junctions: the direction of + travel never jumps, each corner is cut inside its zone, and the + arc is still its circle where no zone reaches.""" + a = _se3([0.1, 0.0, 0.0]) + via = _se3([0.16, 0.0, -0.06]) + b = _se3([0.22, 0.0, 0.0]) + c = _se3([0.32, 0.0, 0.0]) + start = _se3([0.0, 0.0, 0.0]) + segments = [LineSegment(start, a), ArcSegment(a, via, b), LineSegment(b, c)] + path = build_blended_path(segments, [20.0, 20.0], samples_per_segment=60) + pts = np.array([pose[:3, 3] for pose in path]) + steps = np.diff(pts, axis=0) + lengths = np.linalg.norm(steps, axis=1) + keep = lengths > 1e-9 + dirs = steps[keep] / lengths[keep][:, None] + cos = np.clip(np.einsum("ij,ij->i", dirs[:-1], dirs[1:]), -1.0, 1.0) + worst_turn = float(np.degrees(np.arccos(cos)).max()) + assert worst_turn < 12.0, f"the path turned {worst_turn:.1f}° in one step" + for corner in (a, b): + closest = float(np.min(np.linalg.norm(pts - corner[:3, 3], axis=1))) + assert 0.001 < closest <= 0.020 + 1e-9, ( + f"corner cut by {closest * 1000:.2f} mm" + ) + center = np.array([0.16, 0.0, 0.0]) + low = pts[pts[:, 2] < -0.03] + assert len(low) > 10 + radial = np.abs(np.linalg.norm(low - center, axis=1) - 0.06) + assert radial.max() < 1e-6, f"the arc left its circle by {radial.max():e} m" + assert np.allclose(pts[-1], c[:3, 3]) + assert np.allclose(pts[0], start[:3, 3]) diff --git a/tests/unit/test_cartesian_streaming_line.py b/tests/unit/test_cartesian_streaming_line.py index 3ecb47e..c732351 100644 --- a/tests/unit/test_cartesian_streaming_line.py +++ b/tests/unit/test_cartesian_streaming_line.py @@ -121,3 +121,46 @@ def test_cartesian_stream_rations_a_mixed_move_between_both_ceilings(): assert peak_ang <= LIMITS.cart.jog.velocity.angular * 1.01, ( f"angular TCP speed {peak_ang:.4f} rad/s over its ceiling" ) + + +@pytest.mark.unit +def test_a_jog_twist_keeps_its_direction_and_an_empty_one_brakes(): + """A jog_l is a 6-DOF twist: a diagonal moves the TCP along the + diagonal it was given, at the configured TCP ceiling rather than + sqrt(2) times it, and a zero twist ramps the tool to rest.""" + cse = CartesianStreamingExecutor(dt=DT) + start = _pose([0.35, 0.10, 0.20]) + cse.sync_pose(start) + ceiling = LIMITS.cart.jog.velocity.linear + twist = np.array([ceiling, ceiling, 0.0, 0.0, 0.0, 0.0]) + unit = np.array([1.0, 1.0, 0.0]) / math.sqrt(2.0) + + cse.set_jog_twist(twist, wrf=True) + prev = start[:3, 3].copy() + worst_off, peak = 0.0, 0.0 + for _ in range(300): + pose, _vel, _finished = cse.tick() + p = pose[:3, 3] + rel = p - start[:3, 3] + worst_off = max(worst_off, float(np.linalg.norm(rel - (rel @ unit) * unit))) + peak = max(peak, float(np.linalg.norm(p - prev)) / DT) + prev = p.copy() + travelled = float(np.linalg.norm(prev - start[:3, 3])) + assert travelled > 0.05, f"the twist never got the tool moving: {travelled:.4f} m" + assert worst_off < 1e-6, f"the tool left its diagonal by {worst_off * 1000:.3f} mm" + assert peak <= ceiling * 1.01, ( + f"the TCP ran at {peak:.4f} m/s over a {ceiling:.4f} m/s ceiling" + ) + assert peak > ceiling * 0.9, f"the tool never reached its ceiling: {peak:.4f} m/s" + + cse.set_jog_twist(np.zeros(6), wrf=True) + for _ in range(300): + pose, vel, finished = cse.tick() + if finished and float(np.dot(vel, vel)) < 1e-12: + break + else: + pytest.fail("a zero twist never brought the tool to rest") + at_rest = pose[:3, 3].copy() + for _ in range(50): + pose, _vel, _finished = cse.tick() + assert np.linalg.norm(pose[:3, 3] - at_rest) < 1e-9, "the tool crept after braking" diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py index 37e41bc..1e6b153 100644 --- a/tests/unit/test_collision_integration.py +++ b/tests/unit/test_collision_integration.py @@ -326,13 +326,13 @@ def test_pose_to_matrix_is_extrinsic_xyz_rpy(): assert np.allclose(T[:3, 3], [0.1, 0.2, 0.3]) -def test_jogl_release_decel_streams_while_escaping(): +@pytest.mark.parametrize("tool", ["NONE", "SSG-48"]) +def test_jogl_release_decel_streams_while_escaping(tool): """Releasing a Cartesian jog while ESCAPING from inside a keep-out must keep streaming the deceleration (escape-aware gate) — the pre-fix bare ``in_collision`` skipped every decel send, freezing the target at the - release point. Real command, real state, real checker, real IK.""" - import time as _time - + release point. Bare and with a gripper, whose body the cage also holds. + Real command, real state, real checker, real IK.""" from waldoctl import Box from parol6.commands.cartesian_commands import JogLCommand @@ -342,11 +342,13 @@ def test_jogl_release_decel_streams_while_escaping(): from parol6.config import deg_to_steps state = ControllerState() + state.Homed_in[:] = 1 # a cartesian jog is refused unreferenced # IK-friendly physical home (wrist at ~(0.237, 0, 0.334)); q=zeros is an # IK danger zone where the jog never streams. q_home_deg = np.array([0.0, -90.0, 180.0, 0.0, 0.0, 180.0]) deg_to_steps(q_home_deg, state.Position_in) try: + PAROL6_ROBOT.apply_tool(tool) # Box centred 5 cm below the wrist: +Z is unambiguously the escape. PAROL6_ROBOT.apply_shapes( [ @@ -361,12 +363,13 @@ def test_jogl_release_decel_streams_while_escaping(): ) assert PAROL6_ROBOT.collision.in_collision(np.radians(q_home_deg)) is True - cmd = JogLCommand(JogLCmd(velocities=[0.0, 0.0, 1.0, 0, 0, 0], duration=5.0)) + # The jog's duration is counted in ticks: 30 ticks held, released + # on the next. + cmd = JogLCommand(JogLCmd(velocities=[0.0, 0.0, 1.0, 0, 0, 0], duration=0.3)) cmd.setup(state) for _ in range(30): # held phase: build real velocity (CSE dt=0.01/tick) cmd.execute_step(state) assert state.Command_out != 0 # held phase streamed (escape allowed) - cmd._t_end = _time.perf_counter() - 1.0 # deterministic release code = cmd.execute_step(state) pos0 = state.Position_out.copy() @@ -381,6 +384,7 @@ def test_jogl_release_decel_streams_while_escaping(): assert moved, "decel sends were skipped — target frozen at release point" finally: PAROL6_ROBOT.apply_shapes([]) + PAROL6_ROBOT.apply_tool("NONE") def test_jogl_escape_never_streams_into_a_second_keepout(): @@ -397,6 +401,7 @@ def test_jogl_escape_never_streams_into_a_second_keepout(): from parol6.server.state import ControllerState state = ControllerState() + state.Homed_in[:] = 1 # a cartesian jog is refused unreferenced q_home_deg = np.array([0.0, -90.0, 180.0, 0.0, 0.0, 180.0]) deg_to_steps(q_home_deg, state.Position_in) state.Position_out[:] = state.Position_in diff --git a/tests/unit/test_command_completion_wire.py b/tests/unit/test_command_completion_wire.py index f06b969..19f441d 100644 --- a/tests/unit/test_command_completion_wire.py +++ b/tests/unit/test_command_completion_wire.py @@ -43,8 +43,10 @@ def completed(index): assert not completed(10) and completed(9) and completed(1033) state.record_completion(1034) assert not completed(9) and completed(1034) + # The history outlives reset_state: a wait on a command issued before + # the reset still resolves, and indices keep counting. state.reset() - assert not completed(1034) + assert completed(1034) def test_completion_packets_reject_invalid_indices_sessions_and_verdicts(): diff --git a/tests/unit/test_controller_system_commands.py b/tests/unit/test_controller_system_commands.py index 5b15e80..0627a1d 100644 --- a/tests/unit/test_controller_system_commands.py +++ b/tests/unit/test_controller_system_commands.py @@ -2,7 +2,7 @@ Unit tests for system command side-effect signaling. Tests verify that system commands set typed side-effect attributes -(_switch_simulator, _switch_port, _sync_mock) which the controller +(_switch_simulator, _switch_port) which the controller reads to trigger infrastructure changes. """ @@ -68,16 +68,3 @@ def test_set_port_command_fail_leaves_no_side_effect(self): assert code == ExecutionStatusCode.FAILED assert cmd._switch_port is None - - def test_reset_command_sets_sync_mock(self): - """Verify RESET_STATE command sets _sync_mock attribute.""" - from parol6.commands.utility_commands import ResetStateCommand - from parol6.protocol.wire import ResetStateCmd - - cmd = ResetStateCommand(ResetStateCmd()) - state = ControllerState() - - code = cmd.execute_step(state) - - assert code == ExecutionStatusCode.COMPLETED - assert cmd._sync_mock is True diff --git a/tests/unit/test_dry_run_parity.py b/tests/unit/test_dry_run_parity.py new file mode 100644 index 0000000..14b1b95 --- /dev/null +++ b/tests/unit/test_dry_run_parity.py @@ -0,0 +1,174 @@ +"""A program previews as it runs: the dry run accepts, refuses and cancels +what the live client and controller accept, refuse and cancel.""" + +import asyncio + +import numpy as np +import pytest + +from parol6 import AsyncRobotClient +from parol6.client.dry_run_client import DryRunRobotClient +from parol6.tools import get_registry +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import MotionError +from tests.conftest import free_udp_port + +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] + + +def _jaws_at_end(client: DryRunRobotClient, index: int) -> float: + record = client.plan() + block = record.blocks[index] + assert block.error is None and block.rows > 0 + return float(record.tool_closed[block.start_row + block.rows - 1]) + + +def test_tool_verbs_run_as_the_live_tools_run_them(): + """The selected tool offers the live tool's verbs and properties, in the + forms the live tool takes them, and each sends the action the live one + sends.""" + client = DryRunRobotClient(initial_joints_deg=HOME, initial_gripper_calibrated=True) + assert client.select_tool("SSG-48") >= 0 + tool = client.tool + + index = tool.set_position(position=0.4) + assert _jaws_at_end(client, index) == pytest.approx(0.4, abs=0.05) + assert tool.status().position == pytest.approx(0.4) + + assert tool.current_range == get_registry()["SSG-48"].current_range + assert tool.is_open(0.3) and not tool.is_open(0.7) + tool.action_l(False) + assert tool.status().position == 1.0 + tool.action_l(True) + assert tool.status().position == 0.0 + + # A valve is open or shut: a move to 0.7 closes it, as the live valve does. + assert client.select_tool("PNEUMATIC") >= 0 + index = client.tool_action("PNEUMATIC", "move", [0.7]) + assert _jaws_at_end(client, index) > 0.9 + assert client.tool.status().position == 1.0 + + assert client.select_tool("msg") >= 0 + assert client.tool.key == "MSG" + + +def test_malformed_motion_is_refused_as_live_refuses_it(): + """A jog frame or axis list the live client rejects, a keyword the live + signature does not have, and a frame the controller's decoder rejects + are refused in the preview too, and nothing moves.""" + preview = DryRunRobotClient(initial_joints_deg=HOME) + before = preview.angles() + + async def live(call) -> None: + client = AsyncRobotClient(host="127.0.0.1", port=free_udp_port()) + try: + await call(client) + finally: + await client.close() + + def jog_wrf(c): + return c.jog_l("wrf", "X", 0.5, 0.5) + + def jog_mismatch(c): + return c.jog_l("WRF", axes=["X", "Y"], speeds_list=[0.5], duration=0.5) + + for call, raised in ((jog_wrf, ValueError), (jog_mismatch, ValueError)): + with pytest.raises(raised) as live_refusal: + asyncio.run(live(call)) + with pytest.raises(raised) as preview_refusal: + call(preview) + assert str(preview_refusal.value) == str(live_refusal.value) + + with pytest.raises(TypeError): + preview.servo_j(before, sped=0.5) + with pytest.raises(TypeError): + preview.servo_l(preview.pose(), acel=0.5) + + pose = preview.pose() + for refused_move in ( + lambda: preview.move_l([0, 0, -10, 0, 0, 0], frame="trf", rel=True, speed=0.5), + lambda: preview.move_s( + [[pose[0] + 5, *pose[1:]], [pose[0] + 10, pose[1] + 5, *pose[2:]]], + frame="wrf", + speed=0.5, + ), + ): + with pytest.raises(MotionError) as refusal: + refused_move() + assert refusal.value.code == ErrorCode.COMM_VALIDATION_ERROR + block = preview.plan().blocks[-1] + assert block.rows == 0 and block.error is not None + assert block.error.code == ErrorCode.COMM_VALIDATION_ERROR + + assert preview.angles() == pytest.approx(before, abs=1e-6) + + +def test_a_cancel_fails_the_blend_chain_it_drops(): + """A blended move still held for its blend when the program stops, + e-stops, resets, switches to the simulator or teleports never runs: it + fails as cancelled, as the controller fails it, rather than reading as + done.""" + cancels = { + "stop": lambda c: c.stop(), + "estop": lambda c: c.estop(), + "reset_state": lambda c: c.reset_state(), + "simulator": lambda c: c.simulator(True), + "teleport": lambda c: c.teleport(HOME), + } + for name, cancel in cancels.items(): + client = DryRunRobotClient(initial_joints_deg=HOME) + pose = client.pose() + chain = client.move_l([pose[0], pose[1], pose[2] - 20.0, *pose[3:]], r=20) + cancel(client) + assert not client.wait_command(chain), name + record = client.plan() + block = record.blocks[chain] + assert block.rows == 0, name + assert block.error is not None, name + assert block.error.code == ErrorCode.MOTN_CANCELLED, name + assert record.stop == "failed", name + + +def test_teleport_is_refused_and_applied_as_the_controller_does(): + """A teleport is refused while the controller is disabled and when its + tool positions do not match the fitted tool, leaving the arm where it + was; an accepted one puts the jaws where it says.""" + client = DryRunRobotClient(initial_joints_deg=HOME) + client.estop() + with pytest.raises(MotionError) as refusal: + client.teleport(W1) + assert refusal.value.code == ErrorCode.SYS_CONTROLLER_DISABLED + assert client.plan().blocks[-1].error is not None + assert client.angles() == pytest.approx(HOME, abs=0.01) + client.reset() + + with pytest.raises(MotionError) as refusal: + client.teleport(W1, tool_positions=[1.0]) + assert refusal.value.code == ErrorCode.COMM_VALIDATION_ERROR + assert client.angles() == pytest.approx(HOME, abs=0.01) + + assert client.select_tool("SSG-48") >= 0 + assert client.teleport(W1, tool_positions=[1.0]) == 1 + record = client.plan() + assert record.tool_closed[-1] == pytest.approx(1.0) + np.testing.assert_allclose(np.degrees(record.joints_rad[-1]), W1, atol=0.01) + assert client.tool.status().positions == (1.0,) + + +def test_a_transport_switch_forgets_the_gripper_calibration(): + """The gripper behind a new transport has not been calibrated: after a + simulator toggle or a hardware connect, a jaw move is refused until the + program calibrates again, as the controller refuses it.""" + for switch in ( + lambda c: c.simulator(True), + lambda c: c.connect_hardware("/dev/ttyUSB0"), + ): + client = DryRunRobotClient(initial_joints_deg=HOME) + assert client.select_tool("SSG-48") >= 0 + assert client.wait_command(client.tool.calibrate()) + switch(client) + index = client.tool.close() + error = client.plan().blocks[index].error + assert error is not None and error.code == ErrorCode.COMM_VALIDATION_ERROR + assert "not calibrated" in error.cause diff --git a/tests/unit/test_dry_run_record.py b/tests/unit/test_dry_run_record.py index a08a80d..2735a8e 100644 --- a/tests/unit/test_dry_run_record.py +++ b/tests/unit/test_dry_run_record.py @@ -7,7 +7,7 @@ from parol6.client.dry_run_client import DryRunRobotClient from tests.conftest import rows_for -from parol6.tools import get_registry +from parol6.tools import ElectricGripperConfig, 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] @@ -40,11 +40,18 @@ def test_delay_holds_the_pose_for_its_rows(): def test_gripper_close_ramps_the_jaws_over_the_tools_travel(): client = DryRunRobotClient(initial_joints_deg=HOME) - assert client.select_tool("SSG-48") == 1 + assert client.select_tool("SSG-48") >= 0 + # A jaw move before a calibrate previews as the refusal the controller + # would give it. + refused = client.tool.close() + assert client.plan().blocks[refused].error is not None + assert client.tool.calibrate() >= 0 index = client.tool.close() record = client.plan() block = record.blocks[index] - expected = get_registry().get("SSG-48").estimate_duration("close", []) + spec = get_registry().get("SSG-48") + assert isinstance(spec, ElectricGripperConfig) + expected = spec.estimate_duration("move", [1.0, 0.5, 0.5]) assert expected > 0 assert block.rows == pytest.approx(rows_for(expected), abs=1) closed = record.tool_closed[_span(record, block)] @@ -117,3 +124,15 @@ def test_budget_truncates_the_record_and_says_so(): 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" + + +def test_a_move_that_starts_with_a_wrist_turn_takes_the_duration_it_names(): + """From standby a tool-frame reorientation first turns the wrist out of + its singularity; a duration names the whole move, turn included.""" + client = DryRunRobotClient(initial_joints_deg=HOME) + index = client.move_l([0.0, 0.0, 0.0, -15.0, 0.0, 0.0], frame="TRF", duration=2.0) + record = client.plan() + block = record.blocks[index] + assert block.error is None, block.error + # The block's first row is the pose the move starts from, at t = 0. + assert (block.rows - 1) * record.row_dt_s == pytest.approx(2.0, abs=record.row_dt_s) diff --git a/tests/unit/test_dry_run_script_compat.py b/tests/unit/test_dry_run_script_compat.py index eaba405..bc659a9 100644 --- a/tests/unit/test_dry_run_script_compat.py +++ b/tests/unit/test_dry_run_script_compat.py @@ -1,20 +1,25 @@ """Verify that all motion commands users write in scripts work through the dry run client. -The dry run client uses __getattr__ + build_cmd to dispatch calls by mapping -kwargs to wire struct fields. If the client API param names don't match the -struct field names, the kwargs get silently dropped and the command fails. +The dry run client maps kwargs to wire struct fields through build_cmd, from +its own move/servo/jog methods and, for the rest, __getattr__. If the client +API param names don't match the struct field names, the kwargs get silently +dropped and the command fails. This test calls every user-facing motion method with the same signatures shown in the docs / editor auto-complete, ensuring the dry run path doesn't diverge from the real client. """ +import asyncio + import numpy as np import pytest from waldoctl import CommandKind, command_table +from parol6 import AsyncRobotClient from parol6.client.dry_run_client import _CMD_STRUCTS, DryRunRobotClient -from tests.conftest import rows_for +from parol6.config import LIMITS +from tests.conftest import free_udp_port, rows_for 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] @@ -160,7 +165,7 @@ def test_referenced_home_previews_as_return_move(self): def test_snap_carries_the_pending_blend_chain(self): """A blended move still buffered when the script homes with calibrate - (or teleports) is planned under its own command before the snap — + 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) @@ -230,3 +235,73 @@ def test_state_commands_answer_with_the_live_clients_int_codes(client, name): result = getattr(client, name)(*_STATE_ARGS[name]) assert isinstance(result, int) and not isinstance(result, bool) assert result == 1 + + +def test_a_joint_jog_previews_the_distance_it_runs(): + """A jog of J1 at a fifth of the jog speed for one second travels about + a fifth of a second's travel at full jog speed, as on the arm.""" + client = DryRunRobotClient(initial_joints_deg=HOME) + before = np.asarray(client.angles()) + assert client.jog_j(0, 0.2, 1.0) == 1 + record = client.plan() + block = record.blocks[client.program_length - 1] + travel = np.radians( + np.degrees(record.joints_rad[block.start_row + block.rows - 1][0]) - before[0] + ) + expected = 0.2 * float(LIMITS.joint.jog.velocity[0]) * 1.0 + assert travel == pytest.approx(expected, rel=0.15), ( + f"jog_j(0, 0.2, 1.0) previewed {np.degrees(travel):.1f}° of J1 travel, " + f"not about {np.degrees(expected):.1f}°" + ) + + +def test_a_relative_pose_move_j_is_refused_in_preview_and_live(): + """A pose target is absolute: ``move_j(pose=..., rel=True)`` names no move + either client can run, so both refuse it rather than dropping ``rel``.""" + with pytest.raises(ValueError, match="rel=True"): + DryRunRobotClient(initial_joints_deg=HOME).move_j( + pose=POSE_A, speed=0.5, rel=True + ) + + async def live() -> None: + client = AsyncRobotClient(host="127.0.0.1", port=free_udp_port()) + try: + await client.move_j(pose=POSE_A, speed=0.5, rel=True) + finally: + await client.close() + + with pytest.raises(ValueError, match="rel=True"): + asyncio.run(live()) + + +def test_a_move_out_of_the_wrist_singularity_is_timed_by_its_length_not_a_snap(): + """The turn a chain takes out of the singularity is the one whose + chain stays continuous. With the split between J4 and J6 settled a + hair wrong, the solver snaps the wrist round in one row once the + tool has barely left the singularity; TOPP-RA slows over that row, + but a profile that times the path by its rows stretches a 5 mm move + to many times its length.""" + for profile in ("LINEAR", "TRAPEZOID"): + client = DryRunRobotClient(initial_joints_deg=HOME) + assert client.select_profile(profile) == 1 + client.set_tcp_transform(5.0, -3.0, 20.0, 20.0, 25.0, -10.0) + index = client.move_l( + [0.0, 0.0, 5.0, 0.0, 0.0, 0.0], frame="TRF", rel=True, speed=0.2 + ) + record = client.plan() + block = record.blocks[index] + assert block.error is None, block.error + planned = block.rows * record.row_dt_s + assert planned < 3.0, f"{profile}: a 5 mm move planned as {planned:.2f} s" + + +def test_a_move_that_leaves_the_wrist_singularity_previews_as_it_runs(): + """From standby the wrist is singular; a tool-frame reorientation leaves + it through a turn of J4 on the arm, and the preview plans the same turn + every time rather than calling the move unreachable.""" + for _ in range(10): + client = DryRunRobotClient(initial_joints_deg=HOME) + index = client.move_l([0.0, 0.0, 0.0, -15.0, 0.0, 0.0], frame="TRF", speed=0.5) + block = client.plan().blocks[index] + assert block.error is None, block.error + assert block.rows > 1 diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 7567b2d..01abdce 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -6,9 +6,8 @@ from parol6.config import INTERVAL_S, LIMITS, steps_to_rad from parol6.motion import JointPath, ProfileType, Trajectory, TrajectoryBuilder from parol6.motion.trajectory import ( - _IK_OUTLIER_PADDING, - _IK_OUTLIER_RATIO, - _smooth_singularity_outliers, + IK_MAX_JOINT_STEP_RAD, + _ik_branch_hop, ) @@ -242,126 +241,28 @@ def test_getitem_returns_step(self): assert np.array_equal(traj[-1], steps[-1]) -class TestSmoothSingularityOutliers: - """Tests for _smooth_singularity_outliers: LM-IK branch-hop repair.""" +class TestIkBranchHops: + """A cartesian IK chain that hops branches is refused, never smoothed: + the hop is where the arm would whip, and interpolating across it + commands a path nobody checked.""" @staticmethod - def _smooth_chain(n: int = 40, step: float = 0.01) -> np.ndarray: - """A 6-DOF chain with uniform per-sample step magnitude.""" + def _chain(n: int = 40, step: float = 0.01) -> np.ndarray: positions = np.zeros((n, 6), dtype=np.float64) for i in range(n): positions[i] = i * step return positions - def test_smooth_path_unchanged(self): - positions = self._smooth_chain() - original = positions.copy() - n = _smooth_singularity_outliers(positions) - assert n == 0 - assert np.array_equal(positions, original) - - def test_single_sample_hop_is_smoothed(self): - """One outlier sample fires bad-flags on both neighbors (both adjacent - diffs are large) → run of 3, plus pad on each side → 2*pad+3 patched.""" - positions = self._smooth_chain(n=40) - bad_idx = 20 - positions[bad_idx, 3] += 5.0 # 5 rad J4 jump — well past 10× median - - n_patched = _smooth_singularity_outliers(positions) - assert n_patched == 2 * _IK_OUTLIER_PADDING + 3 - # The outlier is gone; uniform-step chain interpolates exactly. - assert positions[bad_idx, 3] == pytest.approx(bad_idx * 0.01, abs=1e-9) - # Samples just outside the patched window are untouched. - outside_lo = bad_idx - 1 - _IK_OUTLIER_PADDING - 1 - outside_hi = bad_idx + 1 + _IK_OUTLIER_PADDING - assert positions[outside_lo, 3] == pytest.approx(outside_lo * 0.01) - assert positions[outside_hi, 3] == pytest.approx(outside_hi * 0.01) - - def test_multi_sample_run_is_smoothed(self): - """Wide hop with monotonically rising outliers → each step is large, - so the whole shelf forms one contiguous bad run.""" - positions = self._smooth_chain(n=40) - # Use increasing magnitudes so every adjacent diff exceeds threshold. - positions[20, 3] += 4.0 - positions[21, 3] += 8.0 - positions[22, 3] += 12.0 - - n_patched = _smooth_singularity_outliers(positions) - # Bad samples: {19, 20, 21, 22, 23} → run [19, 24). - # Patched: [max(1,19-pad), min(n-1,23+pad)+1) = [15, 28) → 13. - assert n_patched == 5 + 2 * _IK_OUTLIER_PADDING - # All run samples land back on the chain. - for k in (20, 21, 22): - assert positions[k, 3] == pytest.approx(k * 0.01, abs=1e-9) - - def test_outlier_near_start_clamps_padding(self): - """Padding clamps at the array boundary; bookend at index 0.""" - positions = self._smooth_chain(n=40) - positions[2, 3] += 5.0 - original_first = positions[0].copy() - original_last = positions[-1].copy() - - _smooth_singularity_outliers(positions) - assert positions[2, 3] == pytest.approx(2 * 0.01, abs=1e-9) - # Endpoints must never be modified. - assert np.array_equal(positions[0], original_first) - assert np.array_equal(positions[-1], original_last) - - def test_outlier_near_end_clamps_padding(self): - positions = self._smooth_chain(n=40) - positions[-3, 3] += 5.0 - original_first = positions[0].copy() - original_last = positions[-1].copy() - - _smooth_singularity_outliers(positions) - assert positions[-3, 3] == pytest.approx((40 - 3) * 0.01, abs=1e-9) - assert np.array_equal(positions[0], original_first) - assert np.array_equal(positions[-1], original_last) - - def test_short_path_is_noop(self): - for n in (0, 1, 2): - positions = np.zeros((n, 6), dtype=np.float64) - assert _smooth_singularity_outliers(positions) == 0 - - def test_zero_motion_is_noop(self): - """Median step = 0 → can't form a ratio threshold; bail.""" - positions = np.zeros((20, 6), dtype=np.float64) - positions[10, 3] = 5.0 # would-be outlier but median is 0 - n = _smooth_singularity_outliers(positions) - assert n == 0 - assert positions[10, 3] == 5.0 - - def test_below_threshold_step_not_smoothed(self): - """Steps within `_IK_OUTLIER_RATIO`× median are normal motion, not hops.""" - positions = self._smooth_chain(n=40) - # Bump one sample by ~5× the median step — should NOT trigger. - positions[20, 3] += 5 * 0.01 - original = positions.copy() - n = _smooth_singularity_outliers(positions) - assert n == 0 - assert np.array_equal(positions, original) - - def test_threshold_exactly_at_ratio_not_smoothed(self): - """Strict > comparison: step == threshold is NOT an outlier.""" - positions = self._smooth_chain(n=40) - # Each step ~ sqrt(6) * 0.01; bump just at the threshold boundary. - median_step = np.sqrt(6) * 0.01 - # Add exactly ratio * median to one component so the step magnitude - # is just over the legitimate motion but well within tolerance. - positions[20, 3] += _IK_OUTLIER_RATIO * median_step - 0.5 * median_step - n = _smooth_singularity_outliers(positions) - assert n == 0 - - def test_repair_preserves_endpoints(self): - """No matter where the hop is, positions[0] and positions[-1] are - always exactly preserved (they're the IK seed and final target).""" - rng = np.random.default_rng(0) - for _ in range(20): - positions = self._smooth_chain(n=50) - hop_idx = int(rng.integers(2, 48)) - positions[hop_idx, rng.integers(0, 6)] += rng.uniform(2.0, 8.0) - first = positions[0].copy() - last = positions[-1].copy() - _smooth_singularity_outliers(positions) - assert np.array_equal(positions[0], first) - assert np.array_equal(positions[-1], last) + def test_a_smooth_chain_and_one_at_the_step_limit_pass(self): + assert _ik_branch_hop(self._chain()) is None + at_limit = self._chain(n=3, step=IK_MAX_JOINT_STEP_RAD) + assert _ik_branch_hop(at_limit) is None + assert _ik_branch_hop(self._chain(n=1)) is None + + def test_a_hop_is_reported_at_the_waypoint_it_lands_on(self): + positions = self._chain() + positions[20, 3] += IK_MAX_JOINT_STEP_RAD * 1.01 + assert _ik_branch_hop(positions) == 20 + wide = self._chain() + wide[25:, 4] -= 2.0 + assert _ik_branch_hop(wide) == 25 diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py deleted file mode 100644 index dc35d34..0000000 --- a/tests/unit/test_query_commands_actions.py +++ /dev/null @@ -1,55 +0,0 @@ -""" -Unit tests for action-related query commands. - -Tests the ACTIVITY query command without requiring a running server. -Uses minimal state objects to test command logic in isolation. -""" - -from waldoctl import ActionState - -from parol6.commands.query_commands import ActivityCommand -from parol6.server.state import ControllerState -from parol6.protocol.wire import ( - ActivityCmd, - CurrentActionResultStruct, -) - - -def test_activity_returns_details(): - """Test that ACTIVITY compute() returns correct data.""" - state = ControllerState( - action_current="MoveJPoseCommand", - action_state=ActionState.EXECUTING, - action_next="HomeCommand", - action_params="angles=[10,20,30,40,50,60]", - ) - - cmd = ActivityCommand(ActivityCmd()) - cmd.setup(state) - result = cmd.compute(state) - - assert isinstance(result, CurrentActionResultStruct) - assert result.current == "MoveJPoseCommand" - assert result.state == "EXECUTING" - assert result.next == "HomeCommand" - assert result.params == "angles=[10,20,30,40,50,60]" - - -def test_activity_with_idle_state(): - """Test ACTIVITY when robot is idle.""" - state = ControllerState( - action_current="", - action_state=ActionState.IDLE, - action_next="", - action_params="", - ) - - cmd = ActivityCommand(ActivityCmd()) - cmd.setup(state) - result = cmd.compute(state) - - assert isinstance(result, CurrentActionResultStruct) - assert result.current == "" - assert result.state == "IDLE" - assert result.next == "" - assert result.params == "" diff --git a/tests/unit/test_reset_command.py b/tests/unit/test_reset_command.py index 49e3518..e3d6b25 100644 --- a/tests/unit/test_reset_command.py +++ b/tests/unit/test_reset_command.py @@ -9,44 +9,49 @@ class TestResetCommandExecution: - """Test ResetStateCommand.tick resets state correctly.""" + """ResetStateCommand.tick resets the program-level state and nothing + physical.""" - def test_reset_clears_positions(self): - """Reset should zero out position buffers.""" + def test_reset_restores_program_state_and_keeps_the_physical(self): state = ControllerState() state.Position_in = np.array( [1000, 2000, 3000, 4000, 5000, 6000], dtype=np.int32 ) state.Speed_in = np.array([10, 20, 30, 40, 50, 60], dtype=np.int32) - - cmd = ResetStateCommand(ResetStateCmd()) - cmd.tick(state) # Reset executes in tick - - assert np.all(state.Position_in == 0) - assert np.all(state.Speed_in == 0) - - def test_reset_clears_errors(self): - """Reset should clear error states.""" - state = ControllerState() + state.Homed_in[:] = 1 + state.InOut_out[2] = 1 + state.Gripper_data_out[0] = 128 + state.gripper_calibrated = True + state.enabled = False + state.disabled_reason = "E-STOP pressed" state.e_stop_active = True state.soft_error = True - state.disabled_reason = "some error" - - cmd = ResetStateCommand(ResetStateCmd()) - cmd.tick(state) - - assert state.e_stop_active is False - assert state.soft_error is False - assert state.disabled_reason == "" - - def test_reset_clears_tool(self): - """Reset should reset tool to NONE.""" - state = ControllerState() + state.execution_paused = True + state.execution_speed = 0.4 + state.motion_profile = "RUCKIG" state._current_tool = "GRIPPER" cmd = ResetStateCommand(ResetStateCmd()) cmd.tick(state) + assert cmd.is_finished is True + # What the firmware reports and what has been commanded to it stay. + assert list(state.Position_in) == [1000, 2000, 3000, 4000, 5000, 6000] + assert list(state.Speed_in) == [10, 20, 30, 40, 50, 60] + assert all(state.Homed_in[:6]) + assert state.InOut_out[2] == 1 + assert state.Gripper_data_out[0] == 128 + assert state.gripper_calibrated + # The protective stop stays latched: only reset() clears it. + assert not state.enabled + assert state.disabled_reason == "E-STOP pressed" + assert state.e_stop_active + # The program-level state is back at its defaults. + assert state.soft_error is False + assert state.error is None + assert not state.execution_paused + assert state.execution_speed == 1.0 + assert state.motion_profile == "TOPPRA" assert state._current_tool == "NONE" def test_reset_preserves_connection_state(self): @@ -65,14 +70,6 @@ def test_reset_preserves_connection_state(self): assert state.start_time == 12345.0 assert state.ser == "mock_serial" - def test_reset_finishes_immediately(self): - """Reset command should complete in single tick.""" - state = ControllerState() - cmd = ResetStateCommand(ResetStateCmd()) - cmd.tick(state) - - assert cmd.is_finished is True - @pytest.mark.integration class TestResetIntegration: diff --git a/tests/unit/test_servo_wire.py b/tests/unit/test_servo_wire.py new file mode 100644 index 0000000..2f7d5fe --- /dev/null +++ b/tests/unit/test_servo_wire.py @@ -0,0 +1,54 @@ +"""A streamed servo target is validated where the controller decodes it, as +the planned moves are: a non-finite value, or a joint angle outside its +range, never reaches a stream.""" + +import math + +import msgspec +import pytest + +from parol6.config import LIMITS +from parol6.protocol.wire import ( + CmdType, + ServoJCmd, + ServoJPoseCmd, + ServoLCmd, + decode_command, + encode, +) + +STANDBY = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] +POSE = [236.8, 0.0, 334.0, -180.0, -90.0, -180.0] + + +def _with(values: list[float], index: int, value: float) -> list[float]: + out = list(values) + out[index] = value + return out + + +@pytest.mark.parametrize( + ("tag", "cls", "valid"), + [ + (CmdType.SERVOJ, ServoJCmd, STANDBY), + (CmdType.SERVOJ_POSE, ServoJPoseCmd, POSE), + (CmdType.SERVOL, ServoLCmd, POSE), + ], +) +def test_a_servo_target_decodes_only_when_finite(tag, cls, valid): + assert decode_command(encode([tag, valid, 0.5, 0.5])) == cls(valid, 0.5, 0.5) + for i in range(6): + for bad in (math.nan, math.inf, -math.inf): + with pytest.raises(msgspec.ValidationError): + decode_command(encode([tag, _with(valid, i, bad), 0.5, 0.5])) + + +def test_a_servo_j_target_decodes_only_inside_the_joint_range(): + lo, hi = LIMITS.joint.position.deg[:, 0], LIMITS.joint.position.deg[:, 1] + for i in range(6): + for edge in (float(lo[i]), float(hi[i])): + angles = _with(STANDBY, i, edge) + assert decode_command(encode([CmdType.SERVOJ, angles])) == ServoJCmd(angles) + for outside in (float(lo[i]) - 0.5, float(hi[i]) + 0.5): + with pytest.raises(msgspec.ValidationError, match=f"Joint {i + 1}"): + decode_command(encode([CmdType.SERVOJ, _with(STANDBY, i, outside)])) diff --git a/tests/unit/test_status_cache_enablement_ascii.py b/tests/unit/test_status_cache_enablement_ascii.py index b1d7ac0..abd521d 100644 --- a/tests/unit/test_status_cache_enablement_ascii.py +++ b/tests/unit/test_status_cache_enablement_ascii.py @@ -11,6 +11,18 @@ import parol6.PAROL6_ROBOT as PAROL6_ROBOT +def _wait_for_ik(cache: StatusCache, timeout: float) -> bool: + """Poll for the worker's answer. Its first one includes the spawned + process importing the robot stack (~2.4 s on a Pi, longer on a loaded + Windows runner); later ones take ~0.08 s.""" + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if cache._poll_ik_results(): + return True + time.sleep(0.02) + return False + + @pytest.mark.integration def test_ik_worker_detects_joint_limits(): """ @@ -40,12 +52,7 @@ def test_ik_worker_detects_joint_limits(): cache._submit_ik_request(q, T_matrix) - ready = False - for _ in range(200): # Longer timeout for CI - IK worker does 24 IK solves - ready = cache._poll_ik_results() - if ready: - break - time.sleep(0.02) + ready = _wait_for_ik(cache, timeout=10.0) assert ready, "IK worker did not return results" joint_en = cache.joint_en @@ -66,12 +73,7 @@ def test_ik_worker_detects_joint_limits(): cache._submit_ik_request(q, T_matrix) - ready = False - for _ in range(200): # Longer timeout for CI - IK worker does 24 IK solves - ready = cache._poll_ik_results() - if ready: - break - time.sleep(0.02) + ready = _wait_for_ik(cache, timeout=2.0) assert ready, "IK worker did not return results for min limit test" joint_en = cache.joint_en @@ -109,12 +111,7 @@ def test_ik_worker_all_enabled_in_safe_position(): cache._submit_ik_request(q_home, T_matrix) - ready = False - for _ in range(200): # Longer timeout for CI - IK worker does 24 IK solves - ready = cache._poll_ik_results() - if ready: - break - time.sleep(0.02) + ready = _wait_for_ik(cache, timeout=10.0) assert ready, "IK worker did not return results in time" joint_en = cache.joint_en