diff --git a/packages/joint_trajectory_controller/README.md b/packages/joint_trajectory_controller/README.md index a764e54..d7af883 100644 --- a/packages/joint_trajectory_controller/README.md +++ b/packages/joint_trajectory_controller/README.md @@ -31,8 +31,22 @@ Outputs: - `trajectory_result`: `forge_msgs.FollowJointTrajectoryResult`, metadata `goal_id` and lowercase `goal_status` +## Abort and hold behavior + +The controller plays the trajectory reference back; it does not judge whether the +plant keeps up with it (see `path_abort_tolerance` / `stuck_*` for the two coarse +abort conditions). After an abort — or a cancel, or a feedback timeout — the +controller enters `HOLDING` and keeps commanding **the position measured at that +moment** on every subsequent tick: + +* no deceleration ramp is generated; +* no automatic retry or replanning is performed; +* the caller decides what to do next (a new goal starts from the held pose). + ## Version 0.1 behavior + + - exact configured trajectory joint order; partial trajectories are rejected; - fresh complete feedback is required, but incoming `JointState` may include extra joints and use a different order; diff --git a/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/config.py b/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/config.py index 14c25ad..625fcc8 100644 --- a/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/config.py +++ b/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/config.py @@ -16,7 +16,17 @@ class ControllerConfig: control_period_ns: int = 20_000_000 feedback_timeout_ns: int = 200_000_000 start_position_tolerance: float = 0.1 + # Deprecated: accepted for YAML compatibility only. Per-tick planned-vs-measured + # deviation is no longer judged against the plant's tracking capability; the + # controller only aborts on a hard deviation or on a stalled plant (below). path_position_tolerance: float = 0.15 + # Hard abort: a deviation above this is a real tracking failure. + path_abort_tolerance: float = 0.5 + # Stall detection: the reference moved >= stuck_reference_epsilon within + # stuck_window_ns while the plant moved < stuck_actual_epsilon. + stuck_window_ns: int = 1_000_000_000 + stuck_reference_epsilon: float = 0.05 + stuck_actual_epsilon: float = 0.02 goal_position_tolerance: float = 0.03 goal_velocity_tolerance: float = 0.05 goal_time_tolerance_ns: int = 500_000_000 @@ -35,6 +45,7 @@ def __post_init__(self) -> None: "control_period_ns", "feedback_timeout_ns", "goal_time_tolerance_ns", + "stuck_window_ns", ): value = getattr(self, name) if isinstance(value, bool) or not isinstance(value, int): @@ -52,12 +63,19 @@ def __post_init__(self) -> None: for name in ( "start_position_tolerance", "path_position_tolerance", + "path_abort_tolerance", + "stuck_reference_epsilon", + "stuck_actual_epsilon", "goal_position_tolerance", "goal_velocity_tolerance", ): value = getattr(self, name) if not math.isfinite(value) or value < 0.0: raise ValueError(f"{name} must be finite and non-negative") + if self.path_abort_tolerance < self.path_position_tolerance: + raise ValueError( + "path_abort_tolerance must be greater than or equal to path_position_tolerance" + ) def load_config(path: str | Path) -> ControllerConfig: @@ -77,6 +95,10 @@ def controller_config_from_mapping(data: dict[str, Any]) -> ControllerConfig: "feedback_timeout_ms", "start_position_tolerance", "path_position_tolerance", + "path_abort_tolerance", + "stuck_window_ms", + "stuck_reference_epsilon", + "stuck_actual_epsilon", "goal_position_tolerance", "goal_velocity_tolerance", "goal_time_tolerance_ms", @@ -98,7 +120,16 @@ def controller_config_from_mapping(data: dict[str, Any]) -> ControllerConfig: data.get("feedback_timeout_ms", 200), "feedback_timeout_ms" ), start_position_tolerance=float(data.get("start_position_tolerance", 0.1)), - path_position_tolerance=float(data.get("path_position_tolerance", 0.15)), + path_position_tolerance=( + _soft := float(data.get("path_position_tolerance", 0.15)) + ), + # Legacy YAML may declare a soft band above 0.5 rad; keep the hard band above it. + path_abort_tolerance=float(data.get("path_abort_tolerance", max(0.5, _soft))), + stuck_window_ns=_milliseconds_to_ns( + data.get("stuck_window_ms", 1000), "stuck_window_ms" + ), + stuck_reference_epsilon=float(data.get("stuck_reference_epsilon", 0.05)), + stuck_actual_epsilon=float(data.get("stuck_actual_epsilon", 0.02)), goal_position_tolerance=float(data.get("goal_position_tolerance", 0.03)), goal_velocity_tolerance=float(data.get("goal_velocity_tolerance", 0.05)), goal_time_tolerance_ns=_milliseconds_to_ns( diff --git a/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/controller.py b/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/controller.py index 725e705..1d16ab6 100644 --- a/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/controller.py +++ b/packages/joint_trajectory_controller/src/forge_joint_trajectory_controller/controller.py @@ -1,7 +1,7 @@ from __future__ import annotations import math -from collections import OrderedDict +from collections import OrderedDict, deque from dataclasses import dataclass from typing import Literal @@ -64,6 +64,8 @@ def __init__(self, config: ControllerConfig) -> None: self.state: ControllerState = "IDLE" self._actual: _ActualState | None = None self._active: _ActiveGoal | None = None + # Rolling (timestamp, measured positions) history used for stall detection. + self._history: deque[tuple[int, tuple[float, ...]]] = deque() self._hold_positions: tuple[float, ...] | None = None self._result_cache: OrderedDict[str, ControllerOutput] = OrderedDict() self._last_now_ns: int | None = None @@ -176,6 +178,7 @@ def accept_goal( ) self.state = "ACTIVE" self._hold_positions = None + self._history.clear() return None def reject_invalid_goal( @@ -243,17 +246,43 @@ def tick(self, now_ns: int) -> ControllerOutput | None: ) if elapsed_ns < active.trajectory.duration_ns: - if any( - abs(error) > tolerance - for error, tolerance in zip( - position_error, - active.path_position_tolerance, - strict=True, + # Plant capability is deliberately NOT judged per tick: a small tracking + # lag is normal and depends on velocity/servo tuning. Only two coarse + # conditions abort the motion: a hard deviation, or a stalled plant. + self._history.append((now_ns, tuple(self._actual.positions))) + window_start_ns = now_ns - self.config.stuck_window_ns + while self._history and self._history[0][0] < window_start_ns: + self._history.popleft() + + worst_index = max( + range(len(position_error)), + key=lambda index: abs(position_error[index]), + ) + worst_error = abs(position_error[worst_index]) + if worst_error > self.config.path_abort_tolerance: + joint = self.config.joint_names[worst_index] + return self._finish_active( + error_code="PATH_TOLERANCE_VIOLATED", + message=( + f"deviation {worst_error:.3f} rad on {joint} exceeds abort tolerance " + f"{self.config.path_abort_tolerance:.3f} rad " + f"(planned {sample.positions[worst_index]:.3f}, " + f"actual {self._actual.positions[worst_index]:.3f})" + ), + goal_status="aborted", + now_ns=now_ns, + desired=sample, + hold_positions=self._actual.positions, ) - ): + + if self._stalled(active, sample, now_ns=now_ns, elapsed_ns=elapsed_ns): return self._finish_active( error_code="PATH_TOLERANCE_VIOLATED", - message="trajectory path position tolerance was violated", + message=( + "no measurable progress for " + f"{self.config.stuck_window_ns / 1_000_000_000:.2f} s while the reference " + "kept moving; the joint is blocked or cannot follow" + ), goal_status="aborted", now_ns=now_ns, desired=sample, @@ -296,6 +325,37 @@ def tick(self, now_ns: int) -> ControllerOutput | None: feedback=feedback, ) + def _stalled( + self, + active: _ActiveGoal, + sample: TrajectorySample, + *, + now_ns: int, + elapsed_ns: int, + ) -> bool: + """True when the reference keeps moving but the plant does not move at all.""" + if len(self._history) < 2 or self._actual is None: + return False + oldest_ns, oldest_positions = self._history[0] + window_elapsed_ns = max(0, elapsed_ns - (now_ns - oldest_ns)) + reference_old = sample_trajectory(active.trajectory, window_elapsed_ns) + reference_moved = max( + abs(current - previous) + for current, previous in zip( + sample.positions, reference_old.positions, strict=True + ) + ) + actual_moved = max( + abs(current - previous) + for current, previous in zip( + self._actual.positions, oldest_positions, strict=True + ) + ) + return ( + reference_moved >= self.config.stuck_reference_epsilon + and actual_moved < self.config.stuck_actual_epsilon + ) + def _goal_reached(self, active: _ActiveGoal, sample: TrajectorySample) -> bool: assert self._actual is not None position_ok = all( diff --git a/packages/joint_trajectory_controller/tests/test_controller.py b/packages/joint_trajectory_controller/tests/test_controller.py index 51932fd..b4db47b 100644 --- a/packages/joint_trajectory_controller/tests/test_controller.py +++ b/packages/joint_trajectory_controller/tests/test_controller.py @@ -23,6 +23,8 @@ def _config(**overrides: object) -> ControllerConfig: "goal_time_tolerance_ns": 50, } values.update(overrides) + if "path_abort_tolerance" not in values: + values["path_abort_tolerance"] = max(0.5, float(values["path_position_tolerance"])) return ControllerConfig(**values) # type: ignore[arg-type] @@ -139,7 +141,9 @@ def test_tick_emits_full_position_command_and_feedback() -> None: def test_path_violation_aborts_and_holds_actual() -> None: - controller = JointTrajectoryController(_config(path_position_tolerance=0.01)) + controller = JointTrajectoryController( + _config(path_position_tolerance=0.01, path_abort_tolerance=0.01) + ) controller.update_joint_state(_state(), 0) controller.accept_goal("goal", _goal(), 0) @@ -271,3 +275,79 @@ def test_goal_velocity_tolerance_compares_desired_and_actual() -> None: assert output is not None and output.result is not None assert output.result.error_code == "SUCCESS" + + +def test_moderate_tracking_lag_does_not_abort() -> None: + """A lag inside the hard band is normal; plant capability is not judged per tick.""" + controller = JointTrajectoryController(_config(path_abort_tolerance=0.5)) + controller.update_joint_state(_state(), 0) + assert controller.accept_goal("goal", _goal(), 0) is None + + controller.update_joint_state(_state(positions=(0.0, 0.0)), 25) + output = controller.tick(25) + + assert output is not None + assert output.result is None + assert output.command is not None + + +def test_hard_deviation_aborts_and_reports_the_joint() -> None: + controller = JointTrajectoryController(_config(path_abort_tolerance=0.5)) + controller.update_joint_state(_state(), 0) + assert controller.accept_goal( + "goal", _goal(target=(0.2, -2.0), duration_ns=1_000_000_000), 0 + ) is None + + controller.update_joint_state(_state(positions=(0.0, 0.0)), 500_000_000) + output = controller.tick(500_000_000) + + assert output is not None and output.result is not None + assert output.result.error_code == "PATH_TOLERANCE_VIOLATED" + assert "j2" in output.result.message + assert "exceeds abort tolerance" in output.result.message + + +def test_stalled_plant_aborts_after_the_window() -> None: + """Reference keeps moving while the plant never moves -> stall abort.""" + controller = JointTrajectoryController( + _config(path_abort_tolerance=5.0, stuck_window_ns=200_000_000) + ) + controller.update_joint_state(_state(), 0) + assert controller.accept_goal("goal", _goal(duration_ns=2_000_000_000), 0) is None + + result = None + for elapsed_ns in range(0, 1_000_000_000, 50_000_000): + controller.update_joint_state(_state(positions=(0.0, 0.0)), elapsed_ns) + output = controller.tick(elapsed_ns) + if output is not None and output.result is not None: + result = output + break + + assert result is not None, "a frozen plant must abort instead of finishing silently" + assert result.result is not None + assert result.result.error_code == "PATH_TOLERANCE_VIOLATED" + assert "no measurable progress" in result.result.message + + +def test_abort_keeps_commanding_the_measured_position() -> None: + """Recovery is deliberately trivial: hold the measured pose, no decel ramp, no retry.""" + controller = JointTrajectoryController( + _config(path_position_tolerance=0.01, path_abort_tolerance=0.01) + ) + controller.update_joint_state(_state(positions=(0.3, -0.4)), 0) + controller.accept_goal("goal", _goal(first=(0.3, -0.4)), 0) + + aborted = controller.tick(25) + assert aborted is not None and aborted.result is not None + assert aborted.result.error_code == "PATH_TOLERANCE_VIOLATED" + assert controller.state == "HOLDING" + + # Every later tick keeps commanding the position measured at the abort. + for elapsed_ns in range(30, 200, 10): + controller.update_joint_state(_state(positions=(0.3, -0.4)), elapsed_ns) + hold = controller.tick(elapsed_ns) + assert hold is not None, "the controller must keep holding after an abort" + assert hold.result is None + assert hold.command is not None + assert hold.command.mode == "position" + assert hold.command.position == pytest.approx([0.3, -0.4])