Fix streaming commands cancelling planned commands (e.g. HOME)

parol6/client/async_client.py

@@ -657,7 +657,7 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg:
     # --------------- Motion / Control ---------------
 
     async def home(
-        self, wait: bool = False, timeout: float = 60.0, **wait_kwargs: Any
+        self, wait: bool = False, timeout: float = 60.0, force: bool = False, **wait_kwargs: Any
     ) -> int:
         """Home the robot to its home position.
 
@@ -675,8 +675,10 @@ async def home(
         Args:
             wait: If True, block until motion completes
             timeout: Maximum time to wait in seconds (only used when wait=True)
+            force: If True, always run the real switch-seeking referencing
+                sequence, even if the robot already believes it's homed.
         """
-        index = await self._send(HomeCmd())
+        index = await self._send(HomeCmd(force=force))
         assert isinstance(index, int)
         if wait and index >= 0:
             ok = await self.wait_command(index, timeout=timeout)

parol6/client/sync_client.py

@@ -175,7 +175,7 @@ def port(self) -> int:
 
     # ---------- motion / control ----------
 
-    def home(self, wait: bool = False, timeout: float = 60.0) -> int:
+    def home(self, wait: bool = False, timeout: float = 60.0, force: bool = False) -> int:
         """Home the robot to its home position.
 
         Unhomed, this runs the full referencing sequence (each joint seeks
@@ -187,8 +187,10 @@ def home(self, wait: bool = False, timeout: float = 60.0) -> int:
         Args:
             wait: If True, block until motion completes.
             timeout: Maximum time to wait in seconds (only used when wait=True).
+            force: If True, always run the real switch-seeking referencing
+                sequence, even if the robot already believes it's homed.
         """
