Skip to content

fix(jtc): abort only on hard deviation or stall, not on plant tracking lag - #4

Merged
mrgh97 merged 2 commits into
PhyAgentOS:mainfrom
mrgh97:fix/jtc-decouple-plant-capability
Nov 18, 2026
Merged

mrgh97 merged 2 commits into
PhyAgentOS:mainfrom
mrgh97:fix/jtc-decouple-plant-capability

Conversation

@mrgh97

@mrgh97 mrgh97 commented Nov 13, 2026

Copy link
Copy Markdown
Contributor

Problem

joint_trajectory_controller compared the planned and the measured joint positions on every tick and aborted as soon as one joint exceeded path_position_tolerance (default 0.15 rad):

position_error = desired - actual
if any(abs(error) > tolerance for error, tolerance in zip(position_error, path_position_tolerance)):
    return self._finish_active(error_code="PATH_TOLERANCE_VIOLATED", ...)

That threshold silently encodes the plant's tracking capability, so motion planning ends up coupled to the specific plant:

  • a motion that is reachable for the target still fails mid-motion when the plant lags (large simultaneous moves, contact, MuJoCo actuator gains vs. a real arm's servo bandwidth);
  • the same bundle/profile cannot serve MuJoCo and a real arm without retuning the controller;
  • the abort message does not even say which joint failed.

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:

Rule Before After
Per-tick tracking error abort at 0.15 rad not judged (field kept for YAML compatibility, documented deprecated/unused)
Hard deviation — abort when any joint deviates more than path_abort_tolerance (default 0.5 rad), message names the joint with planned/measured values
Stall — abort when the reference moved ≥ stuck_reference_epsilon (0.05 rad) inside stuck_window_ms (1000 ms) while the measured position moved < stuck_actual_epsilon (0.02 rad)
Terminal check SETTLING → goal position/velocity/time tolerance unchanged

New config keys (controller.yaml, all optional):

path_abort_tolerance: 0.5      # hard deviation abort (rad)
stuck_window_ms: 1000          # stall window
stuck_reference_epsilon: 0.05  # reference progress required in the window (rad)
stuck_actual_epsilon: 0.02     # measured progress below which the joint is blocked (rad)

Legacy YAML that declares a soft band above 0.5 keeps working: the hard band defaults to max(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:

  • replaced the former path-tolerance abort test with test_moderate_tracking_lag_does_not_abort (0.25 rad lag keeps running);
  • added test_hard_deviation_aborts_and_reports_the_joint (message contains joint name + values);
  • added test_stalled_plant_aborts_after_the_window (frozen plant aborts with no measurable progress);
  • existing feedback, cancel, settling, goal-tolerance and node tests unchanged.

Compatibility

  • error codes unchanged (PATH_TOLERANCE_VIOLATED is reused for both abort reasons, GOAL_TOLERANCE_VIOLATED for the terminal check), so motion_server mapping is unaffected;
  • no wire/schema change;
  • existing controller.yaml files keep loading.

…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.
@mrgh97
mrgh97 merged commit a46583a into PhyAgentOS:main Nov 18, 2026
3 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant