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 comment was written by Claude on behalf of @Jepson2k. Thanks for the diagnosis — the symptom is right: a client streaming at the server's own 100 Hz tick drains the planner queue every packet, so HOME never runs. But the fix inverts a deliberate design decision. Streaming commands preempt planned motion because they are human takeover inputs — Waldo Commander's jog pads and TCP drag gizmo exist so an operator can pull the robot away from a bad planned move — and this PR silently drops those inputs whenever planned work is active. In this API, servoing is a mode, not ambient state: 100 Hz streaming is expected during a servo move, not as the channel homing and planned moves also flow through. ROS 2 handles the same hand-off on the client side — MoveIt Servo, for example, exposes a On the force flag: #34 adds |
2 participants
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