Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .github/workflows/build-ubuntu20.yml
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,11 @@ on:
ref:
description: Git ref to build
required: true
default: v0.1.0
default: v1.0.0
version:
description: Release version without the v prefix
required: true
default: 0.1.0
default: 1.0.0

permissions:
contents: read
Expand Down
5 changes: 3 additions & 2 deletions packages/gripper_action_controller/pyproject.toml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[project]
name = "forge-gripper-action-controller"
version = "0.1.0"
version = "1.0.0"
description = "Dora GripperCommand controller for Forge robotics dataflows."
readme = "README.md"
requires-python = ">=3.12"
Expand All @@ -10,7 +10,8 @@ authors = [
]
dependencies = [
"dora-rs==0.4.1",
"forge-msgs==1.0.1",
"forge-msgs==1.3.1",
"pillow>=13.0.0,<14",
"pyyaml>=6.0",
]

Expand Down
14 changes: 14 additions & 0 deletions packages/joint_trajectory_controller/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
5 changes: 3 additions & 2 deletions packages/joint_trajectory_controller/pyproject.toml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[project]
name = "forge-joint-trajectory-controller"
version = "0.1.0"
version = "1.0.0"
description = "Dora FollowJointTrajectory controller for Forge robotics dataflows."
readme = "README.md"
requires-python = ">=3.12"
Expand All @@ -10,7 +10,8 @@ authors = [
]
dependencies = [
"dora-rs==0.4.1",
"forge-msgs==1.0.1",
"forge-msgs==1.3.1",
"pillow>=13.0.0,<14",
"pyyaml>=6.0",
]

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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):
Expand All @@ -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:
Expand All @@ -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",
Expand All @@ -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(
Expand Down
Original file line number Diff line number Diff line change
@@ -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

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -176,6 +178,7 @@ def accept_goal(
)
self.state = "ACTIVE"
self._hold_positions = None
self._history.clear()
return None

def reject_invalid_goal(
Expand Down Expand Up @@ -243,17 +246,44 @@ 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
# 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(
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,
Expand Down Expand Up @@ -296,6 +326,40 @@ 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]
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)
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(
Expand Down
81 changes: 80 additions & 1 deletion packages/joint_trajectory_controller/tests/test_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -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]


Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -271,3 +275,78 @@ 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

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)
assert output is not None and output.result is 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


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])
2 changes: 1 addition & 1 deletion pyproject.toml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[project]
name = "forge-controls"
version = "0.1.0"
version = "1.0.0"
description = "Reusable motion and control capabilities for Forge robotics dataflows."
readme = "README.md"
requires-python = ">=3.12"
Expand Down
Loading
Loading