Fix streaming commands cancelling planned commands (e.g. HOME) - #33
Open
grahas wants to merge 4 commits into
Open
Fix streaming commands cancelling planned commands (e.g. HOME)#33grahas wants to merge 4 commits into
grahas wants to merge 4 commits into
Conversation
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.
…lback 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.
Brings in upstream changes since 829c2c7, including: - Our merged PR PCrnjak#30 (command_out_locked fix) - Settle-on-progress fix for trajectory settling - waldoctl v0.8.0 and v0.9.0 bumps - ty dispatcher fix, stale ignore cleanup Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
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 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
segment_player.cancel()which drained the planner queue — discarding planned commands like HOME'sInlineSegmentbefore the segment player could execute themcancel_playback()that stops active trajectory playback without draining the planner queue, and drops streaming commands entirely when the segment player has pending or active planned work_pending_plannedcounter to bridge the gap between planner submission and segment arrival, so the segment player knows work is in-flight even before the segment landsTest plan
home(force=True)now completes physically with parol6_bridge running at 100Hz🤖 Generated with Claude Code