diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index d00dc20..b6c2f79 100644 --- a/parol6/client/async_client.py +++ b/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) diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 8e3d058..b92094e 100644 --- a/parol6/client/sync_client.py +++ b/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, diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 204f90f..2acaa8f 100644 --- a/parol6/protocol/wire.py +++ b/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( diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 293f771..b2cc8c4 100644 --- a/parol6/server/controller.py +++ b/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, diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 25e687a..da11390 100644 --- a/parol6/server/motion_planner.py +++ b/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)) diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 080e2a2..2cbdd16 100644 --- a/parol6/server/segment_player.py +++ b/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,11 +350,29 @@ 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 @@ -348,6 +380,7 @@ def cancel(self, state: ControllerState) -> None: 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) diff --git a/tests/unit/test_motion_pipeline.py b/tests/unit/test_motion_pipeline.py index 3d8f77e..347827e 100644 --- a/tests/unit/test_motion_pipeline.py +++ b/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")