Repository navigation
fix(jtc): abort only on hard deviation or stall, not on plant tracking lag - #4
Merged
mrgh97 merged 2 commits intoNov 18, 2026
Merged
Conversation
…g lag The controller compared the planned and measured joint positions on every tick and aborted as soon as one joint exceeded `path_position_tolerance` (0.15 rad). That threshold silently encodes the plant's tracking capability, so a motion planned without knowledge of the specific plant (MuJoCo model vs. a real arm with different servo bandwidth) fails mid-motion even when the target is reachable, and any change of plant requires retuning the controller. Plant capability is not the controller's contract. This change keeps the controller as the trajectory player and moves capability decisions out of it: * the per-tick `path_position_tolerance` abort is removed; the field is kept for YAML compatibility and documented as deprecated/unused; * a hard abort remains for a real tracking failure: any joint deviating more than `path_abort_tolerance` (default 0.5 rad) aborts the goal, and the message now names the offending joint with planned and measured values (observability the previous message lacked); * a stall detector aborts when the reference moved at least `stuck_reference_epsilon` (0.05 rad) within `stuck_window_ms` (1000 ms) while the measured position moved less than `stuck_actual_epsilon` (0.02 rad), which catches a blocked joint instead of letting it push for the whole trajectory; * the terminal goal judgement (SETTLING, goal_position/goal_velocity/ goal_time tolerance) is unchanged, so "did it arrive?" is still verified. Legacy YAML that declares a soft band above 0.5 rad keeps working: the hard band defaults to max(0.5, path_position_tolerance). Tests: the former path-tolerance abort test is replaced by one asserting that a small lag no longer aborts, plus new cases for the hard deviation (message names the joint) and the stall detector. 23 tests pass for the package.
Aborting is only half of a failure story: the caller needs to know the arm is not left drifting. The controller already keeps commanding the position measured at the abort on every later tick (state `HOLDING`, no decel ramp, no retry), but that contract was undocumented and untested. * add `test_abort_keeps_commanding_the_measured_position`, which aborts a goal and then asserts ten consecutive ticks re-emit the same position-mode command; * document the abort/hold behavior in the package README.
This was referenced Nov 18, 2026
Merged
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
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.
Problem
joint_trajectory_controllercompared the planned and the measured joint positions on every tick and aborted as soon as one joint exceededpath_position_tolerance(default0.15 rad):That threshold silently encodes the plant's tracking capability, so motion planning ends up coupled to the specific plant:
Change
The controller keeps doing what it is responsible for — interpolation, command emission, feedback, and the terminal judgement — and stops judging plant capability in flight:
0.15 radpath_abort_tolerance(default0.5 rad), message names the joint with planned/measured valuesstuck_reference_epsilon(0.05 rad) insidestuck_window_ms(1000 ms) while the measured position moved <stuck_actual_epsilon(0.02 rad)SETTLING→ goal position/velocity/time toleranceNew config keys (
controller.yaml, all optional):Legacy YAML that declares a soft band above
0.5keeps working: the hard band defaults tomax(0.5, path_position_tolerance).Why this keeps the safety story
A blocked or overloaded joint is still detected, but on a capability-independent criterion (it does not move while the reference does), and a gross tracking failure still aborts. What is removed is the tuning of a threshold to one particular plant.
Tests
packages/joint_trajectory_controller/tests— 23 passed:test_moderate_tracking_lag_does_not_abort(0.25 rad lag keeps running);test_hard_deviation_aborts_and_reports_the_joint(message contains joint name + values);test_stalled_plant_aborts_after_the_window(frozen plant aborts withno measurable progress);Compatibility
PATH_TOLERANCE_VIOLATEDis reused for both abort reasons,GOAL_TOLERANCE_VIOLATEDfor the terminal check), somotion_servermapping is unaffected;controller.yamlfiles keep loading.