From 2e103fe9d2fc8b78bd2af4f4d247cd2e16d26f4c Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 8 Sep 2026 02:51:58 -0400
Subject: [PATCH 1/7] Report pending planned commands and clear finished
 execution indices

---
 parol6/commands/query_commands.py | 7 ++++++-
 parol6/server/command_executor.py | 5 +++++
 parol6/server/controller.py       | 3 ++-
 parol6/server/segment_player.py   | 8 ++++++++
 parol6/server/state.py            | 3 +++
 5 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py
index c6de72a..bca34be 100644
--- a/parol6/commands/query_commands.py
+++ b/parol6/commands/query_commands.py
@@ -296,7 +296,12 @@ class QueueCommand(QueryCommand[QueueCmd]):
     def compute(self, state: "ControllerState") -> bytes:
         return pack_response(
             QueueResultStruct(
-                queue=state.queue_nonstreamable,
+                queue=state.queue_nonstreamable
+                + [
+                    name
+                    for index, name in state.pending_planned
+                    if index != state.executing_command_index
+                ],
                 executing_index=state.executing_command_index,
                 completed_index=state.completed_command_index,
                 last_checkpoint=state.last_checkpoint,
diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py
index 915fc0e..c9c9a2a 100644
--- a/parol6/server/command_executor.py
+++ b/parol6/server/command_executor.py
@@ -232,6 +232,7 @@ def execute_active_command(self) -> None:
         except Exception as e:
             logger.error("Command execution error: %s", e)
             state.action_current = ""
+            state.executing_command_index = -1
             state.action_params = ""
             state.action_state = ActionState.IDLE
             self._update_queue_state(state)
@@ -272,6 +273,7 @@ def _process_tick_result(
             )
 
             state.action_current = ""
+            state.executing_command_index = -1
             state.action_params = ""
             state.action_state = ActionState.IDLE
             state.completed_command_index = ac.command_index
@@ -288,6 +290,7 @@ def _process_tick_result(
             )
 
             state.action_current = ""
+            state.executing_command_index = -1
             state.action_params = ""
             state.action_state = ActionState.IDLE
 
@@ -320,6 +323,7 @@ def cancel_active_command(self, reason: str = "Cancelled by user") -> None:
 
         state = self._state_manager.get_state()
         state.action_current = ""
+        state.executing_command_index = -1
         state.action_params = ""
         state.action_state = ActionState.IDLE
 
@@ -335,6 +339,7 @@ def cancel_active_streamable(self) -> bool:
         if ac and isinstance(ac.command, MotionCommand) and ac.command.streamable:
             state = self._state_manager.get_state()
             state.action_current = ""
+            state.executing_command_index = -1
             state.action_params = ""
             state.action_state = ActionState.IDLE
             self.active_command = None
diff --git a/parol6/server/controller.py b/parol6/server/controller.py
index 621a5de..d88b7b8 100644
--- a/parol6/server/controller.py
+++ b/parol6/server/controller.py
@@ -826,7 +826,7 @@ def _handle_motion_command(
         # segments are active/queued (e.g. homing), the planner's internal
         # tracking is correct: Position_in may reflect a mid-motion position
         # and the planner has already predicted a queued HOME's homed flags.
-        segment_idle = not self._segment_player.active
+        segment_idle = not self._segment_player.active and not state.pending_planned
         pos_snapshot = state.Position_in.copy() if segment_idle else None
         homed_snapshot: bool | None = None
         if segment_idle:
@@ -843,6 +843,7 @@ def _handle_motion_command(
                 homed=homed_snapshot,
             )
         )
+        state.pending_planned.append((cmd_index, cmd_name))
         if cmd_type and self._ack_policy.requires_ack(cmd_type):
             self._reply_ok_index(addr, cmd_index)
 
diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py
index 363861d..9ae1c54 100644
--- a/parol6/server/segment_player.py
+++ b/parol6/server/segment_player.py
@@ -293,6 +293,7 @@ def tick(self, state: ControllerState) -> bool:
                 state.collision_pairs = tuple(pairs) if pairs else ()
                 state.action_state = ActionState.ERROR
                 state.action_current = ""
+                state.executing_command_index = -1
                 state.action_params = ""
                 self._active = None
                 # Halt: cancel all remaining planned work
@@ -417,7 +418,10 @@ def _complete_segment(self, seg: Segment, state: ControllerState) -> None:
             state.queued_duration -= seg.duration
         state.queued_segments -= 1
         state.completed_command_index = final_idx
+        while state.pending_planned and state.pending_planned[0][0] <= final_idx:
+            state.pending_planned.popleft()
         state.action_current = ""
+        state.executing_command_index = -1
         state.action_params = ""
         state.action_state = ActionState.IDLE
         self._active = None
@@ -428,6 +432,7 @@ def _on_failure(
         """Handle inline command failure: set error state, clear buffer, cancel planner."""
         state.error = error
         state.action_current = ""
+        state.executing_command_index = -1
         state.action_params = ""
         state.action_state = ActionState.ERROR
         self._active = None
@@ -472,6 +477,7 @@ def _world_guard(
             state.collision_pairs = tuple(pairs) if pairs else ()
             state.action_state = ActionState.ERROR
             state.action_current = ""
+            state.executing_command_index = -1
             state.action_params = ""
             self._active = None
             self._buffer.clear()
@@ -486,6 +492,7 @@ def cancel(self, state: ControllerState) -> None:
             # Planned trajectories live here rather than in CommandExecutor.
             # Cancelling its command cannot clear this player's activity.
             state.action_current = ""
+            state.executing_command_index = -1
             state.action_params = ""
             state.action_state = ActionState.IDLE
         self._active = None
@@ -502,5 +509,6 @@ def _drain_planner_queue(self, state: ControllerState) -> None:
         """Drain any remaining segments from the planner's output queue."""
         while self._planner.poll_segment() is not None:
             pass
+        state.pending_planned.clear()
         state.queued_segments = 0
         state.queued_duration = 0.0
diff --git a/parol6/server/state.py b/parol6/server/state.py
index 3cd0901..81e16d8 100644
--- a/parol6/server/state.py
+++ b/parol6/server/state.py
@@ -3,6 +3,7 @@
 import atexit
 import logging
 import secrets
+from collections import deque
 from dataclasses import dataclass, field
 from typing import Any
 
@@ -257,6 +258,7 @@ class ControllerState:
     action_state: ActionState = ActionState.IDLE  # IDLE, EXECUTING, ERROR
     action_next: str = ""
     queue_nonstreamable: list[str] = field(default_factory=list)
+    pending_planned: deque[tuple[int, str]] = field(default_factory=deque)
 
     # Queue progress tracking (monotonically increasing command indices)
     next_command_index: int = 0
@@ -397,6 +399,7 @@ def reset(self) -> None:
         self.action_state = ActionState.IDLE
         self.action_next = ""
         self.queue_nonstreamable.clear()
+        self.pending_planned.clear()
 
         # Queue progress tracking. next_command_index is deliberately NOT
         # reset: indices must stay monotonic across reset so a stale

From 309eb5f700c82d03c77213c9acef902770a02ec0 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Tue, 8 Sep 2026 03:59:20 -0400
Subject: [PATCH 2/7] test: use controller state for queue query fixtures

---
 tests/unit/test_query_commands_actions.py | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py
index 56075e8..0bc2d24 100644
--- a/tests/unit/test_query_commands_actions.py
+++ b/tests/unit/test_query_commands_actions.py
@@ -2,14 +2,13 @@
 Unit tests for action-related query commands.
 
 Tests ACTIVITY and QUEUE query commands without requiring a running server.
-Uses minimal state objects to test command logic in isolation.
+Uses the controller state to test command logic in isolation.
 """
 
-from types import SimpleNamespace
-
 from waldoctl import ActionState
 
 from parol6.commands.query_commands import ActivityCommand, QueueCommand
+from parol6.server.state import ControllerState
 from parol6.protocol.wire import (
     CurrentActionResultStruct,
     ActivityCmd,
@@ -29,7 +28,7 @@ def _unpack_response(data: bytes):
 
 def test_activity_returns_details():
     """Test that ACTIVITY compute() returns correct data."""
-    state = SimpleNamespace(
+    state = ControllerState(
         action_current="MoveJPoseCommand",
         action_state=ActionState.EXECUTING,
         action_next="HomeCommand",
@@ -49,7 +48,7 @@ def test_activity_returns_details():
 
 def test_activity_with_idle_state():
     """Test ACTIVITY when robot is idle."""
-    state = SimpleNamespace(
+    state = ControllerState(
         action_current="",
         action_state=ActionState.IDLE,
         action_next="",
@@ -69,7 +68,7 @@ def test_activity_with_idle_state():
 
 def test_queue_returns_details():
     """Test that QUEUE compute() returns correct data."""
-    state = SimpleNamespace(
+    state = ControllerState(
         queue_nonstreamable=["MoveJPoseCommand", "HomeCommand", "MoveJCommand"],
         executing_command_index=1,
         completed_command_index=0,
@@ -91,7 +90,7 @@ def test_queue_returns_details():
 
 def test_queue_with_empty_queue():
     """Test QUEUE when queue is empty."""
-    state = SimpleNamespace(
+    state = ControllerState(
         queue_nonstreamable=[],
         executing_command_index=-1,
         completed_command_index=-1,
@@ -111,7 +110,7 @@ def test_queue_with_empty_queue():
 
 def test_queue_excludes_streamable():
     """Test that queue only contains non-streamable commands (by design)."""
-    state = SimpleNamespace(
+    state = ControllerState(
         queue_nonstreamable=["MoveJPoseCommand", "HomeCommand"],
         executing_command_index=2,
         completed_command_index=1,

From 9125207c1b56de07114dcbb5a8e518c834973bdd Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Fri, 11 Sep 2026 05:43:26 +0000
Subject: [PATCH 3/7] Drop segments planned before the last cancel

CancelAll travels the planner's command FIFO behind plans already queued,
and the worker only clears its blend buffer on it, so commands still in
the planner's inbox when Stop arrived were emitted afterwards and played:
a stop left an empty queue and an idle executing index for the moment the
restart review looked, then the arm moved. Plans now carry the generation
they were submitted under; a cancel starts a new one and the proxy drops
every segment from an older generation on the way back.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
---
 parol6/server/motion_planner.py          | 33 ++++++++++++++++++++----
 tests/integration/test_stop_semantics.py | 28 ++++++++++++++++++++
 2 files changed, 56 insertions(+), 5 deletions(-)

diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index 32261d2..689f2b4 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -60,6 +60,7 @@ class TrajectorySegment:
     command_name: str = ""
     action_params: str = ""
     blend_consumed_indices: list[int] = field(default_factory=list)
+    generation: int = 0
     velocity_rad_s: np.ndarray = field(init=False)
     acceleration_rad_s2: np.ndarray = field(init=False)
 
@@ -82,6 +83,7 @@ class InlineSegment:
 
     command_index: int
     params: object  # wire struct (msgspec.Struct — picklable)
+    generation: int = 0
 
 
 @dataclass
@@ -93,6 +95,7 @@ class ErrorSegment:
     cartesian_path: np.ndarray | None = None  # (N, 6) full TCP path
     ik_valid: np.ndarray | None = None  # (N,) per-pose bool
     colliding_pairs: list[tuple[str, str]] | None = None  # self-collision viz
+    generation: int = 0
 
 
 Segment = Union[TrajectorySegment, InlineSegment, ErrorSegment]
@@ -112,6 +115,9 @@ class PlanCommand:
         None  # current Position_in (None = use planner internal)
     )
     homed: bool | None = None  # all joints homed (None = use planner internal)
+    # Stamped by the proxy; a cancel starts a new generation and every segment
+    # planned for an older one is dropped on the way back.
+    generation: int = 0
 
 
 @dataclass
@@ -574,6 +580,7 @@ class PlannerWorker:
     def __init__(self, segment_queue: multiprocessing.Queue) -> None:
         self._segment_queue = segment_queue
         self._planner = TrajectoryPlanner(diagnostic=False)
+        self._generation = 0
 
     @property
     def state(self) -> PlannerState:
@@ -586,14 +593,17 @@ def process_command(self, msg: PlanCommand) -> None:
         if msg.homed is not None:
             self._planner.state.Homed_in.fill(1 if msg.homed else 0)
 
+        self._generation = msg.generation
         segments = self._planner.process(msg.params, msg.command_index)
         for seg in segments:
+            seg.generation = self._generation
             self._segment_queue.put(seg)
 
     def flush_stale_blend(self) -> None:
         """Flush any pending blend buffer (called on queue timeout)."""
         segments = self._planner.flush()
         for seg in segments:
+            seg.generation = self._generation
             self._segment_queue.put(seg)
 
     def cancel(self) -> None:
@@ -761,6 +771,10 @@ def __init__(self) -> None:
         self._shutdown_event: EventType = multiprocessing.Event()
         self._ready_event: EventType = multiprocessing.Event()
         self._process: multiprocessing.Process | None = None
+        # CancelAll travels the command FIFO behind plans already queued, so
+        # the worker still emits them after a cancel; the generation is what
+        # tells those late segments from the next program's.
+        self._generation = 0
 
     # -- lifecycle --
 
@@ -832,6 +846,8 @@ def alive(self) -> bool:
 
     def submit(self, msg: PlannerMessage) -> None:
         """Send a message to the planner (non-blocking)."""
+        if isinstance(msg, PlanCommand):
+            msg.generation = self._generation
         self._command_queue.put_nowait(msg)
 
     def sync_position(self, position_in: np.ndarray) -> None:
@@ -865,16 +881,23 @@ def sync_shapes(self, shapes: list) -> None:
 
     def cancel(self) -> None:
         """Cancel all pending work in the planner."""
+        self._generation += 1
         self.submit(CancelAll())
 
     # -- planner → main --
 
     def poll_segment(self) -> Segment | None:
-        """Non-blocking poll for a computed segment. Returns None if empty."""
-        try:
-            return self._segment_queue.get_nowait()
-        except queue.Empty:
-            return None
+        """Non-blocking poll for a computed segment. Returns None if empty.
+
+        Segments planned before the last cancel are discarded here.
+        """
+        while True:
+            try:
+                seg = self._segment_queue.get_nowait()
+            except queue.Empty:
+                return None
+            if seg.generation >= self._generation:
+                return seg
 
 
 def _drain_queue(q: multiprocessing.Queue) -> None:
diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py
index 2db63dd..8f9a097 100644
--- a/tests/integration/test_stop_semantics.py
+++ b/tests/integration/test_stop_semantics.py
@@ -89,3 +89,31 @@ def test_estop_latches_until_reset(client: RobotClient, server_proc):
         "canceled motion resurfaced after reset"
     )
     assert client.home(wait=True, timeout=30.0) >= 0
+
+
+def test_stop_discards_plans_still_in_the_planner(client: RobotClient, server_proc):
+    """Commands the planner has not finished planning when Stop arrives must
+    not play afterwards: a plan finished after the cancel is not a queue."""
+    start = client.angles()
+    assert start is not None
+    pose = client.pose()
+    assert pose is not None
+    away = list(pose)
+    away[0] += 40.0
+    # Cartesian plans take the planner long enough that Stop lands while
+    # most of these are still in its inbox, behind which CancelAll queues.
+    for i in range(40):
+        target = away if i % 2 == 0 else pose
+        assert client.move_l(target, duration=10.0, wait=False) >= 0
+    assert client.stop() == 1
+    time.sleep(0.3)
+    frozen = client.angles()
+    assert frozen is not None
+    time.sleep(1.5)
+    after = client.angles()
+    assert after is not None
+    assert np.allclose(after, frozen, atol=0.05), (
+        f"a plan finished after Stop played: {frozen} -> {after}"
+    )
+    assert client.queue() == []
+    assert client.home(wait=True, timeout=30.0) >= 0

From 2bdc5bb3cde3b2d709c471abc3ab15071258cc57 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Fri, 11 Sep 2026 23:33:18 +0000
Subject: [PATCH 4/7] Test the queue readback the PR body claimed, against the
 controller

The queue's contents are maintained in three places -- the planner's pending
list, the blend consumption that swallows indices, and the executing-index
exclusion -- and none of them were exercised: the only test change swapped a
namespace for a state object with the pending list left empty. Dropping either
mechanism left the suite green.

This drives the client against the simulated controller: a paused queue lists
what is owed, a resumed one drains to empty, a blend chain takes its consumed
indices with it, the executing command is not listed as owed work as well, and
a Stop clears the queue and the pause it was holding. Both mechanisms were
checked by breaking them.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
---
 tests/integration/test_queue_readback.py | 106 +++++++++++++++++++++++
 1 file changed, 106 insertions(+)
 create mode 100644 tests/integration/test_queue_readback.py

diff --git a/tests/integration/test_queue_readback.py b/tests/integration/test_queue_readback.py
new file mode 100644
index 0000000..f23d6a8
--- /dev/null
+++ b/tests/integration/test_queue_readback.py
@@ -0,0 +1,106 @@
+"""What QUEUE reports, against the simulated controller.
+
+The readback is what an operator and the frontend's playback bar read to know
+what is still owed: commands the planner has accepted but not started, the one
+executing now, and nothing at all once a Stop has cleared the queue. Every
+assertion here goes through the client and the real controller, because the
+pieces it is made of -- the planner's pending list, the blend consumption, the
+executing-index exclusion -- are maintained in three different places.
+"""
+
+import numpy as np
+import pytest
+
+from parol6 import RobotClient
+
+
+def _wait(condition, message: str, timeout: float = 10.0) -> None:
+    import time
+
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        if condition():
+            return
+        time.sleep(0.02)
+    pytest.fail(message)
+
+
+def test_the_queue_lists_what_is_owed_and_a_stop_clears_it(client: RobotClient):
+    start = client.angles()
+    assert start is not None
+    first, second = list(start), list(start)
+    first[0] += 6
+    second[0] += 12
+    try:
+        # Paused, so everything accepted stays owed and nothing moves.
+        assert client.pause() == 1
+        held = client.move_j(first, duration=1, wait=False)
+        queued = client.move_j(second, duration=1, wait=False)
+        assert held >= 0 and queued > held
+        _wait(
+            lambda: len(client.queue() or []) >= 2,
+            "the paused queue never listed both accepted commands",
+        )
+        listed = client.queue()
+        assert listed and all(name for name in listed), listed
+        assert any("MoveJ" in name for name in listed)
+        assert np.allclose(client.angles(), start, atol=0.05)
+
+        # Resuming drains it: what the queue reports is what is still owed.
+        assert client.resume() == 1
+        assert client.wait_command(queued, timeout=20)
+        _wait(lambda: client.queue() == [], "the drained queue still reports work")
+        assert np.allclose(client.angles(), second, atol=0.2)
+
+        # A blended chain is consumed as one motion, and the indices it
+        # swallowed leave the queue with it rather than lingering as owed work.
+        assert client.pause() == 1
+        corner = list(second)
+        corner[0] -= 6
+        blended = client.move_j(corner, duration=1, r=15, wait=False)
+        tail = client.move_j(start, duration=1, wait=False)
+        _wait(
+            lambda: len(client.queue() or []) >= 2,
+            "the paused queue never listed the blend chain",
+        )
+        assert client.resume() == 1
+        assert client.wait_command(tail, timeout=20)
+        _wait(
+            lambda: client.queue() == [],
+            "the blend's consumed indices stayed in the queue",
+        )
+        # The blended command completed with the chain that swallowed it.
+        assert blended >= 0 and client.wait_command(blended, timeout=5)
+
+        # With one command executing, the queue reports what is owed after it:
+        # the executing index is reported in its own field and listing it again
+        # would double-count the motion the arm is already making.
+        client.move_j(first, duration=3, wait=False)
+        trailing = client.move_j(second, duration=1, wait=False)
+        _wait(
+            lambda: abs((client.angles() or start)[0] - start[0]) > 0.5,
+            "the first move never started",
+        )
+        listed = client.queue()
+        assert listed is not None and len(listed) == 1, (
+            f"the executing command is listed as owed work as well: {listed}"
+        )
+        assert client.wait_command(trailing, timeout=25)
+
+        # Stop clears what was owed, and the readback says so immediately.
+        assert client.pause() == 1
+        client.move_j(first, duration=2, wait=False)
+        client.move_j(second, duration=2, wait=False)
+        _wait(
+            lambda: len(client.queue() or []) >= 2,
+            "the paused queue never listed the commands a Stop must clear",
+        )
+        assert client.stop() == 1
+        _wait(lambda: client.queue() == [], "Stop left work in the queue")
+        assert not client.execution_speed().paused, (
+            "Stop drops the pause with the queue it was holding"
+        )
+    finally:
+        client.stop()
+        client.resume()
+        client.set_execution_speed(1)

From 8ce741472838b6e895b3258bd67c6820e40ce3c5 Mon Sep 17 00:00:00 2001
From: Claude <noreply@anthropic.com>
Date: Fri, 18 Sep 2026 14:40:46 +0000
Subject: [PATCH 5/7] Keep failed command indices and list only unstarted work

A planner-side failure carries the generation of the command it answers,
so it is not discarded as pre-stop work after a Stop. An ERROR frame
keeps the failed command's index until the next activation or cancel.
The queue readback drops a chain the moment its head starts, consumed
blend members included, so QUEUE lists only commands not yet started.
The polling and dry-run row helpers live in conftest.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01N8zt66KjkzgZLtQuSfJd9r
---
 parol6/server/command_executor.py          |  1 -
 parol6/server/motion_planner.py            |  1 +
 parol6/server/segment_player.py            | 12 ++++++---
 tests/conftest.py                          | 19 ++++++++++++++
 tests/integration/test_queue_readback.py   |  4 +++
 tests/integration/test_shapes_e2e.py       | 30 ++++++++++------------
 tests/integration/test_stale_error_wait.py | 14 ++--------
 tests/integration/test_stop_semantics.py   | 27 +++++++++++++++++++
 tests/unit/test_dry_run_record.py          | 17 ++++--------
 tests/unit/test_dry_run_script_compat.py   | 12 +++------
 10 files changed, 83 insertions(+), 54 deletions(-)

diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py
index 1bd5def..b36186b 100644
--- a/parol6/server/command_executor.py
+++ b/parol6/server/command_executor.py
@@ -290,7 +290,6 @@ def _process_tick_result(
             )
 
             state.action_current = ""
-            state.executing_command_index = -1
             state.action_params = ""
             state.action_state = ActionState.IDLE
 
diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py
index 689f2b4..d1112db 100644
--- a/parol6/server/motion_planner.py
+++ b/parol6/server/motion_planner.py
@@ -737,6 +737,7 @@ def motion_planner_main(
                         ErrorSegment(
                             command_index=msg.command_index,
                             error=robot_error,
+                            generation=msg.generation,
                         )
                     )
                     worker.cancel()
diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py
index 93fe9a4..fa8618e 100644
--- a/parol6/server/segment_player.py
+++ b/parol6/server/segment_player.py
@@ -300,7 +300,6 @@ def tick(self, state: ControllerState) -> bool:
                 state.collision_pairs = tuple(pairs) if pairs else ()
                 state.action_state = ActionState.ERROR
                 state.action_current = ""
-                state.executing_command_index = -1
                 state.action_params = ""
                 self._active = None
                 # Halt: cancel all remaining planned work
@@ -365,6 +364,15 @@ def _activate_next(self, state: ControllerState) -> None:
         self._inline_activated = False
         state.executing_command_index = self._active.command_index
         state.action_state = ActionState.EXECUTING
+        # The chain this segment plays is no longer owed: QUEUE lists only
+        # commands not yet started.
+        started = seg.command_index
+        if isinstance(seg, TrajectorySegment):
+            for idx in seg.blend_consumed_indices:
+                if idx > started:
+                    started = idx
+        while state.pending_planned and state.pending_planned[0][0] <= started:
+            state.pending_planned.popleft()
         # Populate action info for trajectory segments (inline segments set these later)
         if isinstance(self._active, TrajectorySegment):
             self._position_rad[:] = self._active.trajectory_rad[0]
@@ -441,7 +449,6 @@ def _on_failure(
         """Handle inline command failure: set error state, clear buffer, cancel planner."""
         state.error = error
         state.action_current = ""
-        state.executing_command_index = -1
         state.action_params = ""
         state.action_state = ActionState.ERROR
         self._active = None
@@ -486,7 +493,6 @@ def _world_guard(
             state.collision_pairs = tuple(pairs) if pairs else ()
             state.action_state = ActionState.ERROR
             state.action_current = ""
-            state.executing_command_index = -1
             state.action_params = ""
             self._active = None
             self._buffer.clear()
diff --git a/tests/conftest.py b/tests/conftest.py
index bd0c932..64c27e8 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -6,8 +6,10 @@
 """
 
 import logging
+import math
 import os
 import socket
+import time
 from collections.abc import Generator
 from dataclasses import dataclass
 
@@ -27,6 +29,23 @@ def free_udp_port() -> int:
         return sock.getsockname()[1]
 
 
+def wait_until(pred, timeout: float, msg: str) -> None:
+    """Poll ``pred`` until it holds, failing the test with ``msg`` at ``timeout``."""
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        if pred():
+            return
+        time.sleep(0.02)
+    pytest.fail(msg)
+
+
+def rows_for(seconds: float) -> int:
+    """Rows a hold of ``seconds`` occupies in a dry-run record from its first tick."""
+    from parol6.client.dry_run_client import _STRIDE
+
+    return math.ceil(round(seconds / cfg.INTERVAL_S) / _STRIDE)
+
+
 logger = logging.getLogger(__name__)
 
 
diff --git a/tests/integration/test_queue_readback.py b/tests/integration/test_queue_readback.py
index f23d6a8..d44b776 100644
--- a/tests/integration/test_queue_readback.py
+++ b/tests/integration/test_queue_readback.py
@@ -64,6 +64,10 @@ def test_the_queue_lists_what_is_owed_and_a_stop_clears_it(client: RobotClient):
             "the paused queue never listed the blend chain",
         )
         assert client.resume() == 1
+        # The chain plays as one motion: while its head executes, the command
+        # it swallowed is not owed work either.
+        assert client.wait_status(lambda s: s.executing_index == blended, timeout=5)
+        assert client.queue() == [], "a consumed blend member was listed as owed"
         assert client.wait_command(tail, timeout=20)
         _wait(
             lambda: client.queue() == [],
diff --git a/tests/integration/test_shapes_e2e.py b/tests/integration/test_shapes_e2e.py
index 5b28148..91d1959 100644
--- a/tests/integration/test_shapes_e2e.py
+++ b/tests/integration/test_shapes_e2e.py
@@ -18,7 +18,7 @@
 
 from parol6 import MotionError, RobotClient
 
-from tests.conftest import free_udp_port
+from tests.conftest import free_udp_port, wait_until
 from waldoctl import Box, Physical
 
 pytestmark = pytest.mark.integration
@@ -106,7 +106,7 @@ def test_attached_part_blocks_motion_except_for_declared_contacts(client: RobotC
         assert client.shapes().program[-1] == part
 
         assert client.estop() == 1
-        _wait_until(
+        wait_until(
             lambda: not client.shapes().attachments_valid,
             3.0,
             "attachment remained valid after stop",
@@ -153,15 +153,6 @@ def _wrist_box(target_deg: list[float], name: str) -> Box:
     )
 
 
-def _wait_until(pred, timeout: float, msg: str) -> None:
-    deadline = time.monotonic() + timeout
-    while time.monotonic() < deadline:
-        if pred():
-            return
-        time.sleep(0.02)
-    pytest.fail(msg)
-
-
 def _j1(client: RobotClient) -> float:
     angles = client.angles()
     assert angles is not None
@@ -222,18 +213,23 @@ def test_set_shapes_mid_flight_halts_streaming_move(client: RobotClient, server_
     try:
         idx = client.move_j(target, duration=4.0, wait=False)
         assert idx >= 0
-        _wait_until(
+        wait_until(
             lambda: _j1(client) < HOME_J1 - 5.0, 10.0, "move never started streaming"
         )
 
         assert client.set_shapes([_wrist_box(target, "blocker")]) == 1
-        _wait_until(
+        wait_until(
             lambda: client.error() is not None,
             3.0,
             "world change never halted the move",
         )
         err = client.error()
         assert err is not None and "shape:blocker" in err.cause
+        # The ERROR frame names the halted command, so a log keyed on the
+        # executing index sees the failure.
+        assert client.wait_status(
+            lambda s: s.executing_index == idx and s.error is not None, timeout=2.0
+        ), "the ERROR frame dropped the halted command's index"
 
         j1_stop = _j1(client)
         assert j1_stop > 30.0, f"arm reached the keep-out region (J1={j1_stop:.1f})"
@@ -256,13 +252,13 @@ def test_set_shapes_mid_flight_rejects_queued_move_at_activation(
         i1 = client.move_j(t1, duration=1.5, wait=False)
         i2 = client.move_j(t2, duration=2.0, wait=False)
         assert i1 >= 0 and i2 >= 0
-        _wait_until(
+        wait_until(
             lambda: _j1(client) < HOME_J1 - 2.0, 10.0, "first move never started"
         )
 
         assert client.set_shapes([_wrist_box(t2, "late-wall")]) == 1
 
-        _wait_until(
+        wait_until(
             lambda: client.error() is not None,
             10.0,
             "queued move was never invalidated",
@@ -270,7 +266,7 @@ def test_set_shapes_mid_flight_rejects_queued_move_at_activation(
         err = client.error()
         assert err is not None and "shape:late-wall" in err.cause
         # The clear first move finished; the blocked second never streamed.
-        _wait_until(
+        wait_until(
             lambda: abs(_j1(client) - 60.0) < 2.0,
             10.0,
             f"arm not at the first target (J1={_j1(client):.1f})",
@@ -291,7 +287,7 @@ def test_set_shapes_mid_flight_off_path_does_not_disturb_motion(
     try:
         idx = client.move_j(target, duration=2.5, wait=False)
         assert idx >= 0
-        _wait_until(
+        wait_until(
             lambda: _j1(client) < HOME_J1 - 5.0, 10.0, "move never started streaming"
         )
 
diff --git a/tests/integration/test_stale_error_wait.py b/tests/integration/test_stale_error_wait.py
index fa34301..335b584 100644
--- a/tests/integration/test_stale_error_wait.py
+++ b/tests/integration/test_stale_error_wait.py
@@ -8,26 +8,16 @@
 "failing" with a stale self-collision error as the arm visibly homed).
 """
 
-import time
-
 import numpy as np
 import pytest
 
 from parol6 import MotionError, RobotClient
+from tests.conftest import wait_until
 from waldoctl import Box
 
 pytestmark = pytest.mark.integration
 
 
-def _wait_until(pred, timeout: float, msg: str) -> None:
-    deadline = time.monotonic() + timeout
-    while time.monotonic() < deadline:
-        if pred():
-            return
-        time.sleep(0.02)
-    pytest.fail(msg)
-
-
 def _reject_move(client: RobotClient) -> None:
     """Drive a real guard rejection: a keep-out enveloping the target wrist."""
     import parol6.PAROL6_ROBOT as PAROL6_ROBOT
@@ -63,7 +53,7 @@ def test_streaming_accept_clears_stale_error(client: RobotClient, server_proc):
         _reject_move(client)
         assert client.error() is not None
         assert client.jog_j(0, 0.2, 0.2) >= 0
-        _wait_until(
+        wait_until(
             lambda: client.error() is None,
             2.0,
             "jog accept never cleared the stale error",
diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py
index 561c722..b73bece 100644
--- a/tests/integration/test_stop_semantics.py
+++ b/tests/integration/test_stop_semantics.py
@@ -8,12 +8,14 @@
 subsequent command.
 """
 
+import socket
 import time
 
 import numpy as np
 import pytest
 
 from parol6 import MotionError, RobotClient
+from parol6.protocol.wire import SelectToolCmd, encode_command
 
 pytestmark = pytest.mark.integration
 
@@ -119,6 +121,31 @@ def test_stop_discards_plans_still_in_the_planner(client: RobotClient, server_pr
     assert client.home(wait=True, timeout=30.0) >= 0
 
 
+def test_a_planner_failure_after_a_stop_still_surfaces(
+    client: RobotClient, server_proc, ports
+):
+    """A Stop starts a new planner generation; a command the planner itself
+    fails on afterwards must still report, not vanish as pre-stop work."""
+    assert client.stop() == 1
+    assert client.error() is None
+    # The client uppercases tool names; the worker applies the raw name, so a
+    # lowercase one passes wire validation and fails inside the planner.
+    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as raw:
+        raw.sendto(
+            encode_command(SelectToolCmd(tool_name="none")),
+            (ports.server_ip, ports.server_port),
+        )
+    deadline = time.monotonic() + 5
+    while client.error() is None:
+        assert time.monotonic() < deadline, (
+            "the planner failure never surfaced after a Stop"
+        )
+        time.sleep(0.02)
+    error = client.error()
+    assert error is not None and "Unknown tool" in error.cause
+    assert client.queue() == []
+
+
 def test_stop_discards_a_queued_tcp_transform_from_the_planner_too(
     client: RobotClient, server_proc
 ):
diff --git a/tests/unit/test_dry_run_record.py b/tests/unit/test_dry_run_record.py
index 44ace1e..a08a80d 100644
--- a/tests/unit/test_dry_run_record.py
+++ b/tests/unit/test_dry_run_record.py
@@ -1,14 +1,12 @@
 """The commanded record a parol6 dry run returns: one block per command on
 one row axis, with delays, tool travel and refusals all on it."""
 
-import math
-
 import numpy as np
 import pytest
 from waldoctl import following_error
 
-from parol6.client.dry_run_client import _STRIDE, DryRunRobotClient
-from parol6.config import INTERVAL_S
+from parol6.client.dry_run_client import DryRunRobotClient
+from tests.conftest import rows_for
 from parol6.tools import get_registry
 
 HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0]
@@ -16,11 +14,6 @@
 W2 = [70.0, -70.0, 200.0, 20.0, 20.0, 200.0]
 
 
-def _rows_for(seconds: float) -> int:
-    """Rows a hold of *seconds* occupies from the program's first tick."""
-    return math.ceil(round(seconds / INTERVAL_S) / _STRIDE)
-
-
 def _span(record, block):
     return slice(block.start_row, block.start_row + block.rows)
 
@@ -30,7 +23,7 @@ def test_delay_holds_the_pose_for_its_rows():
     index = client.delay(2.0)
     record = client.plan()
     block = record.blocks[index]
-    assert block.command == index and block.rows == _rows_for(2.0) == 100
+    assert block.command == index and block.rows == rows_for(2.0) == 100
     held = record.joints_rad[_span(record, block)]
     # Motor-step quantisation moves the pose by well under a hundredth of a
     # degree; what matters is that every row holds the same pose.
@@ -53,7 +46,7 @@ def test_gripper_close_ramps_the_jaws_over_the_tools_travel():
     block = record.blocks[index]
     expected = get_registry().get("SSG-48").estimate_duration("close", [])
     assert expected > 0
-    assert block.rows == pytest.approx(_rows_for(expected), abs=1)
+    assert block.rows == pytest.approx(rows_for(expected), abs=1)
     closed = record.tool_closed[_span(record, block)]
     assert closed[0] == pytest.approx(0.0, abs=0.05)
     assert np.all(np.diff(closed) >= 0) and closed[-1] > 0.9
@@ -121,6 +114,6 @@ def test_budget_truncates_the_record_and_says_so():
     full = client.plan()
     cut = client.plan(max_seconds=1.0)
     assert cut.stop == "budget_exhausted"
-    assert cut.rows == _rows_for(1.0) < full.rows
+    assert cut.rows == rows_for(1.0) < full.rows
     assert cut.blocks[0].rows == cut.rows and cut.blocks[1].rows == 0
     assert full.stop == "completed"
diff --git a/tests/unit/test_dry_run_script_compat.py b/tests/unit/test_dry_run_script_compat.py
index 4872e84..eaba405 100644
--- a/tests/unit/test_dry_run_script_compat.py
+++ b/tests/unit/test_dry_run_script_compat.py
@@ -13,8 +13,8 @@
 import pytest
 from waldoctl import CommandKind, command_table
 
-from parol6.client.dry_run_client import _CMD_STRUCTS, _STRIDE, DryRunRobotClient
-from parol6.config import INTERVAL_S
+from parol6.client.dry_run_client import _CMD_STRUCTS, DryRunRobotClient
+from tests.conftest import rows_for
 
 HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0]
 POSE_A = [0.0, 280.0, 200.0, 90.0, 0.0, 90.0]
@@ -24,12 +24,6 @@
 ANGLES_B = [70.0, -70.0, 200.0, 20.0, 20.0, 200.0]
 
 
-def _rows_for(seconds: float) -> int:
-    import math
-
-    return math.ceil(round(seconds / INTERVAL_S) / _STRIDE)
-
-
 @pytest.fixture
 def client():
     return DryRunRobotClient()
@@ -111,7 +105,7 @@ def test_flush(self, client):
 
     def test_delay(self, client):
         index = client.delay(1.0)
-        assert client.plan().blocks[index].rows == _rows_for(1.0)
+        assert client.plan().blocks[index].rows == rows_for(1.0)
 
     def test_wait_motion(self, client):
         client.move_j(ANGLES_A, speed=0.5)

From ee670554058869d48354e9005d548279b67048cd Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sat, 19 Sep 2026 12:48:45 -0400
Subject: [PATCH 6/7] Generate trapezoidal and quintic profiles directly

Both profiles were only ever asked for rest-to-rest motion, so interpolatepy
was supplying two closed-form curves through a general API, sampled one
Python call at a time. Sampling them here vectorises that and drops the
dependency, whose 3.3.0 removed the module the trapezoid import named and
ships no Windows or Python 3.14 wheels.

Positions match the previous output to 6e-9 rad across randomised cases,
which is a thousandth of a motor step.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01GMMP6KSdDFv5ZaJXC7ALii
---
 parol6/motion/trajectory.py | 137 +++++++++++++++++-------------------
 pyproject.toml              |   1 -
 2 files changed, 64 insertions(+), 74 deletions(-)

diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py
index 74288c6..24c7c9a 100644
--- a/parol6/motion/trajectory.py
+++ b/parol6/motion/trajectory.py
@@ -50,6 +50,54 @@ def _rad_to_steps_alloc(rad: NDArray) -> NDArray[np.int32]:
     return out
 
 
+def _trapezoid_duration(distance: float, v_max: float, a_max: float) -> float:
+    """Duration of a trapezoidal profile over ``distance``, starting and ending at rest."""
+    distance = abs(distance)
+    if distance < 1e-12:
+        return 0.0
+    if distance * a_max >= v_max * v_max:
+        return v_max / a_max + distance / v_max
+    return 2.0 * float(np.sqrt(distance / a_max))
+
+
+def _trapezoid_samples(
+    times: NDArray[np.float64], q0: float, q1: float, v_max: float, a_max: float
+) -> NDArray[np.float64]:
+    """Sample a trapezoidal profile from ``q0`` to ``q1``, starting and ending at rest."""
+    distance = abs(q1 - q0)
+    duration = _trapezoid_duration(distance, v_max, a_max)
+    if duration <= 0.0:
+        return np.full(times.shape, q0, dtype=np.float64)
+
+    if distance * a_max >= v_max * v_max:
+        t_accel = v_max / a_max
+        v_peak = v_max
+    else:
+        t_accel = duration / 2.0
+        v_peak = a_max * t_accel
+
+    t = np.clip(times, 0.0, duration)
+    t_decel = duration - t_accel
+    travelled = np.where(
+        t < t_accel,
+        0.5 * a_max * t * t,
+        np.where(
+            t < t_decel,
+            0.5 * v_peak * t_accel + v_peak * (t - t_accel),
+            distance - 0.5 * a_max * (duration - t) ** 2,
+        ),
+    )
+    return q0 + np.sign(q1 - q0) * travelled
+
+
+def _quintic_samples(
+    times: NDArray[np.float64], q0: float, q1: float, duration: float
+) -> NDArray[np.float64]:
+    """Sample a quintic profile from ``q0`` to ``q1``, at rest and unaccelerated at both ends."""
+    s = np.clip(times / duration, 0.0, 1.0)
+    return q0 + (q1 - q0) * s * s * s * (10.0 - 15.0 * s + 6.0 * s * s)
+
+
 class _LinearPath:
     """Piecewise linear path wrapper for TOPPRA compatibility.
 
@@ -742,15 +790,10 @@ def _compute_joint_duration_trapezoid(self) -> float:
         """
         Compute duration for joint paths using trapezoidal profile.
 
-        For each joint, uses InterpolatePy to compute the minimum duration
-        for its displacement given its velocity/acceleration limits.
+        For each joint, computes the minimum duration for its displacement
+        given its velocity/acceleration limits.
         Returns the maximum (slowest joint determines overall duration).
         """
-        from interpolatepy.trapezoidal import (
-            TrajectoryParams as TrapParams,
-            TrapezoidalTrajectory,
-        )
-
         positions = self.joint_path.positions
         if len(positions) < 2:
             return self.dt * 2
@@ -763,15 +806,7 @@ def _compute_joint_duration_trapezoid(self) -> float:
             if delta < 1e-6:
                 continue
 
-            params = TrapParams(
-                q0=0.0,
-                q1=delta,
-                v0=0.0,
-                v1=0.0,
-                vmax=self.v_max[j],
-                amax=self.a_max[j],
-            )
-            _, duration = TrapezoidalTrajectory.generate_trajectory(params)
+            duration = _trapezoid_duration(delta, self.v_max[j], self.a_max[j])
             max_duration = max(max_duration, duration)
 
         return max(max_duration, self.dt * 2)
@@ -879,8 +914,6 @@ def _build_quintic_trajectory_joint(self) -> Trajectory:
         Each joint independently follows a quintic polynomial profile,
         synchronized to finish at the same time.
         """
-        from interpolatepy import BoundaryCondition, PolynomialTrajectory, TimeInterval
-
         start_pos = self.joint_path.positions[0]
         end_pos = self.joint_path.positions[-1]
 
@@ -899,17 +932,9 @@ def _build_quintic_trajectory_joint(self) -> Trajectory:
                 trajectory_rad[:, j] = start_pos[j]
                 continue
 
-            bc_start = BoundaryCondition(
-                position=start_pos[j], velocity=0.0, acceleration=0.0
-            )
-            bc_end = BoundaryCondition(
-                position=end_pos[j], velocity=0.0, acceleration=0.0
+            trajectory_rad[:, j] = _quintic_samples(
+                times, start_pos[j], end_pos[j], duration
             )
-            interval = TimeInterval(start=0.0, end=duration)
-            traj = PolynomialTrajectory.order_5_trajectory(bc_start, bc_end, interval)
-
-            for i, t in enumerate(times):
-                trajectory_rad[i, j] = traj(t)[0]
 
         trajectory_rad, duration = self._enforce_segment_limits(
             trajectory_rad, duration
@@ -926,8 +951,6 @@ def _build_quintic_trajectory_cartesian(self) -> Trajectory:
         TCP follows quintic polynomial profile along the path, with local
         slowdown where velocity limits would be exceeded.
         """
-        from interpolatepy import BoundaryCondition, PolynomialTrajectory, TimeInterval
-
         if self.duration:
             duration = self.duration
         else:
@@ -935,17 +958,10 @@ def _build_quintic_trajectory_cartesian(self) -> Trajectory:
             duration = self._compute_cartesian_duration_from_path()
 
         # Quintic profile for the path parameter s, from s=0 to s=1
-        bc_start = BoundaryCondition(position=0.0, velocity=0.0, acceleration=0.0)
-        bc_end = BoundaryCondition(position=1.0, velocity=0.0, acceleration=0.0)
-        interval = TimeInterval(start=0.0, end=duration)
-        traj = PolynomialTrajectory.order_5_trajectory(bc_start, bc_end, interval)
-
         n_output = max(2, int(np.ceil(duration / self.dt)))
         times = np.linspace(0.0, duration, n_output)
 
-        profile_s = np.empty(n_output, dtype=np.float64)
-        for i in range(n_output):
-            profile_s[i] = traj(float(times[i]))[0]
+        profile_s = _quintic_samples(times, 0.0, 1.0, duration)
 
         trajectory_rad = self.joint_path.sample_many(profile_s)
 
@@ -976,11 +992,6 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory:
         Each joint independently follows a trapezoidal velocity profile,
         synchronized to finish at the same time.
         """
-        from interpolatepy.trapezoidal import (
-            TrajectoryParams as TrapParams,
-            TrapezoidalTrajectory,
-        )
-
         start_pos = self.joint_path.positions[0]
         end_pos = self.joint_path.positions[-1]
 
@@ -999,23 +1010,18 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory:
                 trajectory_rad[:, j] = start_pos[j]
                 continue
 
-            params = TrapParams(
-                q0=start_pos[j],
-                q1=end_pos[j],
-                v0=0.0,
-                v1=0.0,
-                vmax=self.v_max[j],
-                amax=self.a_max[j],
-            )
-            traj_fn, profile_duration = TrapezoidalTrajectory.generate_trajectory(
-                params
-            )
+            profile_duration = _trapezoid_duration(delta, self.v_max[j], self.a_max[j])
 
             # Scale this joint's own profile time onto the synchronized duration
             time_scale = profile_duration / duration if duration > 0 else 1.0
 
-            for i, t in enumerate(times):
-                trajectory_rad[i, j] = traj_fn(t * time_scale)[0]
+            trajectory_rad[:, j] = _trapezoid_samples(
+                times * time_scale,
+                start_pos[j],
+                end_pos[j],
+                self.v_max[j],
+                self.a_max[j],
+            )
 
         trajectory_rad, duration = self._enforce_segment_limits(
             trajectory_rad, duration
@@ -1032,11 +1038,6 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory:
         TCP follows trapezoidal velocity profile along the path, with local
         slowdown where velocity limits would be exceeded.
         """
-        from interpolatepy.trapezoidal import (
-            TrajectoryParams as TrapParams,
-            TrapezoidalTrajectory,
-        )
-
         if self.duration:
             duration = self.duration
         else:
@@ -1046,15 +1047,7 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory:
         vmax_s, amax_s, _ = self._compute_s_profile_limits()
 
         # Trapezoidal profile for the path parameter s, from s=0 to s=1
-        params = TrapParams(
-            q0=0.0,
-            q1=1.0,
-            v0=0.0,
-            v1=0.0,
-            vmax=vmax_s,
-            amax=amax_s,
-        )
-        traj_fn, profile_duration = TrapezoidalTrajectory.generate_trajectory(params)
+        profile_duration = _trapezoid_duration(1.0, vmax_s, amax_s)
 
         # If user specified longer duration, scale to match
         if self.duration and self.duration > profile_duration:
@@ -1067,9 +1060,7 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory:
         n_output = max(2, int(np.ceil(duration / self.dt)))
         times = np.linspace(0.0, duration, n_output)
 
-        profile_s = np.array(
-            [traj_fn(t * time_scale)[0] for t in times], dtype=np.float64
-        )
+        profile_s = _trapezoid_samples(times * time_scale, 0.0, 1.0, vmax_s, amax_s)
 
         trajectory_rad = self.joint_path.sample_many(profile_s)
 
diff --git a/pyproject.toml b/pyproject.toml
index dcc440e..92fe83a 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -40,7 +40,6 @@ dependencies = [
     "scipy>=1.11.4",
     "ruckig>=0.12.2",
     "toppra>=0.6.3",
-    "interpolatepy>=2.0.0",
     "numpy>=2.0,<2.5",  # numba (0.6x) requires numpy<2.5
     "numba>=0.59",
     "psutil>=5.9",

From 8c255a670f5000bf6beab5a73d2647aa5079c9ae Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sat, 19 Sep 2026 13:15:15 -0400
Subject: [PATCH 7/7] Let a caller's deadline cancel a status wait

Completion and status waits pace themselves on the status stream, and on
3.11 asyncio.wait_for can swallow an outer cancellation when its child
wakes in the same turn. The status event wakes at the broadcast rate, so
an expired caller deadline was consumed there and the query loop spun on
with no deadline left to fire: a 0.3s wait hung until the suite timeout
killed it. asyncio.timeout re-raises instead, which is why _request was
already written against it.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01GMMP6KSdDFv5ZaJXC7ALii
---
 parol6/client/async_client.py | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py
index 2deb928..3ad438b 100644
--- a/parol6/client/async_client.py
+++ b/parol6/client/async_client.py
@@ -1586,11 +1586,11 @@ async def wait_status(
             if remaining <= 0:
                 return False
             try:
-                await asyncio.wait_for(
-                    self._status_event.wait(),
-                    timeout=min(remaining, 0.5),
-                )
-            except asyncio.TimeoutError:
+                # asyncio.timeout, not wait_for: on 3.11 wait_for can swallow an
+                # outer cancellation when its child wakes in the same turn.
+                async with asyncio.timeout(min(remaining, 0.5)):
+                    await self._status_event.wait()
+            except (asyncio.TimeoutError, TimeoutError):
                 continue
 
             if self._closed:
@@ -1689,8 +1689,12 @@ async def _await_completion_hint(self, command_index: int, timeout: float) -> No
             if remaining <= 0:
                 return
             try:
-                await asyncio.wait_for(self._status_event.wait(), remaining)
-            except asyncio.TimeoutError:
+                # asyncio.timeout, not wait_for: on 3.11 wait_for can swallow an
+                # outer cancellation when its child wakes in the same turn, and
+                # the caller's deadline rides on that cancellation.
+                async with asyncio.timeout(remaining):
+                    await self._status_event.wait()
+            except (asyncio.TimeoutError, TimeoutError):
                 return
 
     # --------------- Move commands (queued, pre-computed trajectory) ---------------