From 749860b46ec5d83c2fec693f1f291d3fc2f146ef Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 25 Jun 2026 17:25:33 -0700 Subject: [PATCH] feat(go2): RPP controller + decoupled pub/sub benchmark & scoring --- dimos/control/coordinator.py | 104 +++ .../tasks/feedforward_gain_compensator.py | 93 ++ .../tasks/path_follower_task/__registry__.py | 17 + .../path_follower_task/path_follower_task.py | 651 ++++++++++++++ .../test_path_follower_task.py | 190 +++++ .../rpp_path_follower_task/__registry__.py | 19 + .../artifacts/go2_posedomain.json | 129 +++ .../rpp_path_follower_task.py | 246 ++++++ .../test_rpp_path_follower_task.py | 297 +++++++ dimos/control/tasks/velocity_profiler.py | 158 ++++ dimos/control/tasks/velocity_tracking_pid.py | 171 ++++ dimos/control/test_coordinator_ports.py | 108 +++ dimos/msgs/std_msgs/Float32.py | 30 + .../replanning_a_star/path_distancer.py | 3 +- dimos/robot/all_blueprints.py | 3 + .../basic/unitree_go2_rpp_benchmark.py | 79 ++ .../basic/unitree_go2_rpp_controller.py | 147 ++++ dimos/robot/unitree/keyboard_teleop.py | 96 ++- dimos/utils/benchmarking/benchmark.py | 531 ++++++++++++ dimos/utils/benchmarking/gate.py | 25 + dimos/utils/benchmarking/paths.py | 482 +++++++++++ dimos/utils/benchmarking/plant.py | 515 ++++++++++++ dimos/utils/benchmarking/score.py | 284 +++++++ dimos/utils/benchmarking/scoring.py | 182 ++++ dimos/utils/benchmarking/test_benchmark.py | 274 ++++++ dimos/utils/benchmarking/tuning.py | 794 ++++++++++++++++++ dimos/utils/benchmarking/velocity_profile.py | 141 ++++ 27 files changed, 5745 insertions(+), 24 deletions(-) create mode 100644 dimos/control/tasks/feedforward_gain_compensator.py create mode 100644 dimos/control/tasks/path_follower_task/__registry__.py create mode 100644 dimos/control/tasks/path_follower_task/path_follower_task.py create mode 100644 dimos/control/tasks/path_follower_task/test_path_follower_task.py create mode 100644 dimos/control/tasks/rpp_path_follower_task/__registry__.py create mode 100644 dimos/control/tasks/rpp_path_follower_task/artifacts/go2_posedomain.json create mode 100644 dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py create mode 100644 dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py create mode 100644 dimos/control/tasks/velocity_profiler.py create mode 100644 dimos/control/tasks/velocity_tracking_pid.py create mode 100644 dimos/control/test_coordinator_ports.py create mode 100644 dimos/msgs/std_msgs/Float32.py create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py create mode 100644 dimos/utils/benchmarking/benchmark.py create mode 100644 dimos/utils/benchmarking/gate.py create mode 100644 dimos/utils/benchmarking/paths.py create mode 100644 dimos/utils/benchmarking/plant.py create mode 100644 dimos/utils/benchmarking/score.py create mode 100644 dimos/utils/benchmarking/scoring.py create mode 100644 dimos/utils/benchmarking/test_benchmark.py create mode 100644 dimos/utils/benchmarking/tuning.py create mode 100644 dimos/utils/benchmarking/velocity_profile.py diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 54022f5892..ccf93879d7 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -56,7 +56,9 @@ from dimos.hardware.whole_body.spec import WholeBodyAdapter from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Float32 import Float32 from dimos.teleop.quest.quest_types import ( Buttons, ) @@ -157,6 +159,18 @@ class ControlCoordinator(Module): # Input: Teleop buttons for engage/disengage signaling teleop_buttons: In[Buttons] + # Input: Planned path for tasks exposing ``set_path()``. Typical source is a + # nav-stack planner or a benchmark publishing a reference path. The coord + # forwards the path (with a fresh odom snapshot) to every such task; the + # tasks read their own latest odom from internal state populated each tick. + path: In[Path] + + # Input: Target/cruise speed (m/s) for tasks exposing ``set_speed()``. An + # optional runtime override — by default the follower's configured speed + # governs; a source (e.g. a benchmark sweeping speeds) wires this to retune + # the follower over the transport without RPC. + speed: In[Float32] + # Arming and dry-run are one-shot RPCs, not streams. def __init__(self, *args: Any, **kwargs: Any) -> None: @@ -181,6 +195,8 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._cartesian_command_unsub: Callable[[], None] | None = None self._twist_command_unsub: Callable[[], None] | None = None self._buttons_unsub: Callable[[], None] | None = None + self._path_unsub: Callable[[], None] | None = None + self._speed_unsub: Callable[[], None] | None = None logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz") @@ -526,6 +542,58 @@ def _on_buttons(self, msg: Buttons) -> None: for task in self._tasks.values(): task.on_buttons(msg) + def _read_base_odom(self) -> PoseStamped | None: + """Snapshot the BASE adapter's current odom as a PoseStamped, or None.""" + with self._hardware_lock: + for hw in self._hardware.values(): + if hw.component.hardware_type != HardwareType.BASE: + continue + read_odometry = getattr(hw.adapter, "read_odometry", None) + if not callable(read_odometry): + continue + try: + xyt = read_odometry() + except Exception: + continue + if xyt is None or len(xyt) < 3: + continue + from dimos.msgs.geometry_msgs.Quaternion import Quaternion + from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + return PoseStamped( + ts=time.perf_counter(), + position=Vector3(float(xyt[0]), float(xyt[1]), 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(xyt[2]))), + ) + return None + + def _on_path(self, msg: Path) -> None: + """Forward a planned path + a fresh odom snapshot to any task exposing + ``set_path(path, odom)``. Single-arg ``set_path(path)`` callers still + work (TypeError fallback).""" + odom = self._read_base_odom() + with self._task_lock: + for task in self._tasks.values(): + set_path = getattr(task, "set_path", None) + if set_path is None: + continue + try: + set_path(msg, odom) + except TypeError: + set_path(msg) # backwards compat with single-arg set_path + + def _on_speed(self, msg: Float32) -> None: + """Forward a target/cruise speed to any task exposing ``set_speed()``.""" + value = float(msg.data) + with self._task_lock: + for task in self._tasks.values(): + set_speed = getattr(task, "set_speed", None) + if callable(set_speed): + try: + set_speed(value) + except Exception: + logger.exception(f"set_speed() raised on task {task.name!r}") + @rpc def set_activated(self, engaged: bool) -> None: """Arm/disarm every task exposing ``arm()`` / ``disarm()``.""" @@ -687,6 +755,36 @@ def start(self) -> None: self._buttons_unsub = self.teleop_buttons.subscribe(self._on_buttons) logger.info("Subscribed to buttons for engage/disengage") + # Subscribe to path if any task implements set_path (nav/benchmark-driven). + with self._task_lock: + has_path_task = any( + callable(getattr(task, "set_path", None)) for task in self._tasks.values() + ) + if has_path_task: + try: + self._path_unsub = self.path.subscribe(self._on_path) + logger.info("Subscribed to path for path-stream-capable tasks") + except Exception: + logger.warning( + "Path-stream-capable task configured but could not subscribe to path. " + "Set transport via blueprint." + ) + + # Subscribe to speed if any task implements set_speed (runtime override). + with self._task_lock: + has_speed_task = any( + callable(getattr(task, "set_speed", None)) for task in self._tasks.values() + ) + if has_speed_task: + try: + self._speed_unsub = self.speed.subscribe(self._on_speed) + logger.info("Subscribed to speed for speed-capable tasks") + except Exception: + logger.warning( + "Speed-capable task configured but could not subscribe to speed. " + "Set transport via blueprint." + ) + # Arming + dry-run are RPC-only; no stream subscription here. logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz") @@ -709,6 +807,12 @@ def stop(self) -> None: if self._buttons_unsub: self._buttons_unsub() self._buttons_unsub = None + if self._path_unsub: + self._path_unsub() + self._path_unsub = None + if self._speed_unsub: + self._speed_unsub() + self._speed_unsub = None if self._tick_loop: self._tick_loop.stop() diff --git a/dimos/control/tasks/feedforward_gain_compensator.py b/dimos/control/tasks/feedforward_gain_compensator.py new file mode 100644 index 0000000000..9c2ab28ed4 --- /dev/null +++ b/dimos/control/tasks/feedforward_gain_compensator.py @@ -0,0 +1,93 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Static feedforward gain compensator (Strategy B). + +Sits between any path-following controller and the platform. Rather than +closing a velocity loop with a PID (which requires actual_velocity feedback +and is fragile when cascaded over a firmware that already tracks velocity), +this compensator just **inverts the steady-state plant gain** so the +controller's "I want vx=X" command actually produces vx=X at the wheels: + + cmd_to_robot = controller_cmd / K_plant + +Stateless, no actual feedback needed, no phase-margin issues. Works as +long as K is reasonably accurate. Trade: doesn't compensate for plant +dynamics (tau, L) - controller's own outer loop handles those via pose +feedback. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +def _clamp(v: float, lo: float, hi: float) -> float: + return max(lo, min(hi, v)) + + +@dataclass +class FeedforwardGainConfig: + """Steady-state plant gains. Default = unity (passthrough). + + For Go2, do not hardcode: read the vendored fit + ``dimos.utils.benchmarking.plant_models.GO2_PLANT_FITTED`` (currently + ``K_vx≈0.92``, ``K_wz≈2.45``). A stale hardcoded ``K_wz=2.175`` copy + silently mis-calibrated every FF controller; the single source of + truth is plant_models. + """ + + K_vx: float = 1.0 + K_vy: float = 1.0 + K_wz: float = 1.0 + output_min_vx: float = -1.0 + output_max_vx: float = 1.0 + output_min_vy: float = -1.0 + output_max_vy: float = 1.0 + output_min_wz: float = -1.5 + output_max_wz: float = 1.5 + + +class FeedforwardGainCompensator: + """Divide controller-output velocities by plant gains; clamp to limits. + + API mirrors :class:`VelocityTrackingPID.compute` so it slots into the + same place in the path-follower task pipeline. ``actual_*`` arguments + are accepted but ignored - this is pure feedforward. + """ + + def __init__(self, config: FeedforwardGainConfig | None = None) -> None: + self.cfg = config or FeedforwardGainConfig() + + def compute( + self, + desired_vx: float, + desired_vy: float, + desired_wz: float, + actual_vx: float = 0.0, + actual_vy: float = 0.0, + actual_wz: float = 0.0, + ) -> tuple[float, float, float]: + return ( + _clamp(desired_vx / self.cfg.K_vx, self.cfg.output_min_vx, self.cfg.output_max_vx), + _clamp(desired_vy / self.cfg.K_vy, self.cfg.output_min_vy, self.cfg.output_max_vy), + _clamp(desired_wz / self.cfg.K_wz, self.cfg.output_min_wz, self.cfg.output_max_wz), + ) + + def reset(self) -> None: + # Stateless. Method exists so it's drop-in for VelocityTrackingPID. + pass + + +__all__ = ["FeedforwardGainCompensator", "FeedforwardGainConfig"] diff --git a/dimos/control/tasks/path_follower_task/__registry__.py b/dimos/control/tasks/path_follower_task/__registry__.py new file mode 100644 index 0000000000..47a7ac1653 --- /dev/null +++ b/dimos/control/tasks/path_follower_task/__registry__.py @@ -0,0 +1,17 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +TASK_FACTORIES = { + "path_follower": "dimos.control.tasks.path_follower_task.path_follower_task:create_task", +} diff --git a/dimos/control/tasks/path_follower_task/path_follower_task.py b/dimos/control/tasks/path_follower_task/path_follower_task.py new file mode 100644 index 0000000000..1afa6469e6 --- /dev/null +++ b/dimos/control/tasks/path_follower_task/path_follower_task.py @@ -0,0 +1,651 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Path-follower ControlTask: production LocalPlanner algorithm, +unwrapped from its daemon thread and rebuilt as a passive ControlTask. + +Algorithm is a faithful port of +:class:`dimos.navigation.replanning_a_star.local_planner.LocalPlanner`: +PController + 0.5 m fixed lookahead + rotate-then-drive heuristic + +state machine (initial_rotation → path_following → final_rotation → arrived). + +Costmap / obstacle-clearance plumbing is intentionally omitted - the +benchmark battery is obstacle-free. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field, replace +from typing import TYPE_CHECKING, Any, Literal + +import numpy as np + +from dimos.control.task import ( + BaseControlTask, + ControlMode, + CoordinatorState, + JointCommandOutput, + ResourceClaim, +) +from dimos.control.tasks.feedforward_gain_compensator import ( + FeedforwardGainCompensator, + FeedforwardGainConfig, +) +from dimos.control.tasks.velocity_tracking_pid import ( + VelocityTrackingConfig, + VelocityTrackingPID, +) +from dimos.core.global_config import global_config as _gc +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.navigation.replanning_a_star.controllers import PController +from dimos.navigation.replanning_a_star.path_distancer import PathDistancer +from dimos.protocol.service.spec import BaseConfig +from dimos.utils.benchmarking.velocity_profile import ( + PathSpeedCap, + PathSpeedCapProtocol, + VelocityProfileConfig, +) +from dimos.utils.logging_config import setup_logger +from dimos.utils.trigonometry import angle_diff + +if TYPE_CHECKING: + from dimos.core.global_config import GlobalConfig + from dimos.msgs.nav_msgs.Path import Path + +logger = setup_logger() + +PathFollowerState = Literal[ + "idle", "initial_rotation", "path_following", "final_rotation", "arrived", "aborted" +] + +# Sentinel so callers can pass ``None`` to configure() to explicitly +# clear ff/profile_config, distinct from "don't touch this field". +_UNSET: object = object() + + +@dataclass +class PathFollowerTaskConfig: + joint_names: list[str] = field(default_factory=lambda: ["base/vx", "base/vy", "base/wz"]) + priority: int = 20 + speed: float = 0.55 + control_frequency: float = 10.0 + goal_tolerance: float = 0.2 + orientation_tolerance: float = 0.35 + # PController outer-loop angular gain. Default 0.5 matches production + # LocalPlanner; sweep on circle_R1.0 found 1.0 gives ~9x lower CTE. + k_angular: float = 0.5 + # Pure-pursuit lookahead distance (m). Default 0.5 matches production + # LocalPlanner. Smaller → tighter curve tracking: the steady-state + # inside-offset on a circle is ~lookahead²/(2·R), so dropping it cuts the + # circle/corner error roughly with the square — but too small wobbles. + # Used as the FIXED lookahead when lookahead_speed_scale == 0 and as the + # initial value before the first adaptive update. + lookahead_dist: float = 0.5 + # --- Regulated-pure-pursuit adaptive lookahead (RPP's own knob) --- + # When lookahead_speed_scale > 0 the lookahead distance adapts to the + # robot's current pursuit speed each tick: + # L = clip(lookahead_speed_scale * v, lookahead_min, lookahead_max) + # Long lookahead at speed (smooth, stable on straights); short lookahead + # in slow corners (tight tracking). 0.0 disables adaptation and the fixed + # ``lookahead_dist`` above is used unchanged. + lookahead_min: float = 0.3 + lookahead_max: float = 0.9 + lookahead_speed_scale: float = 0.0 + # Runtime yaw-rate (steering) saturation, rad/s, applied to the commanded + # wz actually sent to the base. None ⟹ no extra clamp beyond the + # PController's ±speed clip and the FF compensator's ±output_max_wz. Set + # from the artifact's ``velocity_profile.max_angular_speed`` (~1.18 rad/s + # for the Go2) so commanded wz never exceeds the measured turn-rate ceiling. + max_yaw_rate: float | None = None + # Forward-only (car-like / non-holonomic) contract. Pursuit structurally + # commands vy == 0, so this ASSERTS that invariant and clamps vx >= 0 + # (never reverse). The Go2's lidar faces forward; strafing/reversing would + # drive into unsensed space off the collision-free planned path. + forward_only: bool = False + # Optional inner-loop velocity-tracking PID. None ⟹ no closed loop. + # Mutually exclusive with ff_config (PI takes precedence if both set). + pid_config: VelocityTrackingConfig | None = None + # Optional static feedforward plant-gain compensator (Strategy B). + # cmd_to_robot = controller_cmd / K_plant. No actual feedback needed. + ff_config: FeedforwardGainConfig | None = None + # Optional curvature velocity-profile cap. None ⟹ off + velocity_profile_config: VelocityProfileConfig | None = None + + +class PathFollowerTask(BaseControlTask): + """Production LocalPlanner algorithm as a passive ControlTask.""" + + def __init__( + self, + name: str, + config: PathFollowerTaskConfig, + global_config: GlobalConfig, + external_profile_cap: PathSpeedCapProtocol | None = None, + ) -> None: + if len(config.joint_names) != 3: + raise ValueError( + f"PathFollowerTask '{name}' needs 3 joints (vx, vy, wz), " + f"got {len(config.joint_names)}" + ) + + self._name = name + self._config = config + self._joint_names_list = list(config.joint_names) + self._joint_names = frozenset(config.joint_names) + + self._controller = PController(global_config, config.speed, config.control_frequency) + # Override the class-level _k_angular for this instance only. + self._controller._k_angular = config.k_angular + self._pid: VelocityTrackingPID | None = ( + VelocityTrackingPID(config.pid_config) if config.pid_config else None + ) + self._ff: FeedforwardGainCompensator | None = ( + FeedforwardGainCompensator(config.ff_config) if config.ff_config else None + ) + # external_profile_cap (e.g. a ReferenceGovernor) wins over the + # auto-built PathSpeedCap from velocity_profile_config. Either + # path produces a duck-typed PathSpeedCapProtocol object that + # .compute() drives in the same way. + self._profile_cap: PathSpeedCapProtocol | None = ( + external_profile_cap + if external_profile_cap is not None + else PathSpeedCap(config.velocity_profile_config) + if config.velocity_profile_config is not None + else None + ) + + # Optional measured top-speed ceiling for the curvature cap. None ⟹ no + # extra cap (the cap's max_linear_speed follows the requested speed + # directly). Subclasses that derive a profile from a measured envelope + # (e.g. RPPPathFollowerTask) set this to the artifact's max so a runtime + # set_speed() never lets the cap exceed what the plant can hold. + self._v_max_cap: float | None = None + + self._state: PathFollowerState = "idle" + self._path: Path | None = None + self._distancer: PathDistancer | None = None + self._current_odom: PoseStamped | None = None + # Pursuit-speed proxy for adaptive lookahead: the |vx| the controller + # commanded last tick. 0 on the first tick ⟹ lookahead starts at + # lookahead_min and grows as the robot accelerates. + self._v_cur: float = 0.0 + # Closed-path gate: track the furthest-along path index reached so + # that closed paths (where goal==start) don't trip arrival on tick 1. + self._max_progress_idx: int = 0 + # Optional per-waypoint speed cap supplied directly by a caller + # (e.g. Benchmarker handing in RG-derived speeds across RPC). When + # set, takes precedence over self._profile_cap in compute(). See + # start_path() for how it's installed. + self._velocity_profile: np.ndarray | None = None + self._velocity_profile_pts: np.ndarray | None = None + + # ------------------------------------------------------------------ + # ControlTask protocol + # ------------------------------------------------------------------ + + @property + def name(self) -> str: + return self._name + + def claim(self) -> ResourceClaim: + return ResourceClaim( + joints=self._joint_names, + priority=self._config.priority, + mode=ControlMode.VELOCITY, + ) + + def is_active(self) -> bool: + return self._state in ("initial_rotation", "path_following", "final_rotation") + + def compute(self, state: CoordinatorState) -> JointCommandOutput | None: + if not self.is_active(): + return None + if self._path is None or self._distancer is None: + return None + + # Pull pose from CoordinatorState. The twist-base ConnectedHardware + # routes adapter.read_odometry() -> [x, y, yaw] + pos = state.joints.joint_positions + x = pos.get(self._joint_names_list[0]) + y = pos.get(self._joint_names_list[1]) + yaw = pos.get(self._joint_names_list[2]) + if x is not None and y is not None and yaw is not None: + self._current_odom = PoseStamped( + ts=state.t_now, + position=Vector3(float(x), float(y), 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + ) + if self._current_odom is None: + return None + + match self._state: + case "initial_rotation": + vx, vy, wz = self._step_initial_rotation() + case "path_following": + vx, vy, wz = self._step_path_following() + case "final_rotation": + vx, vy, wz = self._step_final_rotation() + case _: + return None + + # Pursuit-speed proxy for next tick's adaptive lookahead: the |vx| + # this controller just commanded (controller frame, pre-FF). Captured + # before FF/profile shaping so lookahead tracks the geometric pursuit + # speed, not the gain-compensated command. + self._v_cur = abs(vx) + + # Speed cap FIRST, on the controller's DESIRED velocities — BEFORE the + # feedforward gain inversion. Capping after FF would clamp the + # gain-inflated command back down to the cap, leaving achieved = cap·K + # (an undershoot when K < 1); capping the desired first lets FF invert + # the plant gain so the robot actually ACHIEVES the capped speed. The + # cap scales (vx, vy, wz) uniformly so the desired turn radius is + # preserved, and FF's per-axis inversion preserves the achieved radius. + # + # Prefer a precomputed per-waypoint profile (e.g. the --rg arm's + # RG-derived speeds shipped as a list[float]) over the auto-built + # curvature-based PathSpeedCap. The lookahead window mirrors + # PathSpeedCap.speed_limit_at — min over the next ~8 waypoints so + # braking starts BEFORE a corner rather than at it. + if self._velocity_profile is not None and self._velocity_profile_pts is not None: + x = self._current_odom.position.x + y = self._current_odom.position.y + i = int(np.argmin(np.sum((self._velocity_profile_pts - np.array([x, y])) ** 2, axis=1))) + j = min(len(self._velocity_profile), i + 8) + vlim = float(np.min(self._velocity_profile[i:j])) + s = abs(vx) + if s > vlim and s > 1e-9: + k = vlim / s + vx, vy, wz = vx * k, vy * k, wz * k + elif self._profile_cap is not None: + vx, vy, wz = self._profile_cap.cap( + self._current_odom.position.x, self._current_odom.position.y, vx, vy, wz + ) + + # Inner-loop gain compensation (mutually exclusive - PI wins if both + # set). Applied AFTER the cap so it inverts the plant gain on the capped + # desired velocity. + if self._pid is not None: + actual_vx = state.joints.joint_velocities.get(self._joint_names_list[0], 0.0) + actual_vy = state.joints.joint_velocities.get(self._joint_names_list[1], 0.0) + actual_wz = state.joints.joint_velocities.get(self._joint_names_list[2], 0.0) + vx, vy, wz = self._pid.compute(vx, vy, wz, actual_vx, actual_vy, actual_wz) + elif self._ff is not None: + # Static gain compensation: cmd_to_robot = controller_cmd / K_plant + vx, vy, wz = self._ff.compute(vx, vy, wz) + + # Regulated-pure-pursuit output conditioning, applied last on the + # commanded values actually sent to the base: + # 1. Yaw-rate saturation to the measured turn-rate ceiling. This is + # a hard actuator limit, so wz is clamped without rescaling vx + # (matching real saturation — the curvature-speed profile above + # already slows corners enough that the clamp rarely binds). + # 2. Forward-only contract: pursuit never strafes (vy == 0) or + # reverses (vx >= 0). + if self._config.max_yaw_rate is not None: + cap = abs(self._config.max_yaw_rate) + wz = max(-cap, min(cap, wz)) + if self._config.forward_only: + assert abs(vy) < 1e-6, f"PathFollowerTask forward_only: vy={vy} must be 0" + vx = max(0.0, vx) + + return JointCommandOutput( + joint_names=self._joint_names_list, + velocities=[vx, vy, wz], + mode=ControlMode.VELOCITY, + ) + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + if joints & self._joint_names and self.is_active(): + logger.warning(f"PathFollowerTask '{self._name}' preempted by {by_task}") + self._state = "aborted" + + # ------------------------------------------------------------------ + # State-machine bodies (mirrors LocalPlanner._compute_*) + # ------------------------------------------------------------------ + + def _step_initial_rotation(self) -> tuple[float, float, float]: + assert self._path is not None and self._current_odom is not None + first_yaw = self._path.poses[0].orientation.euler[2] + robot_yaw = self._current_odom.orientation.euler[2] + yaw_err = angle_diff(first_yaw, robot_yaw) + + if abs(yaw_err) < self._config.orientation_tolerance: + self._state = "path_following" + return self._step_path_following() + + twist = self._controller.rotate(yaw_err) + return float(twist.linear.x), float(twist.linear.y), float(twist.angular.z) + + def _windowed_closest(self, pos: np.ndarray, window: int = 20) -> int: + """Closest path index searched only in a forward window from + ``_max_progress_idx``. Prevents wrap-around matches on closed paths + (e.g. circle where path[0] == path[-1] would otherwise let argmin + return the last index on tick 1 → spurious 'arrived'). + """ + assert self._path is not None + n = len(self._path.poses) + lo = self._max_progress_idx + hi = min(n, lo + window + 1) + best_idx = lo + best_d_sq = float("inf") + for i in range(lo, hi): + p = self._path.poses[i].position + d_sq = (p.x - pos[0]) ** 2 + (p.y - pos[1]) ** 2 + if d_sq < best_d_sq: + best_d_sq = d_sq + best_idx = i + return best_idx + + def _step_path_following(self) -> tuple[float, float, float]: + assert self._path is not None + assert self._distancer is not None + assert self._current_odom is not None + + pos = np.array([self._current_odom.position.x, self._current_odom.position.y]) + + closest = self._windowed_closest(pos) + if closest > self._max_progress_idx: + self._max_progress_idx = closest + + # Arrival is only valid AFTER we've traversed enough of the path. + # Otherwise closed paths (goal==start) would arrive on tick 1. + progress_threshold = max(1, int(0.7 * (len(self._path.poses) - 1))) + if ( + self._max_progress_idx >= progress_threshold + and self._distancer.distance_to_goal(pos) < self._config.goal_tolerance + ): + self._state = "final_rotation" + return self._step_final_rotation() + + # Adaptive lookahead (RPP): size the lookahead to the current pursuit + # speed so straights get a long, stable horizon and slow corners get a + # short, tight one. lookahead_speed_scale == 0 keeps the fixed dist. + if self._config.lookahead_speed_scale > 0.0: + self._distancer._lookahead_dist = float( + np.clip( + self._config.lookahead_speed_scale * self._v_cur, + self._config.lookahead_min, + self._config.lookahead_max, + ) + ) + + lookahead = self._distancer.find_lookahead_point(closest) + twist = self._controller.advance(lookahead, self._current_odom) + return float(twist.linear.x), float(twist.linear.y), float(twist.angular.z) + + def _step_final_rotation(self) -> tuple[float, float, float]: + assert self._path is not None and self._current_odom is not None + goal_yaw = self._path.poses[-1].orientation.euler[2] + robot_yaw = self._current_odom.orientation.euler[2] + yaw_err = angle_diff(goal_yaw, robot_yaw) + + if abs(yaw_err) < self._config.orientation_tolerance: + self._state = "arrived" + logger.info(f"PathFollowerTask '{self._name}' arrived") + return 0.0, 0.0, 0.0 + + twist = self._controller.rotate(yaw_err) + return float(twist.linear.x), float(twist.linear.y), float(twist.angular.z) + + # ------------------------------------------------------------------ + # Public API (called by runner — typically over RPC from a tool) + # ------------------------------------------------------------------ + + def configure( + self, + speed: float | None = None, + k_angular: float | None = None, + lookahead_dist: float | None = None, + lookahead_min: float | None = None, + lookahead_max: float | None = None, + lookahead_speed_scale: float | None = None, + max_yaw_rate: float | None | object = _UNSET, + forward_only: bool | None = None, + ff_config: FeedforwardGainConfig | None | object = _UNSET, + velocity_profile_config: VelocityProfileConfig | None | object = _UNSET, + external_profile_cap: PathSpeedCapProtocol | None | object = _UNSET, + **ignored: Any, + ) -> bool: + """Override per-run knobs before start_path. ``ff_config``, + ``velocity_profile_config``, ``external_profile_cap`` and + ``max_yaw_rate`` use a sentinel so callers can explicitly clear them + by passing ``None`` (distinct from "don't touch"). Only valid while + idle/arrived/aborted — refuses while the task is actively + driving the robot. + + ``external_profile_cap`` wins over ``velocity_profile_config`` + when both are set; this is how a ReferenceGovernor is installed + as the per-tick cap source. + + Unknown kwargs are accepted and logged so callers built for a + sibling task's configure signature (e.g. the trajectory tracker's + eso/deadtime knobs) work unchanged. + """ + if self.is_active(): + logger.warning(f"PathFollowerTask '{self._name}': cannot configure while active") + return False + if speed is not None: + self._config.speed = speed + self._controller._speed = speed # PController exposes _speed + if k_angular is not None: + self._config.k_angular = k_angular + self._controller._k_angular = k_angular + if lookahead_dist is not None: + # Takes effect when the next start_path() rebuilds the PathDistancer. + self._config.lookahead_dist = lookahead_dist + if lookahead_min is not None: + self._config.lookahead_min = lookahead_min + if lookahead_max is not None: + self._config.lookahead_max = lookahead_max + if lookahead_speed_scale is not None: + self._config.lookahead_speed_scale = lookahead_speed_scale + if max_yaw_rate is not _UNSET: + self._config.max_yaw_rate = max_yaw_rate # type: ignore[assignment] + if forward_only is not None: + self._config.forward_only = forward_only + if ff_config is not _UNSET: + self._config.ff_config = ff_config # type: ignore[assignment] + self._ff = FeedforwardGainCompensator(ff_config) if ff_config is not None else None + # external_profile_cap takes precedence over velocity_profile_config. + if external_profile_cap is not _UNSET: + self._profile_cap = external_profile_cap # type: ignore[assignment] + # Track in config only when we're falling back to the auto-built path; + # external_profile_cap is not serialisable into VelocityProfileConfig. + self._config.velocity_profile_config = None + elif velocity_profile_config is not _UNSET: + self._config.velocity_profile_config = velocity_profile_config # type: ignore[assignment] + self._profile_cap = ( + PathSpeedCap(velocity_profile_config) + if velocity_profile_config is not None + else None + ) + if ignored: + logger.info( + f"PathFollowerTask '{self._name}': ignoring unknown configure " + f"kwargs {sorted(ignored)}" + ) + return True + + def start_path( + self, + path: Path, + current_odom: PoseStamped, + ) -> bool: + if path is None or len(path.poses) < 2: + logger.warning(f"PathFollowerTask '{self._name}': invalid path") + return False + self._path = path + self._distancer = PathDistancer(path, self._config.lookahead_dist) + self._current_odom = current_odom + self._max_progress_idx = 0 + self._v_cur = 0.0 + self._controller.reset_errors() + if self._pid is not None: + self._pid.reset() + if self._ff is not None: + self._ff.reset() + if self._profile_cap is not None: + self._profile_cap.for_path(path) + # Reset the per-waypoint speed cap so the parent's compute() path + # is well-defined. Subclasses (e.g. PrecisionPathFollowerTask) may + # repopulate these slots in their own start_path() override. + self._velocity_profile = None + self._velocity_profile_pts = None + + first_yaw = path.poses[0].orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + yaw_err = angle_diff(first_yaw, robot_yaw) + self._controller.reset_yaw_error(yaw_err) + + if abs(yaw_err) < self._config.orientation_tolerance: + # Note: production LocalPlanner transitions to "final_rotation" when + # the robot is exactly at path[0] (pos_d < 0.01). That's broken for + # open paths - we'd snap to "arrived" immediately. Always start in + # path_following when aligned; arrival is detected by distance_to_goal. + self._state = "path_following" + else: + self._state = "initial_rotation" + + logger.info( + f"PathFollowerTask '{self._name}' started " + f"({len(path.poses)} poses, initial state={self._state})" + ) + return True + + def update_odom(self, odom: PoseStamped) -> None: + # Pose now flows in through compute()'s CoordinatorState (sourced + # from the twist-base adapter's read_odometry → joint positions). + # This setter is kept as a no-op-or-override seam so out-of-tree + # callers that still pump odom externally don't break. + self._current_odom = odom + + def set_path(self, path: Path, odom: PoseStamped | None = None) -> None: + """Coordinator broadcast hook for nav-stack-emitted paths. + + ``ControlCoordinator._on_path`` calls this on every task exposing + ``set_path`` with a fresh odom snapshot, so a clicked nav goal (or a + replan) arms the follower the same way the benchmark's RPC + ``start_path`` does. Prefers the caller-supplied ``odom``; falls back to + ``self._current_odom`` for single-arg callers. Each call resets the + follower onto the new path — exactly what makes pursuit robust to + replanning (no clock to re-sync, no re-ramp from rest).""" + use_odom = odom if odom is not None else self._current_odom + if use_odom is None: + logger.warning( + f"PathFollowerTask '{self._name}': received path but no odom available; dropping." + ) + return + logger.info( + f"PathFollowerTask '{self._name}': received path from stream (n={len(path.poses)})" + ) + self.start_path(path, use_odom) + + def set_speed(self, speed: float) -> None: + """Set the follower's target/cruise speed at runtime. + + ``ControlCoordinator._on_speed`` calls this on every task exposing + ``set_speed`` when a ``speed`` message arrives, so an external source + (e.g. the benchmark sweeping speeds) can retune the follower over the + transport without RPC. Updates the pursuit speed and rebuilds the + curvature cap so corner regulation tracks the new top speed (clamped to + the measured ceiling ``_v_max_cap`` when a subclass set one). A no-op + while actively driving — a mid-run jump would discontinuously move the + cap; the next path picks up the new speed cleanly. + """ + if self.is_active(): + logger.warning(f"PathFollowerTask '{self._name}': ignoring set_speed while active") + return + speed = float(speed) + self._config.speed = speed + self._controller._speed = speed # PController exposes _speed + self._apply_profile_speed(speed) + logger.info(f"PathFollowerTask '{self._name}': set_speed({speed:.3f})") + + def _apply_profile_speed(self, speed: float) -> None: + """Rebuild the auto-built curvature cap so its top speed follows + ``speed`` (clamped to ``_v_max_cap``). No-op when no curvature profile + is configured, or when an external non-``PathSpeedCap`` governor is + installed (don't stomp a caller-supplied cap).""" + if self._config.velocity_profile_config is None: + return + if self._profile_cap is not None and not isinstance(self._profile_cap, PathSpeedCap): + return + eff = speed if self._v_max_cap is None else min(speed, self._v_max_cap) + cfg = replace(self._config.velocity_profile_config, max_linear_speed=eff) + self._config.velocity_profile_config = cfg + self._profile_cap = PathSpeedCap(cfg) + + def cancel(self) -> bool: + if not self.is_active(): + return False + self._state = "aborted" + return True + + def reset(self) -> bool: + if self.is_active(): + return False + self._state = "idle" + self._path = None + self._distancer = None + self._current_odom = None + return True + + def get_state(self) -> PathFollowerState: + return self._state + + +__all__ = [ + "PathFollowerTask", + "PathFollowerTaskConfig", +] + + +class PathFollowerTaskParams(BaseConfig): + speed: float = 0.55 + control_frequency: float = 10.0 + goal_tolerance: float = 0.2 + orientation_tolerance: float = 0.35 + k_angular: float = 0.5 + lookahead_dist: float = 0.5 + lookahead_min: float = 0.3 + lookahead_max: float = 0.9 + lookahead_speed_scale: float = 0.0 + max_yaw_rate: float | None = None + forward_only: bool = False + + +def create_task(cfg: Any, hardware: Any) -> PathFollowerTask: + params = PathFollowerTaskParams.model_validate(cfg.params) + return PathFollowerTask( + cfg.name, + PathFollowerTaskConfig( + joint_names=cfg.joint_names, + priority=cfg.priority, + speed=params.speed, + control_frequency=params.control_frequency, + goal_tolerance=params.goal_tolerance, + orientation_tolerance=params.orientation_tolerance, + k_angular=params.k_angular, + lookahead_dist=params.lookahead_dist, + lookahead_min=params.lookahead_min, + lookahead_max=params.lookahead_max, + lookahead_speed_scale=params.lookahead_speed_scale, + max_yaw_rate=params.max_yaw_rate, + forward_only=params.forward_only, + ), + global_config=_gc, + ) diff --git a/dimos/control/tasks/path_follower_task/test_path_follower_task.py b/dimos/control/tasks/path_follower_task/test_path_follower_task.py new file mode 100644 index 0000000000..9c4247e781 --- /dev/null +++ b/dimos/control/tasks/path_follower_task/test_path_follower_task.py @@ -0,0 +1,190 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for the regulated-pure-pursuit knobs added to PathFollowerTask: +adaptive lookahead, runtime yaw-rate clamp, the forward-only contract, and the +extended configure() signature.""" + +from __future__ import annotations + +import math + +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.feedforward_gain_compensator import FeedforwardGainConfig +from dimos.control.tasks.path_follower_task.path_follower_task import ( + PathFollowerTask, + PathFollowerTaskConfig, +) +from dimos.core.global_config import global_config as _gc +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.benchmarking.paths import circle, straight_line +from dimos.utils.benchmarking.velocity_profile import VelocityProfileConfig + +_JOINTS = ["go2/vx", "go2/vy", "go2/wz"] + + +def _state(x: float, y: float, yaw: float, t: float = 0.0) -> CoordinatorState: + return CoordinatorState( + joints=JointStateSnapshot( + joint_positions={_JOINTS[0]: x, _JOINTS[1]: y, _JOINTS[2]: yaw}, + joint_velocities={_JOINTS[0]: 0.0, _JOINTS[1]: 0.0, _JOINTS[2]: 0.0}, + ), + t_now=t, + dt=0.1, + ) + + +def _task(**overrides) -> PathFollowerTask: + cfg = PathFollowerTaskConfig(joint_names=_JOINTS, control_frequency=10.0, **overrides) + return PathFollowerTask("t", cfg, global_config=_gc) + + +def _start_aligned(task: PathFollowerTask, path) -> None: + odom = PoseStamped( + position=Vector3(path.poses[0].position.x, path.poses[0].position.y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, path.poses[0].orientation.euler[2])), + ) + assert task.start_path(path, odom) + + +# --- adaptive lookahead ---------------------------------------------------- + + +def test_adaptive_lookahead_grows_from_min_with_speed(): + """L starts at lookahead_min (v_cur == 0 on tick 0) then climbs to + ~scale * v, staying within [min, max].""" + task = _task(speed=0.6, lookahead_speed_scale=1.0, lookahead_min=0.3, lookahead_max=0.9) + path = straight_line(length=5.0) + _start_aligned(task, path) + + # Tick 0: v_cur == 0 -> L pinned to lookahead_min. + task.compute(_state(0.0, 0.0, 0.0)) + assert math.isclose(task._distancer._lookahead_dist, 0.3, abs_tol=1e-9) + + # Subsequent ticks: L == clip(scale * v_cur, min, max). With FF off, the + # pursuit speed on a straight aligned path is the config speed (0.6). + task.compute(_state(0.0, 0.0, 0.0)) + assert math.isclose(task._distancer._lookahead_dist, 0.6, abs_tol=1e-6) + assert 0.3 <= task._distancer._lookahead_dist <= 0.9 + + +def test_adaptive_lookahead_clamped_to_max(): + task = _task(speed=2.0, lookahead_speed_scale=1.0, lookahead_min=0.3, lookahead_max=0.7) + path = straight_line(length=5.0) + _start_aligned(task, path) + for _ in range(3): + task.compute(_state(0.0, 0.0, 0.0)) + assert math.isclose(task._distancer._lookahead_dist, 0.7, abs_tol=1e-9) + + +def test_fixed_lookahead_when_scale_zero(): + """scale == 0 keeps the fixed lookahead_dist (no adaptation).""" + task = _task(speed=0.6, lookahead_speed_scale=0.0, lookahead_dist=0.5) + path = straight_line(length=5.0) + _start_aligned(task, path) + for _ in range(4): + task.compute(_state(0.0, 0.0, 0.0)) + assert math.isclose(task._distancer._lookahead_dist, 0.5, abs_tol=1e-9) + + +# --- yaw-rate clamp -------------------------------------------------------- + + +def test_max_yaw_rate_clamps_commanded_wz(): + """A tight curve drives |wz| past the cap; the clamp holds it at the cap.""" + cap = 0.25 + task = _task(speed=0.9, max_yaw_rate=cap) + path = circle(radius=0.5) # high curvature -> large desired wz + _start_aligned(task, path) + saw_cap = False + for k in range(15): + out = task.compute(_state(0.0, 0.0, 0.0, t=k * 0.1)) + if out is not None: + wz = out.velocities[2] + assert abs(wz) <= cap + 1e-9 + if abs(abs(wz) - cap) < 1e-6: + saw_cap = True + assert saw_cap, "expected the yaw clamp to bind on a tight circle" + + +def test_no_yaw_clamp_when_none(): + task = _task(speed=0.9, max_yaw_rate=None) + path = circle(radius=0.5) + _start_aligned(task, path) + # Without a clamp the bare PController clips wz to +/- speed (0.9), so + # |wz| can exceed the 0.25 a clamp would have imposed. + peak = 0.0 + for k in range(15): + out = task.compute(_state(0.0, 0.0, 0.0, t=k * 0.1)) + if out is not None: + peak = max(peak, abs(out.velocities[2])) + assert peak > 0.25 + + +# --- forward-only contract ------------------------------------------------- + + +def test_forward_only_holds_over_a_run(): + """Pursuit never strafes or reverses: vy == 0 and vx >= 0 every tick.""" + task = _task( + speed=0.7, + forward_only=True, + lookahead_speed_scale=1.6, + max_yaw_rate=1.18, + ff_config=FeedforwardGainConfig(K_vx=0.8, K_vy=0.8, K_wz=0.9), + velocity_profile_config=VelocityProfileConfig(max_linear_speed=0.7, min_speed=0.2), + ) + path = circle(radius=1.0) + _start_aligned(task, path) + for k in range(30): + out = task.compute(_state(0.0, 0.0, 0.0, t=k * 0.1)) + if out is None: + continue + vx, vy, wz = out.velocities + assert vy == 0.0 + assert vx >= 0.0 + + +# --- configure() signature ------------------------------------------------- + + +def test_configure_sets_rpp_knobs_and_ignores_unknown(): + task = _task() + ok = task.configure( + lookahead_min=0.2, + lookahead_max=0.8, + lookahead_speed_scale=1.4, + max_yaw_rate=1.1, + forward_only=True, + some_trajtrack_only_kwarg=123, # unknown -> logged, not fatal + ) + assert ok + assert task._config.lookahead_min == 0.2 + assert task._config.lookahead_max == 0.8 + assert task._config.lookahead_speed_scale == 1.4 + assert task._config.max_yaw_rate == 1.1 + assert task._config.forward_only is True + + +def test_configure_max_yaw_rate_none_clears_clamp(): + """max_yaw_rate uses a sentinel, so passing None explicitly clears it.""" + task = _task(max_yaw_rate=1.18) + assert task.configure(max_yaw_rate=None) + assert task._config.max_yaw_rate is None + # Omitting it leaves the value untouched. + task2 = _task(max_yaw_rate=1.18) + assert task2.configure(speed=0.5) + assert task2._config.max_yaw_rate == 1.18 diff --git a/dimos/control/tasks/rpp_path_follower_task/__registry__.py b/dimos/control/tasks/rpp_path_follower_task/__registry__.py new file mode 100644 index 0000000000..284bea51a3 --- /dev/null +++ b/dimos/control/tasks/rpp_path_follower_task/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +TASK_FACTORIES = { + "rpp_path_follower": ( + "dimos.control.tasks.rpp_path_follower_task.rpp_path_follower_task:create_task" + ), +} diff --git a/dimos/control/tasks/rpp_path_follower_task/artifacts/go2_posedomain.json b/dimos/control/tasks/rpp_path_follower_task/artifacts/go2_posedomain.json new file mode 100644 index 0000000000..0e9f35286c --- /dev/null +++ b/dimos/control/tasks/rpp_path_follower_task/artifacts/go2_posedomain.json @@ -0,0 +1,129 @@ +{ + "provenance": { + "robot_id": "go2", + "surface": "concrete", + "mode": "default", + "date": "2026-06-23", + "git_sha": "97298ef8c", + "sim_or_hw": "hw", + "characterization_session_dir": "data/characterization/go2/go2_recording_default_2026-06-23_222.db", + "methodology_version": 2 + }, + "plant": { + "vx": { + "K": 0.8031702038136178, + "tau": 0.3, + "L": 0.15 + }, + "vy": { + "K": 0.7359189288019671, + "tau": 0.3, + "L": 0.15 + }, + "wz": { + "K": 0.8988635584229723, + "tau": 0.3, + "L": 0.15 + } + }, + "feedforward": { + "K_vx": 1.2450661083439023, + "K_vy": 1.3588453304604373, + "K_wz": 1.1125158992477884, + "output_min_vx": -1.0, + "output_max_vx": 1.0, + "output_min_vy": -1.0, + "output_max_vy": 1.0, + "output_min_wz": -1.5, + "output_max_wz": 1.5 + }, + "velocity_profile": { + "max_linear_speed": 0.819432592238089, + "max_angular_speed": 1.180671561475521, + "max_centripetal_accel": 1.0, + "max_linear_accel": 2.7314419741269633, + "max_linear_decel": 5.462883948253927, + "min_speed": 0.25, + "lookahead_pts": 8 + }, + "recommended_controller": { + "name": "baseline", + "params": { + "k_angular": 0.5 + }, + "evidence": "Baseline P-controller, hardcoded. The Go2 base is FOPDT per axis; at a given speed the tracking error equals the plant floor (tau + L) * v, which no reactive control law can beat (~zero headroom over the floor \u2014 validated controller bake-off). The only effective lever is speed vs path curvature, which the derived velocity profile + feedforward already apply. See reports/tuning_README.md and the characterization findings (this evidence string cites the Go2 result; a different robot's headroom is TBD until characterized)." + }, + "caveats": [ + "Valid only for surface='concrete', mode='default', hw. Re-run characterization on any surface or gait-mode change.", + "Plant fitted from data/characterization/go2/go2_recording_default_2026-06-23_222.db on 2026-06-23 (git 97298ef8c)." + ], + "operating_point_map": null, + "velocity_envelope": null, + "dynamics_by_amplitude": { + "vx": [ + { + "amp": 0.688, + "K": 0.8031702038136178, + "tau": 0.3, + "L": 0.15, + "r2": 0.9997340425001168, + "source": "sweep" + }, + { + "amp": 1.0, + "K": 0.819432592238089, + "tau": 0.3, + "L": 0.15, + "r2": 0.9997340425001168, + "source": "sweep" + } + ], + "vy": [ + { + "amp": 0.65, + "K": 0.7359189288019671, + "tau": 0.3, + "L": 0.15, + "r2": 0.9998431135682062, + "source": "sweep" + }, + { + "amp": 1.0, + "K": 0.8413264464473194, + "tau": 0.3, + "L": 0.15, + "r2": 0.9998431135682062, + "source": "sweep" + } + ], + "wz": [ + { + "amp": 0.725, + "K": 0.7699677897678445, + "tau": 0.3, + "L": 0.15, + "r2": 0.9999577115547055, + "source": "sweep" + }, + { + "amp": 1.15, + "K": 0.8988635584229723, + "tau": 0.3, + "L": 0.15, + "r2": 0.9999577115547055, + "source": "sweep" + }, + { + "amp": 1.5, + "K": 0.9260169109611931, + "tau": 0.3, + "L": 0.15, + "r2": 0.9999577115547055, + "source": "sweep" + } + ] + }, + "floor_probe_results": null, + "valid_for_tuning": true, + "schema_version": 1 +} diff --git a/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py b/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py new file mode 100644 index 0000000000..e7c2732475 --- /dev/null +++ b/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py @@ -0,0 +1,246 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Regulated-pure-pursuit nav path-follower: PathFollowerTask self-calibrated +from a characterization artifact. + +A thin subclass of :class:`PathFollowerTask` for the navigation stack. The +benchmark configures the bare path_follower over RPC (feedforward + curvature +profile built by the Benchmarker); in nav there is no Benchmarker, so this task +loads the artifact itself — lazily on the first ``start_path()`` so a missing +default never blocks startup — and builds the same calibration: + +* feedforward gain compensation with ``FF.K = plant.K`` (commanded == achieved; + the same convention the trajectory tracker uses), +* curvature speed regulation (``PathSpeedCap`` from the artifact's + ``velocity_profile`` — a_lat / turn-rate / min-speed), +* the runtime yaw-rate clamp from the artifact's measured turn-rate ceiling. + +The pursuit knobs (adaptive lookahead, ``k_angular``, ``forward_only``) come from +the task config. ``set_path`` (inherited from the base) arms it on a clicked nav +goal or a replan; each path resets the pursuit cleanly — no clock to re-sync. +""" + +from __future__ import annotations + +import math +from pathlib import Path as _Path +from typing import TYPE_CHECKING, Any + +from dimos.control.tasks.feedforward_gain_compensator import ( + FeedforwardGainCompensator, + FeedforwardGainConfig, +) +from dimos.control.tasks.path_follower_task.path_follower_task import ( + PathFollowerTask, + PathFollowerTaskConfig, +) +from dimos.core.global_config import global_config as _gc +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Path import Path +from dimos.protocol.service.spec import BaseConfig +from dimos.utils.benchmarking.tuning import TuningConfig +from dimos.utils.benchmarking.velocity_profile import PathSpeedCap, VelocityProfileConfig +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.core.global_config import GlobalConfig + +logger = setup_logger() + +# Pose-domain Go2 tuning artifact, vendored next to this task so the controller +# is self-contained (no gitignored data/ dependency). Blueprints and the task +# params default to it; override per run with params.artifact_path. +DEFAULT_ARTIFACT_PATH = str(_Path(__file__).parent / "artifacts" / "go2_posedomain.json") + +# Heading-degeneracy threshold (rad): if every path pose carries the same yaw +# (e.g. the MLS planner stamps identity orientation on all poses), the spread is +# ~0 and we synthesize per-pose headings from the path tangent. A planner that +# emits real per-pose orientations has a spread above this and is left untouched. +_HEADING_DEGENERATE_EPS = 1e-3 + + +def _with_tangent_headings(path: Path) -> Path: + """Return ``path`` with each pose's yaw set to its forward path tangent when + the path's orientations are degenerate (all equal — e.g. the MLS planner + stamps identity on every pose). RPP's rotate-then-drive + final-settle states + read ``poses[0]``/``poses[-1]`` orientation, so a heading-less path would spin + the robot to face world-yaw 0 at the start and goal. For a forward-only + follower the segment tangent IS the desired heading, so this restores + align-then-drive (first segment) and a settle to the approach heading (last + segment). Paths that already carry per-pose headings are returned unchanged. + """ + poses = path.poses + if poses is None or len(poses) < 2: + return path + + yaws = [p.orientation.euler[2] for p in poses] + if max(yaws) - min(yaws) > _HEADING_DEGENERATE_EPS: + return path # planner provided real per-pose headings; leave them. + + new_poses: list[PoseStamped] = [] + n = len(poses) + for i, p in enumerate(poses): + if i < n - 1: + dx = poses[i + 1].position.x - p.position.x + dy = poses[i + 1].position.y - p.position.y + yaw = ( + math.atan2(dy, dx) + if (dx * dx + dy * dy) > 1e-12 + # Coincident waypoints: keep the previous heading rather than + # snapping to 0. + else (new_poses[-1].orientation.euler[2] if new_poses else 0.0) + ) + else: + # Last pose has no next segment — settle to the approach heading. + yaw = new_poses[-1].orientation.euler[2] if new_poses else 0.0 + new_poses.append( + PoseStamped( + ts=p.ts, + frame_id=p.frame_id, + position=Vector3(p.position.x, p.position.y, p.position.z), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + ) + return Path(ts=path.ts, frame_id=path.frame_id, poses=new_poses) + + +class RPPPathFollowerTask(PathFollowerTask): + """Path follower that self-calibrates its feedforward + curvature velocity + profile from a tuning artifact (loaded lazily on first ``start_path``).""" + + def __init__( + self, + name: str, + config: PathFollowerTaskConfig, + global_config: GlobalConfig, + artifact_path: str, + v_max_override: float | None = None, + ) -> None: + super().__init__(name, config, global_config=global_config) + self._artifact_path = artifact_path + self._v_max_override = float(v_max_override) if v_max_override is not None else None + self._artifact_loaded = False + + def start_path(self, path: Path, current_odom: PoseStamped) -> bool: + if not self._artifact_loaded: + self._load_artifact() + return super().start_path(_with_tangent_headings(path), current_odom) + + def _load_artifact(self) -> None: + if not self._artifact_path: + raise RuntimeError( + f"RPPPathFollowerTask '{self._name}': artifact_path is empty; " + f"pass via params.artifact_path on the TaskConfig." + ) + if not _Path(self._artifact_path).exists(): + raise RuntimeError( + f"RPPPathFollowerTask '{self._name}': artifact not found at {self._artifact_path}" + ) + art = TuningConfig.from_json(self._artifact_path) + + # Feedforward: FF.K = plant.K so the command divides by the plant gain + # and the robot achieves the commanded velocity (same convention as the + # trajectory tracker; the artifact's `feedforward` block stores 1/K and + # is inverted for the compensator, so build from `plant` directly). + self._ff = FeedforwardGainCompensator( + FeedforwardGainConfig( + K_vx=art.plant.vx.K, + K_vy=art.plant.vy.K, + K_wz=art.plant.wz.K, + ) + ) + + # Curvature speed regulation from the artifact's velocity profile. + # Stash the profile config + the measured top-speed ceiling on the base + # so a runtime set_speed() (the coordinator's `speed` port) can rebuild + # the cap to the new cruise speed, clamped to this ceiling. + vp = art.velocity_profile + v_max = self._v_max_override if self._v_max_override is not None else vp.max_linear_speed + self._v_max_cap = v_max + self._config.velocity_profile_config = VelocityProfileConfig( + max_linear_speed=min(self._config.speed, v_max), + max_angular_speed=vp.max_angular_speed, + max_centripetal_accel=vp.max_centripetal_accel, + max_linear_accel=vp.max_linear_accel, + max_linear_decel=vp.max_linear_decel, + min_speed=vp.min_speed, + ) + self._profile_cap = PathSpeedCap(self._config.velocity_profile_config) + + # Runtime yaw-rate clamp = the measured turn-rate ceiling, unless the + # config set it explicitly. + if self._config.max_yaw_rate is None: + self._config.max_yaw_rate = vp.max_angular_speed + + self._artifact_loaded = True + logger.info( + f"RPPPathFollowerTask '{self._name}': loaded artifact {self._artifact_path} " + f"(FF.K=plant.K, v_max={min(self._config.speed, v_max):.3f}, " + f"a_lat={vp.max_centripetal_accel:.2f}, yaw_cap={vp.max_angular_speed:.3f}, " + f"min_speed={vp.min_speed:.3f})" + ) + + +class RPPPathFollowerTaskParams(BaseConfig): + artifact_path: str = DEFAULT_ARTIFACT_PATH + speed: float = 0.7 + control_frequency: float = 10.0 + goal_tolerance: float = 0.2 + orientation_tolerance: float = 0.35 + # CTE-tuned pursuit knobs (see the RPP benchmark report): high heading gain + # + a near-fixed largish lookahead; forward-only for the forward-facing lidar. + k_angular: float = 1.5 + lookahead_dist: float = 0.7 + lookahead_min: float = 0.5 + lookahead_max: float = 0.7 + lookahead_speed_scale: float = 2.0 + max_yaw_rate: float | None = None # None ⟹ artifact's measured turn-rate ceiling + forward_only: bool = True + v_max_override: float | None = None + + +def create_task(cfg: Any, hardware: Any) -> RPPPathFollowerTask: + params = RPPPathFollowerTaskParams.model_validate(cfg.params) + return RPPPathFollowerTask( + cfg.name, + PathFollowerTaskConfig( + joint_names=cfg.joint_names, + priority=cfg.priority, + speed=params.speed, + control_frequency=params.control_frequency, + goal_tolerance=params.goal_tolerance, + orientation_tolerance=params.orientation_tolerance, + k_angular=params.k_angular, + lookahead_dist=params.lookahead_dist, + lookahead_min=params.lookahead_min, + lookahead_max=params.lookahead_max, + lookahead_speed_scale=params.lookahead_speed_scale, + max_yaw_rate=params.max_yaw_rate, + forward_only=params.forward_only, + ), + global_config=_gc, + artifact_path=params.artifact_path, + v_max_override=params.v_max_override, + ) + + +__all__ = [ + "DEFAULT_ARTIFACT_PATH", + "RPPPathFollowerTask", + "RPPPathFollowerTaskParams", + "create_task", +] diff --git a/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py b/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py new file mode 100644 index 0000000000..8220ce6846 --- /dev/null +++ b/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py @@ -0,0 +1,297 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for the RPP nav follower: lazy artifact load, set_path arming, the +feedforward/profile calibration, replan reset, and an end-to-end drive.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.rpp_path_follower_task.rpp_path_follower_task import ( + RPPPathFollowerTask, + create_task, +) +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.benchmarking.paths import circle, straight_line +from dimos.utils.benchmarking.plant import ( + FopdtChannelParams, + TwistBasePlantParams, + TwistBasePlantSim, +) +from dimos.utils.benchmarking.tuning import ( + FeedforwardDC, + FopdtChannelDC, + PlantModelDC, + Provenance, + RecommendedControllerDC, + TuningConfig, + VelocityProfileDC, +) + +_JOINTS = ["go2/vx", "go2/vy", "go2/wz"] + + +@pytest.fixture +def artifact_path(tmp_path): + """A small on-disk artifact (plant gain != 1, real lag) for the task to load.""" + chan = lambda K: FopdtChannelDC(K=K, tau=0.3, L=0.1) # noqa: E731 + art = TuningConfig( + provenance=Provenance(robot_id="go2", git_sha="test", sim_or_hw="hw"), + plant=PlantModelDC(vx=chan(0.8), vy=chan(0.8), wz=chan(0.9)), + feedforward=FeedforwardDC(K_vx=1.25, K_vy=1.25, K_wz=1.11), # 1/K (unused by RPP) + velocity_profile=VelocityProfileDC( + max_linear_speed=0.8, + max_angular_speed=1.18, + max_centripetal_accel=1.0, + max_linear_accel=2.5, + max_linear_decel=5.0, + min_speed=0.2, + ), + recommended_controller=RecommendedControllerDC(), + ) + p = tmp_path / "art.json" + art.to_json(p) + return str(p), art + + +def _task(artifact_path: str, **params) -> RPPPathFollowerTask: + cfg = SimpleNamespace( + name="rpp_follower", + joint_names=_JOINTS, + priority=10, + params={"artifact_path": artifact_path, **params}, + ) + return create_task(cfg, None) + + +def _odom(x=0.0, y=0.0, yaw=0.0) -> PoseStamped: + return PoseStamped( + position=Vector3(x, y, 0.0), orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)) + ) + + +def test_artifact_loaded_lazily_not_at_construction(artifact_path): + path, _ = artifact_path + task = _task(path) + assert not task._artifact_loaded + assert task._ff is None and task._profile_cap is None + + +def test_set_path_arms_and_loads_calibration(artifact_path): + path, art = artifact_path + task = _task(path, speed=0.7) + task.set_path(circle(radius=1.0), _odom()) + assert task._artifact_loaded + assert task.is_active() + assert task.get_state() == "path_following" + # Feedforward built from plant.K (commanded == achieved), not the 1/K block. + assert task._ff.cfg.K_vx == art.plant.vx.K + assert task._ff.cfg.K_wz == art.plant.wz.K + # Curvature profile + yaw clamp from the artifact. + assert task._profile_cap.cfg.max_centripetal_accel == art.velocity_profile.max_centripetal_accel + assert task._profile_cap.cfg.min_speed == art.velocity_profile.min_speed + assert task._config.max_yaw_rate == art.velocity_profile.max_angular_speed + + +def test_profile_speed_capped_to_cruise(artifact_path): + path, _ = artifact_path + # cruise 0.5 below the artifact's 0.8 max -> profile uses the cruise. + task = _task(path, speed=0.5) + task.set_path(straight_line(length=3.0), _odom()) + assert task._profile_cap.cfg.max_linear_speed == pytest.approx(0.5) + + +def test_set_speed_rebuilds_cap_after_load(artifact_path): + """The coordinator's `speed` port -> set_speed() retunes the follower: it + updates the pursuit speed and rebuilds the curvature cap to the new top + speed (still using the artifact's a_lat / min_speed). Valid only while idle, + so the run is cancelled before retuning (as between benchmark runs).""" + path, art = artifact_path + task = _task(path, speed=0.7) + task.set_path(straight_line(length=3.0), _odom()) # loads the artifact + arms + task.cancel() # back to idle so a new speed can be set (as between runs) + task.set_speed(0.4) + assert task._config.speed == pytest.approx(0.4) + assert task._controller._speed == pytest.approx(0.4) + assert task._profile_cap.cfg.max_linear_speed == pytest.approx(0.4) + # regulation constants stay from the artifact + assert task._profile_cap.cfg.max_centripetal_accel == art.velocity_profile.max_centripetal_accel + assert task._profile_cap.cfg.min_speed == art.velocity_profile.min_speed + + +def test_set_speed_clamps_to_artifact_vmax(artifact_path): + """set_speed above the artifact's measured top speed clamps to it — the cap + never exceeds what the plant can hold.""" + path, art = artifact_path # artifact max_linear_speed = 0.8 + task = _task(path, speed=0.5) + task.set_path(straight_line(length=3.0), _odom()) + task.cancel() + task.set_speed(2.0) + assert task._profile_cap.cfg.max_linear_speed == pytest.approx( + art.velocity_profile.max_linear_speed + ) + + +def test_set_speed_before_first_path_is_picked_up(artifact_path): + """set_speed before any path (artifact not yet loaded) stashes the speed; the + lazy artifact load on the first path picks it up.""" + path, _ = artifact_path + task = _task(path, speed=0.7) + assert not task._artifact_loaded + task.set_speed(0.4) + assert task._profile_cap is None # nothing to rebuild yet + task.set_path(straight_line(length=3.0), _odom()) + assert task._profile_cap.cfg.max_linear_speed == pytest.approx(0.4) + + +def test_set_speed_ignored_while_active(artifact_path): + """A mid-run set_speed is ignored (a discontinuous cap jump); the next path + picks up the new speed cleanly.""" + path, _ = artifact_path + task = _task(path, speed=0.7) + task.set_path(straight_line(length=3.0), _odom()) + assert task.is_active() + task.set_speed(0.3) + assert task._config.speed == pytest.approx(0.7) # unchanged while active + + +def test_set_path_resets_cleanly_on_replan(artifact_path): + path, _ = artifact_path + task = _task(path) + task.set_path(straight_line(length=3.0), _odom()) + # Drive a couple ticks so progress advances. + for k in range(5): + task.compute(_state(0.5 * k, 0.0, 0.0)) + advanced = task._max_progress_idx + assert advanced > 0 + # A replan (new path via set_path) resets progress to 0 — no stale carrot. + task.set_path(straight_line(length=3.0), _odom()) + assert task._max_progress_idx == 0 + assert task.get_state() == "path_following" + + +def _state(x, y, yaw, t=0.0): + return CoordinatorState( + joints=JointStateSnapshot( + joint_positions={_JOINTS[0]: x, _JOINTS[1]: y, _JOINTS[2]: yaw}, + joint_velocities={_JOINTS[0]: 0.0, _JOINTS[1]: 0.0, _JOINTS[2]: 0.0}, + ), + t_now=t, + dt=0.1, + ) + + +def test_drives_to_arrival_forward_only(artifact_path): + """End-to-end: drive a straight against the FOPDT plant; arrives, and the + forward-only contract holds (vy == 0, vx >= 0) every tick.""" + path, art = artifact_path + from dimos.core.global_config import global_config as gc # noqa: F401 (task uses _gc) + + task = _task(path, speed=0.7) + plant = TwistBasePlantSim( + TwistBasePlantParams( + vx=FopdtChannelParams(art.plant.vx.K, art.plant.vx.tau, art.plant.vx.L), + vy=FopdtChannelParams(art.plant.vy.K, art.plant.vy.tau, art.plant.vy.L), + wz=FopdtChannelParams(art.plant.wz.K, art.plant.wz.tau, art.plant.wz.L), + ) + ) + p = straight_line(length=3.0) + plant.reset(0.0, 0.0, 0.0, 0.1) + task.set_path(p, _odom()) + arrived = False + for k in range(400): + out = task.compute(_state(plant.x, plant.y, plant.yaw, t=k * 0.1)) + cvx, cvy, cwz = out.velocities if out is not None else (0.0, 0.0, 0.0) + assert cvy == 0.0 + assert cvx >= 0.0 + if task.get_state() == "arrived": + arrived = True + break + plant.step(cvx, cvy, cwz, 0.1) + assert arrived + + +# --- tangent-heading synthesis (for position-only planners e.g. MLS) ----------- + +import math + +from dimos.control.tasks.rpp_path_follower_task.rpp_path_follower_task import ( + _with_tangent_headings, +) +from dimos.msgs.nav_msgs.Path import Path + + +def _ident_path(points): + """Path with identity orientation on every pose (what MLSPlannerNative emits).""" + return Path( + ts=0.0, + frame_id="odom", + poses=[ + PoseStamped( + ts=0.0, position=Vector3(x, y, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + ) + for x, y in points + ], + ) + + +def test_degenerate_path_gets_tangent_headings(): + # Straight +x then turn +y: first segments face 0 rad, the corner faces +pi/2. + out = _with_tangent_headings(_ident_path([(0, 0), (1, 0), (2, 0), (2, 1), (2, 2)])) + yaws = [p.orientation.euler[2] for p in out.poses] + assert math.isclose(yaws[0], 0.0, abs_tol=1e-6) + assert math.isclose(yaws[1], 0.0, abs_tol=1e-6) + assert math.isclose(yaws[2], math.pi / 2, abs_tol=1e-6) # tangent into the turn + assert math.isclose(yaws[3], math.pi / 2, abs_tol=1e-6) + # Last pose inherits the approach heading (no next segment). + assert math.isclose(yaws[4], yaws[3], abs_tol=1e-6) + + +def test_positions_unchanged_by_synthesis(): + pts = [(0, 0), (1, 0), (1, 1)] + out = _with_tangent_headings(_ident_path(pts)) + assert [(round(p.position.x, 6), round(p.position.y, 6)) for p in out.poses] == pts + + +def test_path_with_real_headings_is_untouched(): + # A planner that already provides per-pose orientations must be left alone. + oriented = Path( + ts=0.0, + frame_id="odom", + poses=[ + PoseStamped( + ts=0.0, + position=Vector3(0, 0, 0), + orientation=Quaternion.from_euler(Vector3(0, 0, 0.3)), + ), + PoseStamped( + ts=0.0, + position=Vector3(1, 0, 0), + orientation=Quaternion.from_euler(Vector3(0, 0, 1.4)), + ), + ], + ) + out = _with_tangent_headings(oriented) + assert out is oriented # returned unchanged (identity), not rebuilt + + +def test_short_path_returned_as_is(): + one = _ident_path([(0, 0)]) + assert _with_tangent_headings(one) is one diff --git a/dimos/control/tasks/velocity_profiler.py b/dimos/control/tasks/velocity_profiler.py new file mode 100644 index 0000000000..44a31d3485 --- /dev/null +++ b/dimos/control/tasks/velocity_profiler.py @@ -0,0 +1,158 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Curvature-aware velocity profiler for path-following control. + +Computes a speed limit at each waypoint by: +1. Estimating local curvature via three-point heading change. +2. Limiting speed from centripetal acceleration: v_max = sqrt(a_max / kappa). +3. Forward pass: enforce acceleration constraint. +4. Backward pass: enforce deceleration constraint. +""" + +from __future__ import annotations + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs import Path + + +class VelocityProfiler: + """Compute an optimal speed profile along a path.""" + + def __init__( + self, + max_linear_speed: float = 0.8, + max_angular_speed: float = 1.5, + max_linear_accel: float = 1.0, + max_linear_decel: float = 2.0, + max_centripetal_accel: float = 1.0, + min_speed: float = 0.05, + ) -> None: + self._max_linear_speed = max_linear_speed + self._max_angular_speed = max_angular_speed + self._max_linear_accel = max_linear_accel + self._max_linear_decel = max_linear_decel + self._max_centripetal_accel = max_centripetal_accel + self._min_speed = min_speed + + self._cached_path_id: int | None = None + self._cached_profile: NDArray[np.float64] | None = None + + def compute_profile(self, path: Path) -> NDArray[np.float64]: + """Compute velocity profile for entire path. + + Returns: + Array of speed limits (m/s) per waypoint. + """ + if len(path.poses) < 2: + return np.array([self._min_speed]) + + pts = np.array([[p.position.x, p.position.y] for p in path.poses]) + curvatures = self._compute_curvatures(pts) + max_speeds = self._curvature_speed_limits(curvatures) + velocities = self._acceleration_pass(pts, max_speeds, forward=True) + velocities = self._acceleration_pass(pts, velocities, forward=False) + return np.maximum(velocities, self._min_speed) + + def get_velocity_at_index(self, path: Path, index: int) -> float: + """Get cached velocity at a specific path index.""" + path_id = id(path) + if self._cached_path_id != path_id or self._cached_profile is None: + self._cached_profile = self.compute_profile(path) + self._cached_path_id = path_id + idx = min(max(0, index), len(self._cached_profile) - 1) + return float(self._cached_profile[idx]) + + # ------------------------------------------------------------------ + # Internal helpers + # ------------------------------------------------------------------ + + def _compute_curvatures(self, pts: NDArray[np.float64]) -> NDArray[np.float64]: + n = len(pts) + if n < 3: + return np.zeros(n) + + curvatures = np.zeros(n) + + # First point + ds1 = float(np.linalg.norm(pts[1] - pts[0])) + if n > 2: + ds2 = float(np.linalg.norm(pts[2] - pts[1])) + dtheta = self._angle_between(pts[0], pts[1], pts[2]) + if ds1 + ds2 > 1e-6: + curvatures[0] = abs(dtheta) / (ds1 + ds2) + + # Middle points + for i in range(1, n - 1): + d1 = float(np.linalg.norm(pts[i] - pts[i - 1])) + d2 = float(np.linalg.norm(pts[i + 1] - pts[i])) + dtheta = self._angle_between(pts[i - 1], pts[i], pts[i + 1]) + if d1 + d2 > 1e-6: + curvatures[i] = abs(dtheta) / (d1 + d2) + + # Last point + if n > 2: + ds1 = float(np.linalg.norm(pts[-1] - pts[-2])) + ds2 = float(np.linalg.norm(pts[-2] - pts[-3])) + dtheta = self._angle_between(pts[-3], pts[-2], pts[-1]) + if ds1 + ds2 > 1e-6: + curvatures[-1] = abs(dtheta) / (ds1 + ds2) + + return curvatures + + @staticmethod + def _angle_between( + p0: NDArray[np.float64], p1: NDArray[np.float64], p2: NDArray[np.float64] + ) -> float: + v1 = p1 - p0 + v2 = p2 - p1 + n1 = float(np.linalg.norm(v1)) + n2 = float(np.linalg.norm(v2)) + if n1 < 1e-6 or n2 < 1e-6: + return 0.0 + cos_a = float(np.clip(np.dot(v1 / n1, v2 / n2), -1.0, 1.0)) + angle = float(np.arccos(cos_a)) + cross = v1[0] * v2[1] - v1[1] * v2[0] + return -angle if cross < 0 else angle + + def _curvature_speed_limits(self, curvatures: NDArray[np.float64]) -> NDArray[np.float64]: + limits = np.full(len(curvatures), self._max_linear_speed) + mask = curvatures > 1e-6 + if np.any(mask): + limits[mask] = np.minimum( + limits[mask], + np.sqrt(self._max_centripetal_accel / curvatures[mask]), + ) + return limits + + def _acceleration_pass( + self, + pts: NDArray[np.float64], + max_speeds: NDArray[np.float64], + forward: bool, + ) -> NDArray[np.float64]: + v = max_speeds.copy() + a = self._max_linear_accel if forward else self._max_linear_decel + rng = range(1, len(pts)) if forward else range(len(pts) - 2, -1, -1) + for i in rng: + j = i - 1 if forward else i + 1 + ds = float(np.linalg.norm(pts[i] - pts[j])) + if ds > 1e-6: + v[i] = min(v[i], float(np.sqrt(v[j] ** 2 + 2 * a * ds))) + return v + + +__all__ = ["VelocityProfiler"] diff --git a/dimos/control/tasks/velocity_tracking_pid.py b/dimos/control/tasks/velocity_tracking_pid.py new file mode 100644 index 0000000000..74ae871730 --- /dev/null +++ b/dimos/control/tasks/velocity_tracking_pid.py @@ -0,0 +1,171 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Per-channel velocity tracking PID controllers. + +Sits between the path-following controller and the robot hardware. +Ensures that when the outer loop requests vx=0.4 m/s, the robot +actually tracks 0.4 m/s by comparing against odom feedback and +adjusting the command sent to the robot. + +Each channel (vx, vy, wz) has an independent PID with anti-windup. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass +class VelocityPIDConfig: + """PID gains for one velocity channel. + + Start with P-only (Ki=Kd=0), then add I to eliminate steady-state + error, then D if needed for damping. + """ + + kp: float = 1.0 + ki: float = 0.0 + kd: float = 0.0 + max_integral: float = 0.5 # anti-windup clamp + output_min: float = -1.0 + output_max: float = 1.0 + + +@dataclass +class VelocityTrackingConfig: + """Configuration for all three velocity channels.""" + + vx: VelocityPIDConfig = None # type: ignore[assignment] + vy: VelocityPIDConfig = None # type: ignore[assignment] + wz: VelocityPIDConfig = None # type: ignore[assignment] + dt: float = 0.1 # control period (s) + + def __post_init__(self) -> None: + if self.vx is None: + self.vx = VelocityPIDConfig(kp=1.0, ki=0.0, kd=0.0, output_min=-1.0, output_max=1.0) + if self.vy is None: + self.vy = VelocityPIDConfig(kp=1.0, ki=0.0, kd=0.0, output_min=-1.0, output_max=1.0) + if self.wz is None: + self.wz = VelocityPIDConfig(kp=1.0, ki=0.0, kd=0.0, output_min=-1.0, output_max=1.0) + + +class SingleChannelPID: + """PID controller for one velocity channel.""" + + def __init__(self, config: VelocityPIDConfig, dt: float) -> None: + self._cfg = config + self._dt = dt + self._integral = 0.0 + self._prev_error = 0.0 + self._first_call = True + + def reset(self) -> None: + self._integral = 0.0 + self._prev_error = 0.0 + self._first_call = True + + def compute(self, desired: float, actual: float) -> float: + """Compute adjusted command to track desired velocity. + + Args: + desired: Target velocity from outer loop. + actual: Measured velocity from odom. + + Returns: + Adjusted command to send to robot. + """ + error = desired - actual + + # Proportional + p_term = self._cfg.kp * error + + # Integral with anti-windup + self._integral += error * self._dt + self._integral = _clamp(self._integral, -self._cfg.max_integral, self._cfg.max_integral) + i_term = self._cfg.ki * self._integral + + # Derivative (skip on first call to avoid spike) + if self._first_call: + d_term = 0.0 + self._first_call = False + else: + d_term = self._cfg.kd * (error - self._prev_error) / self._dt + + self._prev_error = error + + # Feedforward + PID correction + # The feedforward is the desired value itself - PID corrects the error + output = desired + p_term + i_term + d_term + return _clamp(output, self._cfg.output_min, self._cfg.output_max) + + +class VelocityTrackingPID: + """Three independent PIDs for (vx, vy, wz) velocity tracking. + + Usage: + pid = VelocityTrackingPID(config) + + # Each control tick: + adjusted_vx, adjusted_vy, adjusted_wz = pid.compute( + desired_vx, desired_vy, desired_wz, + actual_vx, actual_vy, actual_wz, + ) + """ + + def __init__(self, config: VelocityTrackingConfig | None = None) -> None: + cfg = config or VelocityTrackingConfig() + self._pid_vx = SingleChannelPID(cfg.vx, cfg.dt) + self._pid_vy = SingleChannelPID(cfg.vy, cfg.dt) + self._pid_wz = SingleChannelPID(cfg.wz, cfg.dt) + + def compute( + self, + desired_vx: float, + desired_vy: float, + desired_wz: float, + actual_vx: float, + actual_vy: float, + actual_wz: float, + ) -> tuple[float, float, float]: + """Compute adjusted commands for all three channels. + + Args: + desired_*: Target velocities from outer loop (Lyapunov controller). + actual_*: Measured velocities from odom. + + Returns: + (adjusted_vx, adjusted_vy, adjusted_wz) to send to robot. + """ + return ( + self._pid_vx.compute(desired_vx, actual_vx), + self._pid_vy.compute(desired_vy, actual_vy), + self._pid_wz.compute(desired_wz, actual_wz), + ) + + def reset(self) -> None: + self._pid_vx.reset() + self._pid_vy.reset() + self._pid_wz.reset() + + +def _clamp(value: float, lo: float, hi: float) -> float: + return max(lo, min(hi, value)) + + +__all__ = [ + "VelocityPIDConfig", + "VelocityTrackingConfig", + "VelocityTrackingPID", +] diff --git a/dimos/control/test_coordinator_ports.py b/dimos/control/test_coordinator_ports.py new file mode 100644 index 0000000000..91ab3cdb6c --- /dev/null +++ b/dimos/control/test_coordinator_ports.py @@ -0,0 +1,108 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The coordinator's path/speed broadcast: ``_on_path`` -> ``set_path`` and +``_on_speed`` -> ``set_speed`` on every task that exposes them.""" + +from __future__ import annotations + +import threading + +from dimos.control.coordinator import ControlCoordinator +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.std_msgs.Float32 import Float32 + + +class _FakeCoord: + """Binds the real coordinator broadcast methods onto a lightweight object so + we exercise their logic without constructing a Module (which spawns threads). + No hardware is wired, so ``_read_base_odom`` returns None.""" + + # Bind the real implementations as methods. + _on_speed = ControlCoordinator._on_speed + _on_path = ControlCoordinator._on_path + _read_base_odom = ControlCoordinator._read_base_odom + + def __init__(self, tasks: dict) -> None: + self._tasks = tasks + self._task_lock = threading.Lock() + self._hardware: dict = {} + self._hardware_lock = threading.Lock() + + +class _StubTask: + """A task exposing set_path/set_speed; records what it received.""" + + def __init__(self, name: str = "stub") -> None: + self.name = name + self.paths: list = [] + self.speeds: list[float] = [] + + def set_path(self, path, odom=None) -> None: + self.paths.append((path, odom)) + + def set_speed(self, speed: float) -> None: + self.speeds.append(speed) + + +class _NoBroadcastTask: + """A task with neither set_path nor set_speed — must be skipped, not error.""" + + name = "no_broadcast" + + +def _path() -> Path: + return Path( + poses=[ + PoseStamped(position=Vector3(0.0, 0.0, 0.0)), + PoseStamped(position=Vector3(1.0, 0.0, 0.0)), + ] + ) + + +def test_on_speed_broadcasts_to_tasks_with_set_speed(): + stub, other = _StubTask(), _NoBroadcastTask() + coord = _FakeCoord({"stub": stub, "other": other}) + coord._on_speed(Float32(data=0.6)) + assert stub.speeds == [0.6] + + +def test_on_path_broadcasts_to_tasks_with_set_path(): + stub = _StubTask() + coord = _FakeCoord({"stub": stub}) + path = _path() + coord._on_path(path) + assert len(stub.paths) == 1 + received_path, received_odom = stub.paths[0] + assert received_path is path + # No hardware wired -> no odom snapshot; set_path still called with None. + assert received_odom is None + + +def test_on_path_falls_back_to_single_arg_set_path(): + """Tasks with single-arg set_path(path) still work (TypeError fallback).""" + received: list = [] + + class _SingleArg: + name = "single" + + def set_path(self, path) -> None: # no odom param + received.append(path) + + coord = _FakeCoord({"single": _SingleArg()}) + path = _path() + coord._on_path(path) + assert received == [path] diff --git a/dimos/msgs/std_msgs/Float32.py b/dimos/msgs/std_msgs/Float32.py new file mode 100644 index 0000000000..f5a508d21c --- /dev/null +++ b/dimos/msgs/std_msgs/Float32.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Float32 message type.""" + +from typing import ClassVar + +from dimos_lcm.std_msgs import Float32 as LCMFloat32 + + +class Float32(LCMFloat32): # type: ignore[misc] + """Float32 message.""" + + msg_name: ClassVar[str] = "std_msgs.Float32" + + def __init__(self, data: float = 0.0) -> None: + """Initialize Float32 with data value.""" + self.data = data diff --git a/dimos/navigation/replanning_a_star/path_distancer.py b/dimos/navigation/replanning_a_star/path_distancer.py index c50583ca33..6dccd46454 100644 --- a/dimos/navigation/replanning_a_star/path_distancer.py +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -25,7 +25,8 @@ class PathDistancer: _path: NDArray[np.float64] _cumulative_dists: NDArray[np.float64] - def __init__(self, path: Path) -> None: + def __init__(self, path: Path, lookahead_dist: float = 0.5) -> None: + self._lookahead_dist = lookahead_dist self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) self._cumulative_dists = _make_cumulative_distance_array(self._path) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index eecbde13f6..d688b8b5d9 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -123,6 +123,8 @@ "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", + "unitree-go2-rpp-benchmark": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_rpp_benchmark:unitree_go2_rpp_benchmark", + "unitree-go2-rpp-controller": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_rpp_controller:unitree_go2_rpp_controller", "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", "unitree-go2-spatial": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_spatial:unitree_go2_spatial", "unitree-go2-temporal-memory": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_temporal_memory:unitree_go2_temporal_memory", @@ -146,6 +148,7 @@ "b-box-navigation-module": "dimos.navigation.bbox_navigation.BBoxNavigationModule", "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", "basic-path-follower": "dimos.navigation.basic_path_follower.module.BasicPathFollower", + "benchmarker": "dimos.utils.benchmarking.benchmark.Benchmarker", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", "click-start-goal-router": "dimos.navigation.cmu_nav.modules.click_start_goal_router.click_start_goal_router.ClickStartGoalRouter", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py new file mode 100644 index 0000000000..00cf9d27c9 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unitree Go2 RPP benchmark — controller + benchmark in ONE launchable. + +A dimos blueprint is a composition of Modules that each run as their own worker +and communicate over the transport. So "two independently-launchable entities +that talk only over LCM" is realized as ONE ``dimos run`` that deploys both +module-sets — the controller (``unitree-go2-rpp-controller``: GO2Connection + +ControlCoordinator + KeyboardTeleop) and the ``Benchmarker``. They are still +fully decoupled: the Benchmarker imports nothing from the controller and only +talks to it over LCM topics — + + Benchmarker.path -> /path -> coordinator arms the follower + Benchmarker.speed -> /speed -> coordinator retunes the follower + Benchmarker.odom <- /go2/odom -- the executed pose + Benchmarker.cmd_vel <- /cmd_vel -- the command sent to the robot + Benchmarker.gate <- /benchmark/gate -- operator advance/skip/quit (teleop) + +(Two separate ``dimos run`` processes can't share an LCM bus — the runtime's +``Coordinator`` RPC service is a per-bus singleton — so one blueprint is the +correct dimos shape. The Benchmarker stays controller-agnostic: point it at a +different controller just by composing a different one here.) + +Run (one terminal). Focus the pygame window: WASD to position the robot, then +ENTER to start each run (K=skip, Backspace=quit). Completion is detected from +odom automatically:: + + dimos run unitree-go2-rpp-benchmark + # afterwards, score offline: + python -m dimos.utils.benchmarking.score data/benchmark/go2 + +To drive paths from some OTHER source instead of the benchmark, launch the +controller alone: ``dimos run unitree-go2-rpp-controller``. +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_rpp_controller import ( + unitree_go2_rpp_controller, +) +from dimos.utils.benchmarking.benchmark import Benchmarker + +# The Benchmarker's ``odom`` In must read the same topic the controller emits leg +# odom on (/go2/odom). path/speed/cmd_vel/gate already share names+topics with +# the controller blueprint, so they wire up by the controller's transports. +_BENCHMARK_TRANSPORTS = { + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), +} + + +# NOTE: the public blueprint var ends in a chained ``.global_config(...)`` (a +# recognized blueprint method) so the AST-based all_blueprints generator +# discovers it. A bare ``X = autoconnect(...)`` is invisible to the generator. +unitree_go2_rpp_benchmark = ( + autoconnect( + unitree_go2_rpp_controller, + Benchmarker.blueprint(robot="go2", gate_source="stream"), + ) + .transports(_BENCHMARK_TRANSPORTS) + .global_config(obstacle_avoidance=False, n_workers=6) +) + +__all__ = ["unitree_go2_rpp_benchmark"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py new file mode 100644 index 0000000000..a9212044a2 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unitree Go2 RPP controller — a standalone, path-following control process. + +GO2Connection + a ControlCoordinator running the calibrated regulated-pure- +pursuit (RPP) follower, plus a KeyboardTeleop for manual positioning. There is +NO planner and NO map here: this blueprint is a controller you feed paths to +from any source over the transport. + +Interface (pure LCM pub/sub — the ONLY coupling to whatever drives it): + + IN path (nav_msgs/Path) -> coordinator.path -> rpp_follower.set_path + IN speed (std_msgs/Float32, m/s)-> coordinator.speed -> rpp_follower.set_speed + OUT odom (geometry_msgs/PoseStamped, /go2/odom) -- the Go2 leg odom + OUT cmd_vel(geometry_msgs/Twist, /cmd_vel) -- aggregated command echo + +Two tasks ship: ``vel_go2`` (priority 20, idle/teleop) and ``rpp_follower`` +(priority 10). The follower is the only task the coordinator's ``set_path`` +broadcast arms, so a published path drives pursuit and nothing else. Holding a +WASD key publishes a Twist on ``/cmd_vel`` (the coordinator's ``twist_command`` +input), driving the priority-20 ``vel_go2`` task and preempting the follower — +that is how the operator repositions/aims the robot between runs. Preemption +aborts the active path, so publish a new path to resume autonomous following. + +KeyboardTeleop also emits a per-run **gate** (``ENTER``/``K``/``Backspace`` -> +advance/skip/quit) on ``/benchmark/gate``, which the benchmark process consumes +to pace its runs. The follower self-calibrates from a vendored tuning artifact +on the first path (feedforward so commanded == achieved, curvature speed +regulation, measured yaw-rate clamp) and runs forward-only (the Go2 lidar faces +forward; no strafe/reverse off the path). + +Run (one of two processes; the benchmark is the other):: + + dimos run unitree-go2-rpp-controller +""" + +from __future__ import annotations + +from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop + +_go2_joints = make_twist_base_joints("go2") + + +unitree_go2_rpp_controller = ( + autoconnect( + GO2Connection.blueprint(), + ControlCoordinator.blueprint( + publish_joint_state=True, + hardware=[ + HardwareComponent( + hardware_id="go2", + hardware_type=HardwareType.BASE, + joints=_go2_joints, + adapter_type="transport_lcm", + ), + ], + tasks=[ + # Idle/teleop velocity task. Priority 20 so a held WASD key + # preempts the priority-10 follower for repositioning. + TaskConfig( + name="vel_go2", + type="velocity", + joint_names=_go2_joints, + priority=20, + params={"zero_on_timeout": False}, + ), + # Sole set_path/set_speed responder. Self-calibrates from the + # vendored artifact on the first path; pursuit knobs are the + # CTE-tuned defaults; forward_only for the forward-facing lidar. + TaskConfig( + name="rpp_follower", + type="rpp_path_follower", + joint_names=_go2_joints, + priority=10, + params={ + "speed": 0.7, + "goal_tolerance": 0.20, + "orientation_tolerance": 0.35, + "k_angular": 1.5, + "lookahead_min": 0.5, + "lookahead_max": 0.7, + "lookahead_speed_scale": 2.0, + "forward_only": True, + }, + ), + ], + ), + # Manual override for positioning the robot between runs + the per-run + # gate the benchmark consumes. publish_only_when_active=True => emits a + # Twist only while a key is held, so it coexists with the follower. + KeyboardTeleop.blueprint(publish_only_when_active=True), + ) + .remappings( + [ + (GO2Connection, "cmd_vel", "go2_cmd_vel"), + (GO2Connection, "odom", "go2_odom"), + ] + ) + .transports( + { + # External command interface: teleop publishes on /cmd_vel; the + # coordinator's twist_command reads it and echoes cmd_vel back. + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + # Base adapter <-> GO2Connection link (drives the robot, reads odom). + ("go2_cmd_vel", Twist): LCMTransport("/go2/cmd_vel", Twist), + ("go2_odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + # Controller IN: planned path + target speed (from any source). + ("path", Path): LCMTransport("/path", Path), + ("speed", Float32): LCMTransport("/speed", Float32), + # Operator gate (teleop -> benchmark) to pace runs. + ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + # Aggregated joint state for observability (positions = [x,y,yaw]). + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("coordinator_joint_state", JointState): LCMTransport( + "/coordinator/joint_state", JointState + ), + } + ) + .global_config(obstacle_avoidance=False) +) + +__all__ = ["unitree_go2_rpp_controller"] diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index 07af844c60..e405457600 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -25,6 +25,14 @@ from dimos.core.stream import Out from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.std_msgs.Int8 import Int8 + +# Gate event codes published on KeyboardTeleop.gate for tools that need +# operator-confirmation per step. Defined in a dependency-free module so offline +# consumers (e.g. the benchmark scorer) don't pull pygame just to read them; +# re-exported here for back-compat with `from keyboard_teleop import GATE_*`. +from dimos.utils.benchmarking.gate import GATE_ADVANCE, GATE_QUIT, GATE_SKIP from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -47,9 +55,22 @@ class KeyboardTeleop(Module): - """Pygame-based keyboard control. Outputs Twist on cmd_vel.""" + """Pygame-based keyboard control. Outputs Twist on cmd_vel. + + Also emits operator "gate" events on ``gate: Out[str]`` for tools + that need to pause for operator confirmation between steps (e.g. the + one-terminal Go2 characterization blueprint). Three keys: + ``ENTER`` -> ``"advance"``, ``K`` -> ``"skip"``, ``Backspace`` -> + ``"quit"``. Existing blueprints that don't wire the ``gate`` port + are unaffected — the events publish into a stream nobody listens + to. + """ cmd_vel: Out[Twist] + gate: Out[Int8] + # Reference-governor corridor half-width (m). Number keys 0-9 map + # to 0.0–0.9 m so an operator can dial precision live during a run. + e_max: Out[Float32] _stop_event: threading.Event _keys_held: set[int] | None = None @@ -65,6 +86,7 @@ def __init__( boost_multiplier: float = DEFAULT_BOOST_MULTIPLIER, slow_multiplier: float = DEFAULT_SLOW_MULTIPLIER, publish_only_when_active: bool = False, + disable_movement: bool = False, **kwargs: Any, ) -> None: super().__init__(**kwargs) @@ -78,6 +100,11 @@ def __init__( # Lets the teleop coexist with another /cmd_vel publisher # (e.g. the SI / benchmark tools) instead of flooding zeros. self.publish_only_when_active = publish_only_when_active + # When True, WASD/QE movement keys are no-ops and the window is a + # pure 0-9 e_max slider. Used by blueprints that drive cmd_vel + # from another source (e.g. nav-stack-driven precision controller) + # but still want the operator's live e_max input. + self.disable_movement = disable_movement self._was_active = False @rpc @@ -133,6 +160,15 @@ def _pygame_loop(self) -> None: elif event.key == pygame.K_ESCAPE: # ESC quits self._stop_event.set() + elif event.key == pygame.K_RETURN: + self.gate.publish(Int8(GATE_ADVANCE)) + elif event.key == pygame.K_k: + self.gate.publish(Int8(GATE_SKIP)) + elif event.key == pygame.K_BACKSPACE: + self.gate.publish(Int8(GATE_QUIT)) + elif pygame.K_0 <= event.key <= pygame.K_9: + # 0 → 0.0 m, 1 → 0.1 m, …, 9 → 0.9 m corridor half-width. + self.e_max.publish(Float32(data=(event.key - pygame.K_0) * 0.1)) elif event.type == pygame.KEYUP: self._keys_held.discard(event.key) @@ -142,23 +178,27 @@ def _pygame_loop(self) -> None: twist.linear = Vector3(0, 0, 0) twist.angular = Vector3(0, 0, 0) - # Forward/backward (W/S) - if pygame.K_w in self._keys_held: - twist.linear.x = self.linear_speed - if pygame.K_s in self._keys_held: - twist.linear.x = -self.linear_speed - - # Strafe left/right (Q/E) - if pygame.K_q in self._keys_held: - twist.linear.y = self.linear_speed - if pygame.K_e in self._keys_held: - twist.linear.y = -self.linear_speed - - # Turning (A/D) - if pygame.K_a in self._keys_held: - twist.angular.z = self.angular_speed - if pygame.K_d in self._keys_held: - twist.angular.z = -self.angular_speed + # Movement keys (WASD/QE) — guarded by disable_movement so the + # window can run as a pure e_max slider (0-9 keys stay live in + # the KEYDOWN handler above). + if not self.disable_movement: + # Forward/backward (W/S) + if pygame.K_w in self._keys_held: + twist.linear.x = self.linear_speed + if pygame.K_s in self._keys_held: + twist.linear.x = -self.linear_speed + + # Strafe left/right (Q/E) + if pygame.K_q in self._keys_held: + twist.linear.y = self.linear_speed + if pygame.K_e in self._keys_held: + twist.linear.y = -self.linear_speed + + # Turning (A/D) + if pygame.K_a in self._keys_held: + twist.angular.z = self.angular_speed + if pygame.K_d in self._keys_held: + twist.angular.z = -self.angular_speed # Apply speed modifiers (Shift = boost, Ctrl = slow) speed_multiplier = 1.0 @@ -229,11 +269,21 @@ def _update_display(self, twist: Twist) -> None: pygame.draw.circle(self._screen, (0, 255, 0), (450, 30), _INDICATOR_RADIUS) y_pos = 280 - help_texts = [ - "WS: Move | AD: Turn | QE: Strafe", - "Shift: Boost | Ctrl: Slow", - "Space: E-Stop | ESC: Quit", - ] + if self.disable_movement: + help_texts = [ + "Movement disabled (e_max slider mode)", + "Space: E-Stop | ESC: Quit", + "Enter: Advance | K: Skip | Backspace: Quit (tools)", + "0-9: e_max corridor (0.0-0.9 m, for RG)", + ] + else: + help_texts = [ + "WS: Move | AD: Turn | QE: Strafe", + "Shift: Boost | Ctrl: Slow", + "Space: E-Stop | ESC: Quit", + "Enter: Advance | K: Skip | Backspace: Quit (tools)", + "0-9: e_max corridor (0.0-0.9 m, for RG)", + ] for text in help_texts: surf = self._font.render(text, True, _HELP_TEXT_COLOR) self._screen.blit(surf, (20, y_pos)) diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py new file mode 100644 index 0000000000..422bbdc1c5 --- /dev/null +++ b/dimos/utils/benchmarking/benchmark.py @@ -0,0 +1,531 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Path-following benchmark — a controller-agnostic publisher/recorder. + +This module knows NOTHING about the controller's internals. It talks to whatever +follows paths purely over the transport: + + OUT path (nav_msgs/Path) -- the reference path for one run + OUT speed (std_msgs/Float32, m/s) -- the target/cruise speed for one run + IN odom (geometry_msgs/PoseStamped) -- the robot's executed pose + IN cmd_vel(geometry_msgs/Twist) -- the command sent to the robot + IN gate (std_msgs/Int8) -- operator advance/skip/quit (hw) + +For each (path, speed) in a fixed battery it: waits for an operator gate (so the +robot can be teleoped into position), anchors the reference path to the robot's +current odom, publishes ``speed`` then ``path``, records incoming odom + cmd_vel, +detects completion **from odom alone** (within a goal tolerance of the path's +last pose AND near-zero velocity for a short dwell, or a per-run timeout), and +writes the run (reference path + executed trace + metadata) to disk as one flat +JSON. It does NOT score inline — scoring is a separate offline step (see +:mod:`dimos.utils.benchmarking.score`). + +Launch as a SEPARATE process from the controller:: + + dimos run unitree-go2-rpp-benchmark # the benchmark + dimos run unitree-go2-rpp-controller # the controller (other terminal) +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import json +import math +from pathlib import Path +import queue +import threading +import time +from typing import Any, Literal + +import numpy as np +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Path import Path as NavPath +from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.utils.benchmarking.gate import GATE_QUIT, GATE_SKIP +from dimos.utils.benchmarking.paths import ( + circle, + rounded_square, + single_corner, + smooth_corner, + square, + straight_line, +) +from dimos.utils.logging_config import setup_logger +from dimos.utils.path_utils import get_project_root + +logger = setup_logger() + +DEFAULT_OUT_DIR = get_project_root() / "data" / "benchmark" + +# Recording schema version — bump if the on-disk JSON shape changes. +RECORDING_SCHEMA = 1 + + +def path_set() -> dict[str, NavPath]: + """Real-space-constrained fixed path battery. + + Sharp corners (single_corner, square) keep the infinite-curvature 90° + geometry; their curved counterparts (smooth_corner, rounded_square) fillet + the vertices so a tracker can hold them at speed. + """ + return { + "straight_line": straight_line(), + "single_corner": single_corner(leg_length=2.0, angle_deg=90.0), + "smooth_corner": smooth_corner(leg_length=2.0, angle_deg=90.0, arc_radius=0.5), + "square": square(side=2.0), + "rounded_square": rounded_square(side=2.0, arc_radius=0.5), + "circle": circle(radius=1.0), + } + + +def shift_path_to_start_at_pose(path: NavPath, start_pose: PoseStamped) -> NavPath: + """Rigid-transform a robot-centric reference path into the odom frame anchored + at the robot's current pose, so the operator only has to roughly aim the + robot. Scoring is then in the executed frame regardless of where the plant + starts.""" + px0, py0 = path.poses[0].position.x, path.poses[0].position.y + pyaw0 = path.poses[0].orientation.euler[2] + sx, sy = start_pose.position.x, start_pose.position.y + dyaw = start_pose.orientation.euler[2] - pyaw0 + cd, sd = math.cos(dyaw), math.sin(dyaw) + new = [] + for p in path.poses: + rx, ry = p.position.x - px0, p.position.y - py0 + new.append( + PoseStamped( + position=Vector3(sx + rx * cd - ry * sd, sy + rx * sd + ry * cd, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, p.orientation.euler[2] + dyaw)), + ) + ) + return NavPath(poses=new) + + +# --------------------------------------------------------------------------- +# Recording (flat per-run JSON) + round-trip +# --------------------------------------------------------------------------- + + +@dataclass +class RunRecording: + """One (path, speed) run: the anchored reference + executed trace + metadata. + + ``reference`` is a list of ``[x, y, yaw]``; ``ticks`` is a list of + ``[t, x, y, yaw, cmd_vx, cmd_vy, cmd_wz]``. The offline scorer rebuilds a + ``Path`` + ``ExecutedTrajectory`` from these and runs ``score_run``. + """ + + robot: str + path: str + speed: float + arrived: bool + reason: str + goal_tolerance: float + velocity_threshold: float + timeout: float + reference: list[list[float]] = field(default_factory=list) + ticks: list[list[float]] = field(default_factory=list) + schema: int = RECORDING_SCHEMA + + @classmethod + def from_path( + cls, + *, + robot: str, + path_name: str, + speed: float, + reference: NavPath, + goal_tolerance: float, + velocity_threshold: float, + timeout: float, + ) -> RunRecording: + ref = [[p.position.x, p.position.y, p.orientation.euler[2]] for p in reference.poses] + return cls( + robot=robot, + path=path_name, + speed=float(speed), + arrived=False, + reason="", + goal_tolerance=goal_tolerance, + velocity_threshold=velocity_threshold, + timeout=timeout, + reference=ref, + ) + + def to_json(self, out_path: str | Path) -> None: + Path(out_path).write_text(json.dumps(self.__dict__, indent=2)) + + @classmethod + def from_json(cls, in_path: str | Path) -> RunRecording: + return cls(**json.loads(Path(in_path).read_text())) + + def reference_path(self) -> NavPath: + """Rebuild the reference ``Path`` (for the offline scorer).""" + return NavPath( + poses=[ + PoseStamped( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + for x, y, yaw in self.reference + ] + ) + + +# --------------------------------------------------------------------------- +# Odom recorder — accumulates ticks; recovers body-frame velocity by diff +# --------------------------------------------------------------------------- + + +class OdomRecorder: + """Subscribe to ``odom`` (and ``cmd_vel``); turn each odom into a tick. + + Body-frame velocity is recovered by pose differentiation (legged-base odom + reports pose, not body velocity), EMA-smoothed (``alpha``). The live body + speed is what the completion monitor reads; the recorded ``cmd_*`` columns + come from the most recent ``cmd_vel``. + """ + + def __init__(self, alpha: float = 0.5) -> None: + self._alpha = alpha + self._lock = threading.Lock() + self._ticks: list[list[float]] = [] + self._t0: float | None = None + self._prev_pose: PoseStamped | None = None + self._prev_t: float | None = None + self._vx = self._vy = self._wz = 0.0 + self._cmd_vx = self._cmd_vy = self._cmd_wz = 0.0 + self._latest_pose: PoseStamped | None = None + + def on_cmd_vel(self, msg: Twist) -> None: + with self._lock: + self._cmd_vx = float(msg.linear.x) + self._cmd_vy = float(msg.linear.y) + self._cmd_wz = float(msg.angular.z) + + def on_odom(self, pose: PoseStamped, now: float | None = None) -> None: + now = time.perf_counter() if now is None else now + with self._lock: + self._latest_pose = pose + if self._t0 is None: + self._t0 = now + t_rel = now - self._t0 + if self._prev_pose is None or self._prev_t is None: + self._prev_pose, self._prev_t = pose, now + else: + dt = now - self._prev_t + if dt > 0: + dx = pose.position.x - self._prev_pose.position.x + dy = pose.position.y - self._prev_pose.position.y + y1 = pose.orientation.euler[2] + dyaw = (y1 - self._prev_pose.orientation.euler[2] + math.pi) % ( + 2 * math.pi + ) - math.pi + c, s = math.cos(y1), math.sin(y1) + bx = (dx / dt) * c + (dy / dt) * s + by = -(dx / dt) * s + (dy / dt) * c + a = self._alpha + self._vx = a * bx + (1 - a) * self._vx + self._vy = a * by + (1 - a) * self._vy + self._wz = a * (dyaw / dt) + (1 - a) * self._wz + self._prev_pose, self._prev_t = pose, now + self._ticks.append( + [ + t_rel, + pose.position.x, + pose.position.y, + pose.orientation.euler[2], + self._cmd_vx, + self._cmd_vy, + self._cmd_wz, + ] + ) + + def body_speed(self) -> tuple[float, float]: + """(linear body speed, |angular|) from the latest differentiation.""" + with self._lock: + return math.hypot(self._vx, self._vy), abs(self._wz) + + def latest_pose(self) -> PoseStamped | None: + with self._lock: + return self._latest_pose + + def snapshot(self) -> list[list[float]]: + with self._lock: + return [list(t) for t in self._ticks] + + def reset(self) -> None: + with self._lock: + self._ticks.clear() + self._t0 = None + self._prev_pose = self._prev_t = None + self._vx = self._vy = self._wz = 0.0 + + def wait_first_pose(self, timeout_s: float, poll_s: float = 0.02) -> PoseStamped: + deadline = time.perf_counter() + timeout_s + while time.perf_counter() < deadline: + pose = self.latest_pose() + if pose is not None: + return pose + time.sleep(poll_s) + raise RuntimeError(f"no odom within {timeout_s:.1f}s") + + +# --------------------------------------------------------------------------- +# Completion monitor — odom-only arrival detection +# --------------------------------------------------------------------------- + + +class CompletionMonitor: + """Declare a run complete from odom alone. + + Tracks forward progress along the reference path (windowed nearest index, so + closed paths don't trip arrival at the start) and declares completion once + the robot has covered ``progress_frac`` of the path, is within + ``goal_tolerance`` of the last pose, and has near-zero linear+angular speed + sustained for ``dwell_s``. + """ + + def __init__( + self, + reference: NavPath, + *, + goal_tolerance: float, + velocity_threshold: float, + angular_threshold: float, + dwell_s: float, + progress_frac: float = 0.7, + window: int = 20, + ) -> None: + self._xy = np.array([[p.position.x, p.position.y] for p in reference.poses], dtype=float) + self._goal = self._xy[-1] + self._n = len(self._xy) + self._goal_tol = goal_tolerance + self._v_thresh = velocity_threshold + self._w_thresh = angular_threshold + self._dwell_s = dwell_s + self._window = window + self._progress_idx = 0 + self._progress_threshold = max(1, int(progress_frac * (self._n - 1))) + self._settled_since: float | None = None + + def update(self, x: float, y: float, lin_speed: float, ang_speed: float, t: float) -> bool: + pos = np.array([x, y]) + lo = self._progress_idx + hi = min(self._n, lo + self._window + 1) + idx = lo + int(np.argmin(np.sum((self._xy[lo:hi] - pos) ** 2, axis=1))) + self._progress_idx = max(self._progress_idx, idx) + + near_goal = float(np.linalg.norm(self._goal - pos)) < self._goal_tol + stopped = lin_speed < self._v_thresh and ang_speed < self._w_thresh + covered = self._progress_idx >= self._progress_threshold + if covered and near_goal and stopped: + if self._settled_since is None: + self._settled_since = t + return (t - self._settled_since) >= self._dwell_s + self._settled_since = None + return False + + +# --------------------------------------------------------------------------- +# Benchmarker module +# --------------------------------------------------------------------------- + + +class BenchmarkerConfig(ModuleConfig): + """Config for :class:`Benchmarker`. + + ``gate_source`` selects how each (path, speed) cell is paced. ``"stream"`` + (default, hardware) waits for the operator's gate (ENTER/skip/quit from the + controller's KeyboardTeleop) so they can reposition the robot first; + ``"auto"`` advances immediately (headless sim / validation). + """ + + robot: str = "go2" + speeds: str = "0.3,0.5,0.7,0.9,1.0" + tolerances: str = "5,10,15" # cm — carried through to the offline scorer + goal_tolerance: float = 0.25 # m — arrival radius around the last pose + velocity_threshold: float = 0.05 # m/s — "stopped" linear-speed gate + angular_threshold: float = 0.10 # rad/s — "stopped" angular-speed gate + settle_dwell_s: float = 0.5 # s within tolerance+stopped before declaring done + timeout: float = 60.0 # s per run before giving up (recorded not-arrived) + odom_warmup_s: float = 10.0 # s to wait for the first odom each run + speed_settle_s: float = 0.3 # s between publishing speed and the path + gate_source: Literal["stream", "auto"] = "stream" + out_dir: str | None = None + + +class Benchmarker(Module): + """Publishes the path battery + speeds, records odom, detects odom-based + completion, and writes per-run recordings. Pure pub/sub — never calls into + the controller.""" + + config: BenchmarkerConfig + + path: Out[NavPath] + speed: Out[Float32] + odom: In[PoseStamped] + cmd_vel: In[Twist] + gate: In[Int8] + + _gate_queue: queue.Queue[int] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._gate_queue = queue.Queue() + self._recorder = OdomRecorder() + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.odom.subscribe(self._recorder.on_odom))) + if self.cmd_vel.transport is not None: + self.register_disposable(Disposable(self.cmd_vel.subscribe(self._recorder.on_cmd_vel))) + if self.config.gate_source == "stream" and self.gate.transport is not None: + self.register_disposable(Disposable(self.gate.subscribe(self._on_gate_event))) + # Run on a background thread so start() returns immediately (the session + # is operator-paced and easily outlives the start RPC's timeout). + threading.Thread(target=self._run, name="benchmarker", daemon=True).start() + + def _on_gate_event(self, msg: Int8) -> None: + self._gate_queue.put(int(msg.data)) + + def _wait_gate(self) -> int: + return self._gate_queue.get() + + # -- the session loop ---------------------------------------------------- + + def _run(self) -> None: + cfg = self.config + speeds = [float(s) for s in cfg.speeds.split(",") if s.strip()] + out_root = Path(cfg.out_dir).expanduser() if cfg.out_dir else DEFAULT_OUT_DIR / cfg.robot + out_root.mkdir(parents=True, exist_ok=True) + + logger.info( + f"Benchmarker: {cfg.robot} speeds={speeds} over {len(path_set())} paths " + f"(gate_source={cfg.gate_source}, out={out_root})" + ) + + idx = 0 + for path_name, path in path_set().items(): + for speed in speeds: + if cfg.gate_source == "stream": + logger.info( + f"[{path_name} v={speed:.2f}] reposition+aim robot, then ENTER " + f"(K=skip, Backspace=quit)" + ) + ev = self._wait_gate() + if ev == GATE_QUIT: + logger.info("operator quit — ending session") + return + if ev == GATE_SKIP: + logger.info(" skipped") + continue + # GATE_ADVANCE falls through to run. + try: + rec = self._run_one(cfg.robot, path_name, path, speed) + except RuntimeError as e: + logger.warning(f"[{path_name} v={speed:.2f}] {e}; skipping") + continue + out_path = out_root / f"{cfg.robot}_{path_name}_v{speed:.2f}_{idx:03d}.json" + rec.to_json(out_path) + idx += 1 + logger.info( + f" [{path_name} v={speed:.2f}] {rec.reason} " + f"(arrived={rec.arrived}, ticks={len(rec.ticks)}) -> {out_path.name}" + ) + logger.info(f"Benchmarker: done — {idx} run(s) recorded under {out_root}") + + def _run_one(self, robot: str, path_name: str, path: NavPath, speed: float) -> RunRecording: + """Publish one (path, speed), record odom until odom-based completion or + timeout, and return the recording.""" + cfg = self.config + pose0 = self._recorder.wait_first_pose(cfg.odom_warmup_s) + path_w = shift_path_to_start_at_pose(path, pose0) + + rec = RunRecording.from_path( + robot=robot, + path_name=path_name, + speed=speed, + reference=path_w, + goal_tolerance=cfg.goal_tolerance, + velocity_threshold=cfg.velocity_threshold, + timeout=cfg.timeout, + ) + + # Fresh trace; publish speed first so the follower arms at this speed, + # then the anchored path (which arms the follower). + self._recorder.reset() + self.speed.publish(Float32(data=float(speed))) + time.sleep(cfg.speed_settle_s) + self.path.publish(path_w) + + monitor = CompletionMonitor( + path_w, + goal_tolerance=cfg.goal_tolerance, + velocity_threshold=cfg.velocity_threshold, + angular_threshold=cfg.angular_threshold, + dwell_s=cfg.settle_dwell_s, + ) + arrived, reason = self._await_completion(monitor, cfg.timeout) + rec.arrived = arrived + rec.reason = reason + rec.ticks = self._recorder.snapshot() + return rec + + def _await_completion(self, monitor: CompletionMonitor, timeout_s: float) -> tuple[bool, str]: + deadline = time.perf_counter() + timeout_s + while time.perf_counter() < deadline: + pose = self._recorder.latest_pose() + if pose is not None: + lin, ang = self._recorder.body_speed() + if monitor.update(pose.position.x, pose.position.y, lin, ang, time.perf_counter()): + return True, "goal+stop" + time.sleep(0.05) + return False, "timeout" + + +def main() -> None: + """Run the benchmark standalone (blueprint-free). Mostly for headless/auto + use; the operator-paced flow is launched via the benchmark blueprint.""" + import argparse + + ap = argparse.ArgumentParser(description="Path-following benchmark (pub/sub)") + ap.add_argument("--robot", default="go2") + ap.add_argument("--speeds", default="0.3,0.5,0.7,0.9,1.0") + ap.add_argument("--gate-source", choices=["stream", "auto"], default="auto") + ap.add_argument("--timeout", type=float, default=60.0) + ap.add_argument("--out", default=None) + args = ap.parse_args() + + Benchmarker( + robot=args.robot, + speeds=args.speeds, + gate_source=args.gate_source, + timeout=args.timeout, + out_dir=args.out, + ).start() + + +if __name__ == "__main__": + main() diff --git a/dimos/utils/benchmarking/gate.py b/dimos/utils/benchmarking/gate.py new file mode 100644 index 0000000000..2fb0522acd --- /dev/null +++ b/dimos/utils/benchmarking/gate.py @@ -0,0 +1,25 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Operator gate event codes — a tiny wire protocol shared by the gate producer +(``KeyboardTeleop``) and consumers (the benchmark). Kept dependency-free so +offline tools (e.g. the scorer) never pull pygame just to read the codes.""" + +from __future__ import annotations + +GATE_ADVANCE = 0 +GATE_SKIP = 1 +GATE_QUIT = 2 + +__all__ = ["GATE_ADVANCE", "GATE_QUIT", "GATE_SKIP"] diff --git a/dimos/utils/benchmarking/paths.py b/dimos/utils/benchmarking/paths.py new file mode 100644 index 0000000000..95562e9799 --- /dev/null +++ b/dimos/utils/benchmarking/paths.py @@ -0,0 +1,482 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Canonical reference path battery for the controller benchmark. + +Every path starts at the origin facing +x in the robot frame. Each +:class:`PoseStamped` waypoint carries the path-tangent yaw at that point. +""" + +from __future__ import annotations + +import math + +from dimos.memory2.vis.space.elements import Point, Polyline, Text +from dimos.memory2.vis.space.space import Space +from dimos.msgs.geometry_msgs.Point import Point as GeoPoint +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Path import Path + +# Plot styling constants for the trajectory renderers below. +_REF_COLOR = "#cccccc" # reference path = light gray +_EXE_COLOR = "#1f77b4" # single-cohort executed path = blue +_START_COLOR = "#2ecc71" # green start marker +_END_COLOR = "#e74c3c" # red end marker +_REF_WIDTH = 0.06 +_EXE_WIDTH = 0.03 +_MARKER_RADIUS = 0.06 + + +def _xy_to_path(executed_xy: list[tuple[float, float]]) -> Path: + """Wrap (x, y) tuples in a nav_msgs.Path so memory2 Polyline can render them.""" + poses = [ + PoseStamped( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), + ) + for x, y in executed_xy + ] + return Path(poses=poses) + + +def _pose(x: float, y: float, yaw: float) -> PoseStamped: + return PoseStamped( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + + +def _path_from_xy(xs: list[float], ys: list[float]) -> Path: + """Build a Path with tangent yaw at each waypoint.""" + n = len(xs) + poses: list[PoseStamped] = [] + for i in range(n): + if i < n - 1: + dx = xs[i + 1] - xs[i] + dy = ys[i + 1] - ys[i] + else: + dx = xs[i] - xs[i - 1] + dy = ys[i] - ys[i - 1] + yaw = math.atan2(dy, dx) + poses.append(_pose(xs[i], ys[i], yaw)) + return Path(poses=poses) + + +# --------------------------------------------------------------------------- +# Path generators +# --------------------------------------------------------------------------- + + +def straight_line(length: float = 5.0, step: float = 0.05) -> Path: + n = round(length / step) + xs = [i * step for i in range(n + 1)] + ys = [0.0] * (n + 1) + return _path_from_xy(xs, ys) + + +def single_corner(leg_length: float = 2.0, angle_deg: float = 90.0, step: float = 0.05) -> Path: + """Two straight legs meeting at one corner. + + Robot starts at origin going +x, drives ``leg_length``, turns by + ``angle_deg`` (left positive), drives another ``leg_length``. + """ + angle = math.radians(angle_deg) + n_leg = round(leg_length / step) + + xs: list[float] = [] + ys: list[float] = [] + for i in range(n_leg + 1): + xs.append(i * step) + ys.append(0.0) + corner_x, corner_y = xs[-1], ys[-1] + cos_a, sin_a = math.cos(angle), math.sin(angle) + for i in range(1, n_leg + 1): + d = i * step + xs.append(corner_x + d * cos_a) + ys.append(corner_y + d * sin_a) + return _path_from_xy(xs, ys) + + +def circle(radius: float = 1.0, n_points: int = 100) -> Path: + """Closed circle, robot starts at origin going +x, curves left. + + Center at (0, ``radius``). Last waypoint coincides with the first. + """ + xs: list[float] = [] + ys: list[float] = [] + for i in range(n_points + 1): + theta = 2.0 * math.pi * i / n_points + xs.append(radius * math.sin(theta)) + ys.append(radius * (1.0 - math.cos(theta))) + return _path_from_xy(xs, ys) + + +def figure_eight(loop_radius: float = 1.0, n_points: int = 200) -> Path: + """Lemniscate of Gerono. + + x(t) = R sin(2t), y(t) = R sin(t), t in [0, 2pi]. + Starts at origin going +x. + """ + xs: list[float] = [] + ys: list[float] = [] + for i in range(n_points + 1): + t = 2.0 * math.pi * i / n_points + xs.append(loop_radius * math.sin(2.0 * t)) + ys.append(loop_radius * math.sin(t)) + return _path_from_xy(xs, ys) + + +def slalom( + cone_spacing: float = 1.0, + lateral_offset: float = 0.5, + n_cones: int = 5, + points_per_cone: int = 20, +) -> Path: + """Smooth slalom past ``n_cones`` cones, alternating sides. + + Cones sit at (i * cone_spacing, +/-lateral_offset). The path is a + sinusoid that crosses the centerline between cones. + """ + total_length = (n_cones + 1) * cone_spacing + n = n_cones * points_per_cone + points_per_cone + xs: list[float] = [] + ys: list[float] = [] + for i in range(n + 1): + x = total_length * i / n + y = lateral_offset * math.sin(math.pi * x / cone_spacing) + xs.append(x) + ys.append(y) + return _path_from_xy(xs, ys) + + +def square(side: float = 2.0, step: float = 0.05) -> Path: + """Closed square. Origin → +x → +y → -x → -y back to origin.""" + n_side = round(side / step) + + xs: list[float] = [] + ys: list[float] = [] + # leg 1: +x + for i in range(n_side + 1): + xs.append(i * step) + ys.append(0.0) + # leg 2: +y + for i in range(1, n_side + 1): + xs.append(side) + ys.append(i * step) + # leg 3: -x + for i in range(1, n_side + 1): + xs.append(side - i * step) + ys.append(side) + # leg 4: -y + for i in range(1, n_side + 1): + xs.append(0.0) + ys.append(side - i * step) + return _path_from_xy(xs, ys) + + +# --------------------------------------------------------------------------- +# Battery registry +# --------------------------------------------------------------------------- + + +def smooth_corner( + leg_length: float = 2.0, + angle_deg: float = 90.0, + arc_radius: float = 0.5, + step: float = 0.05, +) -> Path: + """Two straight legs joined by a finite-radius arc — geometrically tunable. + + Unlike :func:`single_corner` (sharp 90° point with effectively zero radius), + this path replaces the corner with an arc of radius ``arc_radius``. That + gives a well-defined minimum curvature, so geometric tuning methods + (lookahead = 2·R, etc.) can compute an actual answer instead of "infeasible". + """ + angle = math.radians(angle_deg) + n_leg = round(leg_length / step) + cos_a, sin_a = math.cos(angle), math.sin(angle) + + xs: list[float] = [i * step for i in range(n_leg + 1)] + ys: list[float] = [0.0] * (n_leg + 1) + + # Center of the arc: perpendicular to leg 1, at distance arc_radius + cx = xs[-1] + cy = ys[-1] + arc_radius # arc center is to the left for a +90° turn + # Arc starts at (xs[-1], ys[-1]) heading +x (angle from center = -π/2) + # and ends heading at angle `angle_deg` (angle from center = -π/2 + angle) + n_arc = max(2, round(abs(angle) * arc_radius / step)) + for i in range(1, n_arc + 1): + theta_offset = (angle * i / n_arc) - math.pi / 2 + xs.append(cx + arc_radius * math.cos(theta_offset)) + ys.append(cy + arc_radius * math.sin(theta_offset)) + + # Second leg: starts at end of arc, heads in direction `angle` + end_x, end_y = xs[-1], ys[-1] + for i in range(1, n_leg + 1): + d = i * step + xs.append(end_x + d * cos_a) + ys.append(end_y + d * sin_a) + return _path_from_xy(xs, ys) + + +def rounded_square(side: float = 2.0, arc_radius: float = 0.5, step: float = 0.05) -> Path: + """Closed square with filleted corners — the curved counterpart to + :func:`square`. + + Each 90° vertex is replaced by a quarter-circle arc of radius + ``arc_radius``, so the path has bounded curvature (kappa = 1/arc_radius) + everywhere instead of the infinite curvature of a sharp corner. Lets a + trajectory tracker hold the geometry at speed without slowing to a stop + at the vertices. Straight legs are shortened by ``arc_radius`` at each + end to make room for the arcs. Same traversal sense as ``square`` + (origin → +x → +y → -x → -y), starting at the midpoint of the bottom + edge so the start lies on a straight segment. + """ + if arc_radius > side / 2.0: + raise ValueError("arc_radius must be <= side/2") + leg = side - 2.0 * arc_radius # straight portion of each edge + + # Corner centers (inset by arc_radius from each true vertex). + corners = [ + (side - arc_radius, arc_radius), # bottom-right + (side - arc_radius, side - arc_radius), # top-right + (arc_radius, side - arc_radius), # top-left + (arc_radius, arc_radius), # bottom-left + ] + # Outgoing direction of each leg (after the corner before it). + leg_dirs = [(1.0, 0.0), (0.0, 1.0), (-1.0, 0.0), (0.0, -1.0)] + + xs: list[float] = [] + ys: list[float] = [] + n_arc = max(2, round((math.pi / 2.0) * arc_radius / step)) + + # Start at the midpoint of the bottom edge, heading +x. + px, py = side / 2.0, 0.0 + xs.append(px) + ys.append(py) + # First half-leg up to the bottom-right corner arc. + half_leg = (side / 2.0) - arc_radius + for i in range(1, max(1, round(half_leg / step)) + 1): + xs.append(px + i * step) + ys.append(0.0) + + for k in range(4): + cx, cy = corners[k] + # Quarter-circle arc around this corner: start angle points from the + # center back toward the incoming leg, sweeping +90° (left turn). + start_angle = math.atan2(ys[-1] - cy, xs[-1] - cx) + for i in range(1, n_arc + 1): + a = start_angle + (math.pi / 2.0) * i / n_arc + xs.append(cx + arc_radius * math.cos(a)) + ys.append(cy + arc_radius * math.sin(a)) + # Straight leg out to the next corner (or back to start on the last). + dx, dy = leg_dirs[(k + 1) % 4] + ex, ey = xs[-1], ys[-1] + run = leg if k < 3 else half_leg + for i in range(1, max(1, round(run / step)) + 1): + xs.append(ex + i * step * dx) + ys.append(ey + i * step * dy) + return _path_from_xy(xs, ys) + + +def sidestep_1m(distance: float = 1.0, n_points: int = 20) -> Path: + """End up ``distance`` m to the left of start, facing forward. + + Path waypoints sit on a straight line from (0, 0) to (0, distance), all + with yaw=0. Path-followers will interpret this as a goal 90° to the + left — they typically rotate to face it, drive there, then rotate back + to yaw=0 for arrival. Tests off-axis-goal handling more than true + lateral velocity (Go2 has minimal native vy authority over WebRTC). + """ + poses: list[PoseStamped] = [] + for i in range(n_points + 1): + a = i / n_points + poses.append(_pose(0.0, a * distance, 0.0)) + return Path(poses=poses) + + +def short_battery() -> dict[str, Path]: + """3-path battery for the hardware setpoint benchmark. + + Tighter than `default_battery()` — only enough to get a 6-controller + comparison done in 15-20 min of robot time. Exercises: + - ``straight_2m``: trivial forward driving (best-case test). + - ``corner_90``: in-path heading change (steering authority test). + - ``sidestep_1m``: off-axis goal (turn-then-drive test). + """ + return { + "straight_2m": straight_line(length=2.0), + "corner_90": single_corner(leg_length=2.0, angle_deg=90.0), + "sidestep_1m": sidestep_1m(), + } + + +def default_battery() -> dict[str, Path]: + """All canonical paths used for the standard benchmark report.""" + return { + "straight_2m": straight_line(length=2.0), + "straight_5m": straight_line(length=5.0), + "corner_90": single_corner(leg_length=2.0, angle_deg=90.0), + "smooth_corner_R0.5": smooth_corner(leg_length=2.0, angle_deg=90.0, arc_radius=0.5), + "sidestep_1m": sidestep_1m(), + "circle_R0.5": circle(radius=0.5), + "circle_R1.0": circle(radius=1.0), + "circle_R2.0": circle(radius=2.0), + "figure_eight_R1.0": figure_eight(loop_radius=1.0), + "slalom_5cones": slalom(), + "square_2m": square(side=2.0), + } + + +# --------------------------------------------------------------------------- +# SVG rendering (for visual fixtures) +# --------------------------------------------------------------------------- + + +# Cohort palette for multi_trajectory_to_svg overlays. 10 distinct colors so +# the current cohort matrix (10 entries) doesn't have any color collisions. +_COHORT_COLORS = ( + "#1f77b4", # blue + "#d62728", # red + "#2ca02c", # green + "#ff7f0e", # orange + "#9467bd", # purple + "#17becf", # cyan + "#e377c2", # pink + "#8c564b", # brown + "#bcbd22", # olive + "#000000", # black +) + + +def path_to_svg(path: Path, size_px: int = 400, margin_px: int = 20) -> str: + """Render a Path as an SVG polyline via memory2.vis.space. + + ``size_px`` / ``margin_px`` are kept for API compatibility but ignored; + Space picks its own dimensions from world-space content bounds. + """ + if not path.poses: + return Space().to_svg() + + sp = Space() + sp.add(Polyline(msg=path, color="#000000", width=_REF_WIDTH)) + sp.add(Point(msg=path.poses[0], color=_START_COLOR, radius=_MARKER_RADIUS)) + sp.add(Point(msg=path.poses[-1], color=_END_COLOR, radius=_MARKER_RADIUS)) + return sp.to_svg() + + +def trajectory_to_svg( + reference: Path, + executed_xy: list[tuple[float, float]], + size_px: int = 500, + margin_px: int = 20, +) -> str: + """Reference path (gray) + executed trajectory (blue), via memory2.vis.space.""" + if not reference.poses or not executed_xy: + return Space().to_svg() + + sp = Space() + sp.add(Polyline(msg=reference, color=_REF_COLOR, width=_REF_WIDTH)) + sp.add(Polyline(msg=_xy_to_path(executed_xy), color=_EXE_COLOR, width=_EXE_WIDTH)) + sx, sy = executed_xy[0] + ex, ey = executed_xy[-1] + sp.add(Point(msg=GeoPoint(sx, sy, 0.0), color=_START_COLOR, radius=_MARKER_RADIUS)) + sp.add(Point(msg=GeoPoint(ex, ey, 0.0), color=_END_COLOR, radius=_MARKER_RADIUS)) + return sp.to_svg() + + +def multi_trajectory_to_svg( + reference: Path, + cohorts: dict[str, list[tuple[float, float]]], + size_px: int = 600, + margin_px: int = 30, + title: str | None = None, +) -> str: + """Reference + multiple executed trajectories overlaid, via memory2.vis.space. + + Each cohort gets a distinct color from ``_COHORT_COLORS`` (10 unique + entries; no collisions for the current cohort matrix). A small dot at + each cohort's start position helps disambiguate when overlapping lines + converge. The legend is emitted as ``Polyline`` stubs + ``Text`` labels + placed in world space below the plot bounds, so it sits inside the + auto-fit viewBox alongside the trajectories. Axes / grid / tick labels + are drawn by memory2's Space renderer (`show_axes=True`). + """ + if not reference.poses: + return Space().to_svg() + + sp = Space() + sp.add(Polyline(msg=reference, color=_REF_COLOR, width=_REF_WIDTH * 1.4)) + + # Establish bounds for legend placement (below the path) and title (above). + all_ys = [p.position.y for p in reference.poses] + all_xs = [p.position.x for p in reference.poses] + for xy in cohorts.values(): + all_ys.extend(y for _, y in xy) + all_xs.extend(x for x, _ in xy) + y_min = min(all_ys) if all_ys else 0.0 + y_max = max(all_ys) if all_ys else 1.0 + x_min = min(all_xs) if all_xs else 0.0 + + if title: + sp.add(Text(position=(x_min, y_max + 0.3, 0.0), text=title, font_size=14.0)) + + # Cohort polylines + start dots. + for i, (name, xy) in enumerate(cohorts.items()): + color = _COHORT_COLORS[i % len(_COHORT_COLORS)] + if xy: + sp.add(Polyline(msg=_xy_to_path(xy), color=color, width=_EXE_WIDTH)) + sx, sy = xy[0] + sp.add(Point(msg=GeoPoint(sx, sy, 0.0), color=color, radius=_MARKER_RADIUS * 0.7)) + # Legend row (world coords below the plot). + ly = y_min - 0.4 - i * 0.25 + sp.add( + Polyline( + msg=Path( + poses=[ + PoseStamped( + position=Vector3(x_min, ly, 0.0), + orientation=Quaternion.from_euler(Vector3(0, 0, 0)), + ), + PoseStamped( + position=Vector3(x_min + 0.4, ly, 0.0), + orientation=Quaternion.from_euler(Vector3(0, 0, 0)), + ), + ] + ), + color=color, + width=_EXE_WIDTH, + ) + ) + sp.add(Text(position=(x_min + 0.5, ly, 0.0), text=name, font_size=12.0, color=color)) + + return sp.to_svg() + + +__all__ = [ + "circle", + "default_battery", + "figure_eight", + "multi_trajectory_to_svg", + "path_to_svg", + "rounded_square", + "single_corner", + "slalom", + "smooth_corner", + "square", + "straight_line", + "trajectory_to_svg", +] diff --git a/dimos/utils/benchmarking/plant.py b/dimos/utils/benchmarking/plant.py new file mode 100644 index 0000000000..d4420299df --- /dev/null +++ b/dimos/utils/benchmarking/plant.py @@ -0,0 +1,515 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Sim plant + per-robot profile for the twist-base tuning tools. + +Per-channel FOPDT velocity tracking + unicycle kinematics (robot-agnostic; +the ``(vx, vy, wz)`` twist-base contract). Tick-based: each call to +:meth:`TwistBasePlantSim.step` advances one control period. + +The bottom of this module holds the per-robot plant + control config +(``RobotPlantProfile`` + ``ROBOT_PLANT_PROFILES``). The vendored Go2 fit +(``GO2_PLANT_FITTED``) is the Go2 profile's ground truth — it keeps its +``GO2_`` name because it is genuinely Go2-measured data, not generic. +""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass, field +import math + + +@dataclass +class FopdtChannelParams: + """First-order-plus-dead-time params for a single velocity channel. + + Symbols match the characterization fitter: + K - steady-state gain (output / commanded) + tau - first-order time constant (s) + L - pure dead-time (s) + """ + + K: float + tau: float + L: float + + +@dataclass +class AmplitudeFit: + """One FOPDT fit at a specific commanded amplitude. Used to record the + full K(amp), tau(amp), L(amp) tables alongside the canonical single + fit, so future lookup-based controllers can interpolate without re- + collecting data.""" + + amp: float + K: float + tau: float + L: float + r2: float + source: str = "sweep" # "sweep" | "ceiling_probe" + + +@dataclass +class FloorProbeResult: + """Pass/fail of the D3 floor-detection AND-test at one probe amplitude. + + A sample passes when |body_vel| > motion_threshold AND > frac_threshold * + |amp|. Floor = lowest amp where the test is sustained for N consecutive + samples within the step window.""" + + amp: float + motion_detected: bool + sustained_samples: int # longest contiguous run of passing samples + net_displacement: float = 0.0 # signed body-frame displacement in cmd dir + + +@dataclass +class ChannelEnvelope: + """Measured speed envelope for a single channel. + + ``ceiling`` is the operational ``min(max(K·amp), envelope_cap)`` value + that DERIVE feeds into RG's v_max / ω_max. ``saturating_at_amp`` is a + forensic-only diagnostic (lowest amp where K dropped 15% below the + linear-regime K) and is NOT used as the operational cap.""" + + floor: float + ceiling: float + floor_not_found: bool = False + ceiling_not_found: bool = False + saturating_at_amp: float | None = None + + +@dataclass +class VelocityEnvelope: + """Per-channel measured floor/ceiling — m/s for vx/vy, rad/s for wz. + + Saturation is not a dynamics parameter so this is a separate section + in the artifact (not folded into the FOPDT plant).""" + + vx: ChannelEnvelope + vy: ChannelEnvelope + wz: ChannelEnvelope + + +class FOPDTChannel: + """First-order lag + dead-time for one velocity axis. + + Tick-based: feed one commanded value per :meth:`step` call, get the + delayed/lagged actual velocity back. + """ + + def __init__(self, params: FopdtChannelParams) -> None: + self.params = params + self._delay_buf: deque[float] = deque() + self._delay_samples = 0 + self._y = 0.0 + + def reset(self, dt: float) -> None: + # step() appends before reading the head, so a buffer of N slots + # delays by N-1 ticks; size for int(L/dt) ticks of dead time. + self._delay_samples = max(1, int(self.params.L / dt) + 1) + self._delay_buf = deque([0.0] * self._delay_samples, maxlen=self._delay_samples) + self._y = 0.0 + + def step(self, u: float, dt: float) -> float: + self._delay_buf.append(u) + u_delayed = self._delay_buf[0] + alpha = dt / (self.params.tau + dt) + self._y += alpha * (self.params.K * u_delayed - self._y) + return self._y + + +@dataclass +class TwistBasePlantParams: + """FOPDT params for the three twist-base velocity channels.""" + + vx: FopdtChannelParams + vy: FopdtChannelParams + wz: FopdtChannelParams + + +class CommandLimiter: + """Ruckig-style per-axis velocity + acceleration limiter in COMMAND units. + + Mirrors the limiter that lives inside the FlowBase firmware, applied to + the commanded twist BEFORE the plant dynamics. Trajectories planned + within the physical margins never engage it; the sim reproduces the + saturation behavior when they do. + """ + + def __init__(self, max_vel: tuple[float, float, float], max_acc: tuple[float, float, float]): + self.max_vel = max_vel + self.max_acc = max_acc + self._prev = [0.0, 0.0, 0.0] + + def reset(self) -> None: + self._prev = [0.0, 0.0, 0.0] + + def step(self, cmds: tuple[float, float, float], dt: float) -> tuple[float, float, float]: + out = [] + for i, cmd in enumerate(cmds): + target = max(-self.max_vel[i], min(self.max_vel[i], cmd)) + dv_max = self.max_acc[i] * dt + dv = max(-dv_max, min(dv_max, target - self._prev[i])) + self._prev[i] += dv + out.append(self._prev[i]) + return out[0], out[1], out[2] + + +class TwistBasePlantSim: + """Unicycle kinematic sim with FOPDT velocity response per channel. + + Body-frame velocities `(vx, vy, wz)` are commanded; the plant produces + actual velocities (filtered + delayed) that drive a unicycle integrator + in the world frame. An optional ``limiter`` clamps the commands first + (command units), reproducing a firmware-side rate limiter. + """ + + def __init__(self, params: TwistBasePlantParams, limiter: CommandLimiter | None = None) -> None: + self.params = params + self.limiter = limiter + self.ch_vx = FOPDTChannel(params.vx) + self.ch_vy = FOPDTChannel(params.vy) + self.ch_wz = FOPDTChannel(params.wz) + self.x = 0.0 + self.y = 0.0 + self.yaw = 0.0 + self.vx = 0.0 + self.vy = 0.0 + self.wz = 0.0 + + def reset(self, x: float, y: float, yaw: float, dt: float) -> None: + self.x, self.y, self.yaw = x, y, yaw + self.vx = self.vy = self.wz = 0.0 + for ch in (self.ch_vx, self.ch_vy, self.ch_wz): + ch.reset(dt) + if self.limiter is not None: + self.limiter.reset() + + def step(self, cmd_vx: float, cmd_vy: float, cmd_wz: float, dt: float) -> None: + if self.limiter is not None: + cmd_vx, cmd_vy, cmd_wz = self.limiter.step((cmd_vx, cmd_vy, cmd_wz), dt) + self.vx = self.ch_vx.step(cmd_vx, dt) + self.vy = self.ch_vy.step(cmd_vy, dt) + self.wz = self.ch_wz.step(cmd_wz, dt) + + self.x += (self.vx * math.cos(self.yaw) - self.vy * math.sin(self.yaw)) * dt + self.y += (self.vx * math.sin(self.yaw) + self.vy * math.cos(self.yaw)) * dt + self.yaw = (self.yaw + self.wz * dt + math.pi) % (2 * math.pi) - math.pi + + +# --- Vendored fitted FOPDT plant for the Go2 base ------------------------ +# +# Source: concrete surface, normal/default mode, data collected +# 2026-05-07, fitted by the characterization pipeline. RISE tau/L +# corrected 2026-05-16: an earlier pooled fit was degenerate (tau pinned +# at the solver lower bound with all lag collapsed into L); a per-run +# re-fit of the same raw E1/E2 data (median over converged forward +# trials, fresh-fit r2=0.92 vx / 0.82 wz) gives the true structure — +# small dead-time (L ~ 0.05-0.07 s), larger tau (vx ~ 0.40, +# wz ~ 0.55-0.60 s). K is unchanged (independently validated). +# +# This is the Go2 profile's sim ground truth (self-test recovers it; the +# sim adapter / DERIVE fallback use it). It keeps its GO2_ name because +# it is genuinely Go2-measured data. vy is a placeholder copy of vx until +# vy is characterized on hardware — the Go2 DOES strafe (it just needs a +# higher floor amplitude than vx to start), so vy is now excited in the +# sweep; this sim FOPDT just has no measured lateral model yet. + +GO2_VX_RISE = FopdtChannelParams(K=0.922, tau=0.395, L=0.065) +GO2_WZ_RISE = FopdtChannelParams(K=2.453, tau=0.596, L=0.052) + +GO2_PLANT_FITTED = TwistBasePlantParams( + vx=GO2_VX_RISE, + vy=GO2_VX_RISE, # placeholder; Go2 doesn't strafe in default gait + wz=GO2_WZ_RISE, +) + + +# --- Vendored fitted FOPDT plant for the FlowBase ------------------------ +# +# Source: concrete surface, default mode, real FlowBase over LCM SI, +# characterized 2026-06-09 (git 704a591f5, methodology v2, 17 fit points). +# Artifact: data/characterization/flowbase/ +# flowbase_config_hw_concrete_2026-06-09_704a591f5.json +# +# K is actuation-side (the robot genuinely moves K x the command; odometry +# is honest) — caused by a kinematic inconsistency in the sealed firmware. +# All compensation lives controller-side. + +FLOWBASE_VX_FIT = FopdtChannelParams(K=0.778, tau=0.288, L=0.010) +FLOWBASE_VY_FIT = FopdtChannelParams(K=0.773, tau=0.267, L=0.033) +FLOWBASE_WZ_FIT = FopdtChannelParams(K=2.929, tau=0.607, L=0.017) + +FLOWBASE_PLANT_FITTED = TwistBasePlantParams( + vx=FLOWBASE_VX_FIT, + vy=FLOWBASE_VY_FIT, + wz=FLOWBASE_WZ_FIT, +) + +# FlowBase firmware Ruckig limiter, COMMAND units (x, y, yaw). Physical +# limits are K x these. max_vel == max_accel in the firmware config. +FLOWBASE_CMD_MAX_VEL: tuple[float, float, float] = (0.8, 0.8, 3.0) +FLOWBASE_CMD_MAX_ACC: tuple[float, float, float] = FLOWBASE_CMD_MAX_VEL + + +def flowbase_command_limiter() -> CommandLimiter: + """Limiter matching the FlowBase firmware's Ruckig config.""" + return CommandLimiter(max_vel=FLOWBASE_CMD_MAX_VEL, max_acc=FLOWBASE_CMD_MAX_ACC) + + +# --- Per-robot profile (single source of truth for robot specifics) ----- + + +@dataclass(frozen=True) +class RobotPlantProfile: + """Everything the characterization + benchmark tools need to know + about a specific velocity-commanded twist base: the FOPDT plant and + the control-loop knobs that surround it. Add a robot by appending + one instance to ``ROBOT_PLANT_PROFILES``. + + The tuning tools talk to the operator's running coord on two + well-known LCM topics (the contract every coord blueprint already + honors): + + * publish Twist on ``/cmd_vel`` (the coord's ``twist_command`` In) + * subscribe JointState on ``/coordinator/joint_state`` (the + coord's published Out — joint *positions* carry ``[x, y, yaw]`` + because ``positions = adapter.read_odometry()``; see + :class:`~dimos.control.hardware_interface.ConnectedTwistBase`). + + ``robot_id`` is provenance/cosmetic. ``joint_prefix`` is what the + operator coord's hardware uses for joint names — Go2's coord uses + ``go2/{vx,vy,wz}``, FlowBase's uses ``base/{vx,vy,wz}`` — so the + tool knows which positions to pick out of joint_state. + """ + + # identity / cosmetic + name: str + robot_id: str # provenance + artifact filename + # transport / bring-up + blueprint: str # the `dimos run ` the operator starts (hw) + sim_blueprint: str # the `dimos run ` for sim + # joint name prefix the operator coord's hardware uses. Defaults to + # robot_id; set explicitly when the coord blueprint uses a different + # prefix (e.g. flowbase coord uses "base/..."). + joint_prefix: str | None = None + # physical envelope + vx_max: float = 1.0 + wz_max: float = 1.5 + tick_rate_hz: float = 10.0 + odom_warmup_s: float = 10.0 + odom_stale_s: float = 1.0 + # SI plan / kinematics + excited_channels: tuple[str, ...] = ("vx", "wz") # omit vy => non-strafing + si_amplitudes: dict[str, list[float]] = field( + default_factory=lambda: {"vx": [0.3, 0.6, 0.9], "vy": [0.2, 0.4], "wz": [0.4, 0.8, 1.2]} + ) + # Floor / ceiling probe ladders (methodology v2 — densification). + # Floor probe runs FIRST per channel: tiny amplitudes to find the + # smallest commanded value that produces actual robot motion (the D3 + # AND-test in characterization.py). Ceiling probe runs LAST: + # supra-sweep amplitudes to detect the K-sag onset (saturation). + floor_probe_amplitudes: dict[str, list[float]] = field( + default_factory=lambda: { + "vx": [0.02, 0.05, 0.10, 0.15], + "vy": [0.1, 0.15, 0.20], + "wz": [0.05, 0.10, 0.20, 0.30], + } + ) + # Ascending floor search: when the predefined floor_probe_amplitudes + # ladder is exhausted without detected motion, keep probing at + # last_amp + floor_probe_step[channel] until motion is found or the + # amplitude exceeds floor_probe_max[channel] (safety cap). Only when + # the cap is reached without motion is floor_not_found set. + floor_probe_step: dict[str, float] = field( + default_factory=lambda: {"vx": 0.05, "vy": 0.05, "wz": 0.10} + ) + floor_probe_max: dict[str, float] = field( + default_factory=lambda: {"vx": 0.5, "vy": 0.5, "wz": 1.0} + ) + ceiling_probe_amplitudes: dict[str, list[float]] = field( + default_factory=lambda: {"vx": [2.5, 3.0], "vy": [1.5, 2.0], "wz": [2.5, 3.0]} + ) + # Floor D3 thresholds. AND of (absolute motion above noise floor) and + # (fractional response above tracking noise), sustained for N samples. + floor_motion_threshold: float = 0.02 # m/s (vx/vy) or rad/s (wz) + floor_fractional_threshold: float = 0.05 # |v_body| > 5% of |amp| + floor_sustained_samples: int = 5 + # Minimum NET signed displacement (commanded direction) over the probe + # window to count as real translation. Rejects net-zero posture wobble + # whose |v| spikes but whose integral cancels. m for vx/vy, rad for wz. + floor_displacement_threshold: dict[str, float] = field( + default_factory=lambda: {"vx": 0.05, "vy": 0.05, "wz": 0.10} + ) + # Ceiling K-sag: |K(amp)| drops below (1 - sag) * K_linear -> saturated. + ceiling_k_sag_threshold: float = 0.15 + step_s: float = 8.0 + pre_roll_s: float = 1.0 + max_dist_m: float = 6.0 + # Hard manual floor — if a profile sets this >0, DERIVE will not let + # the measured floor go below it. Off by default (use measured). + min_speed_floor: float = 0.0 + # Sim ground truth: drives the sim blueprint's FOPDT plant + the + # characterization self-test path + DERIVE ceiling fallback. + sim_plant: TwistBasePlantParams = field(default_factory=lambda: GO2_PLANT_FITTED) + + @property + def joints_prefix(self) -> str: + return self.joint_prefix if self.joint_prefix is not None else self.robot_id + + +GO2_PLANT_PROFILE = RobotPlantProfile( + name="Go2", + robot_id="go2", + blueprint="unitree-go2-webrtc-keyboard-teleop", + sim_blueprint="coordinator-sim-fopdt", + joint_prefix="go2", # unitree_go2_coordinator uses make_twist_base_joints("go2") + vx_max=1.0, + wz_max=1.5, + tick_rate_hz=10.0, + odom_warmup_s=10.0, + odom_stale_s=1.0, + excited_channels=("vx", "vy", "wz"), + # Densified sweep (methodology v2): unified numeric ladder across + # channels (vx/vy in m/s, wz in rad/s). vy data on Go2 is expected + # noisier — Go2 doesn't strafe natively — but we collect it anyway + # so future work has something to look at. + si_amplitudes={ + "vx": [0.2, 0.5, 1.0, 1.5, 2.0], + "vy": [0.2, 0.5, 1.0, 1.5, 2.0], + "wz": [0.2, 0.5, 1.0, 1.5, 2.0], + }, + floor_probe_amplitudes={ + "vx": [0.02, 0.05, 0.10, 0.15], + "vy": [0.02, 0.05, 0.10], + "wz": [0.05, 0.10, 0.20, 0.30], + }, + # Go2: vx true floor ~0.2 (ladder tops at 0.15); step past it in 0.05 + # increments up to 0.5 m/s before declaring floor_not_found. wz in + # 0.10 rad/s increments up to 1.0 rad/s. + floor_probe_step={"vx": 0.05, "vy": 0.05, "wz": 0.10}, + floor_probe_max={"vx": 0.5, "vy": 0.5, "wz": 1.0}, + ceiling_probe_amplitudes={"vx": [2.5, 3.0], "vy": [1.5, 2.0], "wz": [2.5, 3.0]}, + # Net-displacement floor gate. A genuine step at the true Go2 floor + # (~0.1 m/s held a couple seconds) covers >=0.2 m. Go2 odom can drift + # ~0.07 m over a window with NO real translation, so require 0.10 m / + # 0.10 rad net to count as motion (above odom drift, below real travel). + floor_displacement_threshold={"vx": 0.20, "vy": 0.10, "wz": 0.10}, + step_s=8.0, + pre_roll_s=1.0, + max_dist_m=6.0, + sim_plant=GO2_PLANT_FITTED, +) + +# FlowBase (holonomic Portal-RPC twist base). Operator-side blueprint: +# the existing `coordinator-flowbase-keyboard-teleop` already publishes +# /coordinator/joint_state with positions=[x,y,yaw] from the flowbase +# adapter's read_odometry. No new blueprint, no bridge, no Connection +# module needed — just this profile entry. +# +# The vy channel is excited (FlowBase strafes natively) so vy is in the +# excited_channels tuple. sim_plant is the vendored 2026-06-09 hw fit. +FLOWBASE_PLANT_PROFILE = RobotPlantProfile( + name="FlowBase", + robot_id="flowbase", + blueprint="coordinator-flowbase-keyboard-teleop", + sim_blueprint="coordinator-sim-fopdt-flowbase", + joint_prefix="base", # coordinator_flowbase uses make_twist_base_joints("base") + vx_max=0.8, + wz_max=1.2, + tick_rate_hz=10.0, + odom_warmup_s=10.0, + odom_stale_s=1.0, + excited_channels=("vx", "vy", "wz"), # holonomic — strafes + # Densified sweep (2026-06-09): ~5-6 fit points/axis (was the sparse + # placeholder [0.2,0.4,0.6]/[0.2,0.4]/[0.3,0.6,1.0] = 8 total fits, vs Go2's + # 15). Stays WITHIN the already-tested envelope — vx/vy ≤0.6 (achieved + # ceiling ~0.63 m/s), wz ≤1.0 rad/s — so this is denser, NOT faster (same + # speeds + risk as the prior run). Extending the RANGE to find the real top + # speed is separate: raise vx_max after the i2rt firmware-limit check. + si_amplitudes={ + "vx": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6], + "vy": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6], + "wz": [0.2, 0.4, 0.6, 0.8, 1.0], + }, + step_s=6.0, + pre_roll_s=1.0, + max_dist_m=4.0, + sim_plant=FLOWBASE_PLANT_FITTED, +) + +# Conservative SHAKEOUT profile for the FIRST on-hardware run of the tall, +# top-heavy FlowBase: low speed caps (0.3 m/s, 0.5 rad/s), gentle amplitudes, +# short 1 m / 5 s steps (wz never completes a full spin). Use +# `--robot flowbase-slow` to validate direction / mast stability / e-stop / +# wiring at a snail's pace. The fit it produces is LOW-SPEED ONLY (truncated +# envelope) — re-run `--robot flowbase` for the real full-envelope fit once the +# bot is trusted. All amplitudes stay below the caps so nothing is clamped +# (clamped steps would corrupt the K fit). Leaves the canonical profile intact. +FLOWBASE_SLOW_PLANT_PROFILE = RobotPlantProfile( + name="FlowBase (slow shakeout)", + robot_id="flowbase_slow", + blueprint="coordinator-flowbase-keyboard-teleop", + sim_blueprint="coordinator-sim-fopdt-flowbase", + joint_prefix="base", + vx_max=0.3, + wz_max=0.5, + tick_rate_hz=10.0, + odom_warmup_s=10.0, + odom_stale_s=1.0, + excited_channels=("vx", "vy", "wz"), + si_amplitudes={"vx": [0.1, 0.2], "vy": [0.1, 0.2], "wz": [0.15, 0.3]}, + floor_probe_amplitudes={"vx": [0.05, 0.1], "vy": [0.1], "wz": [0.1, 0.2]}, + ceiling_probe_amplitudes={"vx": [0.3], "vy": [0.2], "wz": [0.45]}, + step_s=5.0, + pre_roll_s=1.0, + max_dist_m=1.0, + sim_plant=GO2_PLANT_FITTED, +) + +ROBOT_PLANT_PROFILES: dict[str, RobotPlantProfile] = { + "go2": GO2_PLANT_PROFILE, + "flowbase": FLOWBASE_PLANT_PROFILE, + "flowbase-slow": FLOWBASE_SLOW_PLANT_PROFILE, +} + + +__all__ = [ + "FLOWBASE_CMD_MAX_ACC", + "FLOWBASE_CMD_MAX_VEL", + "FLOWBASE_PLANT_FITTED", + "FLOWBASE_PLANT_PROFILE", + "FLOWBASE_SLOW_PLANT_PROFILE", + "FLOWBASE_VX_FIT", + "FLOWBASE_VY_FIT", + "FLOWBASE_WZ_FIT", + "GO2_PLANT_FITTED", + "GO2_PLANT_PROFILE", + "GO2_VX_RISE", + "GO2_WZ_RISE", + "ROBOT_PLANT_PROFILES", + "AmplitudeFit", + "ChannelEnvelope", + "CommandLimiter", + "FOPDTChannel", + "FloorProbeResult", + "FopdtChannelParams", + "RobotPlantProfile", + "TwistBasePlantParams", + "TwistBasePlantSim", + "VelocityEnvelope", + "flowbase_command_limiter", +] diff --git a/dimos/utils/benchmarking/score.py b/dimos/utils/benchmarking/score.py new file mode 100644 index 0000000000..5311ec9a9f --- /dev/null +++ b/dimos/utils/benchmarking/score.py @@ -0,0 +1,284 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Offline scoring for path-following benchmark recordings. + +A SEPARATE step from acquisition: the benchmark (``benchmark.py``) records each +run as a flat per-run JSON; this reads those recordings, scores each geometrically +against its reference path (``score_run``), and emits the operating-point map + +tolerance->max-safe-speed inversion in the existing JSON metric format. + + python -m dimos.utils.benchmarking.score [--tolerances 5,10,15] +""" + +from __future__ import annotations + +import argparse +from dataclasses import asdict +import json +import math +from pathlib import Path + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.benchmarking.benchmark import RunRecording +from dimos.utils.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run +from dimos.utils.benchmarking.tuning import OperatingPoint, OperatingPointMap, invert_tolerance +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _executed_from_recording(rec: RunRecording) -> ExecutedTrajectory: + ticks: list[TrajectoryTick] = [] + for t, x, y, yaw, cvx, cvy, cwz in rec.ticks: + ticks.append( + TrajectoryTick( + t=t, + pose=PoseStamped( + ts=t, + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ), + cmd_twist=Twist(linear=Vector3(cvx, cvy, 0.0), angular=Vector3(0.0, 0.0, cwz)), + actual_twist=Twist(), + ) + ) + return ExecutedTrajectory(ticks=ticks, arrived=rec.arrived) + + +def _looks_like_recording(data: object) -> bool: + """A run recording has a trace + a reference + the schema tag. Other JSONs in + the directory (old operating-point maps, this scorer's own output, etc.) lack + these and are skipped silently.""" + return isinstance(data, dict) and {"ticks", "reference", "schema"} <= data.keys() + + +def load_recordings(recordings_dir: str | Path) -> list[RunRecording]: + """Load every run-recording ``*.json`` in ``recordings_dir`` (sorted), + quietly skipping any non-recording JSON files that share the directory.""" + d = Path(recordings_dir) + recs: list[RunRecording] = [] + skipped = 0 + for f in sorted(d.glob("*.json")): + try: + data = json.loads(f.read_text()) + except json.JSONDecodeError: + skipped += 1 + continue + if not _looks_like_recording(data): + skipped += 1 + continue + try: + recs.append(RunRecording(**data)) + except TypeError: + skipped += 1 + if skipped: + logger.info(f"skipped {skipped} non-recording JSON file(s) in {d}") + return recs + + +def score_recordings( + recs: list[RunRecording], tolerances_cm: list[float] +) -> tuple[OperatingPointMap, list[dict]]: + """Score every recording into an operating-point map + per-run diagnostics.""" + points: list[OperatingPoint] = [] + runs: list[dict] = [] + for rec in recs: + ref = rec.reference_path() + executed = _executed_from_recording(rec) + s = score_run(ref, executed) + points.append( + OperatingPoint( + path=rec.path, + speed=rec.speed, + cte_max=s.cte_max, + cte_rms=s.cte_rms, + arrived=s.arrived, + ) + ) + runs.append( + { + "path": rec.path, + "speed": rec.speed, + "cte_max": s.cte_max, + "cte_rms": s.cte_rms, + "arrived": s.arrived, + "reason": rec.reason, + "ref": [(p[0], p[1]) for p in rec.reference], + "exec": [(tk[1], tk[2]) for tk in rec.ticks], + } + ) + speeds = sorted({p.speed for p in points}) + inversion = invert_tolerance(points, tolerances_cm) + return OperatingPointMap(speeds=speeds, points=points, tolerance_inversion=inversion), runs + + +# --------------------------------------------------------------------------- +# Diagnostic plots (optional; reused from the prior inline benchmark) +# --------------------------------------------------------------------------- + + +def _plot_cte_vs_speed(points: list[OperatingPoint], out: Path, robot: str) -> None: + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(7, 4.5)) + for name in sorted({p.path for p in points}): + xs = [p.speed for p in points if p.path == name] + ys = [p.cte_max * 100 for p in points if p.path == name] + ax.plot(xs, ys, marker="o", label=name) + ax.set_xlabel("commanded speed (m/s)") + ax.set_ylabel("cte_max (cm)") + ax.set_title(f"{robot}: cross-track error vs speed") + ax.grid(True, alpha=0.3) + ax.legend() + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) + + +def _canonicalize(ref: list, exec_: list) -> tuple[list, list]: + """Rigid-transform a run into the canonical path frame: reference start -> + (0,0), initial heading -> +x; same transform on the executed trajectory.""" + if len(ref) < 2: + return ref, exec_ + ox, oy = ref[0] + th = 0.0 + for px, py in ref[1:]: + if math.hypot(px - ox, py - oy) > 1e-6: + th = math.atan2(py - oy, px - ox) + break + c, s = math.cos(-th), math.sin(-th) + + def tf(pts): + return [((x - ox) * c - (y - oy) * s, (x - ox) * s + (y - oy) * c) for x, y in pts] + + return tf(ref), tf(exec_) + + +def _plot_xy(runs: list[dict], out: Path, robot: str) -> None: + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + if not runs: + return + paths = list(dict.fromkeys(r["path"] for r in runs)) + n = len(paths) + cols = min(n, 2) + rows = (n + cols - 1) // cols + fig, axes = plt.subplots(rows, cols, figsize=(6.0 * cols, 5.0 * rows), squeeze=False) + flat = [ax for row in axes for ax in row] + for ax, name in zip(flat, paths, strict=False): + prs = [r for r in runs if r["path"] == name] + ref_drawn = False + for r in prs: + ref_c, ex_c = _canonicalize(r["ref"], r["exec"]) + if not ref_drawn: + ax.plot( + [p[0] for p in ref_c], [p[1] for p in ref_c], "k-", lw=2.0, label="reference" + ) + ax.plot(0.0, 0.0, "ko", ms=5) + ref_drawn = True + if not ex_c: + continue + ax.plot( + [p[0] for p in ex_c], + [p[1] for p in ex_c], + lw=1.3, + label=f"v={r['speed']:g} (cte_max={r['cte_max'] * 100:.0f}cm" + f"{'' if r['arrived'] else ', NOT arrived'})", + ) + ax.set_title(name) + ax.set_xlabel("x (m)") + ax.set_ylabel("y (m)") + ax.set_aspect("equal", adjustable="datalim") + ax.grid(True, alpha=0.3) + ax.legend(fontsize=7) + for ax in flat[n:]: + ax.set_visible(False) + fig.suptitle(f"{robot}: executed trajectory vs reference path") + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) + + +def score_dir( + recordings_dir: str | Path, + *, + tolerances_cm: list[float], + out_path: str | Path | None = None, + plots: bool = True, +) -> OperatingPointMap: + """Score a directory of recordings; write the metric JSON (+ plots) beside it.""" + recs = load_recordings(recordings_dir) + if not recs: + raise SystemExit(f"no run recordings found in {recordings_dir}") + opm, runs = score_recordings(recs, tolerances_cm) + + d = Path(recordings_dir) + robot = recs[0].robot + out_json = Path(out_path) if out_path else d / f"{robot}_benchmark_scores.json" + out_json.write_text(json.dumps(asdict(opm), indent=2)) + logger.info(f"scored {len(recs)} run(s) -> {out_json}") + for row in opm.tolerance_inversion: + if row.max_speed is None: + logger.info( + f" tolerance {row.tol_cm:g} cm: NO tested speed keeps every path within tolerance" + ) + else: + logger.info( + f" tolerance {row.tol_cm:g} cm: run at {row.max_speed:.2f} m/s " + f"(binding path: {row.binding_path})" + ) + + if plots: + try: + _plot_cte_vs_speed(opm.points, d / f"{robot}_benchmark_cte_vs_speed.png", robot) + _plot_xy(runs, d / f"{robot}_benchmark_xy.png", robot) + logger.info(f" plots -> {d}/{robot}_benchmark_*.png") + except Exception as e: # plotting is best-effort + logger.warning(f"plotting failed: {e}") + return opm + + +def main() -> None: + ap = argparse.ArgumentParser(description="Score path-following benchmark recordings") + ap.add_argument("recordings_dir", help="directory of per-run *.json recordings") + ap.add_argument("--tolerances", default="5,10,15", help="cm, comma-separated") + ap.add_argument( + "--out", + default=None, + help="output JSON path (default: /_benchmark_scores.json)", + ) + ap.add_argument("--no-plots", action="store_true") + args = ap.parse_args() + + tolerances = [float(t) for t in args.tolerances.split(",") if t.strip()] + score_dir( + args.recordings_dir, + tolerances_cm=tolerances, + out_path=args.out, + plots=not args.no_plots, + ) + + +if __name__ == "__main__": + main() diff --git a/dimos/utils/benchmarking/scoring.py b/dimos/utils/benchmarking/scoring.py new file mode 100644 index 0000000000..2d89bca500 --- /dev/null +++ b/dimos/utils/benchmarking/scoring.py @@ -0,0 +1,182 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Offline scoring for path-following benchmark runs. + +Source-agnostic: an :class:`ExecutedTrajectory` from sim and from hardware +score identically. Scoring is purely geometric against the reference +:class:`~dimos.msgs.nav_msgs.Path.Path` (cross-track error, heading error, +arrival) — no time-parameterized reference is needed. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import math + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Path import Path +from dimos.utils.trigonometry import angle_diff + + +@dataclass +class TrajectoryTick: + """One control period worth of recorded state.""" + + t: float # seconds since trajectory start + pose: PoseStamped + cmd_twist: Twist + actual_twist: Twist # plant output (sim) or measured (hw) + + +@dataclass +class ExecutedTrajectory: + ticks: list[TrajectoryTick] = field(default_factory=list) + arrived: bool = False + + +@dataclass +class ScoreResult: + # Path-following (spatial CTE — measured against a Path). + cte_rms: float = 0.0 # m + cte_max: float = 0.0 # m + heading_err_rms: float = 0.0 # rad + heading_err_max: float = 0.0 # rad + time_to_complete: float = 0.0 # s + linear_speed_rms: float = 0.0 # m/s, |cmd linear| + angular_speed_rms: float = 0.0 # rad/s, |cmd wz| + cmd_rate_integral: float = 0.0 # Sum |dcmd| (smoothness; lower is smoother) + arrived: bool = False + n_ticks: int = 0 + + +# --------------------------------------------------------------------------- +# Geometry helpers +# --------------------------------------------------------------------------- + + +def _path_xy(path: Path) -> NDArray[np.float64]: + return np.array([[p.position.x, p.position.y] for p in path.poses], dtype=np.float64) + + +def nearest_segment( + pt: NDArray[np.float64], path_xy: NDArray[np.float64] +) -> tuple[int, float, float]: + """Find nearest path segment to ``pt``. + + Returns ``(seg_idx, perp_dist, t_along_seg)`` where ``seg_idx`` indexes + the segment from ``path_xy[seg_idx]`` to ``path_xy[seg_idx+1]`` and + ``t_along_seg`` is the parameter (clamped to [0, 1]) of the foot of + the perpendicular. + """ + if len(path_xy) < 2: + d = float(np.linalg.norm(pt - path_xy[0])) + return 0, d, 0.0 + + starts = path_xy[:-1] + ends = path_xy[1:] + segs = ends - starts + seg_len_sq = np.sum(segs * segs, axis=1) + seg_len_sq = np.where(seg_len_sq < 1e-12, 1.0, seg_len_sq) + + rel = pt[None, :] - starts + t = np.clip(np.sum(rel * segs, axis=1) / seg_len_sq, 0.0, 1.0) + foot = starts + t[:, None] * segs + dists = np.linalg.norm(pt[None, :] - foot, axis=1) + + idx = int(np.argmin(dists)) + return idx, float(dists[idx]), float(t[idx]) + + +def _segment_yaw(path_xy: NDArray[np.float64], seg_idx: int) -> float: + if len(path_xy) < 2: + return 0.0 + seg_idx = max(0, min(seg_idx, len(path_xy) - 2)) + dx = path_xy[seg_idx + 1, 0] - path_xy[seg_idx, 0] + dy = path_xy[seg_idx + 1, 1] - path_xy[seg_idx, 1] + return float(math.atan2(dy, dx)) + + +# --------------------------------------------------------------------------- +# Scoring +# --------------------------------------------------------------------------- + + +def _twist_linear_speed(t: Twist) -> float: + return float(math.hypot(t.linear.x, t.linear.y)) + + +def _twist_angular_speed(t: Twist) -> float: + return float(abs(t.angular.z)) + + +def _cmd_delta(a: Twist, b: Twist) -> float: + """L2 norm of (a - b) over the (vx, vy, wz) channels.""" + dvx = a.linear.x - b.linear.x + dvy = a.linear.y - b.linear.y + dwz = a.angular.z - b.angular.z + return float(math.sqrt(dvx * dvx + dvy * dvy + dwz * dwz)) + + +def score_run(reference_path: Path, executed: ExecutedTrajectory) -> ScoreResult: + """Score an executed trajectory against its reference path.""" + if not executed.ticks: + return ScoreResult(arrived=executed.arrived, n_ticks=0) + + path_xy = _path_xy(reference_path) + if len(path_xy) == 0: + return ScoreResult(arrived=executed.arrived, n_ticks=len(executed.ticks)) + + cte_sq: list[float] = [] + cte_abs: list[float] = [] + he_abs: list[float] = [] + he_sq: list[float] = [] + lin_sq: list[float] = [] + ang_sq: list[float] = [] + + for tick in executed.ticks: + pt = np.array([tick.pose.position.x, tick.pose.position.y], dtype=np.float64) + seg_idx, d, _ = nearest_segment(pt, path_xy) + cte_abs.append(d) + cte_sq.append(d * d) + + path_yaw = _segment_yaw(path_xy, seg_idx) + he = abs(angle_diff(tick.pose.orientation.euler[2], path_yaw)) + he_abs.append(he) + he_sq.append(he * he) + + lin_sq.append(_twist_linear_speed(tick.cmd_twist) ** 2) + ang_sq.append(_twist_angular_speed(tick.cmd_twist) ** 2) + + cmd_rate_integral = sum( + _cmd_delta(executed.ticks[i].cmd_twist, executed.ticks[i - 1].cmd_twist) + for i in range(1, len(executed.ticks)) + ) + + return ScoreResult( + cte_rms=math.sqrt(sum(cte_sq) / len(cte_sq)), + cte_max=max(cte_abs), + heading_err_rms=math.sqrt(sum(he_sq) / len(he_sq)), + heading_err_max=max(he_abs), + time_to_complete=executed.ticks[-1].t - executed.ticks[0].t, + linear_speed_rms=math.sqrt(sum(lin_sq) / len(lin_sq)), + angular_speed_rms=math.sqrt(sum(ang_sq) / len(ang_sq)), + cmd_rate_integral=cmd_rate_integral, + arrived=executed.arrived, + n_ticks=len(executed.ticks), + ) diff --git a/dimos/utils/benchmarking/test_benchmark.py b/dimos/utils/benchmarking/test_benchmark.py new file mode 100644 index 0000000000..852e44ad61 --- /dev/null +++ b/dimos/utils/benchmarking/test_benchmark.py @@ -0,0 +1,274 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Benchmark pub/sub pieces: odom recorder, odom-based completion, the flat +per-run recording round-trip, and an end-to-end in-process loop (RPP follower +driven against the FOPDT sim plant -> benchmark records odom -> completion fires +-> recording -> offline scoring).""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.rpp_path_follower_task.rpp_path_follower_task import ( + DEFAULT_ARTIFACT_PATH, + create_task, +) +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.benchmarking.benchmark import ( + CompletionMonitor, + OdomRecorder, + RunRecording, + path_set, + shift_path_to_start_at_pose, +) +from dimos.utils.benchmarking.paths import circle, straight_line +from dimos.utils.benchmarking.plant import ( + FopdtChannelParams, + TwistBasePlantParams, + TwistBasePlantSim, +) +from dimos.utils.benchmarking.score import score_dir +from dimos.utils.benchmarking.tuning import TuningConfig + +_JOINTS = ["go2/vx", "go2/vy", "go2/wz"] + + +def _pose(x=0.0, y=0.0, yaw=0.0) -> PoseStamped: + return PoseStamped( + position=Vector3(x, y, 0.0), orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)) + ) + + +# --------------------------------------------------------------------------- +# Recording round-trip +# --------------------------------------------------------------------------- + + +def test_recording_round_trip(tmp_path): + ref = straight_line(length=2.0) + rec = RunRecording.from_path( + robot="go2", + path_name="straight_line", + speed=0.5, + reference=ref, + goal_tolerance=0.25, + velocity_threshold=0.05, + timeout=60.0, + ) + rec.arrived = True + rec.reason = "goal+stop" + rec.ticks = [[i * 0.1, i * 0.1, 0.0, 0.0, 0.5, 0.0, 0.0] for i in range(20)] + out = tmp_path / "run.json" + rec.to_json(out) + + back = RunRecording.from_json(out) + assert back.robot == "go2" + assert back.path == "straight_line" + assert back.speed == 0.5 + assert back.arrived is True + assert back.reason == "goal+stop" + assert len(back.ticks) == 20 + # reference rebuilds into a Path matching the original pose count + assert len(back.reference_path().poses) == len(ref.poses) + + +def test_path_set_is_the_full_battery(): + names = set(path_set()) + assert names == { + "straight_line", + "single_corner", + "smooth_corner", + "square", + "rounded_square", + "circle", + } + + +def test_anchor_shifts_path_to_pose(): + ref = straight_line(length=2.0) + anchored = shift_path_to_start_at_pose(ref, _pose(5.0, 3.0, 0.0)) + p0 = anchored.poses[0].position + assert p0.x == 5.0 and p0.y == 3.0 + + +# --------------------------------------------------------------------------- +# Odom recorder +# --------------------------------------------------------------------------- + + +def test_odom_recorder_differentiates_forward_velocity(): + rec = OdomRecorder(alpha=1.0) # no smoothing -> exact diff + rec.on_odom(_pose(0.0, 0.0, 0.0), now=0.0) + rec.on_odom(_pose(0.1, 0.0, 0.0), now=0.1) # 1 m/s forward + lin, ang = rec.body_speed() + assert lin == pytest.approx(1.0, abs=1e-6) + assert ang == pytest.approx(0.0, abs=1e-6) + assert len(rec.snapshot()) == 2 + + +def test_odom_recorder_reset_clears_ticks(): + rec = OdomRecorder() + rec.on_odom(_pose(0.0), now=0.0) + rec.on_odom(_pose(0.1), now=0.1) + assert rec.snapshot() + rec.reset() + assert rec.snapshot() == [] + assert rec.latest_pose() is not None # latest pose is retained for warm-up + + +# --------------------------------------------------------------------------- +# Completion monitor (odom-only) +# --------------------------------------------------------------------------- + + +def _monitor(ref, dwell_s=0.3): + return CompletionMonitor( + ref, + goal_tolerance=0.25, + velocity_threshold=0.05, + angular_threshold=0.1, + dwell_s=dwell_s, + ) + + +def test_completion_fires_on_goal_and_stop(): + ref = straight_line(length=2.0) + mon = _monitor(ref) + t = 0.0 + done = False + # drive to the goal (moving) + for i in range(40): + x = min(2.0, i * 0.1) + done = mon.update(x, 0.0, 1.0, 0.0, t) # moving -> not done + t += 0.1 + assert not done + # sit at goal, stopped, for the dwell + for _ in range(10): + done = mon.update(2.0, 0.0, 0.0, 0.0, t) + t += 0.1 + if done: + break + assert done + + +def test_completion_does_not_fire_prematurely_on_closed_path(): + """A closed path (circle: goal == start) must not trip arrival at the start + just because the robot is near the last pose and momentarily slow.""" + ref = circle(radius=1.0) + mon = _monitor(ref) + # robot sits at the start (== goal) and is stopped, but hasn't covered the + # path yet -> not complete. + done = False + for i in range(10): + done = mon.update(ref.poses[0].position.x, ref.poses[0].position.y, 0.0, 0.0, i * 0.1) + assert not done + + +def test_completion_never_fires_when_robot_never_reaches_goal(): + ref = straight_line(length=2.0) + mon = _monitor(ref) + done = False + for i in range(60): # robot stuck at start, stopped + done = mon.update(0.0, 0.0, 0.0, 0.0, i * 0.1) + assert not done # never covered the path -> would hit the per-run timeout + + +# --------------------------------------------------------------------------- +# End-to-end: RPP follower vs sim plant -> benchmark record -> score +# --------------------------------------------------------------------------- + + +def _state(x, y, yaw, t): + return CoordinatorState( + joints=JointStateSnapshot( + joint_positions={_JOINTS[0]: x, _JOINTS[1]: y, _JOINTS[2]: yaw}, + joint_velocities={_JOINTS[0]: 0.0, _JOINTS[1]: 0.0, _JOINTS[2]: 0.0}, + ), + t_now=t, + dt=0.1, + ) + + +def test_end_to_end_controller_benchmark_scoring(tmp_path): + """Prove the full decoupled chain in-process against the in-repo FOPDT sim: + the benchmark publishes a path+speed (here driven directly), the RPP follower + tracks it against TwistBasePlantSim, the benchmark records the executed odom, + odom-based completion fires, a run is written, and the offline scorer emits + metrics.""" + art = TuningConfig.from_json(DEFAULT_ARTIFACT_PATH) + task = create_task( + SimpleNamespace( + name="rpp_follower", joint_names=_JOINTS, priority=10, params={"speed": 0.5} + ), + None, + ) + plant = TwistBasePlantSim( + TwistBasePlantParams( + vx=FopdtChannelParams(art.plant.vx.K, art.plant.vx.tau, art.plant.vx.L), + vy=FopdtChannelParams(art.plant.vy.K, art.plant.vy.tau, art.plant.vy.L), + wz=FopdtChannelParams(art.plant.wz.K, art.plant.wz.tau, art.plant.wz.L), + ) + ) + plant.reset(0.0, 0.0, 0.0, 0.1) + + # Benchmark anchors the path to the robot's first odom (here the origin). + ref = shift_path_to_start_at_pose(straight_line(length=2.0), _pose(plant.x, plant.y, plant.yaw)) + task.set_path(ref, _pose(plant.x, plant.y, plant.yaw)) + + recorder = OdomRecorder() + monitor = _monitor(ref, dwell_s=0.3) + done = False + for k in range(800): + out = task.compute(_state(plant.x, plant.y, plant.yaw, t=k * 0.1)) + cvx, cvy, cwz = out.velocities if out is not None else (0.0, 0.0, 0.0) + # Benchmark records what it sees over the transport: cmd_vel + odom. + recorder.on_cmd_vel(Twist(linear=Vector3(cvx, cvy, 0.0), angular=Vector3(0.0, 0.0, cwz))) + recorder.on_odom(_pose(plant.x, plant.y, plant.yaw), now=k * 0.1) + lin, ang = recorder.body_speed() + if monitor.update(plant.x, plant.y, lin, ang, k * 0.1): + done = True + break + plant.step(cvx, cvy, cwz, 0.1) + assert done, "odom-based completion never fired" + + # Write the run, then score it offline. + rec = RunRecording.from_path( + robot="go2", + path_name="straight_line", + speed=0.5, + reference=ref, + goal_tolerance=0.25, + velocity_threshold=0.05, + timeout=60.0, + ) + rec.arrived = True + rec.reason = "goal+stop" + rec.ticks = recorder.snapshot() + rec.to_json(tmp_path / "go2_straight_line_v0.50_000.json") + + opm = score_dir(tmp_path, tolerances_cm=[10, 15], plots=False) + assert len(opm.points) == 1 + pt = opm.points[0] + assert pt.arrived + assert pt.path == "straight_line" + # tracked a straight line reasonably well + assert pt.cte_max < 0.3 + assert len(opm.tolerance_inversion) == 2 diff --git a/dimos/utils/benchmarking/tuning.py b/dimos/utils/benchmarking/tuning.py new file mode 100644 index 0000000000..4c747d1697 --- /dev/null +++ b/dimos/utils/benchmarking/tuning.py @@ -0,0 +1,794 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Twist-base tuning config artifact + the DERIVE step (model -> config). + +Robot-agnostic. This is the contract the two tuning tools share: + +* :func:`derive_config` is the **pure** DERIVE step — a fitted FOPDT + plant model in, a fully-populated controller config out. No file or + robot I/O, so it is unit-tested in isolation (``test_tuning.py``). +* :class:`TuningConfig` is the versioned artifact. It owns the JSON + (de)serialization (``to_json`` / ``from_json``) and the + runtime-config converters the benchmark tool consumes. +* :func:`invert_tolerance` is the pure tolerance -> max-safe-speed + inversion the benchmark tool fills section 5 with (also unit-tested). + +Why these numbers (the settled characterization findings, not re-derived +here — see ``reports/tuning_README.md``): a velocity-commanded base is +FOPDT per axis; at a given speed the tracking error is the plant floor +``(tau + L) * v``; reactive controllers have ~zero headroom over that +floor; the dominant lever is speed vs path curvature; the simple +production baseline P-controller is the recommended controller. +""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +import json +from pathlib import Path +import subprocess + +from dimos.control.tasks.feedforward_gain_compensator import FeedforwardGainConfig +from dimos.utils.benchmarking.plant import TwistBasePlantParams +from dimos.utils.benchmarking.velocity_profile import ( + GO2_VX_MAX, + GO2_WZ_MAX, + VelocityProfileConfig, +) + +SCHEMA_VERSION = 1 +# SCHEMA_VERSION = breaking field/type change. +# METHODOLOGY_VERSION = how data was collected (additive). +# +# Methodology log (append; don't edit prior): +# v1 — sparse sweep (3 amps), one FOPDT/channel. +# v2 — dense sweep + floor/ceiling probes. +METHODOLOGY_VERSION = 2 + +# --- DERIVE tunable constants (documented; single source of truth) ------- + +# Cross-track headroom margin on the measured angular-rate ceiling. The +# baseline P-controller adds a cross-track correction term on top of the +# nominal turn rate; if the profile lets wz ride at the saturation +# ceiling there is no authority left for that correction and corners get +# cut (the oscillation/cut-corner failure mode). Reserve 15%. +WZ_HEADROOM_MARGIN = 0.15 + +# Lateral (centripetal) comfort acceleration cap for the curvature +# profile, m/s^2. Constant, not derived: it is a ride-quality / stability +# choice, not a plant property. 1.0 matches the shipped VelocityProfiler +# default and is conservative for a ~15 kg quadruped — it keeps the +# corner-speed cap inside the regime the curvature-profile R&D validated. +A_LAT_MAX = 1.0 + +# Braking authority exceeds forward-accel authority: a robot can decel +# harder than it can accel. Mirrors the shipped VelocityProfileConfig +# 1.0 / 2.0 accel/decel ratio. +DECEL_ACCEL_RATIO = 2.0 + +RECOMMENDED_CONTROLLER_EVIDENCE = ( + "Baseline P-controller, hardcoded. The Go2 base is FOPDT per axis; at " + "a given speed the tracking error equals the plant floor (tau + L) * " + "v, which no reactive control law can beat (~zero headroom over the " + "floor — validated controller bake-off). The only effective lever is " + "speed vs path curvature, which the derived velocity profile + " + "feedforward already apply. See reports/tuning_README.md and the " + "characterization findings (this evidence string cites the Go2 " + "result; a different robot's headroom is TBD until characterized)." +) + + +# --- Artifact schema ----------------------------------------------------- + + +@dataclass +class Provenance: + """Where/when this model was measured — defines its validity scope.""" + + robot_id: str = "unknown" + surface: str = "unknown" + mode: str = "default" + date: str = "unknown" + git_sha: str = "unknown" + sim_or_hw: str = "sim" + characterization_session_dir: str = "" + methodology_version: int = METHODOLOGY_VERSION + + +@dataclass +class FopdtChannelDC: + K: float + tau: float + L: float + + +@dataclass +class PlantModelDC: + vx: FopdtChannelDC + vy: FopdtChannelDC + wz: FopdtChannelDC + + +@dataclass +class FeedforwardDC: + K_vx: float + K_vy: float + K_wz: float + output_min_vx: float = -GO2_VX_MAX + output_max_vx: float = GO2_VX_MAX + output_min_vy: float = -GO2_VX_MAX + output_max_vy: float = GO2_VX_MAX + output_min_wz: float = -GO2_WZ_MAX + output_max_wz: float = GO2_WZ_MAX + + def to_runtime(self) -> FeedforwardGainConfig: + """Build the live :class:`FeedforwardGainConfig` the controller + consumes (the benchmark tool's single mapping point).""" + return FeedforwardGainConfig( + K_vx=self.K_vx, + K_vy=self.K_vy, + K_wz=self.K_wz, + output_min_vx=self.output_min_vx, + output_max_vx=self.output_max_vx, + output_min_vy=self.output_min_vy, + output_max_vy=self.output_max_vy, + output_min_wz=self.output_min_wz, + output_max_wz=self.output_max_wz, + ) + + +@dataclass +class VelocityProfileDC: + max_linear_speed: float + max_angular_speed: float + max_centripetal_accel: float + max_linear_accel: float + max_linear_decel: float + min_speed: float = 0.05 + lookahead_pts: int = 8 + + def to_runtime(self, max_linear_speed: float | None = None) -> VelocityProfileConfig: + """Build the live :class:`VelocityProfileConfig`. The benchmark + tool overrides ``max_linear_speed`` per speed-ladder rung.""" + return VelocityProfileConfig( + max_linear_speed=( + self.max_linear_speed if max_linear_speed is None else max_linear_speed + ), + max_angular_speed=self.max_angular_speed, + max_centripetal_accel=self.max_centripetal_accel, + max_linear_accel=self.max_linear_accel, + max_linear_decel=self.max_linear_decel, + min_speed=self.min_speed, + lookahead_pts=self.lookahead_pts, + ) + + +@dataclass +class RecommendedControllerDC: + name: str = "baseline" + params: dict = field(default_factory=lambda: {"k_angular": 0.5}) + evidence: str = RECOMMENDED_CONTROLLER_EVIDENCE + + +@dataclass +class OperatingPoint: + path: str + speed: float + cte_max: float + cte_rms: float + arrived: bool + + +@dataclass +class ToleranceRow: + tol_cm: float + max_speed: float | None # None = no tested speed meets the tolerance + binding_path: str | None + + +@dataclass +class OperatingPointMap: + speeds: list[float] + points: list[OperatingPoint] + tolerance_inversion: list[ToleranceRow] + + +# --- methodology v2: floor/ceiling envelope + per-amplitude tables ------- + + +@dataclass +class ChannelEnvelopeDC: + """Measured floor + ceiling for one velocity channel (serialized form).""" + + floor: float + ceiling: float + floor_not_found: bool = False + ceiling_not_found: bool = False + saturating_at_amp: float | None = None + + +@dataclass +class VelocityEnvelopeDC: + """Section 5: per-channel velocity envelope. m/s for vx/vy, rad/s for wz.""" + + vx: ChannelEnvelopeDC + vy: ChannelEnvelopeDC + wz: ChannelEnvelopeDC + + +@dataclass +class AmplitudeFitDC: + """One FOPDT fit at a specific amplitude (sweep or ceiling probe).""" + + amp: float + K: float + tau: float + L: float + r2: float + source: str = "sweep" # "sweep" | "ceiling_probe" + + +@dataclass +class FloorProbeResultDC: + """One floor-probe row (D3 AND-test pass/fail at one amplitude).""" + + amp: float + motion_detected: bool + sustained_samples: int + net_displacement: float = 0.0 # signed body-frame displacement in cmd dir + + +@dataclass +class DynamicsByAmplitude: + """Section 7: full per-amplitude K/τ/L table across regular sweep + + ceiling probe (forensics + future lookup-based RG).""" + + vx: list[AmplitudeFitDC] = field(default_factory=list) + vy: list[AmplitudeFitDC] = field(default_factory=list) + wz: list[AmplitudeFitDC] = field(default_factory=list) + + +@dataclass +class FloorProbeResults: + """Sibling forensic record of the floor-probe AND-test outcomes per + amplitude (not FOPDT-fit — only pass/fail).""" + + vx: list[FloorProbeResultDC] = field(default_factory=list) + vy: list[FloorProbeResultDC] = field(default_factory=list) + wz: list[FloorProbeResultDC] = field(default_factory=list) + + +@dataclass +class TuningConfig: + provenance: Provenance + plant: PlantModelDC + feedforward: FeedforwardDC + velocity_profile: VelocityProfileDC + recommended_controller: RecommendedControllerDC + caveats: list[str] = field(default_factory=list) + operating_point_map: OperatingPointMap | None = None + velocity_envelope: VelocityEnvelopeDC | None = None + dynamics_by_amplitude: DynamicsByAmplitude | None = None + floor_probe_results: FloorProbeResults | None = None + # False = a sim/self-test plumbing check, NOT measured on the robot. + # Operators must never tune from an artifact with this False. + valid_for_tuning: bool = True + schema_version: int = SCHEMA_VERSION + + # --- serialization --- + + def to_json(self, path: str | Path) -> Path: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(json.dumps(asdict(self), indent=2, sort_keys=False)) + return path + + @classmethod + def from_json(cls, path: str | Path) -> TuningConfig: + data = json.loads(Path(path).read_text()) + return cls.from_dict(data) + + @classmethod + def from_dict(cls, data: dict) -> TuningConfig: + sv = data.get("schema_version") + if sv != SCHEMA_VERSION: + raise ValueError( + f"unsupported go2 tuning artifact schema_version={sv!r} " + f"(this build understands {SCHEMA_VERSION})" + ) + + def _chan(d: dict) -> FopdtChannelDC: + return FopdtChannelDC(K=d["K"], tau=d["tau"], L=d["L"]) + + opm = None + if data.get("operating_point_map") is not None: + m = data["operating_point_map"] + opm = OperatingPointMap( + speeds=list(m["speeds"]), + points=[OperatingPoint(**p) for p in m["points"]], + tolerance_inversion=[ToleranceRow(**t) for t in m["tolerance_inversion"]], + ) + + env = None + if data.get("velocity_envelope") is not None: + e = data["velocity_envelope"] + env = VelocityEnvelopeDC( + vx=ChannelEnvelopeDC(**e["vx"]), + vy=ChannelEnvelopeDC(**e["vy"]), + wz=ChannelEnvelopeDC(**e["wz"]), + ) + + dba = None + if data.get("dynamics_by_amplitude") is not None: + d = data["dynamics_by_amplitude"] + dba = DynamicsByAmplitude( + vx=[AmplitudeFitDC(**a) for a in d.get("vx", [])], + vy=[AmplitudeFitDC(**a) for a in d.get("vy", [])], + wz=[AmplitudeFitDC(**a) for a in d.get("wz", [])], + ) + + fpr = None + if data.get("floor_probe_results") is not None: + f = data["floor_probe_results"] + fpr = FloorProbeResults( + vx=[FloorProbeResultDC(**r) for r in f.get("vx", [])], + vy=[FloorProbeResultDC(**r) for r in f.get("vy", [])], + wz=[FloorProbeResultDC(**r) for r in f.get("wz", [])], + ) + + # Tolerate v1 provenance (no methodology_version field). + prov_data = dict(data["provenance"]) + prov_data.setdefault("methodology_version", 1) + return cls( + provenance=Provenance(**prov_data), + plant=PlantModelDC( + vx=_chan(data["plant"]["vx"]), + vy=_chan(data["plant"]["vy"]), + wz=_chan(data["plant"]["wz"]), + ), + feedforward=FeedforwardDC(**data["feedforward"]), + velocity_profile=VelocityProfileDC(**data["velocity_profile"]), + recommended_controller=RecommendedControllerDC(**data["recommended_controller"]), + caveats=list(data.get("caveats", [])), + operating_point_map=opm, + velocity_envelope=env, + dynamics_by_amplitude=dba, + floor_probe_results=fpr, + valid_for_tuning=bool(data.get("valid_for_tuning", True)), + schema_version=sv, + ) + + +# --- helpers ------------------------------------------------------------- + + +def git_sha() -> str: + """Short HEAD sha, best-effort (``unknown`` off a repo).""" + try: + return ( + subprocess.run( + ["git", "rev-parse", "--short", "HEAD"], + capture_output=True, + text=True, + check=True, + ).stdout.strip() + or "unknown" + ) + except Exception: + return "unknown" + + +def _safe_inv_gain(K: float) -> float: + """1/K with a guard for a degenerate (near-zero) fitted gain.""" + if abs(K) < 1e-6: + return 1.0 + return 1.0 / K + + +def _output_ceiling(fits: list[dict | AmplitudeFitDC], cap: float) -> tuple[float, bool]: + """Operational ceiling for one channel: ``min(max(|K·amp|), cap)``. + + Falls back to ``cap`` and ``not_found=True`` when no fits are given.""" + vals: list[float] = [] + for f in fits: + K = getattr(f, "K", None) if not isinstance(f, dict) else f.get("K") + amp = ( + getattr(f, "amp", None) + if not isinstance(f, dict) + else (f.get("amp", f.get("amplitude"))) + ) + if K is None or amp is None: + continue + try: + vals.append(abs(float(K) * float(amp))) + except (TypeError, ValueError): + continue + if not vals: + return cap, True + # `not_found` is reserved for "no data". Clamping to the platform + # cap is silent — it just means the robot can output more than the + # profile says is safe; that's not a failure of the measurement. + return min(max(vals), cap), False + + +def _saturating_at_amp( + fits: list[dict | AmplitudeFitDC], K_linear: float, sag_threshold: float +) -> float | None: + """Forensic-only: the lowest amp where |K| drops below + ``(1 - sag) · |K_linear|``. ``None`` if no fit saturates.""" + if not fits or K_linear == 0.0: + return None + threshold = (1.0 - sag_threshold) * abs(K_linear) + saturating: list[float] = [] + for f in fits: + K = getattr(f, "K", None) if not isinstance(f, dict) else f.get("K") + amp = ( + getattr(f, "amp", None) + if not isinstance(f, dict) + else (f.get("amp", f.get("amplitude"))) + ) + if K is None or amp is None: + continue + try: + if abs(float(K)) < threshold: + saturating.append(float(amp)) + except (TypeError, ValueError): + continue + return min(saturating) if saturating else None + + +def _floor_from_probe(probe_rows: list, fallback_amps: list[float]) -> tuple[float, bool]: + """Floor = lowest amp where D3 ``motion_detected`` is true. Falls back + to max probed amp when nothing passes.""" + passing: list[float] = [] + for r in probe_rows: + amp = getattr(r, "amp", None) if not isinstance(r, dict) else r.get("amp") + det = ( + getattr(r, "motion_detected", None) + if not isinstance(r, dict) + else (r.get("motion_detected")) + ) + if amp is None or not det: + continue + try: + passing.append(float(amp)) + except (TypeError, ValueError): + continue + if passing: + return min(passing), False + return (max(fallback_amps) if fallback_amps else 0.0), True + + +def compute_envelope( + floor_probe_results: FloorProbeResults | None, + dynamics_by_amplitude: DynamicsByAmplitude | None, + *, + vx_cap: float, + wz_cap: float, + floor_probe_amplitudes: dict[str, list[float]] | None = None, + K_linear: dict[str, float] | None = None, + sag_threshold: float = 0.15, +) -> VelocityEnvelopeDC: + """Pure reducer: per-channel floor + ceiling from the densification + data. Used by both the live characterization run and the post-hoc + ``re-derive`` mode (where the user re-applies the current logic to an + existing artifact's stored sweep without re-running on hardware). + + Floor = lowest amp where ``motion_detected`` is true in the floor + probe; falls back to max probe amp with ``floor_not_found=True``. + + Ceiling = ``min(max(|K(amp)·amp|), {vx,wz}_cap)`` across the FULL + sweep + ceiling-probe table. Robust to noisy K because the OUTPUT + magnitude is what matters for RG (max achievable v_actual). + ``ceiling_not_found=True`` only when no fits are available.""" + caps = {"vx": vx_cap, "vy": vx_cap, "wz": wz_cap} + fpa = floor_probe_amplitudes or {} + Kl = K_linear or {} + out: dict[str, ChannelEnvelopeDC] = {} + for ch in ("vx", "vy", "wz"): + probe_rows = getattr(floor_probe_results, ch, []) if floor_probe_results is not None else [] + floor, floor_nf = _floor_from_probe(list(probe_rows), fpa.get(ch, [])) + fits = getattr(dynamics_by_amplitude, ch, []) if dynamics_by_amplitude is not None else [] + ceiling, ceiling_nf = _output_ceiling(list(fits), caps[ch]) + sat = _saturating_at_amp(list(fits), Kl.get(ch, 0.0), sag_threshold) if Kl else None + out[ch] = ChannelEnvelopeDC( + floor=floor, + ceiling=ceiling, + floor_not_found=floor_nf, + ceiling_not_found=ceiling_nf, + saturating_at_amp=sat, + ) + return VelocityEnvelopeDC(vx=out["vx"], vy=out["vy"], wz=out["wz"]) + + +def _channel_ceiling(per_amplitude: dict | None, channel: str, fallback: float) -> float: + """Measured steady-state magnitude ceiling for a channel: + ``max(K_at_amp * |amplitude|)`` over the swept amplitudes. Falls back + to the robot's saturation envelope when per-amplitude data is missing + or too sparse to be trustworthy.""" + if not per_amplitude: + return fallback + entries = per_amplitude.get(channel) or [] + vals: list[float] = [] + for e in entries: + K = e.get("K") + amp = e.get("amplitude") + if K is None or amp is None: + continue + try: + vals.append(abs(float(K) * float(amp))) + except (TypeError, ValueError): + continue + if not vals: + return fallback + return max(vals) + + +# --- DERIVE: pure model -> config --------------------------------------- + + +def derive_config( + plant: TwistBasePlantParams, + provenance: Provenance, + *, + per_amplitude: dict | None = None, + vx_max: float = GO2_VX_MAX, + wz_max: float = GO2_WZ_MAX, + velocity_envelope: VelocityEnvelopeDC | None = None, + dynamics_by_amplitude: DynamicsByAmplitude | None = None, + floor_probe_results: FloorProbeResults | None = None, + min_speed_floor: float = 0.0, +) -> TuningConfig: + """Derive the full controller config from a fitted FOPDT plant model. + + Pure: model + provenance in, :class:`TuningConfig` out. No I/O. + + - Feedforward gain per axis = ``1 / K`` (the compensator divides the + controller command by the plant gain so commanded == achieved). + ``plant`` is the **canonical** (linear-regime, methodology v2) fit. + - ``max_linear_speed`` / ``max_angular_speed`` = the measured ceilings + from ``velocity_envelope`` when present (clamped to ``vx_max``/ + ``wz_max``); otherwise fall back to the deprecated per-amplitude + ``K*amp`` estimator, then the saturation envelope. + - ``min_speed`` (RG floor) = measured ``velocity_envelope.vx.floor`` + when present, otherwise the legacy default (``VelocityProfileDC`` + class default 0.05). A profile-supplied ``min_speed_floor > 0`` + hard-clamps the result from below. + - ``max_centripetal_accel`` = the lateral comfort constant. + - ``max_linear_accel`` ~= ``vx_ceiling / tau_vx`` (first-order rise); + decel = ``DECEL_ACCEL_RATIO x`` that. + - recommended controller = baseline, hardcoded, with cited evidence. + """ + caveats: list[str] = [] + + # Ceilings. Prefer the measured envelope (methodology v2). Fall back + # to per-amplitude K*amp (legacy) then to the saturation envelope. + if velocity_envelope is not None: + vx_ceiling = min(velocity_envelope.vx.ceiling, vx_max) + wz_ceiling = min(velocity_envelope.wz.ceiling, wz_max) + if velocity_envelope.vx.ceiling_not_found: + caveats.append( + "vx ceiling probe did not saturate within the safe sweep; " + "DERIVE used the highest probed amplitude. True ceiling " + "may be higher — re-probe with a wider range if needed." + ) + vx_ceiling = vx_max + if velocity_envelope.wz.ceiling_not_found: + caveats.append( + "wz ceiling probe did not saturate within the safe sweep; " + "DERIVE used the wz_max envelope as a fallback." + ) + wz_ceiling = wz_max + else: + vx_ceiling = min(_channel_ceiling(per_amplitude, "vx", vx_max), vx_max) + wz_ceiling = min(_channel_ceiling(per_amplitude, "wz", wz_max), wz_max) + + # RG min_speed: prefer measured floor, then profile hard-clamp. + legacy_min_speed = VelocityProfileDC.__dataclass_fields__["min_speed"].default + if velocity_envelope is not None and not velocity_envelope.vx.floor_not_found: + min_speed = max(velocity_envelope.vx.floor, min_speed_floor) + else: + if velocity_envelope is not None and velocity_envelope.vx.floor_not_found: + caveats.append( + "vx floor probe did not detect motion within the probe " + "ladder; DERIVE fell back to the legacy min_speed default " + f"({legacy_min_speed:g} m/s)." + ) + min_speed = max(legacy_min_speed, min_speed_floor) + + feedforward = FeedforwardDC( + K_vx=_safe_inv_gain(plant.vx.K), + K_vy=_safe_inv_gain(plant.vy.K), + K_wz=_safe_inv_gain(plant.wz.K), + ) + + max_linear_accel = vx_ceiling / plant.vx.tau if plant.vx.tau > 1e-6 else vx_max + velocity_profile = VelocityProfileDC( + max_linear_speed=vx_ceiling, + max_angular_speed=wz_ceiling * (1.0 - WZ_HEADROOM_MARGIN), + max_centripetal_accel=A_LAT_MAX, + max_linear_accel=max_linear_accel, + max_linear_decel=max_linear_accel * DECEL_ACCEL_RATIO, + min_speed=min_speed, + ) + + caveats.extend( + [ + f"Valid only for surface={provenance.surface!r}, " + f"mode={provenance.mode!r}, {provenance.sim_or_hw}. Re-run " + f"characterization on any surface or gait-mode change.", + f"Plant fitted from {provenance.characterization_session_dir or 'n/a'} " + f"on {provenance.date} (git {provenance.git_sha}).", + ] + ) + valid_for_tuning = provenance.sim_or_hw == "hw" + if not valid_for_tuning: + caveats.insert( + 0, + "*** PIPELINE CHECK ONLY — NOT ROBOT-VALID — DO NOT TUNE FROM " + "THIS *** Derived from the in-process FOPDT sim plant " + "(self-test): it only proves the measure->fit->derive plumbing " + "runs and re-recovers its own injected model. Re-run " + "`characterization --mode hw` on the real robot for a " + "tuning-valid artifact.", + ) + + return TuningConfig( + provenance=provenance, + plant=PlantModelDC( + vx=FopdtChannelDC(plant.vx.K, plant.vx.tau, plant.vx.L), + vy=FopdtChannelDC(plant.vy.K, plant.vy.tau, plant.vy.L), + wz=FopdtChannelDC(plant.wz.K, plant.wz.tau, plant.wz.L), + ), + feedforward=feedforward, + velocity_profile=velocity_profile, + recommended_controller=RecommendedControllerDC(), + caveats=caveats, + operating_point_map=None, + velocity_envelope=velocity_envelope, + dynamics_by_amplitude=dynamics_by_amplitude, + floor_probe_results=floor_probe_results, + valid_for_tuning=valid_for_tuning, + ) + + +def re_derive_config( + artifact: TuningConfig, + *, + vx_max: float = GO2_VX_MAX, + wz_max: float = GO2_WZ_MAX, + floor_probe_amplitudes: dict[str, list[float]] | None = None, + min_speed_floor: float = 0.0, + sag_threshold: float = 0.15, +) -> TuningConfig: + """Post-hoc apply the current envelope + DERIVE logic to an existing + artifact. Uses the stored ``dynamics_by_amplitude`` + + ``floor_probe_results`` — no re-run on hardware needed. + + Useful after a methodology bugfix (the K-sag ceiling was too + conservative on noisy fits; switched to ``max(K·amp)`` for + operational use): pass the artifact through here and you get a + corrected JSON without re-collecting data. + + Plant, FF (the canonical FOPDT) and provenance are passed through + unchanged — this only recomputes envelope + velocity_profile + + caveats.""" + K_linear = { + "vx": artifact.plant.vx.K, + "vy": artifact.plant.vy.K, + "wz": artifact.plant.wz.K, + } + env = compute_envelope( + artifact.floor_probe_results, + artifact.dynamics_by_amplitude, + vx_cap=vx_max, + wz_cap=wz_max, + floor_probe_amplitudes=floor_probe_amplitudes, + K_linear=K_linear, + sag_threshold=sag_threshold, + ) + plant = TwistBasePlantParams( + vx=FopdtChannelParamsLike(artifact.plant.vx), + vy=FopdtChannelParamsLike(artifact.plant.vy), + wz=FopdtChannelParamsLike(artifact.plant.wz), + ) + return derive_config( + plant, + artifact.provenance, + vx_max=vx_max, + wz_max=wz_max, + velocity_envelope=env, + dynamics_by_amplitude=artifact.dynamics_by_amplitude, + floor_probe_results=artifact.floor_probe_results, + min_speed_floor=min_speed_floor, + ) + + +def FopdtChannelParamsLike(dc: FopdtChannelDC): + """Lightweight adapter: derive_config wants a TwistBasePlantParams + (made of FopdtChannelParams), but the artifact stores them as + FopdtChannelDC. Return a duck-typed object with .K, .tau, .L.""" + from dimos.utils.benchmarking.plant import FopdtChannelParams + + return FopdtChannelParams(K=dc.K, tau=dc.tau, L=dc.L) + + +# --- tolerance -> max-safe-speed inversion (pure) ------------------------ + + +def invert_tolerance( + points: list[OperatingPoint], tolerances_cm: list[float] +) -> list[ToleranceRow]: + """For each tolerance, the fastest speed that keeps every path within + ``cte_max <= tol`` *and* arrives. + + Per path: the max speed whose run satisfies the tolerance and + arrived. The recommendation is the *binding* (minimum across paths) + such speed — the slowest path's limit gates the fleet. Speeds where a + path fails the tolerance or did not arrive are excluded; if no speed + satisfies a path, that tolerance yields ``max_speed=None``. + """ + paths = sorted({p.path for p in points}) + rows: list[ToleranceRow] = [] + for tol in tolerances_cm: + tol_m = tol / 100.0 + per_path_best: dict[str, float] = {} + feasible = True + binding_path: str | None = None + binding_speed = float("inf") + for path in paths: + ok_speeds = [ + p.speed for p in points if p.path == path and p.arrived and p.cte_max <= tol_m + ] + if not ok_speeds: + feasible = False + break + best = max(ok_speeds) + per_path_best[path] = best + if best < binding_speed: + binding_speed = best + binding_path = path + if feasible and per_path_best: + rows.append( + ToleranceRow(tol_cm=tol, max_speed=binding_speed, binding_path=binding_path) + ) + else: + rows.append(ToleranceRow(tol_cm=tol, max_speed=None, binding_path=None)) + return rows + + +__all__ = [ + "METHODOLOGY_VERSION", + "SCHEMA_VERSION", + "AmplitudeFitDC", + "ChannelEnvelopeDC", + "DynamicsByAmplitude", + "FeedforwardDC", + "FloorProbeResultDC", + "FloorProbeResults", + "FopdtChannelDC", + "OperatingPoint", + "OperatingPointMap", + "PlantModelDC", + "Provenance", + "RecommendedControllerDC", + "ToleranceRow", + "TuningConfig", + "VelocityEnvelopeDC", + "VelocityProfileDC", + "derive_config", + "git_sha", + "invert_tolerance", +] diff --git a/dimos/utils/benchmarking/velocity_profile.py b/dimos/utils/benchmarking/velocity_profile.py new file mode 100644 index 0000000000..e70b292442 --- /dev/null +++ b/dimos/utils/benchmarking/velocity_profile.py @@ -0,0 +1,141 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Curvature velocity profiling as a controller-agnostic benchmark wrapper. + +Option 2 ("B"). Unlike the command smoothers (LPF / rate limiter, which +only shape the command and cannot beat the plant floor), this attacks +the floor itself: the FOPDT spatial lag is ~(tau+L)*v, so slowing down +where path curvature is high *lowers the lag exactly where tracking is +worst*. It is the architecturally correct tracking lever. + +Wraps the pre-existing +:class:`dimos.control.tasks.velocity_profiler.VelocityProfiler` +(curvature -> centripetal-accel speed limit -> fwd/bwd accel passes) and +applies it as a per-tick cap on the controller's commanded ``(vx, wz)``: +at the robot's current path index it caps ``|vx|`` to the profile speed +and scales ``wz`` by the same factor so the commanded turn radius +(vx/wz) — i.e. the path geometry — is preserved; the robot just +traverses the corner slower. + +``max_angular_speed`` defaults to the Go2 Rung-1 saturation envelope +(``WZ_MAX = 1.5 rad/s``); ``max_linear_speed`` is the cohort target +speed. No control-law change — a pure output wrapper, same seam as the +rate limiter. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Protocol + +import numpy as np + +from dimos.control.tasks.velocity_profiler import VelocityProfiler +from dimos.msgs.nav_msgs.Path import Path + +# Go2 Rung-1 saturation envelope (mirrors runner.VX_MAX / WZ_MAX). +GO2_VX_MAX = 1.0 # m/s +GO2_WZ_MAX = 1.5 # rad/s + + +class PathSpeedCapProtocol(Protocol): + """Duck-type contract for objects installed as a follower's ``_profile_cap``. + + :class:`PathSpeedCap` (below) is the curvature-based implementation; any + object with this shape (e.g. a reference governor) can be dropped in instead. + """ + + def for_path(self, path: Path) -> None: ... + + def speed_limit_at(self, x: float, y: float) -> float: ... + + def cap( + self, x: float, y: float, vx: float, vy: float, wz: float + ) -> tuple[float, float, float]: ... + + +@dataclass +class VelocityProfileConfig: + """Curvature-profile knobs. Defaults come from the Go2 saturation + envelope; ``max_linear_speed`` should be set to the cohort speed. + + ``lookahead_pts`` makes the cap use the *minimum* profile speed over + the next N path points so the robot starts slowing *before* the + corner (a pure at-index cap brakes too late). + """ + + max_linear_speed: float = 0.55 + max_angular_speed: float = GO2_WZ_MAX + max_centripetal_accel: float = 1.0 + max_linear_accel: float = 1.0 + max_linear_decel: float = 2.0 + min_speed: float = 0.05 + lookahead_pts: int = 8 + + +class PathSpeedCap: + """Per-tick curvature speed cap for one path. + + Build once per run (``for_path``); call :meth:`cap` each tick with + the robot xy and the controller's commanded ``(vx, wz)``. + """ + + def __init__(self, cfg: VelocityProfileConfig | None = None) -> None: + self.cfg = cfg or VelocityProfileConfig() + self._profiler = VelocityProfiler( + max_linear_speed=self.cfg.max_linear_speed, + max_angular_speed=self.cfg.max_angular_speed, + max_linear_accel=self.cfg.max_linear_accel, + max_linear_decel=self.cfg.max_linear_decel, + max_centripetal_accel=self.cfg.max_centripetal_accel, + min_speed=self.cfg.min_speed, + ) + self._pts: np.ndarray | None = None + self._profile: np.ndarray | None = None + + def for_path(self, path: Path) -> None: + """(Re)compute the speed profile for ``path``. Call on path start.""" + self._profile = np.asarray(self._profiler.compute_profile(path), dtype=float) + self._pts = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) + + def speed_limit_at(self, x: float, y: float) -> float: + """Profile speed at the nearest path index, min over the lookahead + window (so braking starts before the corner).""" + if self._pts is None or self._profile is None or len(self._profile) == 0: + return self.cfg.max_linear_speed + i = int(np.argmin(np.sum((self._pts - np.array([x, y])) ** 2, axis=1))) + j = min(len(self._profile), i + max(1, self.cfg.lookahead_pts)) + return float(np.min(self._profile[i:j])) + + def cap( + self, x: float, y: float, vx: float, vy: float, wz: float + ) -> tuple[float, float, float]: + """Cap |vx| to the profile speed; scale vy/wz by the same factor + so the commanded path geometry (turn radius) is preserved.""" + vlim = self.speed_limit_at(x, y) + s = abs(vx) + if s <= vlim or s < 1e-9: + return vx, vy, wz + k = vlim / s + return vx * k, vy * k, wz * k + + +__all__ = [ + "GO2_VX_MAX", + "GO2_WZ_MAX", + "PathSpeedCap", + "PathSpeedCapProtocol", + "VelocityProfileConfig", +]