Fix SystemCommand Command_out getting clobbered same-tick by exec fallback - #30
Open
grahas wants to merge 1 commit into
Open
Fix SystemCommand Command_out getting clobbered same-tick by exec fallback#30grahas wants to merge 1 commit into
grahas wants to merge 1 commit into
Conversation
…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.
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
After an ESTOP latches
PAROL6.disabledat the firmware level,RESET(and anySystemCommand) silently fails to re-enable the robot --HOME/JOG/MOVEall get accepted, planned, and streamed with correct, smoothly-changing setpoints, and the server reports success throughout, but the arm never actually moves. No error is raised anywhere in the stack.Root cause
Each 100Hz control-loop tick runs
_poll_commands()(dispatchesSystemCommands like RESET) before_execute_commands()._execute_commands()'s "nothing active" fallback unconditionally setsstate.Command_out = CommandCode.IDLE-- which clobbersResetCommand.execute_step()'sstate.Command_out = CommandCode.ENABLEbefore_write_to_firmware()ever observes it, on every single RESET call (RESET never queues an active segment, so it always hits this fallback branch).ENABLE(101)therefore never reaches the firmware, andPAROL6.disabledstays latched forever at the firmware'sif (PAROL6.disabled == 0) {...}gate -- while every higher layer (planner, segment player, server) believes the command succeeded.Found via live per-tick instrumentation of the pipeline (
_handle_motion_command->planner.submit-> subprocess ->segment_queue->SegmentPlayer.tick/_activate_next->_write_to_firmware) rather than static reading, which had repeatedly given false confidence at every layer.Fix
ControllerStategainscommand_out_locked: bool = False:_poll_commands()resets it toFalseat the top of every tick._handle_system_command()sets itTrueaftercommand.tick(state)ifstate.Command_out != CommandCode.IDLE._execute_commands()'s fallback checks the lock first -- if set, it consumes it (= False) instead of stompingCommand_outback toIDLE.Testing
home()verified on real hardware: robot genuinely drives to standby, confirmed both via continuousstatus().anglespolling during the move and by physically observing it.python3 -m pytest -q -m "unit or integration"(90 tests).Open item
Whether the same same-tick clobbering affects STOP/ESTOP's own
Command_outassignments wasn't independently verified -- their handling additionally calls_segment_player.cancel()/_executor.cancel_active_command(), which may sidestep the issue differently, but I haven't confirmed either way. Flagging for review/follow-up rather than asserting it's fine.