From c9009a9a386c6a18be13befc7e801160dd912ca6 Mon Sep 17 00:00:00 2001 From: mrgh97 Date: Wed, 18 Nov 2026 17:04:52 +0800 Subject: [PATCH] fix(jtc): wait for the full stall observation window --- .github/workflows/build-ubuntu20.yml | 4 ++-- packages/gripper_action_controller/pyproject.toml | 2 +- .../joint_trajectory_controller/pyproject.toml | 2 +- .../controller.py | 8 ++++++-- .../tests/test_controller.py | 15 +++++++-------- pyproject.toml | 2 +- uv.lock | 6 +++--- 7 files changed, 21 insertions(+), 18 deletions(-) diff --git a/.github/workflows/build-ubuntu20.yml b/.github/workflows/build-ubuntu20.yml index ea1930d..2298d6c 100644 --- a/.github/workflows/build-ubuntu20.yml +++ b/.github/workflows/build-ubuntu20.yml @@ -8,11 +8,11 @@ on: ref: description: Git ref to build required: true - default: v2.0.0 + default: v2.0.1 version: description: Release version without the v prefix required: true - default: 2.0.0 + default: 2.0.1 permissions: contents: read diff --git a/packages/gripper_action_controller/pyproject.toml b/packages/gripper_action_controller/pyproject.toml index 18a5928..41e289b 100644 --- a/packages/gripper_action_controller/pyproject.toml +++ b/packages/gripper_action_controller/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "forge-gripper-action-controller" -version = "2.0.0" +version = "2.0.1" description = "Dora GripperCommand controller for Forge robotics dataflows." readme = "README.md" requires-python = ">=3.12" diff --git a/packages/joint_trajectory_controller/pyproject.toml b/packages/joint_trajectory_controller/pyproject.toml index c8bbbd1..f7b2330 100644 --- a/packages/joint_trajectory_controller/pyproject.toml +++ b/packages/joint_trajectory_controller/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "forge-joint-trajectory-controller" -version = "2.0.0" +version = "2.0.1" description = "Dora FollowJointTrajectory controller for Forge robotics dataflows." readme = "README.md" requires-python = ">=3.12" 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 1d16ab6..ad2d1ab 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 @@ -251,7 +251,8 @@ def tick(self, now_ns: int) -> ControllerOutput | None: # 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: + # Keep the newest sample at or before the start of the window. + while len(self._history) > 1 and self._history[1][0] <= window_start_ns: self._history.popleft() worst_index = max( @@ -337,7 +338,10 @@ def _stalled( 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)) + observed_ns = now_ns - oldest_ns + if observed_ns < self.config.stuck_window_ns: + return False + window_elapsed_ns = max(0, elapsed_ns - observed_ns) reference_old = sample_trajectory(active.trajectory, window_elapsed_ns) reference_moved = max( abs(current - previous) diff --git a/packages/joint_trajectory_controller/tests/test_controller.py b/packages/joint_trajectory_controller/tests/test_controller.py index b4db47b..5fb6b65 100644 --- a/packages/joint_trajectory_controller/tests/test_controller.py +++ b/packages/joint_trajectory_controller/tests/test_controller.py @@ -315,16 +315,15 @@ def test_stalled_plant_aborts_after_the_window() -> None: 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) + for elapsed_ns in (0, 50_000_000, 100_000_000, 199_000_000): + controller.update_joint_state(_state(), elapsed_ns) output = controller.tick(elapsed_ns) - if output is not None and output.result is not None: - result = output - break + assert output is not None and output.result is None - assert result is not None, "a frozen plant must abort instead of finishing silently" - assert result.result is not None + # The first tick after the window has elapsed is slightly late. + controller.update_joint_state(_state(), 201_000_000) + result = controller.tick(201_000_000) + assert result is not None and result.result is not None assert result.result.error_code == "PATH_TOLERANCE_VIOLATED" assert "no measurable progress" in result.result.message diff --git a/pyproject.toml b/pyproject.toml index 0737050..632d644 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "forge-controls" -version = "2.0.0" +version = "2.0.1" description = "Reusable motion and control capabilities for Forge robotics dataflows." readme = "README.md" requires-python = ">=3.12" diff --git a/uv.lock b/uv.lock index 1a71ad0..6fc18aa 100644 --- a/uv.lock +++ b/uv.lock @@ -61,7 +61,7 @@ wheels = [ [[package]] name = "forge-controls" -version = "2.0.0" +version = "2.0.1" source = { virtual = "." } dependencies = [ { name = "forge-gripper-action-controller" }, @@ -92,7 +92,7 @@ dev = [ [[package]] name = "forge-gripper-action-controller" -version = "2.0.0" +version = "2.0.1" source = { editable = "packages/gripper_action_controller" } dependencies = [ { name = "dora-rs" }, @@ -109,7 +109,7 @@ requires-dist = [ [[package]] name = "forge-joint-trajectory-controller" -version = "2.0.0" +version = "2.0.1" source = { editable = "packages/joint_trajectory_controller" } dependencies = [ { name = "dora-rs" },