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/2] 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/2] 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()