From 4b242c8398381e6185b1205f5d5cad105f7ed7d1 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 19:01:47 -0400 Subject: [PATCH 1/4] Retime queued motion and preserve explicit pause state --- README.md | 26 ++++ parol6/ack_policy.py | 3 + parol6/client/async_client.py | 118 ++++++++++++++-- parol6/client/dry_run_client.py | 44 +++++- parol6/client/sync_client.py | 17 +++ parol6/commands/base.py | 3 +- parol6/commands/cartesian_commands.py | 2 + parol6/commands/curved_commands.py | 1 + parol6/commands/joint_commands.py | 2 + parol6/commands/query_commands.py | 20 +++ parol6/commands/system_commands.py | 28 ++++ parol6/commands/utility_commands.py | 9 +- parol6/config.py | 10 ++ parol6/motion/trajectory.py | 25 ++-- parol6/protocol/wire.py | 74 ++++++++++ parol6/server/motion_planner.py | 17 +++ parol6/server/segment_player.py | 148 +++++++++++++++++++- parol6/server/state.py | 3 + tests/integration/test_execution_speed.py | 67 +++++++++ tests/unit/test_dry_run_blend.py | 26 ++++ tests/unit/test_execution_speed_playback.py | 82 +++++++++++ tests/unit/test_execution_speed_wire.py | 75 ++++++++++ tests/unit/test_motion.py | 8 +- tests/unit/test_motion_pipeline.py | 32 ++++- 24 files changed, 800 insertions(+), 40 deletions(-) create mode 100644 tests/integration/test_execution_speed.py create mode 100644 tests/unit/test_execution_speed_playback.py create mode 100644 tests/unit/test_execution_speed_wire.py diff --git a/README.md b/README.md index c28f699..938ab70 100644 --- a/README.md +++ b/README.md @@ -303,6 +303,32 @@ Speed and accel are fractions of maximum (0.0–1.0), not percentages. For Cartesian moves, joint limits stay at 100% as hard bounds—the speed fraction only affects the Cartesian velocity constraint. +### Queued execution speed and pause + +`set_execution_speed(scale)` selects 10–100% of an already planned trajectory's +speed. The command's `speed`, `accel` and `duration` still define the original +plan. Jog and streamed servo commands retain their own timing. + +Override transitions use a separate rate ramp and acceleration checks. The +nominal motion profile's jerk ceiling is not guaranteed during a transition. + +Use `pause()` to retain the queue and decelerate queued motion to a hold, and +`resume()` to continue at the selected scale. Changing speed while paused keeps +the pause. The speed setter rejects zero. These controls return 1 when their +request is confirmed, or 0 when confirmation times out. + +Fresh `execution_speed()` readback exposes `target_scale`, `applied_scale` and +`resume_scale`. Its `paused` property confirms the applied scale reached zero; +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. + +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 +preview retimes trajectories and reports paused queued operations as +`UnresolvedPreview` instead of claiming completion. + ## 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. diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index efca3f3..e13eb8f 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -14,6 +14,8 @@ CmdType.WRITE_IO, CmdType.SET_SHAPES, CmdType.SET_STATUS_RATE, + CmdType.SET_EXECUTION_SPEED, + CmdType.PAUSE, } # Query command types (use request/response, not ACK) @@ -38,6 +40,7 @@ CmdType.TCP_TRANSFORM, CmdType.SHAPES, CmdType.STATUS_RATE, + CmdType.EXECUTION_SPEED, } # Streaming commands are fire-and-forget (no ACK needed) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index b159124..52bf0c3 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -27,6 +27,8 @@ ) from waldoctl.tools import ToolSpec +from waldoctl.execution import ExecutionSpeed, validate_execution_scale + from .. import config as cfg from ..ack_policy import QUERY_CMD_TYPES, SYSTEM_CMD_TYPES, AckPolicy from ..utils.error_catalog import RobotError @@ -43,6 +45,10 @@ DelayCmd, EnablementResultStruct, ErrorCmd, + ExecutionSpeedCmd, + ExecutionSpeedResultStruct, + SetExecutionSpeedCmd, + PauseCmd, ErrorResultStruct, ErrorMsg, IOCmd, @@ -260,6 +266,7 @@ class AsyncRobotClient(_RobotClientABC): def skill_capabilities(self) -> frozenset[str]: return super().skill_capabilities | { "backend.parol6", + "execution.speed", "tool.gripper", "io.digital", } @@ -962,6 +969,89 @@ async def reset_loop_stats(self) -> int: """ return await self._send(ResetLoopStatsCmd()) + async def execution_speed(self, *, timeout: float = 3.0) -> ExecutionSpeed: + """Read fresh requested, applied, and retained execution scales. + + Category: Query + + Example: + speed = rbt.execution_speed() + """ + self._validate_execution_timeout(timeout) + async with asyncio.timeout(timeout): + response = await self._request(ExecutionSpeedCmd()) + if not isinstance(response, ExecutionSpeedResultStruct): + raise ConnectionError("Controller execution speed is unavailable") + return ExecutionSpeed( + response.target_scale, response.applied_scale, response.resume_scale + ) + + @staticmethod + def _validate_execution_timeout(timeout: float) -> None: + if isinstance(timeout, bool) or not math.isfinite(timeout) or timeout <= 0: + raise ValueError("Execution control timeout must be positive and finite") + + async def _request_execution_state( + self, *, timeout: float, scale: float | None = None, paused: bool = False + ) -> int: + self._validate_execution_timeout(timeout) + try: + async with asyncio.timeout(timeout): + command = ( + SetExecutionSpeedCmd(scale) + if scale is not None + else PauseCmd(paused) + ) + if await self._send(command) <= 0: + return 0 + while True: + state = await self.execution_speed(timeout=timeout) + confirmed = ( + state.resume_scale == scale + if scale is not None + else (state.target_scale == 0) == paused + ) + if confirmed: + return 1 + await asyncio.sleep(0.01) + except TimeoutError: + return 0 + + async def set_execution_speed(self, scale: float, *, timeout: float = 3.0) -> int: + """Select 10–100% of planned queued-motion speed, preserving pause. + + Category: Control + + Example: + rbt.set_execution_speed(0.5) + """ + return await self._request_execution_state( + scale=validate_execution_scale(scale), timeout=timeout + ) + + async def pause(self, *, timeout: float = 3.0) -> int: + """Request a controlled hold, retaining queued trajectory progress. + + A confirmed request returns 1. Read ``execution_speed().paused`` + to confirm the hold. Standalone Python completion timeouts continue. + + Category: Control + + Example: + rbt.pause() + """ + return await self._request_execution_state(paused=True, timeout=timeout) + + async def resume(self, *, timeout: float = 3.0) -> int: + """Resume the retained queue at its selected positive speed. + + Category: Control + + Example: + rbt.resume() + """ + return await self._request_execution_state(paused=False, timeout=timeout) + async def set_status_rate(self, hz: float) -> int: """Set the rate the controller broadcasts status at. @@ -1515,8 +1605,8 @@ async def move_j( rel=rel, ) ) - if wait and index >= 0: - await self.wait_command(index, timeout=timeout) + if wait and index >= 0 and not await self.wait_command(index, timeout=timeout): + raise TimeoutError(f"Command {index} did not complete within {timeout}s") return index async def move_l( @@ -1562,8 +1652,8 @@ async def move_l( rel=rel, ) index = await self._send(cmd) - if wait and index >= 0: - await self.wait_command(index, timeout=timeout) + if wait and index >= 0 and not await self.wait_command(index, timeout=timeout): + raise TimeoutError(f"Command {index} did not complete within {timeout}s") return index async def move_c( @@ -1609,8 +1699,8 @@ async def move_c( r=r, ) index = await self._send(cmd) - if wait and index >= 0: - await self.wait_command(index, timeout=timeout) + if wait and index >= 0 and not await self.wait_command(index, timeout=timeout): + raise TimeoutError(f"Command {index} did not complete within {timeout}s") return index async def move_s( @@ -1650,8 +1740,8 @@ async def move_s( accel=accel, ) index = await self._send(cmd) - if wait and index >= 0: - await self.wait_command(index, timeout=timeout) + if wait and index >= 0 and not await self.wait_command(index, timeout=timeout): + raise TimeoutError(f"Command {index} did not complete within {timeout}s") return index async def move_p( @@ -1691,8 +1781,8 @@ async def move_p( accel=accel, ) index = await self._send(cmd) - if wait and index >= 0: - await self.wait_command(index, timeout=timeout) + if wait and index >= 0 and not await self.wait_command(index, timeout=timeout): + raise TimeoutError(f"Command {index} did not complete within {timeout}s") return index async def checkpoint(self, label: str) -> int: @@ -1942,6 +2032,10 @@ async def tool_action( params=params or [], ) result = await self._send(cmd) - if wait and result >= 0: - await self.wait_command(result, timeout=timeout) + if ( + wait + and result >= 0 + and not await self.wait_command(result, timeout=timeout) + ): + raise TimeoutError(f"Command {result} did not complete within {timeout}s") return result diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 9387727..59c934d 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -13,6 +13,8 @@ from typing import Any import numpy as np +from waldoctl.execution import ExecutionSpeed, validate_execution_scale +from waldoctl.skills import UnresolvedPreview import parol6.PAROL6_ROBOT as PAROL6_ROBOT from ..commands.base import MotionCommand @@ -177,8 +179,7 @@ class DryRunRobotClient: simulated separately since the planner doesn't handle streaming. Most methods are auto-dispatched via __getattr__ using CMD_MAP. - Explicit methods exist only for angles/pose (read from state) - and delay (no-op). + Execution controls change the planning clock; observations read local state. """ def __init__( @@ -247,6 +248,8 @@ def tcp_transform(self) -> list[float]: def flush(self) -> list[DryRunResult]: """Flush pending blend buffer. Call after script completion.""" + if self._planner._blend_buffer: + self._require_running() segments = self._planner.flush() self._state.Position_in[:] = self._planner.state.Position_in results: list[DryRunResult] = [] @@ -279,6 +282,13 @@ def _snap_to_angles(self, angles_deg: list[float]) -> DryRunResult: def _dispatch(self, params: Any) -> DryRunResult | None: """Route a command struct through the trajectory planner.""" + cmd_cls = self._registry.get_command_for_struct(type(params)) + if ( + cmd_cls is not None + and issubclass(cmd_cls, MotionCommand) + and not cmd_cls.streamable + ): + self._require_running() if isinstance(params, HomeCmd): if params.calibrate or not self._planner.state.Homed_in[:6].all(): return self._snap_to_angles(HOME_ANGLES_DEG) @@ -308,7 +318,6 @@ def _dispatch(self, params: Any) -> DryRunResult | None: # Detect jog/servo commands — planner doesn't handle streaming. # Other non-trajectory MotionCommands (SelectTool, Home) fall through # to the planner which handles them as inline segments. - cmd_cls = self._registry.get_command_for_struct(type(params)) if cmd_cls is not None and issubclass(cmd_cls, (JogJCommand, JogLCommand)): self._planner.flush() self._state.Position_in[:] = self._planner.state.Position_in @@ -356,7 +365,7 @@ def _trajectory_segment_to_result(self, seg: TrajectorySegment) -> DryRunResult: for i in range(len(sampled)): steps_to_rad(sampled[i], radians[i]) - return _build_result(radians, seg.duration) + return _build_result(radians, seg.duration / self._state.execution_speed) def _error_segment_to_result(self, seg: ErrorSegment) -> DryRunResult: """Convert an ErrorSegment to a DryRunResult with per-pose validity.""" @@ -554,6 +563,7 @@ def skill_capabilities(self) -> frozenset[str]: "backend.parol6", "io.digital", "execution.preview", + "execution.speed", } ) @@ -619,8 +629,32 @@ def write_io(self, index: int, value: int, *, timeout: float | None = None) -> i raise RuntimeError(str(result.error)) return 0 + def _require_running(self) -> None: + if self._state.execution_paused: + raise UnresolvedPreview( + "Queued execution is paused; preview needs an explicit resume " + "before it can predict completion" + ) + + def set_execution_speed(self, scale: float, *, timeout: float = 3.0) -> int: + self._state.execution_speed = validate_execution_scale(scale) + return 1 + + def execution_speed(self, *, timeout: float = 3.0) -> ExecutionSpeed: + scale = self._state.execution_speed + applied = 0.0 if self._state.execution_paused else scale + return ExecutionSpeed(applied, applied, scale) + + def pause(self, *, timeout: float = 3.0) -> int: + self._state.execution_paused = True + return 1 + + def resume(self, *, timeout: float = 3.0) -> int: + self._state.execution_paused = False + return 1 + def delay(self, seconds: float = 0.0) -> None: - pass + self._require_running() def wait_motion(self, **kwargs: Any) -> None: self.flush() diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index b1d3598..854e46e 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -11,6 +11,7 @@ from collections.abc import Callable, Coroutine from typing import Any, TypeVar, overload +from waldoctl.execution import ExecutionSpeed from waldoctl.sync_tools import SyncTool from waldoctl import PingResult, ToolStatus @@ -334,6 +335,22 @@ def reset_loop_stats(self) -> int: """Reset control-loop min/max metrics and overrun count.""" return _run(self._inner.reset_loop_stats()) + def execution_speed(self, *, timeout: float = 3.0) -> ExecutionSpeed: + """Read fresh controller execution timing.""" + return _run(self._inner.execution_speed(timeout=timeout)) + + def set_execution_speed(self, scale: float, *, timeout: float = 3.0) -> int: + """Select queued-motion speed without releasing pause.""" + return _run(self._inner.set_execution_speed(scale, timeout=timeout)) + + def pause(self, *, timeout: float = 3.0) -> int: + """Request a controlled hold of the retained queue.""" + return _run(self._inner.pause(timeout=timeout)) + + def resume(self, *, timeout: float = 3.0) -> int: + """Resume the retained queue at its selected speed.""" + return _run(self._inner.resume(timeout=timeout)) + def set_status_rate(self, hz: float) -> int: """Set the rate the controller broadcasts status at.""" return _run(self._inner.set_status_rate(hz)) diff --git a/parol6/commands/base.py b/parol6/commands/base.py index 02e4716..f0f66e0 100644 --- a/parol6/commands/base.py +++ b/parol6/commands/base.py @@ -301,11 +301,12 @@ class TrajectoryMoveCommandBase(MotionCommand[P]): so execute_step() simply outputs waypoints tick-by-tick. """ - __slots__ = ("trajectory_steps", "command_step", "_duration") + __slots__ = ("trajectory_steps", "trajectory_rad", "command_step", "_duration") def __init__(self, p: P): super().__init__(p) self.trajectory_steps: np.ndarray = np.empty((0, 6), dtype=np.int32) + self.trajectory_rad: np.ndarray = np.empty((0, 6), dtype=np.float64) self.command_step = 0 self._duration: float = 0.0 diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py index 9721fe7..c1ce792 100644 --- a/parol6/commands/cartesian_commands.py +++ b/parol6/commands/cartesian_commands.py @@ -359,6 +359,7 @@ def _precompute_trajectory(self, state: "ControllerState") -> None: trajectory = builder.build() self.trajectory_steps = trajectory.steps + self.trajectory_rad = trajectory.positions_rad self._duration = trajectory.duration self.log_debug( @@ -519,6 +520,7 @@ def do_setup_with_blend( trajectory = builder.build() self.trajectory_steps = trajectory.steps + self.trajectory_rad = trajectory.positions_rad self._duration = trajectory.duration consumed = len(chain) - 1 diff --git a/parol6/commands/curved_commands.py b/parol6/commands/curved_commands.py index caa14f9..368fc54 100644 --- a/parol6/commands/curved_commands.py +++ b/parol6/commands/curved_commands.py @@ -171,6 +171,7 @@ def do_setup(self, state: "ControllerState") -> None: trajectory = builder.build() self.trajectory_steps = trajectory.steps + self.trajectory_rad = trajectory.positions_rad self._duration = trajectory.duration self.log_info( diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py index 6fa33d3..fe109a7 100644 --- a/parol6/commands/joint_commands.py +++ b/parol6/commands/joint_commands.py @@ -97,6 +97,7 @@ def do_setup(self, state: ControllerState) -> None: trajectory = builder.build() self.trajectory_steps = trajectory.steps + self.trajectory_rad = trajectory.positions_rad self._duration = trajectory.duration if len(self.trajectory_steps) == 0: @@ -229,6 +230,7 @@ def do_setup_with_blend( trajectory = builder.build() self.trajectory_steps = trajectory.steps + self.trajectory_rad = trajectory.positions_rad self._duration = trajectory.duration consumed = len(chain) - 1 diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 7fd8f49..bb21e55 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -16,6 +16,8 @@ CurrentActionResultStruct, EnablementResultStruct, ErrorCmd, + ExecutionSpeedCmd, + ExecutionSpeedResultStruct, ErrorResultStruct, IOCmd, IOResultStruct, @@ -453,3 +455,21 @@ def compute(self, state: "ControllerState") -> bytes: yaw=degrees(rpy[2]), ) ) + + +@register_command(CmdType.EXECUTION_SPEED) +class ExecutionSpeedCommand(QueryCommand[ExecutionSpeedCmd]): + """Read the trajectory clock owned by the control loop.""" + + PARAMS_TYPE = ExecutionSpeedCmd + QUERY_TYPE = QueryType.EXECUTION_SPEED + __slots__ = () + + def compute(self, state: "ControllerState") -> bytes: + return pack_response( + ExecutionSpeedResultStruct( + target_scale=0.0 if state.execution_paused else state.execution_speed, + applied_scale=state.execution_applied_speed, + resume_scale=state.execution_speed, + ) + ) diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index bd409ed..260c212 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -23,6 +23,8 @@ SetTcpTransformCmd, SimulatorCmd, StopCmd, + PauseCmd, + SetExecutionSpeedCmd, WriteIOCmd, ) from parol6.protocol.wire import CommandCode @@ -241,3 +243,29 @@ def do_setup(self, state: ControllerState) -> None: def execute_step(self, state: ControllerState) -> ExecutionStatusCode: self.finish() return ExecutionStatusCode.COMPLETED + + +@register_command(CmdType.SET_EXECUTION_SPEED) +class SetExecutionSpeedCommand(SystemCommand[SetExecutionSpeedCmd]): + """Select the queued trajectory clock rate without changing pause.""" + + PARAMS_TYPE = SetExecutionSpeedCmd + __slots__ = () + + def execute_step(self, state: ControllerState) -> ExecutionStatusCode: + state.execution_speed = self.p.scale + self.finish() + return ExecutionStatusCode.COMPLETED + + +@register_command(CmdType.PAUSE) +class PauseCommand(SystemCommand[PauseCmd]): + """Retain the queue and request a controlled pause or explicit resume.""" + + PARAMS_TYPE = PauseCmd + __slots__ = () + + def execute_step(self, state: ControllerState) -> ExecutionStatusCode: + state.execution_paused = self.p.on + self.finish() + return ExecutionStatusCode.COMPLETED diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 6ec7821..ad9de05 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -11,7 +11,7 @@ MotionCommand, SystemCommand, ) -from parol6.config import CONTROL_RATE_HZ +from parol6.config import CONTROL_RATE_HZ, INTERVAL_S from parol6.protocol.wire import ( CheckpointCmd, CmdType, @@ -37,10 +37,10 @@ class DelayCommand(CommandBase[DelayCmd]): PARAMS_TYPE = DelayCmd - __slots__ = () + __slots__ = ("_remaining_s",) def do_setup(self, state: "ControllerState") -> None: - self.start_timer(self.p.seconds) + self._remaining_s = self.p.seconds logger.info(f" -> Delay starting for {self.p.seconds} seconds...") def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: @@ -48,7 +48,8 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) - if self.timer_expired(): + self._remaining_s -= INTERVAL_S + if self._remaining_s <= 0: logger.info(f"Delay finished after {self.p.seconds} seconds.") self.finish() return ExecutionStatusCode.COMPLETED diff --git a/parol6/config.py b/parol6/config.py index f3f3046..0c174f2 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -43,6 +43,16 @@ def _trace(self, msg, *args, **kwargs): CONTROL_RATE_HZ: float = float(os.getenv("PAROL6_CONTROL_RATE_HZ", "100")) DEFAULT_ACCEL_PERCENT: float = 100.0 +EXECUTION_OVERRIDE_TRANSITION_S: float = float( + os.getenv("PAROL6_EXECUTION_OVERRIDE_TRANSITION_S", "1.0") +) +if ( + not np.isfinite(EXECUTION_OVERRIDE_TRANSITION_S) + or EXECUTION_OVERRIDE_TRANSITION_S <= 0 +): + raise ValueError( + "PAROL6_EXECUTION_OVERRIDE_TRANSITION_S must be positive and finite" + ) # Motion thresholds (mm) NEAR_MM_TOL_MM: float = 2.0 # Proximity threshold for considering positions "near" (mm) diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 06fcda5..74288c6 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -377,6 +377,7 @@ class Trajectory: steps: NDArray[np.int32] # (M, 6) motor steps duration: float # seconds + positions_rad: NDArray[np.float64] # Before motor-step quantization def __len__(self) -> int: return len(self.steps) @@ -471,7 +472,11 @@ def build(self) -> Trajectory: steps = _rad_to_steps_alloc( self.joint_path.positions[0:1] # Keep 2D shape (1, 6) ) - return Trajectory(steps=steps, duration=0.0) + return Trajectory( + steps=steps, + duration=0.0, + positions_rad=self.joint_path.positions[0:1].copy(), + ) if self.profile == ProfileType.RUCKIG: # Point-to-point jerk-limited motion; ignores intermediate waypoints @@ -574,7 +579,9 @@ def _build_toppra_trajectory(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=duration) + return Trajectory( + steps=steps, duration=duration, positions_rad=trajectory_rad + ) except Exception as e: logger.warning("TOPPRA failed: %s. Falling back to LINEAR profile.", e) @@ -604,7 +611,7 @@ def _build_simple_trajectory(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=duration) + return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) def _is_cartesian_path(self) -> bool: """Check if this is a Cartesian path (has Cartesian velocity limits set).""" @@ -910,7 +917,7 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=duration) + return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) def _build_quintic_trajectory_cartesian(self) -> Trajectory: """ @@ -948,7 +955,7 @@ def _build_quintic_trajectory_cartesian(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=duration) + return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) def _build_trapezoid_trajectory(self) -> Trajectory: """ @@ -1016,7 +1023,7 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=duration) + return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) def _build_trapezoid_trajectory_cartesian(self) -> Trajectory: """ @@ -1072,7 +1079,7 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=duration) + return Trajectory(steps=steps, duration=duration, positions_rad=trajectory_rad) def _build_cart_vel_constraint( self, path: ta.SplineInterpolator | _LinearPath, ss_waypoints: NDArray @@ -1220,7 +1227,9 @@ def _build_ruckig_trajectory(self) -> Trajectory: steps = _rad_to_steps_alloc(trajectory_rad) - return Trajectory(steps=steps, duration=actual_duration) + return Trajectory( + steps=steps, duration=actual_duration, positions_rad=trajectory_rad + ) 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 a6566c3..5dbed44 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -28,6 +28,7 @@ from parol6.config import LIMITS from waldoctl import ActionState, ToolStatus +from waldoctl.execution import ExecutionSpeed, validate_execution_scale from waldoctl.tools import ToolState from parol6.tools import get_registry, list_tools @@ -95,6 +96,7 @@ class QueryType(IntEnum): SHAPES = auto() STATUS_RATE = auto() TCP_TRANSFORM = auto() + EXECUTION_SPEED = auto() class CmdType(IntEnum): @@ -168,6 +170,9 @@ class CmdType(IntEnum): STATUS_RATE = auto() SET_TCP_TRANSFORM = auto() TCP_TRANSFORM = auto() + PAUSE = auto() + SET_EXECUTION_SPEED = auto() + EXECUTION_SPEED = auto() # ============================================================================= @@ -911,6 +916,52 @@ class ActivityCmd( pass +class SetExecutionSpeedCmd( + msgspec.Struct, + tag=int(CmdType.SET_EXECUTION_SPEED), + array_like=True, + frozen=True, + gc=False, + forbid_unknown_fields=True, +): + """Select 10–100% of queued trajectory speed without releasing pause.""" + + scale: float + + def __post_init__(self) -> None: + validate_execution_scale(self.scale) + + +class PauseCmd( + msgspec.Struct, + tag=int(CmdType.PAUSE), + array_like=True, + frozen=True, + gc=False, + forbid_unknown_fields=True, +): + """Explicit pause or resume, retaining the selected execution speed.""" + + on: bool + + def __post_init__(self) -> None: + if not isinstance(self.on, bool): + raise ValueError("Pause requires a boolean") + + +class ExecutionSpeedCmd( + msgspec.Struct, + tag=int(CmdType.EXECUTION_SPEED), + array_like=True, + frozen=True, + gc=False, + forbid_unknown_fields=True, +): + """Fresh controller-owned trajectory timing readback.""" + + pass + + class StatusRateCmd( msgspec.Struct, tag=int(CmdType.STATUS_RATE), @@ -1071,6 +1122,24 @@ class StatusResultStruct( tool_status: list +class ExecutionSpeedResultStruct( + msgspec.Struct, + tag=int(QueryType.EXECUTION_SPEED), + array_like=True, + frozen=True, + gc=False, + forbid_unknown_fields=True, +): + """Requested, applied, and retained positive queued-execution scales.""" + + target_scale: float + applied_scale: float + resume_scale: float + + def __post_init__(self) -> None: + ExecutionSpeed(self.target_scale, self.applied_scale, self.resume_scale) + + class StatusRateResultStruct( msgspec.Struct, tag=int(QueryType.STATUS_RATE), @@ -1310,6 +1379,7 @@ class ShapesResultStruct( StatusResultStruct | LoopStatsResultStruct | StatusRateResultStruct + | ExecutionSpeedResultStruct | ToolResultStruct | CurrentActionResultStruct | PingResultStruct @@ -2080,6 +2150,10 @@ def unpack_rx_frame_into( "LoopStatsCmd", "StatusRateCmd", "SetStatusRateCmd", + "SetExecutionSpeedCmd", + "PauseCmd", + "ExecutionSpeedCmd", + "ExecutionSpeedResultStruct", "ProfileCmd", "Command", # Mixin diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 4d1e2ba..32261d2 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -55,10 +55,25 @@ class TrajectorySegment: command_index: int trajectory_steps: np.ndarray # (M, 6) int32 + trajectory_rad: np.ndarray # (M, 6) float64, before motor-step quantization duration: float command_name: str = "" action_params: str = "" blend_consumed_indices: list[int] = field(default_factory=list) + velocity_rad_s: np.ndarray = field(init=False) + acceleration_rad_s2: np.ndarray = field(init=False) + + def __post_init__(self) -> None: + from parol6.config import INTERVAL_S + + if len(self.trajectory_rad) < 2: + self.velocity_rad_s = np.zeros_like(self.trajectory_rad) + self.acceleration_rad_s2 = np.zeros_like(self.trajectory_rad) + else: + self.velocity_rad_s = np.gradient(self.trajectory_rad, INTERVAL_S, axis=0) + self.acceleration_rad_s2 = np.gradient( + self.velocity_rad_s, INTERVAL_S, axis=0 + ) @dataclass @@ -376,6 +391,7 @@ def _flush_blend(self) -> None: TrajectorySegment( command_index=head_idx, trajectory_steps=head_cmd.trajectory_steps.copy(), + trajectory_rad=head_cmd.trajectory_rad.copy(), duration=head_cmd._duration, command_name=type(head_cmd).__name__, action_params=_format_cmd_params(head_cmd.p), @@ -411,6 +427,7 @@ def _emit_trajectory( TrajectorySegment( command_index=command_index, trajectory_steps=cmd.trajectory_steps.copy(), + trajectory_rad=cmd.trajectory_rad.copy(), duration=cmd._duration, command_name=type(cmd).__name__, action_params=_format_cmd_params(params) if params is not None else "", diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 29d686b..363861d 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -20,8 +20,16 @@ from parol6.commands._collision_guard import guard_joint_path from parol6.commands.base import CommandBase, ExecutionStatusCode -from parol6.config import COLLISION_PATH_SAMPLES, SETTLE_MAX_TICKS, steps_to_rad -from parol6.protocol.wire import CommandCode +from parol6.config import ( + COLLISION_PATH_SAMPLES, + EXECUTION_OVERRIDE_TRANSITION_S, + INTERVAL_S, + LIMITS, + SETTLE_MAX_TICKS, + rad_to_steps, + steps_to_rad, +) +from parol6.protocol.wire import CommandCode, DelayCmd 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 ( @@ -54,6 +62,10 @@ class SegmentPlayer: "_planner", "_active", "_step", + "_phase", + "_position_rad", + "_candidate_position_rad", + "_velocity_rad_s", "_buffer", "_inline_cmd", "_inline_activated", @@ -67,6 +79,11 @@ def __init__(self, planner: MotionPlanner) -> None: self._planner = planner self._active: Segment | None = None self._step: int = 0 + self._phase = -1.0 + self._position_rad = np.zeros(6, dtype=np.float64) + self._candidate_position_rad = np.zeros(6, dtype=np.float64) + self._velocity_rad_s = np.zeros(6, dtype=np.float64) + rad_to_steps(self._position_rad, np.empty(6, dtype=np.int32)) self._buffer: deque[Segment] = deque() self._inline_cmd: CommandBase | None = None self._inline_activated: bool = False @@ -113,7 +130,16 @@ def tick(self, state: ControllerState) -> bool: # Activate next segment if idle if self._active is None: if not self._buffer: + state.execution_applied_speed = ( + 0.0 if state.execution_paused else state.execution_speed + ) return False + if state.execution_paused and not isinstance( + self._buffer[0], ErrorSegment + ): + state.execution_applied_speed = 0.0 + state.Speed_out.fill(0) + return True self._activate_next(state) if self._active is None: continue # activation-time world guard rejected the segment @@ -122,10 +148,86 @@ def tick(self, state: ControllerState) -> bool: # --- Trajectory segment: index into waypoints --- if isinstance(active, TrajectorySegment): + if state.execution_paused and ( + state.execution_applied_speed == 0.0 + or self._step >= len(active.trajectory_steps) + ): + state.execution_applied_speed = 0.0 + self._velocity_rad_s.fill(0.0) + state.Command_out = CommandCode.MOVE + state.Speed_out.fill(0) + return True if self._step < len(active.trajectory_steps): - state.Position_out[:] = active.trajectory_steps[self._step] + old_scale = state.execution_applied_speed + target_scale = ( + 0.0 if state.execution_paused else state.execution_speed + ) + low = -1.0 / EXECUTION_OVERRIDE_TRANSITION_S + high = -low + for joint in range(6): + velocity = active.velocity_rad_s[self._step, joint] + if abs(velocity) > 1e-12: + base = ( + old_scale + * old_scale + * active.acceleration_rad_s2[self._step, joint] + ) + limit = LIMITS.joint.hard.acceleration[joint] + first = (-limit - base) / velocity + second = (limit - base) / velocity + low = max(low, min(first, second)) + high = min(high, max(first, second)) + requested = (target_scale - old_scale) / INTERVAL_S + rate = min(high, max(low, requested)) if low <= high else 0.0 + if requested == 0.0 or rate * requested < 0.0: + rate = 0.0 + new_scale = min(1.0, max(0.0, old_scale + rate * INTERVAL_S)) + if abs(new_scale - target_scale) < 1e-12: + new_scale = target_scale + if new_scale != old_scale and not self._rate_is_admissible( + active, old_scale, new_scale + ): + allowed = 0.0 + refused = 1.0 + for _ in range(16): + fraction = 0.5 * (allowed + refused) + if self._rate_is_admissible( + active, + old_scale, + old_scale + fraction * (new_scale - old_scale), + ): + allowed = fraction + else: + refused = fraction + new_scale = old_scale + allowed * (new_scale - old_scale) + state.execution_applied_speed = new_scale + self._candidate_position( + active, self._phase + 0.5 * (old_scale + new_scale) + ) + for joint in range(6): + self._velocity_rad_s[joint] = ( + self._candidate_position_rad[joint] + - self._position_rad[joint] + ) / INTERVAL_S + self._position_rad[:] = self._candidate_position_rad + self._phase += 0.5 * (old_scale + new_scale) + if self._phase >= 1.0: + self._step += 1 + self._phase -= 1.0 + if self._phase <= 0.0 or self._step + 1 == len( + active.trajectory_steps + ): + state.Position_out[:] = active.trajectory_steps[self._step] + else: + # Preserve the planner's piecewise-linear joint path; + # rounding happens only at the firmware boundary. + rad_to_steps(self._position_rad, state.Position_out) state.Command_out = CommandCode.MOVE - self._step += 1 + if ( + self._step + 1 == len(active.trajectory_steps) + and self._phase >= 0.0 + ): + self._step += 1 self._settling = False return True # All waypoints sent — hold MOVE at target until Position_in @@ -168,6 +270,12 @@ def tick(self, state: ControllerState) -> bool: # --- Inline segment: tick the command --- if isinstance(active, InlineSegment): + state.execution_applied_speed = ( + 0.0 if state.execution_paused else state.execution_speed + ) + if state.execution_paused and isinstance(active.params, DelayCmd): + state.Speed_out.fill(0) + return True result = self._tick_inline(active, state) if result is None: # Instant completion — try next immediately @@ -201,6 +309,34 @@ def tick(self, state: ControllerState) -> bool: # Exhausted immediate iterations (unlikely) return self._active is not None + def _candidate_position(self, segment: TrajectorySegment, phase: float) -> None: + step = self._step + if phase >= 1.0: + step += 1 + phase -= 1.0 + step = min(step, len(segment.trajectory_rad) - 1) + following = min(step + 1, len(segment.trajectory_rad) - 1) + phase = max(0.0, phase) + for joint in range(6): + left = segment.trajectory_rad[step, joint] + right = segment.trajectory_rad[following, joint] + self._candidate_position_rad[joint] = left + phase * (right - left) + + def _rate_is_admissible( + self, segment: TrajectorySegment, old_scale: float, scale: float + ) -> bool: + # Central planner derivatives do not bound the second difference + # of a fractional, piecewise-linear position command. + self._candidate_position(segment, self._phase + 0.5 * (old_scale + scale)) + for joint in range(6): + velocity = ( + self._candidate_position_rad[joint] - self._position_rad[joint] + ) / INTERVAL_S + acceleration = (velocity - self._velocity_rad_s[joint]) / INTERVAL_S + if abs(acceleration) > LIMITS.joint.hard.acceleration[joint] * (1.0 + 1e-6): + return False + return True + def _activate_next(self, state: ControllerState) -> None: """Promote next buffered segment to active. @@ -216,12 +352,15 @@ def _activate_next(self, state: ControllerState) -> None: return self._active = seg self._step = 0 + self._phase = -1.0 self._inline_cmd = None self._inline_activated = False state.executing_command_index = self._active.command_index state.action_state = ActionState.EXECUTING # Populate action info for trajectory segments (inline segments set these later) if isinstance(self._active, TrajectorySegment): + self._position_rad[:] = self._active.trajectory_rad[0] + self._velocity_rad_s.fill(0.0) state.action_current = self._active.command_name state.action_params = self._active.action_params @@ -351,6 +490,7 @@ def cancel(self, state: ControllerState) -> None: state.action_state = ActionState.IDLE self._active = None self._step = 0 + self._phase = -1.0 self._inline_cmd = None self._inline_activated = False self._buffer.clear() diff --git a/parol6/server/state.py b/parol6/server/state.py index a35182e..b07b04f 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -272,6 +272,9 @@ class ControllerState: # Pipeline depth (maintained by segment player) queued_segments: int = 0 queued_duration: float = 0.0 + execution_paused: bool = False + execution_speed: float = 1.0 + execution_applied_speed: float = 1.0 # Self-collision viz: colliding pairs captured at the predicted colliding # config when a move is blocked or a jog is stopped; cleared when a diff --git a/tests/integration/test_execution_speed.py b/tests/integration/test_execution_speed.py new file mode 100644 index 0000000..ac6c97e --- /dev/null +++ b/tests/integration/test_execution_speed.py @@ -0,0 +1,67 @@ +"""Queued execution controls through the client and simulated controller.""" + +import math +import time + +import numpy as np +import pytest + +from parol6 import RobotClient + + +def test_execution_pause_speed_dwell_and_standalone_deadlines(client: RobotClient): + start = client.angles() + assert start is not None + target = list(start) + target[0] += 8 + try: + for invalid in [0, -1, 0.09, 1.01, 2, True, math.nan, math.inf, -math.inf]: + with pytest.raises(ValueError): + client.set_execution_speed(invalid) + assert client.pause() == 1 + index = client.move_j(target, duration=2, wait=False) + before = time.monotonic() + assert not client.wait_command(index, timeout=0.3) + assert time.monotonic() - before < 1 + assert client.set_execution_speed(0.5) == 1 + state = client.execution_speed() + assert state.paused and state.resume_scale == 0.5 + assert not client.wait_command(index, timeout=0.3) + assert np.allclose(client.angles(), start, atol=0.05) + + assert client.resume() == 1 + assert client.wait_status(lambda s: s.angles[0] > start[0] + 1, timeout=10) + assert client.pause() == 1 + deadline = time.monotonic() + 5 + while not client.execution_speed().paused: + assert time.monotonic() < deadline, "pause never reached a hold" + time.sleep(0.02) + assert not client.wait_command(index, timeout=0.3) + held = client.angles() + assert client.set_execution_speed(0.6) == 1 + assert client.execution_speed().paused + assert not client.wait_command(index, timeout=0.3) + assert np.allclose(client.angles(), held, atol=0.05) + assert client.resume() == 1 + assert client.wait_command(index, timeout=10) + assert np.allclose(client.angles(), target, atol=0.1) + + index = client.delay(1) + assert client.wait_status(lambda s: s.executing_index == index, timeout=3) + assert client.pause() == 1 + assert not client.wait_command(index, timeout=1.3) + assert client.ping() is not None + assert client.resume() == 1 + assert client.wait_command(index, timeout=3) + + assert client.pause() == 1 + with pytest.raises(TimeoutError): + client.move_j(start, duration=1, wait=True, timeout=0.2) + assert client.stop() == 1 + assert client.execution_speed().paused + assert client.resume() == 1 + assert client.queue() == [] + finally: + client.stop() + client.resume() + client.set_execution_speed(1) diff --git a/tests/unit/test_dry_run_blend.py b/tests/unit/test_dry_run_blend.py index ab38997..5de568c 100644 --- a/tests/unit/test_dry_run_blend.py +++ b/tests/unit/test_dry_run_blend.py @@ -4,6 +4,7 @@ import pytest from parol6.client.dry_run_client import DryRunRobotClient +from waldoctl.skills import UnresolvedPreview # Valid PAROL6 joint angles (deg) within limits: # J1: [-123, 123], J2: [-145, -3.375], J3: [107.9, 287.9], @@ -86,3 +87,28 @@ def test_state_updated_after_blend(self, client): angles_after = client.angles() assert len(angles_after) == 6 np.testing.assert_allclose(angles_after, W2, atol=0.5) + + def test_execution_override_preserves_path_and_pause(self): + normal = DryRunRobotClient(initial_joints_deg=W0) + slow = DryRunRobotClient(initial_joints_deg=W0) + normal_result = normal.move_j(W1, duration=2) + assert slow.set_execution_speed(0.5) == 1 + slow_result = slow.move_j(W1, duration=2) + assert normal_result is not None and slow_result is not None + assert slow_result.duration == pytest.approx(normal_result.duration * 2) + np.testing.assert_allclose( + slow_result.joint_trajectory_rad, normal_result.joint_trajectory_rad + ) + assert slow.pause() == 1 + assert slow.set_execution_speed(0.3) == 1 + assert slow.execution_speed().paused + for operation in (lambda: slow.move_j(W2, duration=2), lambda: slow.delay(1)): + with pytest.raises(UnresolvedPreview, match="paused"): + operation() + np.testing.assert_allclose(slow.angles(), W1, atol=0.05) + assert slow.resume() == 1 + assert slow.execution_speed().applied_scale == 0.3 + assert slow.move_j(W2, duration=2).error is None + for invalid in (0, True, 2, float("nan")): + with pytest.raises(ValueError): + slow.set_execution_speed(invalid) diff --git a/tests/unit/test_execution_speed_playback.py b/tests/unit/test_execution_speed_playback.py new file mode 100644 index 0000000..db057d5 --- /dev/null +++ b/tests/unit/test_execution_speed_playback.py @@ -0,0 +1,82 @@ +"""Observe firmware position commands from the real planner/player pipeline.""" + +import numpy as np + +from parol6.config import INTERVAL_S, LIMITS, deg_to_steps, steps_to_rad +from parol6.protocol.wire import MoveJCmd, PauseCmd, SetExecutionSpeedCmd +from parol6.server.command_registry import create_command_from_struct +from parol6.server.motion_planner import MotionPlanner, PlanCommand, PlannerWorker +from parol6.server.segment_player import SegmentPlayer +from parol6.server.state import ControllerState + + +def test_override_transitions_bound_commanded_acceleration(monkeypatch): + import parol6.server.segment_player as module + + quantum = np.empty(6) + steps_to_rad(np.ones(6, dtype=np.int32), quantum) + # Each emitted position has at most half a step of rounding error; + # the second difference has coefficients 1, -2, 1. + rounding = 2 * quantum / INTERVAL_S**2 + limits = np.asarray(LIMITS.joint.hard.acceleration) + target = [105.0, -75.0, 195.0, 15.0, 15.0, 195.0] + for transition in (1.0, 0.03): + monkeypatch.setattr(module, "EXECUTION_OVERRIDE_TRANSITION_S", transition) + planner = MotionPlanner() + worker = PlannerWorker(planner._segment_queue) + state = ControllerState() + deg_to_steps(np.array([90.0, -90.0, 180.0, 0.0, 0.0, 180.0]), state.Position_in) + state.Position_out[:] = state.Position_in + worker.process_command( + PlanCommand( + command_index=1, + params=MoveJCmd(angles=target, speed=1.0), + position_in=state.Position_in.copy(), + homed=True, + ) + ) + assert planner._segment_queue._reader.poll(3), "planner produced no result" + player = SegmentPlayer(planner) + trace = [] + held_at = None + + def apply(params): + command, _, error = create_command_from_struct(params) + assert command is not None, error + command.setup(state) + command.tick(state) + + try: + for k in range(round(20 / INTERVAL_S)): + if k == round(0.15 / INTERVAL_S): + apply(SetExecutionSpeedCmd(0.1)) + if k == round(0.35 / INTERVAL_S): + apply(PauseCmd(True)) + if held_at is not None and k == held_at + round(0.2 / INTERVAL_S): + apply(PauseCmd(False)) + apply(SetExecutionSpeedCmd(0.6)) + active = player.tick(state) + assert state.error is None + q = np.empty(6) + steps_to_rad(state.Position_out, q) + trace.append(q) + state.Position_in[:] = state.Position_out + if state.execution_paused and state.execution_applied_speed == 0: + if held_at is None: + held_at = k + else: + np.testing.assert_array_equal(q, trace[held_at]) + if not active and state.completed_command_index == 1: + break + assert state.completed_command_index == 1 and held_at is not None + np.testing.assert_allclose( + trace[-1], np.radians(target), atol=quantum.max() + ) + acceleration = np.diff(np.asarray(trace), n=2, axis=0) / INTERVAL_S**2 + excess = np.maximum(0.0, np.abs(acceleration) - rounding) / limits + assert excess.max() <= 1.01, ( + f"{transition}s override exceeded acceleration: " + f"{excess.max(axis=0).tolist()} times the joint limits after quantization" + ) + finally: + planner.stop() diff --git a/tests/unit/test_execution_speed_wire.py b/tests/unit/test_execution_speed_wire.py new file mode 100644 index 0000000..bda6283 --- /dev/null +++ b/tests/unit/test_execution_speed_wire.py @@ -0,0 +1,75 @@ +"""Execution controls reject invalid values and malformed wire frames.""" + +import math + +import msgspec +import pytest + +from parol6.protocol.wire import ( + CmdType, + ExecutionSpeedCmd, + ExecutionSpeedResultStruct, + MsgType, + PauseCmd, + QueryType, + ResponseMsg, + SetExecutionSpeedCmd, + decode_command, + decode_message, + encode, +) + + +def test_execution_controls_roundtrip_and_reject_malformed_frames(): + for command in ( + SetExecutionSpeedCmd(0.1), + SetExecutionSpeedCmd(1), + PauseCmd(True), + PauseCmd(False), + ExecutionSpeedCmd(), + ): + assert decode_command(encode(command)) == command + for values in ((0.5, 0.7, 0.5), (0, 0.03, 0.6), (0, 0, 1)): + response = ResponseMsg(ExecutionSpeedResultStruct(*values)) + assert decode_message(encode(response)) == response + + invalid_commands = [ + [CmdType.SET_EXECUTION_SPEED], + [CmdType.SET_EXECUTION_SPEED, 0.5, 1], + [CmdType.PAUSE], + [CmdType.PAUSE, True, False], + [CmdType.EXECUTION_SPEED, 0], + *( + [CmdType.SET_EXECUTION_SPEED, value] + for value in ( + True, + 0, + -1, + 0.09, + 1.01, + 2, + math.inf, + -math.inf, + math.nan, + "0.5", + ) + ), + *([CmdType.PAUSE, value] for value in (0, 1, "true", None)), + ] + for wire in invalid_commands: + with pytest.raises(msgspec.ValidationError): + decode_command(encode(wire)) + for values in ( + (0.5, 0.7), + (0.5, 0.7, 0.5, 1), + (0.5, 0.7, 0.6), + (0, -0.1, 0.5), + (0, 1.1, 0.5), + (0, math.nan, 0.5), + (0, 0, 0), + (True, 0, 1), + ): + with pytest.raises(msgspec.ValidationError): + decode_message( + encode([MsgType.RESPONSE, [QueryType.EXECUTION_SPEED, *values]]) + ) diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index dd2d3c9..7567b2d 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -224,14 +224,18 @@ class TestTrajectory: def test_len_returns_step_count(self): """len() should return number of steps.""" steps = np.zeros((100, 6), dtype=np.int32) - traj = Trajectory(steps=steps, duration=1.0) + traj = Trajectory( + steps=steps, duration=1.0, positions_rad=np.zeros(steps.shape) + ) assert len(traj) == 100 def test_getitem_returns_step(self): """Indexing should return individual step.""" steps = np.arange(60, dtype=np.int32).reshape(10, 6) - traj = Trajectory(steps=steps, duration=1.0) + traj = Trajectory( + steps=steps, duration=1.0, positions_rad=np.zeros(steps.shape) + ) assert np.array_equal(traj[0], steps[0]) assert np.array_equal(traj[5], steps[5]) diff --git a/tests/unit/test_motion_pipeline.py b/tests/unit/test_motion_pipeline.py index 8a91212..d01b952 100644 --- a/tests/unit/test_motion_pipeline.py +++ b/tests/unit/test_motion_pipeline.py @@ -10,7 +10,7 @@ import numpy as np import pytest -from parol6.config import deg_to_steps +from parol6.config import deg_to_steps, steps_to_rad from parol6.protocol.wire import ( CheckpointCmd, DelayCmd, @@ -51,6 +51,13 @@ def _deg_to_steps(angles: list[float]) -> np.ndarray: return buf +def _path_radians(steps: np.ndarray) -> np.ndarray: + radians = np.empty(steps.shape, dtype=np.float64) + for source, target in zip(steps, radians): + steps_to_rad(source, target) + return radians + + def _make_movej_cmd( angles: list[float], speed: float = 0.5, r: float = 0.0 ) -> MoveJCmd: @@ -400,6 +407,7 @@ def test_trajectory_playback(self, state): seg = TrajectorySegment( command_index=0, trajectory_steps=steps, + trajectory_rad=_path_radians(steps), duration=0.05, ) player._buffer.append(seg) @@ -440,7 +448,12 @@ def test_segment_ordering(self, state): # Trajectory segment steps = np.tile(_home_steps(), (3, 1)) - traj = TrajectorySegment(command_index=0, trajectory_steps=steps, duration=0.03) + traj = TrajectorySegment( + command_index=0, + trajectory_steps=steps, + trajectory_rad=_path_radians(steps), + duration=0.03, + ) # Inline segment inline = InlineSegment( @@ -465,7 +478,12 @@ def test_cancel_clears_everything(self, state): player = SegmentPlayer(planner) steps = np.tile(_home_steps(), (100, 1)) - seg = TrajectorySegment(command_index=0, trajectory_steps=steps, duration=1.0) + seg = TrajectorySegment( + command_index=0, + trajectory_steps=steps, + trajectory_rad=_path_radians(steps), + duration=1.0, + ) player._buffer.append(seg) # Start playing @@ -488,6 +506,7 @@ def test_blend_consumed_indices(self, state): seg = TrajectorySegment( command_index=0, trajectory_steps=steps, + trajectory_rad=_path_radians(steps), duration=0.02, blend_consumed_indices=[1, 2], ) @@ -510,7 +529,12 @@ def test_active_property(self, state): steps = np.tile(_home_steps(), (2, 1)) player._buffer.append( - TrajectorySegment(command_index=0, trajectory_steps=steps, duration=0.02) + TrajectorySegment( + command_index=0, + trajectory_steps=steps, + trajectory_rad=_path_radians(steps), + duration=0.02, + ) ) assert player.active is True From ae5f97938f83a2e8a4937868bcf9b2b06512b44f Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 19:33:49 -0400 Subject: [PATCH 2/4] Verify multicast recovery deterministically and bound the spline example wait --- .github/workflows/tests.yml | 14 ++- examples/draw_circle.py | 2 +- .../test_status_broadcast_autofailover.py | 87 ++++++++----------- 3 files changed, 50 insertions(+), 53 deletions(-) diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 6992620..4bcf250 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -144,6 +144,12 @@ jobs: PYTHONUTF8: '1' run: | pytest + - name: Preserve test report + if: always() + uses: actions/upload-artifact@v4 + with: + name: junit-${{ matrix.os }}-python-${{ matrix.python-version }} + path: test-results.xml # The examples are the scripts a user copies, and `--examples` is opt-in, # so nothing was running them: four had rotted into refusals on their # own happy path. They run each script as a subprocess against the @@ -152,4 +158,10 @@ jobs: env: PYTHONUNBUFFERED: '1' PYTHONUTF8: '1' - run: pytest tests/test_examples.py --examples + run: pytest tests/test_examples.py --examples --junitxml=example-results.xml + - name: Preserve example report + if: always() + uses: actions/upload-artifact@v4 + with: + name: examples-${{ matrix.os }}-python-${{ matrix.python-version }} + path: example-results.xml diff --git a/examples/draw_circle.py b/examples/draw_circle.py index 34ceb01..e22eec5 100644 --- a/examples/draw_circle.py +++ b/examples/draw_circle.py @@ -75,7 +75,7 @@ def circle_pt(cx, cz, angle_deg): z = z_min + t * (z_max - z_min) x = RADIUS * math.cos(t * 3 * 2 * math.pi) spline.append([x, CIRCLE_Y, z] + ORIENTATION) - rbt.move_s(spline, speed=SPEED, wait=True) + rbt.move_s(spline, speed=SPEED, wait=True, timeout=60) rbt.home(wait=True) print("Done!") diff --git a/tests/integration/test_status_broadcast_autofailover.py b/tests/integration/test_status_broadcast_autofailover.py index fa603d9..9b39083 100644 --- a/tests/integration/test_status_broadcast_autofailover.py +++ b/tests/integration/test_status_broadcast_autofailover.py @@ -131,60 +131,45 @@ async def _consume_one(timeout: float = 3.0) -> bool: assert ok, "Subscriber did not receive unicast datagram on multicast socket" -def _raise_sendto(*args, **kwargs): - raise OSError("simulated send failure") - - -@pytest.mark.timeout(5) @pytest.mark.asyncio -async def test_multicast_send_errors_should_trigger_fallback_but_currently_do_not( - monkeypatch, -): - """ - Demonstrate the bug: if multicast setup succeeds but subsequent send() calls fail, - the broadcaster should fall back to UNICAST. Current implementation does not, - so this test is expected to FAIL until the logic is improved. - """ - port = _free_udp_port() - # Ensure we attempt multicast path - monkeypatch.setattr(cfg, "STATUS_TRANSPORT", "MULTICAST", raising=False) - - cache = get_cache() - cache.mark_serial_observed() +async def test_multicast_send_failure_recovers_with_unicast_delivery(monkeypatch): + from parol6.protocol.wire import StatusBuffer, decode_status_bin_into - state_mgr = StateManager() - broadcaster = StatusBroadcaster( - state_mgr=state_mgr, port=port, iface_ip="127.0.0.1", stale_s=2.0 + monkeypatch.setattr(cfg, "STATUS_TRANSPORT", "MULTICAST") + monkeypatch.setattr(cfg, "STATUS_UNICAST_HOST", "127.0.0.1") + monkeypatch.setattr( + StatusBroadcaster, "_verify_multicast_reachable", lambda *args: True ) + cache = get_cache() + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as receiver: + receiver.bind(("127.0.0.1", 0)) + receiver.setblocking(False) + broadcaster = StatusBroadcaster( + state_mgr=StateManager(), + port=receiver.getsockname()[1], + iface_ip="127.0.0.1", + stale_s=2.0, + ) + failed_socket = broadcaster._sock + sendto = socket.socket.sendto - # StatusBroadcaster is now a polling class - call tick() manually - stop_flag = False + def fail_multicast(sock, *args, **kwargs): + if sock is failed_socket: + raise OSError("simulated multicast send failure") + return sendto(sock, *args, **kwargs) - async def _tick_loop(): - while not stop_flag: + monkeypatch.setattr(socket.socket, "sendto", fail_multicast) + try: + for _ in range(broadcaster._max_send_failures): + cache.mark_serial_observed() + broadcaster.tick() + assert broadcaster._use_unicast + cache.mark_serial_observed() broadcaster.tick() - await asyncio.sleep(0.05) - - tick_task = asyncio.create_task(_tick_loop()) - - try: - # Allow setup to complete and at least one send to work - await asyncio.sleep(0.1) - - # From now on, every sendto should fail - monkeypatch.setattr(socket.socket, "sendto", _raise_sendto) - - # Give it a few cycles to "detect" and hypothetically fall back - await asyncio.sleep(0.3) - - # The desired behavior would be to switch to unicast after persistent errors. - # Current code does not, so this assertion should FAIL, making the problem visible. - assert broadcaster._use_unicast is True, ( - "Broadcaster did not fall back to unicast on repeated send errors" - ) - finally: - stop_flag = True - tick_task.cancel() - with contextlib.suppress(asyncio.CancelledError): - await tick_task - broadcaster.close() + data = await asyncio.wait_for( + asyncio.get_running_loop().sock_recv(receiver, 65536), + timeout=3, + ) + assert decode_status_bin_into(data, StatusBuffer()) + finally: + broadcaster.close() From 17096e538a620f14a648e08bfa88d70bb93aa415 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 05:20:11 +0000 Subject: [PATCH 3/4] Clear a standing pause on Stop, Estop and reset execution_paused was only ever cleared by an explicit resume, so a pause followed by a program stop left every later queued command silently held by the segment player with no error and an empty-looking pipeline. Co-Authored-By: Claude Fable 5.1 --- parol6/server/controller.py | 3 +++ parol6/server/state.py | 1 + tests/integration/test_execution_speed.py | 11 +++++++++-- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 2dda415..f5f611e 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -842,6 +842,8 @@ def _handle_system_command( self._segment_player.cancel(state) self._executor.cancel_active_command(reason) self._executor.clear_queue(reason) + # 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 @@ -850,6 +852,7 @@ def _handle_system_command( self._segment_player.cancel(state) self._executor.cancel_active_command("Reset") self._executor.clear_queue("Reset") + state.execution_paused = False self._planner.sync_tool( state.current_tool, variant_key=state.current_tool_variant, diff --git a/parol6/server/state.py b/parol6/server/state.py index b07b04f..24da08c 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -353,6 +353,7 @@ def reset(self) -> None: """ # Safety and control flags self.enabled = True + self.execution_paused = False self.soft_error = False self.disabled_reason = "" self.e_stop_active = False diff --git a/tests/integration/test_execution_speed.py b/tests/integration/test_execution_speed.py index ac6c97e..d883eeb 100644 --- a/tests/integration/test_execution_speed.py +++ b/tests/integration/test_execution_speed.py @@ -57,10 +57,17 @@ def test_execution_pause_speed_dwell_and_standalone_deadlines(client: RobotClien assert client.pause() == 1 with pytest.raises(TimeoutError): client.move_j(start, duration=1, wait=True, timeout=0.2) + # Stop discards the queue the pause was holding, and the pause with + # it: the next queued command runs without a resume. assert client.stop() == 1 - assert client.execution_speed().paused - assert client.resume() == 1 assert client.queue() == [] + assert not client.execution_speed().paused + assert client.move_j(start, duration=1, wait=True, timeout=5) >= 0 + assert np.allclose(client.angles(), start, atol=0.1) + assert client.pause() == 1 + assert client.reset_state() == 1 + assert not client.execution_speed().paused + assert client.resume() == 1 finally: client.stop() client.resume() From 5c81956cd8de67b547189eb38f9717e16e5207fa Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 22:40:35 +0000 Subject: [PATCH 4/4] Dwell on the clock, skip the rate search in steady state, report unconfirmed as 0 A queued delay counted nominal control ticks, so on a loaded controller -- where periods routinely exceed the nominal one -- every delay stretched by the loop's cumulative overrun: five seconds became six at 20% overrun. It measures real time now, with each tick's contribution capped, because the player stops ticking a paused delay altogether and that gap arrives as one enormous tick. The trajectory hot path ran the per-joint acceleration-window search and the admissibility bisection on every tick of every playback, including the common case where nobody has overridden the speed and the rate they compute is forced to zero. They run when the scale is actually changing. pause(), resume() and set_execution_speed() document 0 as "unconfirmed", but a readback whose reply was lost inside the confirmation window raised ConnectionError -- telling the caller the controller was unreachable when it had acked the command moments earlier. Co-Authored-By: Claude Opus 5 --- parol6/client/async_client.py | 6 ++- parol6/commands/utility_commands.py | 20 +++++++- parol6/server/segment_player.py | 51 ++++++++++++--------- tests/unit/test_execution_speed_playback.py | 38 +++++++++++++++ 4 files changed, 90 insertions(+), 25 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 46ae475..14cd977 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -1049,7 +1049,11 @@ async def _request_execution_state( if confirmed: return 1 await asyncio.sleep(0.01) - except TimeoutError: + except (TimeoutError, ConnectionError): + # 0 is "unconfirmed": the command may or may not have been applied. + # A readback whose reply was lost inside the confirmation window is + # exactly that, and raising instead told the caller the controller + # was unreachable when it had acked the command a moment earlier. return 0 async def set_execution_speed(self, scale: float, *, timeout: float = 3.0) -> int: diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index ad9de05..02dd3b9 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -4,6 +4,7 @@ """ import logging +import time from parol6.commands.base import ( CommandBase, @@ -29,18 +30,31 @@ logger = logging.getLogger(__name__) +#: Most real time one tick of a delay may count toward its dwell. A control +#: loop running late still spends real seconds, so the dwell follows the clock; +#: but the segment player stops ticking a paused delay altogether, so the gap +#: across a pause arrives as one enormous tick and must not be counted as +#: waiting the program asked for. +_MAX_DELAY_TICK_S = 4 * INTERVAL_S + + @register_command(CmdType.DELAY) class DelayCommand(CommandBase[DelayCmd]): """ A non-blocking command that pauses execution for a specified duration. + + The dwell is measured on the clock, not in nominal ticks: a loaded + controller's periods exceed the nominal one, and counting ticks stretched + every queued delay by the loop's cumulative overrun. """ PARAMS_TYPE = DelayCmd - __slots__ = ("_remaining_s",) + __slots__ = ("_remaining_s", "_ticked_at") def do_setup(self, state: "ControllerState") -> None: self._remaining_s = self.p.seconds + self._ticked_at = time.perf_counter() logger.info(f" -> Delay starting for {self.p.seconds} seconds...") def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: @@ -48,7 +62,9 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) - self._remaining_s -= INTERVAL_S + now = time.perf_counter() + self._remaining_s -= min(now - self._ticked_at, _MAX_DELAY_TICK_S) + self._ticked_at = now if self._remaining_s <= 0: logger.info(f"Delay finished after {self.p.seconds} seconds.") self.finish() diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 363861d..6e97e1b 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -162,28 +162,35 @@ def tick(self, state: ControllerState) -> bool: target_scale = ( 0.0 if state.execution_paused else state.execution_speed ) - low = -1.0 / EXECUTION_OVERRIDE_TRANSITION_S - high = -low - for joint in range(6): - velocity = active.velocity_rad_s[self._step, joint] - if abs(velocity) > 1e-12: - base = ( - old_scale - * old_scale - * active.acceleration_rad_s2[self._step, joint] - ) - limit = LIMITS.joint.hard.acceleration[joint] - first = (-limit - base) / velocity - second = (limit - base) / velocity - low = max(low, min(first, second)) - high = min(high, max(first, second)) - requested = (target_scale - old_scale) / INTERVAL_S - rate = min(high, max(low, requested)) if low <= high else 0.0 - if requested == 0.0 or rate * requested < 0.0: - rate = 0.0 - new_scale = min(1.0, max(0.0, old_scale + rate * INTERVAL_S)) - if abs(new_scale - target_scale) < 1e-12: - new_scale = target_scale + # Steady state -- every tick of a playback nobody has + # overridden -- holds the scale, so the acceleration-window + # search and the admissibility bisection below are skipped: + # they exist to bound a change, and compute a rate that is + # forced to zero when there is none. + new_scale = old_scale + if target_scale != old_scale: + low = -1.0 / EXECUTION_OVERRIDE_TRANSITION_S + high = -low + for joint in range(6): + velocity = active.velocity_rad_s[self._step, joint] + if abs(velocity) > 1e-12: + base = ( + old_scale + * old_scale + * active.acceleration_rad_s2[self._step, joint] + ) + limit = LIMITS.joint.hard.acceleration[joint] + first = (-limit - base) / velocity + second = (limit - base) / velocity + low = max(low, min(first, second)) + high = min(high, max(first, second)) + requested = (target_scale - old_scale) / INTERVAL_S + rate = min(high, max(low, requested)) if low <= high else 0.0 + if rate * requested < 0.0: + rate = 0.0 + new_scale = min(1.0, max(0.0, old_scale + rate * INTERVAL_S)) + if abs(new_scale - target_scale) < 1e-12: + new_scale = target_scale if new_scale != old_scale and not self._rate_is_admissible( active, old_scale, new_scale ): diff --git a/tests/unit/test_execution_speed_playback.py b/tests/unit/test_execution_speed_playback.py index db057d5..fb13948 100644 --- a/tests/unit/test_execution_speed_playback.py +++ b/tests/unit/test_execution_speed_playback.py @@ -1,6 +1,7 @@ """Observe firmware position commands from the real planner/player pipeline.""" import numpy as np +import pytest from parol6.config import INTERVAL_S, LIMITS, deg_to_steps, steps_to_rad from parol6.protocol.wire import MoveJCmd, PauseCmd, SetExecutionSpeedCmd @@ -80,3 +81,40 @@ def apply(params): ) finally: planner.stop() + + +def test_a_queued_delay_dwells_on_the_clock_and_not_through_a_pause(monkeypatch): + """A delay is a dwell in seconds. + + Counting nominal ticks stretched it by whatever the control loop's real + period is -- on a loaded controller, visibly. Counting raw wall time + instead would swallow the gap where the player holds a paused delay + without ticking it, ending the dwell the moment it resumes. + """ + import parol6.commands.utility_commands as module + from parol6.protocol.wire import DelayCmd + from parol6.server.command_executor import ExecutionStatusCode + + clock = [1_000.0] + monkeypatch.setattr(module.time, "perf_counter", lambda: clock[0]) + state = ControllerState() + + def dwell(step_s: float, pause_s: float = 0.0) -> float: + command, _, error = create_command_from_struct(DelayCmd(seconds=1.0)) + assert command is not None, error + started = clock[0] + command.setup(state) + paused_at = started + 0.4 + while True: + clock[0] += step_s + if pause_s and clock[0] >= paused_at: + clock[0] += pause_s # the player stops ticking a paused delay + pause_s = 0.0 + if command.tick(state) == ExecutionStatusCode.COMPLETED: + return clock[0] - started + + # Ticks arriving 60% late still end the dwell after a second of real time. + assert dwell(INTERVAL_S) == pytest.approx(1.0, abs=2 * INTERVAL_S) + assert dwell(1.6 * INTERVAL_S) == pytest.approx(1.0, abs=3 * INTERVAL_S) + # A two-second hold is not two seconds of waiting the program asked for. + assert dwell(INTERVAL_S, pause_s=2.0) == pytest.approx(3.0, abs=0.1)