Report accepted queued commands and clear finished execution indices

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) ---------------

parol6/commands/query_commands.py

@@ -301,7 +301,12 @@ class QueueCommand(QueryCommand[QueueCmd]):
 
     def compute(self, state: "ControllerState") -> Response:
         return 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,

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,26 +951,17 @@ 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:
             # Use per-segment analysis to handle singularities and wrist flips
             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)
 

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.record_completion(ac.command_index)
@@ -320,6 +322,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 +338,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

parol6/server/controller.py

@@ -859,7 +859,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:
@@ -876,6 +876,7 @@ def _handle_motion_command(
                 homed=homed_snapshot,
             )
         )
+        state.pending_planned.append((cmd_index, cmd_name))
         state.plan_submitted_index = cmd_index
         if cmd_type and self._ack_policy.requires_ack(cmd_type):
             self._reply_ok_index(req_id, addr, cmd_index)

parol6/server/motion_planner.py

@@ -735,6 +735,7 @@ def motion_planner_main(
                         ErrorSegment(
                             command_index=msg.command_index,
                             error=robot_error,
+                            generation=msg.generation,
                         )
                     )
                     worker.cancel()

parol6/server/segment_player.py

@@ -370,6 +370,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]
@@ -422,14 +431,20 @@ def _tick_inline(self, seg: InlineSegment, state: ControllerState) -> bool | Non
 
     def _complete_segment(self, seg: Segment, state: ControllerState) -> None:
         """Mark segment as completed and update tracking indices."""
+        final_idx = seg.command_index
         if isinstance(seg, TrajectorySegment):
             for idx in seg.blend_consumed_indices:
                 if idx != seg.command_index:
                     state.record_completion(idx)
+                if idx > final_idx:
+                    final_idx = idx
             state.queued_duration -= seg.duration
         state.queued_segments -= 1
         state.record_completion(seg.command_index)
+        while state.pending_planned and state.pending_planned[0][0] <= final_idx:
+            state.pending_planned.popleft()
         state.action_current = ""
+        state.executing_command_index = -1
         state.action_params = ""
         state.action_state = ActionState.IDLE
         self._active = None
@@ -498,6 +513,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
@@ -514,6 +530,7 @@ 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
         state.plan_received_index = state.plan_submitted_index