-        return _run(self._inner.home(wait=wait, timeout=timeout))
+        return _run(self._inner.home(wait=wait, timeout=timeout, force=force))
 
     def teleport(
         self,

parol6/protocol/wire.py

@@ -500,9 +500,18 @@ def __post_init__(self) -> None:
 class HomeCmd(
     msgspec.Struct, tag=int(CmdType.HOME), array_like=True, frozen=True, gc=False
 ):
-    """HOME: [CmdType.HOME]"""
+    """HOME: [CmdType.HOME, force]
+
+    force: if True, always run the firmware's real switch-seeking
+    referencing sequence, bypassing TrajectoryPlanner.process()'s
+    fast-path substitution (HomeCmd -> MoveJCmd) when Homed_in is
+    already all-true. The firmware's own HOME opcode (PAROL6.command
+    == 100) always runs the real sequence unconditionally -- the fast
+    path is purely a host-side planner decision, not anything the
+    firmware itself gates.
+    """
 
-    pass
+    force: bool = False
 
 
 class ResetCmd(

parol6/server/controller.py

@@ -680,9 +680,14 @@ def _handle_motion_command(
             )
             return
 
-        # Streaming commands: cancel segment playback + existing streamable handling
+        # Streaming commands: cancel segment playback + existing streamable handling.
+        # If the segment player has pending or active work (e.g. a HOME inline
+        # command), drop the streaming command — the bridge will send another
+        # one next tick, and the segment player's work takes priority.
         if getattr(command, "streamable", False):
-            self._segment_player.cancel(state)
+            if self._segment_player.active:
+                return
+            self._segment_player.cancel_playback(state)
             # Unconditional: a jog self-collision sets the viz but no state.error.
             state.clear_collision()
             if self.udp_transport:
@@ -768,6 +773,7 @@ def _handle_motion_command(
                 if not state.Homed_in[i]:
                     homed_snapshot = False
                     break
+        self._segment_player.notify_planned()
         self._planner.submit(
             PlanCommand(
                 command_index=cmd_index,

parol6/server/motion_planner.py

@@ -235,8 +235,16 @@ def process(self, params: object, command_index: int = 0) -> list[Segment]:
 
         # Fast-path home: an already-referenced robot returns to the standby
         # pose with a normal planned (collision-checked) joint move instead
-        # of re-running the firmware switch-seek.
-        if isinstance(params, HomeCmd) and bool(self.state.Homed_in[:6].all()):
+        # of re-running the firmware switch-seek. HomeCmd.force skips this
+        # substitution entirely, so the raw HOME opcode always reaches the
+        # firmware -- which runs the real referencing sequence
+        # unconditionally regardless of any homed state (see home_all() in
+        # the firmware source).
+        if (
+            isinstance(params, HomeCmd)
+            and not params.force
+            and bool(self.state.Homed_in[:6].all())
+        ):
             params = MoveJCmd(angles=self._home_deg, speed=self._home_return_speed)
 
         cmd_class = self._registry.get_command_for_struct(type(params))

parol6/server/segment_player.py

@@ -61,6 +61,7 @@ class SegmentPlayer:
         "_settle_ticks",
         "_settle_err",
         "_last_shapes_version",
+        "_pending_planned",
     )
 
     def __init__(self, planner: MotionPlanner) -> None:
@@ -74,11 +75,20 @@ def __init__(self, planner: MotionPlanner) -> None:
         self._settle_ticks: int = 0
         self._settle_err: int = -1
         self._last_shapes_version: int = 0
+        self._pending_planned: int = 0
+
+    def notify_planned(self) -> None:
+        """Called when a non-streaming command is submitted to the planner."""
+        self._pending_planned += 1
 
     @property
     def active(self) -> bool:
-        """True if playing a segment or has buffered segments."""
-        return self._active is not None or bool(self._buffer)
+        """True if playing/buffered segments or awaiting planner output."""
+        return (
+            self._active is not None
+            or bool(self._buffer)
+            or self._pending_planned > 0
+        )
 
     def tick(self, state: ControllerState) -> bool:
         """Execute one tick. Returns True if actively playing/executing.
@@ -90,6 +100,8 @@ def tick(self, state: ControllerState) -> bool:
         seg = self._planner.poll_segment()
         while seg is not None:
             self._buffer.append(seg)
+            if self._pending_planned > 0:
+                self._pending_planned -= 1
             state.queued_segments += 1
             if isinstance(seg, TrajectorySegment):
                 state.queued_duration += seg.duration
@@ -189,6 +201,7 @@ def tick(self, state: ControllerState) -> bool:
                 self._active = None
                 # Halt: cancel all remaining planned work
                 self._buffer.clear()
+                self._pending_planned = 0
                 self._planner.cancel()
                 self._drain_planner_queue(state)
                 return False
@@ -293,6 +306,7 @@ def _on_failure(
         state.action_state = ActionState.ERROR
         self._active = None
         self._buffer.clear()
+        self._pending_planned = 0
         self._planner.cancel()
         self._drain_planner_queue(state)
 
@@ -336,18 +350,37 @@ def _world_guard(
             state.action_params = ""
             self._active = None
             self._buffer.clear()
+            self._pending_planned = 0
             self._planner.cancel()
             self._drain_planner_queue(state)
             return False
         return True
 
+    def cancel_playback(self, state: ControllerState) -> None:
+        """Stop active playback without draining the planner queue.
+
+        Used by the streaming command path: a jog/servo takes over from any
+        in-progress trajectory, but planned commands (like HOME) that are
+        still in-flight in the planner subprocess must survive so they can
+        be picked up once streaming stops.
+        """
+        self._active = None
+        self._step = 0
+        self._inline_cmd = None
+        self._inline_activated = False
+        self._settling = False
+        self._buffer.clear()
+        state.queued_segments = 0
+        state.queued_duration = 0.0
+
     def cancel(self, state: ControllerState) -> None:
         """Clear buffer, drain stale segments, and stop playback."""
         self._active = None
         self._step = 0
         self._inline_cmd = None
         self._inline_activated = False
         self._buffer.clear()
+        self._pending_planned = 0
         self._planner.cancel()
         # Drain stale segments from planner output queue
         self._drain_planner_queue(state)

tests/unit/test_motion_pipeline.py

@@ -127,6 +127,21 @@ def test_home_routes_by_referenced_state(self, worker, segment_queue):
         np.testing.assert_allclose(seg.trajectory_steps[-1], home_steps, atol=2)
         np.testing.assert_allclose(worker.state.Position_in, home_steps, atol=2)
 
+    def test_home_force_bypasses_referenced_fastpath(self, worker, segment_queue):
+        """HomeCmd(force=True) always produces an InlineSegment (the real
+        firmware referencing sequence), even when already homed -- unlike
+        the plain HomeCmd() case in test_home_routes_by_referenced_state,
+        which fast-paths to a TrajectorySegment once referenced."""
+        home_steps = _home_steps()
+
+        worker.state.Position_in[:] = _deg_to_steps(W1)
+        worker.process_command(
+            PlanCommand(command_index=0, params=HomeCmd(force=True), homed=True)
+        )
+        seg = segment_queue.get(timeout=1.0)
+        assert isinstance(seg, InlineSegment)
+        np.testing.assert_array_equal(worker.state.Position_in, home_steps)
+
     def test_checkpoint_produces_inline_segment(self, worker, segment_queue):
         """Checkpoint should produce an InlineSegment."""
         params = CheckpointCmd(label="step1")