From d3761f153bb9f002b4ec3b8a4aa001b4db663bee Mon Sep 17 00:00:00 2001 From: gupyfish Date: Sat, 8 Aug 2026 02:49:22 -0600 Subject: [PATCH 1/3] Add home(force=True) to always run the real referencing sequence The fast-path (HomeCmd -> MoveJCmd substitution in TrajectoryPlanner. process(), based on Homed_in) exists so an already-referenced robot returns to standby via a normal planned move instead of re-running the firmware switch-seek. But there was no way to explicitly request the real sequence when needed -- e.g. after a fall/collision where the robot's believed position no longer matches physical reality, homed_in being (correctly) still true meant home() would only ever plan a move to a stale/wrong target instead of re-referencing. The firmware's own HOME opcode (PAROL6.command == 100) already runs home_all() unconditionally regardless of any homed state -- the fast path is purely a host-side planner decision, not something the firmware itself gates. force=True on HomeCmd skips the substitution, so the raw command always reaches the firmware. Threaded through HomeCmd's wire struct, both client home() methods, and the planner's fast-path check. Verified: the new unit test fails against the pre-fix planner logic (confirmed by temporarily reverting motion_planner.py) and passes with the fix; full unit suite (195 tests) and the existing home-fastpath integration test still pass unchanged. --- parol6/client/async_client.py | 6 ++++-- parol6/client/sync_client.py | 6 ++++-- parol6/protocol/wire.py | 13 +++++++++++-- parol6/server/motion_planner.py | 12 ++++++++++-- tests/unit/test_motion_pipeline.py | 15 +++++++++++++++ 5 files changed, 44 insertions(+), 8 deletions(-) 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 231d7e0..7b13013 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/motion_planner.py b/parol6/server/motion_planner.py index 9a3a7fe..e9bfff2 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/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") From f4a6ff907af98e6046ff8a17c96139f4ccb75af1 Mon Sep 17 00:00:00 2001 From: Graham Harison Date: Mon, 3 Aug 2026 01:59:10 -0600 Subject: [PATCH 2/3] Fix SystemCommand Command_out getting clobbered same-tick by exec fallback RESET (and any other one-shot SystemCommand that sets state.Command_out, e.g. to CommandCode.ENABLE) had its signal silently overwritten before it ever reached the firmware: _poll_commands() (which dispatches SystemCommands during the "poll_cmd" phase) runs before _execute_commands() (the "exec" phase) in the same control-loop tick, and _execute_commands()'s "nothing active" fallback unconditionally reset state.Command_out = CommandCode.IDLE whenever no segment/streaming command was active -- which is the case right after a plain RESET, since RESET itself doesn't queue any motion. The practical symptom: RESET appeared to succeed (state.enabled is pure Python state, set unconditionally), but PAROL6.disabled on the firmware never actually got cleared, because the ENABLE(101) command code set by ResetCommand.execute_step() never survived to _write_to_firmware(). Once PAROL6.disabled was latched from an earlier ESTOP, every subsequent HOME/JOG/MOVE was silently dropped by the firmware's `if (PAROL6.disabled == 0)` gate -- while the server-side planner/segment-player pipeline computed and "sent" a perfectly valid trajectory the whole time, believing it succeeded. Fixed with a same-tick lock flag (ControllerState.command_out_locked): set whenever a SystemCommand assigns a non-IDLE Command_out during poll_cmd, consumed by _execute_commands()'s fallback instead of blindly resetting to IDLE, and cleared fresh at the top of every _poll_commands() call. Verified against real hardware: home() on an unhomed-but-referenced robot now actually drives the arm to standby (confirmed via continuous status().angles polling during the move, and visually). Full test suite (90 tests, unit + integration) passes unchanged. --- parol6/server/controller.py | 13 +++++++++++++ parol6/server/state.py | 12 ++++++++++++ 2 files changed, 25 insertions(+) diff --git a/parol6/server/controller.py b/parol6/server/controller.py index ab250d4..f9864af 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -385,6 +385,11 @@ def _execute_commands(self, state: ControllerState) -> None: # Streaming command executor (jog/servo) if self._executor.active_command or self._executor.command_queue: self._executor.execute_active_command() + elif state.command_out_locked: + # A SystemCommand (e.g. RESET) set Command_out earlier this same + # tick during poll_cmd -- consume the lock instead of stomping + # it back to IDLE before _write_to_firmware() sees it. + state.command_out_locked = False else: state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) @@ -591,6 +596,7 @@ def _poll_commands(self, state: ControllerState) -> None: """Poll and process UDP commands (non-blocking).""" assert self.udp_transport is not None + state.command_out_locked = False msgs = self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT) for data, addr in msgs: self._process_command(data, addr, state) @@ -802,6 +808,13 @@ def _handle_system_command( command.setup(state) code = command.tick(state) + # This SystemCommand set a real signal (e.g. RESET's ENABLE) for + # firmware to see on this tick's write phase -- don't let + # _execute_commands()'s later "nothing active" fallback stomp it + # back to IDLE before _write_to_firmware() runs. + if state.Command_out != CommandCode.IDLE: + state.command_out_locked = True + # Stop/estop: cancel the motion pipeline, or the segment player # keeps playing the active trajectory (rewriting Command_out and # fresh speeds every tick) and the "stopped" robot drives on. diff --git a/parol6/server/state.py b/parol6/server/state.py index e5a8f03..49a7bd4 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -184,6 +184,18 @@ class ControllerState: # Robot telemetry and command buffers - using ndarray for efficiency Command_out: CommandCode = CommandCode.IDLE # The command code to send to firmware + # True for the remainder of the tick in which a SystemCommand (RESET's + # ENABLE, ESTOP/STOP's IDLE, etc.) explicitly set Command_out to a + # meaningful value during poll_cmd. Without this, _execute_commands()'s + # "nothing active" fallback (which also runs every tick, after poll_cmd) + # unconditionally overwrites Command_out back to IDLE before + # _write_to_firmware() ever sees the SystemCommand's signal -- so e.g. + # RESET's ENABLE(101) never actually reaches the firmware, leaving + # PAROL6.disabled latched from an earlier ESTOP forever. Reset to False + # at the top of every _poll_commands() call; consumed (and cleared) by + # _execute_commands()'s fallback the same tick it's set. + command_out_locked: bool = False + Position_out: np.ndarray = field( default_factory=lambda: np.zeros((6,), dtype=np.int32) ) From 79ff44ed5eebc6c881d5f183c9ec65bbc1dd7430 Mon Sep 17 00:00:00 2001 From: Graham Harison Date: Tue, 1 Sep 2026 02:46:52 -0600 Subject: [PATCH 3/3] Fix streaming commands cancelling planned commands (e.g. HOME) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When parol6_bridge sends streaming ServoJ at 100Hz, each call triggered segment_player.cancel() which drained the planner queue — discarding HOME's InlineSegment before the segment player could execute it. This caused home(force=True) to silently complete without physical movement whenever the bridge was running. Fix: add cancel_playback() that stops active trajectory playback without draining the planner queue, and drop streaming commands entirely when the segment player has pending or active planned work. A pending-planned counter bridges the gap between planner submission and segment arrival. Verified on real hardware: homing now completes physically with the bridge running. Co-Authored-By: Claude Opus 4.6 --- parol6/server/controller.py | 10 +++++++-- parol6/server/segment_player.py | 37 +++++++++++++++++++++++++++++++-- 2 files changed, 43 insertions(+), 4 deletions(-) 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/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)