From cf02719460f7e2055fb45a7e44432d9df289c9de Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:07:12 -0700 Subject: [PATCH 01/51] control tasks for nav2 controller + modifiers --- .../tasks/baseline_path_follower_task.py | 317 ++++++++++++++++++ .../tasks/feedforward_gain_compensator.py | 93 +++++ dimos/control/tasks/velocity_profiler.py | 158 +++++++++ dimos/control/tasks/velocity_tracking_pid.py | 171 ++++++++++ 4 files changed, 739 insertions(+) create mode 100644 dimos/control/tasks/baseline_path_follower_task.py create mode 100644 dimos/control/tasks/feedforward_gain_compensator.py create mode 100644 dimos/control/tasks/velocity_profiler.py create mode 100644 dimos/control/tasks/velocity_tracking_pid.py diff --git a/dimos/control/tasks/baseline_path_follower_task.py b/dimos/control/tasks/baseline_path_follower_task.py new file mode 100644 index 0000000000..42bf48de76 --- /dev/null +++ b/dimos/control/tasks/baseline_path_follower_task.py @@ -0,0 +1,317 @@ +# 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. + +"""Baseline 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 +from typing import TYPE_CHECKING, 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.navigation.replanning_a_star.controllers import PController +from dimos.navigation.replanning_a_star.path_distancer import PathDistancer +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.geometry_msgs.PoseStamped import PoseStamped + from dimos.msgs.nav_msgs.Path import Path + +logger = setup_logger() + +BaselineState = Literal[ + "idle", "initial_rotation", "path_following", "final_rotation", "arrived", "aborted" +] + + +@dataclass +class BaselinePathFollowerTaskConfig: + 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 + # 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 + + +class BaselinePathFollowerTask(BaseControlTask): + """Production LocalPlanner algorithm as a passive ControlTask.""" + + def __init__( + self, + name: str, + config: BaselinePathFollowerTaskConfig, + global_config: GlobalConfig, + ) -> None: + if len(config.joint_names) != 3: + raise ValueError( + f"BaselinePathFollowerTask '{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 + ) + + self._state: BaselineState = "idle" + self._path: Path | None = None + self._distancer: PathDistancer | None = None + self._current_odom: PoseStamped | None = None + # 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 + + # ------------------------------------------------------------------ + # 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 or 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 + + # Inner-loop options (mutually exclusive - PI wins if both set). + 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) + + 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"BaselinePathFollowerTask '{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() + + 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"BaselinePathFollowerTask '{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) + # ------------------------------------------------------------------ + + def start_path(self, path: Path, current_odom: PoseStamped) -> bool: + if path is None or len(path.poses) < 2: + logger.warning(f"BaselinePathFollowerTask '{self._name}': invalid path") + return False + self._path = path + self._distancer = PathDistancer(path) + self._current_odom = current_odom + self._max_progress_idx = 0 + self._controller.reset_errors() + if self._pid is not None: + self._pid.reset() + if self._ff is not None: + self._ff.reset() + + 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"BaselinePathFollowerTask '{self._name}' started " + f"({len(path.poses)} poses, initial state={self._state})" + ) + return True + + def update_odom(self, odom: PoseStamped) -> None: + self._current_odom = odom + + 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) -> BaselineState: + return self._state + + +__all__ = [ + "BaselinePathFollowerTask", + "BaselinePathFollowerTaskConfig", +] 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/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", +] From cfcc988c3373de493fc5089a1a4c6b33b1399d24 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:08:11 -0700 Subject: [PATCH 02/51] plant model for system identification --- dimos/utils/benchmarking/paths.py | 420 +++++++++++++++++++ dimos/utils/benchmarking/plant.py | 141 +++++++ dimos/utils/benchmarking/scoring.py | 292 +++++++++++++ dimos/utils/benchmarking/velocity_profile.py | 118 ++++++ 4 files changed, 971 insertions(+) create mode 100644 dimos/utils/benchmarking/paths.py create mode 100644 dimos/utils/benchmarking/plant.py create mode 100644 dimos/utils/benchmarking/scoring.py create mode 100644 dimos/utils/benchmarking/velocity_profile.py diff --git a/dimos/utils/benchmarking/paths.py b/dimos/utils/benchmarking/paths.py new file mode 100644 index 0000000000..c419919981 --- /dev/null +++ b/dimos/utils/benchmarking/paths.py @@ -0,0 +1,420 @@ +# 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 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", + "single_corner", + "slalom", + "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..d68d2e63b8 --- /dev/null +++ b/dimos/utils/benchmarking/plant.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. + +"""Layer 1 sim plant for the Go2 base. + +Per-channel FOPDT velocity tracking + unicycle kinematics. Tick-based: +each call to :meth:`Go2PlantSim.step` advances one control period. + +The vendored fitted parameters (``GO2_PLANT_FITTED``) live at the bottom +of this module — types, simulator, and the measured values in one place. +""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass +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 + + +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: + self._delay_samples = max(1, int(self.params.L / dt)) + 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 Go2PlantParams: + """FOPDT params for all three velocity channels.""" + + vx: FopdtChannelParams + vy: FopdtChannelParams + wz: FopdtChannelParams + + +class Go2PlantSim: + """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. + """ + + def __init__(self, params: Go2PlantParams) -> None: + self.params = params + 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) + + def step(self, cmd_vx: float, cmd_vy: float, cmd_wz: float, dt: float) -> None: + 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 ground truth the sim self-test injects and recovers, and +# the documented rationale for the derived feedforward gains. vy on the +# real robot strafes and should be characterized for real (--mode hw); +# the sim ground truth copies vx into vy only because the sim FOPDT has +# no independent lateral model. + +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 = Go2PlantParams( + vx=GO2_VX_RISE, + vy=GO2_VX_RISE, # sim ground-truth placeholder; real vy via --mode hw + wz=GO2_WZ_RISE, +) diff --git a/dimos/utils/benchmarking/scoring.py b/dimos/utils/benchmarking/scoring.py new file mode 100644 index 0000000000..698c19f33b --- /dev/null +++ b/dimos/utils/benchmarking/scoring.py @@ -0,0 +1,292 @@ +# 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. +""" + +from __future__ import annotations + +from collections.abc import Callable +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.characterization.trajectories import TrajRefState +from dimos.utils.trigonometry import angle_diff + +RefFn = Callable[[float], TrajRefState] + + +@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 + # Trajectory tracking (time-indexed — measured against r(t)). Decomposed + # in the reference yaw frame at each tick. Positive along-track lag + # means the robot is BEHIND the reference along the reference's + # heading. ``traj_completed_on_time_pct`` is the fraction of the + # expected duration spanned by the run (1.0 if duration unknown). + along_track_lag_rms: float = 0.0 # m + along_track_lag_max: float = 0.0 # m + cross_track_traj_rms: float = 0.0 # m + cross_track_traj_max: float = 0.0 # m + heading_err_traj_rms: float = 0.0 # rad + heading_err_traj_max: float = 0.0 # rad + traj_completed_on_time_pct: float = 0.0 # 0..1 + + +# --------------------------------------------------------------------------- +# 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), + ) + + +# --------------------------------------------------------------------------- +# Trajectory-tracking scoring (time-indexed, decomposed in ref-yaw frame) +# --------------------------------------------------------------------------- + + +def score_run_with_trajectory( + executed: ExecutedTrajectory, + ref_fn: RefFn, + *, + duration_s: float | None = None, +) -> ScoreResult: + """Score against a time-indexed reference ``r(t)``. + + The error vector ``(pose.x - ref.x, pose.y - ref.y)`` is rotated into + the **reference yaw frame** at each tick: + + - ``along_track`` (+ if robot is ahead of reference along its heading) + - ``cross_track`` (+ if robot is to the LEFT of the reference direction) + + Heading error uses :func:`angle_diff` so wrap-around is handled. + + ``traj_completed_on_time_pct`` reports the fraction of ``duration_s`` + spanned by the run. When ``duration_s`` is ``None``, defaults to 1.0 + if any ticks were recorded (analysis is responsible for supplying + the expected duration). + + Path-following fields on the returned :class:`ScoreResult` are zero — + call :func:`score_run` separately if both are needed. + """ + if not executed.ticks: + return ScoreResult(arrived=executed.arrived, n_ticks=0) + + along_lag_sq: list[float] = [] + along_lag_abs: list[float] = [] + cross_sq: list[float] = [] + cross_abs: list[float] = [] + he_sq: list[float] = [] + he_abs: list[float] = [] + lin_sq: list[float] = [] + ang_sq: list[float] = [] + + for tick in executed.ticks: + ref = ref_fn(tick.t) + ex = tick.pose.position.x - ref.x + ey = tick.pose.position.y - ref.y + + cos_y = math.cos(ref.yaw) + sin_y = math.sin(ref.yaw) + # Project world error into ref-yaw frame. Along-track is the + # ref-x component (positive = robot ahead). Lag (the diagnostic + # quantity) is the negative of along-track signed offset: + along_signed = cos_y * ex + sin_y * ey # + = ahead + lag = -along_signed # + = behind, matches "robot is X behind ref" + cross = -sin_y * ex + cos_y * ey # + = left of ref direction + + along_lag_sq.append(lag * lag) + along_lag_abs.append(abs(lag)) + cross_sq.append(cross * cross) + cross_abs.append(abs(cross)) + + he = angle_diff(tick.pose.orientation.euler[2], ref.yaw) + he_sq.append(he * he) + he_abs.append(abs(he)) + + lin_sq.append(_twist_linear_speed(tick.cmd_twist) ** 2) + ang_sq.append(_twist_angular_speed(tick.cmd_twist) ** 2) + + n = len(executed.ticks) + cmd_rate_integral = sum( + _cmd_delta(executed.ticks[i].cmd_twist, executed.ticks[i - 1].cmd_twist) + for i in range(1, n) + ) + + span = executed.ticks[-1].t - executed.ticks[0].t + if duration_s is not None and duration_s > 0.0: + completed_pct = min(1.0, span / duration_s) + else: + completed_pct = 1.0 + + return ScoreResult( + time_to_complete=span, + linear_speed_rms=math.sqrt(sum(lin_sq) / n), + angular_speed_rms=math.sqrt(sum(ang_sq) / n), + cmd_rate_integral=cmd_rate_integral, + arrived=executed.arrived, + n_ticks=n, + along_track_lag_rms=math.sqrt(sum(along_lag_sq) / n), + along_track_lag_max=max(along_lag_abs), + cross_track_traj_rms=math.sqrt(sum(cross_sq) / n), + cross_track_traj_max=max(cross_abs), + heading_err_traj_rms=math.sqrt(sum(he_sq) / n), + heading_err_traj_max=max(he_abs), + traj_completed_on_time_pct=completed_pct, + ) diff --git a/dimos/utils/benchmarking/velocity_profile.py b/dimos/utils/benchmarking/velocity_profile.py new file mode 100644 index 0000000000..96c1be8cc4 --- /dev/null +++ b/dimos/utils/benchmarking/velocity_profile.py @@ -0,0 +1,118 @@ +# 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 + +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 + + +@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", "VelocityProfileConfig"] From e77b511c06e58b5487b803e7e3b0ce65befac8be Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:08:41 -0700 Subject: [PATCH 03/51] fopdt modelling and go2 sim adapter --- .../hardware/drive_trains/go2_sim/adapter.py | 154 +++++++ .../utils/characterization/modeling/fopdt.py | 295 ++++++++++++++ dimos/utils/characterization/trajectories.py | 376 ++++++++++++++++++ 3 files changed, 825 insertions(+) create mode 100644 dimos/hardware/drive_trains/go2_sim/adapter.py create mode 100644 dimos/utils/characterization/modeling/fopdt.py create mode 100644 dimos/utils/characterization/trajectories.py diff --git a/dimos/hardware/drive_trains/go2_sim/adapter.py b/dimos/hardware/drive_trains/go2_sim/adapter.py new file mode 100644 index 0000000000..77e62f7c70 --- /dev/null +++ b/dimos/hardware/drive_trains/go2_sim/adapter.py @@ -0,0 +1,154 @@ +# 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. + +"""Layer 1 sim TwistBase adapter for the Unitree Go2. + +Wraps :class:`~dimos.utils.benchmarking.plant.Go2PlantSim` and presents +the standard :class:`TwistBaseAdapter` protocol so any controller / task / +coordinator that talks to a real Go2 base can be exercised in pure-Python +sim with no hardware. + +Plant integration is wall-clock driven: each :meth:`write_velocities` +call advances the plant by ``time.perf_counter()`` delta since the last +write. The ControlCoordinator's tick loop calls write_velocities once +per tick, so the plant ticks at the coordinator's tick_rate. +""" + +from __future__ import annotations + +import time +from typing import TYPE_CHECKING + +from dimos.utils.benchmarking.plant import GO2_PLANT_FITTED, Go2PlantParams, Go2PlantSim + +if TYPE_CHECKING: + from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry + + +class Go2SimTwistBaseAdapter: + """FOPDT + unicycle sim posing as a real Go2 twist base. + + Implements :class:`TwistBaseAdapter`. ``dof`` is fixed at 3 for the + holonomic Go2 model (vx, vy, wz); commanding non-zero vy is a sim + artifact in the sim ground truth (the real Go2 strafes — characterize + real vy via ``go2_characterization --mode hw``). + """ + + def __init__( + self, + dof: int = 3, + params: Go2PlantParams | None = None, + initial_pose: tuple[float, float, float] = (0.0, 0.0, 0.0), + nominal_dt: float = 0.1, + **_: object, + ) -> None: + if dof != 3: + raise ValueError(f"Go2SimTwistBaseAdapter requires dof=3, got {dof}") + self._dof = dof + self._params = params if params is not None else GO2_PLANT_FITTED + self._plant = Go2PlantSim(self._params) + self._initial_pose = initial_pose + self._nominal_dt = nominal_dt + + self._cmd: list[float] = [0.0, 0.0, 0.0] + self._last_step_time: float | None = None + self._enabled = False + self._connected = False + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + self._plant.reset(*self._initial_pose, dt=self._nominal_dt) + self._last_step_time = None + self._connected = True + return True + + def disconnect(self) -> None: + self._connected = False + + def is_connected(self) -> bool: + return self._connected + + # ========================================================================= + # Info + # ========================================================================= + + def get_dof(self) -> int: + return self._dof + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_velocities(self) -> list[float]: + """Return plant's actual filtered velocities (m/s, m/s, rad/s).""" + return [self._plant.vx, self._plant.vy, self._plant.wz] + + def read_odometry(self) -> list[float] | None: + """Return plant's integrated pose [x (m), y (m), yaw (rad)].""" + return [self._plant.x, self._plant.y, self._plant.yaw] + + # ========================================================================= + # Control + # ========================================================================= + + def write_velocities(self, velocities: list[float]) -> bool: + """Advance plant under ZOH of the prior cmd, then latch new cmd.""" + if len(velocities) != self._dof: + return False + now = time.perf_counter() + if self._last_step_time is not None: + dt = now - self._last_step_time + if dt > 0: + self._plant.step(self._cmd[0], self._cmd[1], self._cmd[2], dt) + self._cmd = list(velocities) + self._last_step_time = now + return True + + def write_stop(self) -> bool: + return self.write_velocities([0.0, 0.0, 0.0]) + + # ========================================================================= + # Enable/Disable + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + self._enabled = enable + return True + + def read_enabled(self) -> bool: + return self._enabled + + # ========================================================================= + # Sim helpers (not part of Protocol) + # ========================================================================= + + @property + def plant(self) -> Go2PlantSim: + """Direct access to the underlying plant (for inspection / tests).""" + return self._plant + + def set_initial_pose(self, x: float, y: float, yaw: float) -> None: + """Override the start pose. Takes effect on next :meth:`connect`.""" + self._initial_pose = (x, y, yaw) + + +def register(registry: TwistBaseAdapterRegistry) -> None: + """Register this adapter with the registry under ``go2_sim_twist_base``.""" + registry.register("go2_sim_twist_base", Go2SimTwistBaseAdapter) + + +__all__ = ["Go2SimTwistBaseAdapter"] diff --git a/dimos/utils/characterization/modeling/fopdt.py b/dimos/utils/characterization/modeling/fopdt.py new file mode 100644 index 0000000000..deac609dd0 --- /dev/null +++ b/dimos/utils/characterization/modeling/fopdt.py @@ -0,0 +1,295 @@ +# Copyright 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. + +"""First-Order Plus Deadtime (FOPDT) model + fitter. + +Step response (cmd 0 -> ``u_step`` at t=0, sample times in ``t``): + + y(t) = 0 for t < L + y(t) = K * u_step * (1 - exp(-(t - L) / tau)) for t >= L + +Three parameters per channel: + + K steady-state gain + tau time constant (~63% of K * u_step reached at t = L + tau) + L deadtime / pure delay before any response begins + +Fit uses ``scipy.optimize.curve_fit`` (Levenberg-Marquardt with bounds). +Initial guesses are derived from the trace itself (steady-state span, +time-to-63%, first-sample-above-noise-floor) - bad initial guesses send +the optimizer to bad local minima for nonlinear fits. +""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +from typing import Any + +import numpy as np + +# --- Bounds. Channel-aware bounds would be slightly tighter but these are +# generous enough to avoid clipping good fits while still preventing the +# optimizer from running off into the weeds. +_K_ABS_MAX = 5.0 +_TAU_MIN = 1e-3 +_TAU_MAX = 5.0 +_L_MIN = 0.0 +_L_MAX = 1.0 + + +@dataclass +class FopdtParams: + """Result of a single FOPDT fit. ``converged=False`` means the optimizer + reported failure or the input was degenerate; in that case all numeric + fields are NaN and ``reason`` explains why. + """ + + K: float + tau: float + L: float + K_ci: tuple[float, float] # 95% CI as (low, high); (nan, nan) if degenerate + tau_ci: tuple[float, float] + L_ci: tuple[float, float] + rmse: float + r_squared: float + n_samples: int + fit_window_s: tuple[float, float] # (t_start, t_end) relative to step edge + degenerate: bool # singular covariance => point estimates only + converged: bool + reason: str | None = None + initial_guess: dict[str, float] = field(default_factory=dict) + + def asdict(self) -> dict[str, Any]: + return asdict(self) + + +def fopdt_step_response(t: np.ndarray, K: float, tau: float, L: float, u_step: float) -> np.ndarray: + """Vectorized FOPDT step response. ``t`` is time relative to step edge.""" + t = np.asarray(t, dtype=float) + out = np.zeros_like(t) + mask = t >= L + if tau <= 0.0: + return out + out[mask] = K * u_step * (1.0 - np.exp(-(t[mask] - L) / tau)) + return out + + +def _initial_guess( + t: np.ndarray, y: np.ndarray, u_step: float, noise_std: float | None +) -> tuple[float, float, float]: + """Derive (K, tau, L) initial guesses from the data. + + K_init: steady-state span (mean of last 20% of post-step samples) + divided by ``u_step``. + L_init: first time the response leaves the noise band ``3 * noise_std`` + (or ``1e-3`` fallback). Pulled in slightly from where rise + actually begins so curve_fit doesn't have to climb out of a + zero-gradient region. + tau_init: first time the response crosses ``0.63 * (K * u_step)``, + minus L. Falls back to a sensible default if the trace + never makes it that far. + """ + if t.size < 4: + # Caller should have rejected this; provide a non-zero guess so we + # don't hit divide-by-zero before the fit fails cleanly. + return (1.0, 0.2, 0.05) + + n = t.size + tail_n = max(1, int(n * 0.2)) + y_tail = float(np.mean(y[-tail_n:])) + K_init = y_tail / u_step if abs(u_step) > 1e-9 else 1.0 + K_init = float(np.clip(K_init, -_K_ABS_MAX * 0.99, _K_ABS_MAX * 0.99)) + if abs(K_init) < 1e-3: + K_init = 0.5 if u_step >= 0 else -0.5 + + band = 3.0 * (noise_std if noise_std and noise_std > 0 else 1e-3) + above = np.flatnonzero(np.abs(y) > band) + if above.size: + L_init = float(t[above[0]]) + else: + L_init = 0.05 + L_init = float(np.clip(L_init, _L_MIN, _L_MAX * 0.99)) + + target = 0.63 * K_init * u_step + if abs(target) > 1e-6: + if K_init * u_step > 0: + crossed = np.flatnonzero(y >= target) + else: + crossed = np.flatnonzero(y <= target) + if crossed.size: + tau_init = float(t[crossed[0]] - L_init) + else: + tau_init = 0.3 + else: + tau_init = 0.3 + tau_init = float(np.clip(tau_init, _TAU_MIN * 10, _TAU_MAX * 0.99)) + + return (K_init, tau_init, L_init) + + +def _bounds_for(u_step: float) -> tuple[tuple[float, float, float], tuple[float, float, float]]: + """Bounds tuple ((lo_K, lo_tau, lo_L), (hi_K, hi_tau, hi_L)). + + K is unsigned-bounded: the fit recovers the signed gain from the data, + independent of ``u_step``'s sign. Bounding K to one sign would + actually rule out reasonable fits where the plant inverts. + """ + return ( + (-_K_ABS_MAX, _TAU_MIN, _L_MIN), + (_K_ABS_MAX, _TAU_MAX, _L_MAX), + ) + + +def fit_fopdt( + t: np.ndarray, + y: np.ndarray, + u_step: float, + *, + noise_std: float | None = None, + fit_window_s: tuple[float, float] | None = None, +) -> FopdtParams: + """Fit FOPDT to a step-response trace. + + ``t`` is time relative to the step edge (so the step happens at t=0). + ``y`` is the measured response with pre-step baseline already + subtracted. ``u_step`` is the commanded step amplitude (signed). + + ``noise_std`` (optional) is per-sample sigma for weighted least + squares. ``fit_window_s`` is recorded into the result for traceability. + """ + from scipy.optimize import curve_fit + + t = np.asarray(t, dtype=float) + y = np.asarray(y, dtype=float) + + fit_window = ( + fit_window_s + if fit_window_s is not None + else ((float(t[0]), float(t[-1])) if t.size else (0.0, 0.0)) + ) + + if t.size < 4: + return FopdtParams( + K=float("nan"), + tau=float("nan"), + L=float("nan"), + K_ci=(float("nan"), float("nan")), + tau_ci=(float("nan"), float("nan")), + L_ci=(float("nan"), float("nan")), + rmse=float("nan"), + r_squared=float("nan"), + n_samples=int(t.size), + fit_window_s=fit_window, + degenerate=True, + converged=False, + reason="fewer than 4 samples in fit window", + ) + + if abs(u_step) < 1e-9: + return FopdtParams( + K=float("nan"), + tau=float("nan"), + L=float("nan"), + K_ci=(float("nan"), float("nan")), + tau_ci=(float("nan"), float("nan")), + L_ci=(float("nan"), float("nan")), + rmse=float("nan"), + r_squared=float("nan"), + n_samples=int(t.size), + fit_window_s=fit_window, + degenerate=True, + converged=False, + reason="u_step is zero - cannot identify K", + ) + + K0, tau0, L0 = _initial_guess(t, y, u_step, noise_std) + p0 = (K0, tau0, L0) + lo, hi = _bounds_for(u_step) + + sigma = None + if noise_std is not None and noise_std > 0: + sigma = np.full_like(y, float(noise_std)) + + def _model(t_, K, tau, L): + return fopdt_step_response(t_, K, tau, L, u_step) + + try: + popt, pcov = curve_fit( + _model, + t, + y, + p0=p0, + bounds=(lo, hi), + sigma=sigma, + absolute_sigma=False, + maxfev=5000, + ) + except Exception as e: + return FopdtParams( + K=float("nan"), + tau=float("nan"), + L=float("nan"), + K_ci=(float("nan"), float("nan")), + tau_ci=(float("nan"), float("nan")), + L_ci=(float("nan"), float("nan")), + rmse=float("nan"), + r_squared=float("nan"), + n_samples=int(t.size), + fit_window_s=fit_window, + degenerate=True, + converged=False, + reason=f"curve_fit failed: {type(e).__name__}: {e}", + initial_guess={"K": K0, "tau": tau0, "L": L0}, + ) + + K, tau, L = (float(popt[0]), float(popt[1]), float(popt[2])) + y_hat = _model(t, K, tau, L) + resid = y - y_hat + rmse = float(np.sqrt(np.mean(resid**2))) if resid.size else float("nan") + ss_res = float(np.sum(resid**2)) + y_mean = float(np.mean(y)) + ss_tot = float(np.sum((y - y_mean) ** 2)) + r2 = 1.0 - ss_res / ss_tot if ss_tot > 0 else float("nan") + + diag = np.diag(pcov) + degenerate = bool(np.any(~np.isfinite(diag)) or np.any(diag <= 0)) + if degenerate: + K_ci = (float("nan"), float("nan")) + tau_ci = (float("nan"), float("nan")) + L_ci = (float("nan"), float("nan")) + else: + sigmas = np.sqrt(diag) + K_ci = (K - 1.96 * float(sigmas[0]), K + 1.96 * float(sigmas[0])) + tau_ci = (tau - 1.96 * float(sigmas[1]), tau + 1.96 * float(sigmas[1])) + L_ci = (L - 1.96 * float(sigmas[2]), L + 1.96 * float(sigmas[2])) + + return FopdtParams( + K=K, + tau=tau, + L=L, + K_ci=K_ci, + tau_ci=tau_ci, + L_ci=L_ci, + rmse=rmse, + r_squared=r2, + n_samples=int(t.size), + fit_window_s=fit_window, + degenerate=degenerate, + converged=True, + reason=None, + initial_guess={"K": K0, "tau": tau0, "L": L0}, + ) + + +__all__ = ["FopdtParams", "fit_fopdt", "fopdt_step_response"] diff --git a/dimos/utils/characterization/trajectories.py b/dimos/utils/characterization/trajectories.py new file mode 100644 index 0000000000..5f0da2cc65 --- /dev/null +++ b/dimos/utils/characterization/trajectories.py @@ -0,0 +1,376 @@ +# 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. + +"""Time-indexed SE(2) reference trajectories for plant-bottleneck diagnosis. + +A ``Trajectory`` is a callable ``ref_fn(t) -> TrajRefState`` plus a +duration, a recommended controller mode, and a serializable spec for +replay from ``run.json``. + +The reference is the **kinematic ideal** under the unicycle model +starting at the origin facing +x. A perfectly-behaved plant would still +show ``along_track_lag ~ L + tau`` against this reference — that is the +expected baseline, not a failure mode. See ``score_run_with_trajectory``. + +Each helper picks a recommended controller mode. Open-loop FF is reserved +for short trials and trials with no sustained yaw rate (no compounding +integration drift in yaw). Sustained or oscillating ``wz`` needs the +low-gain P fallback to keep the robot on the reference long enough for +the e(t) decomposition to be interpretable. +""" + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import dataclass, field +import math +from typing import Any, Literal + +import numpy as np + +ControllerMode = Literal["openloop_ff", "lowgain_p"] + + +@dataclass(frozen=True) +class TrajRefState: + """Reference pose + velocity at a given time.""" + + t: float + x: float + y: float + yaw: float + vx: float + wz: float + + +RefFn = Callable[[float], TrajRefState] + + +@dataclass(frozen=True) +class Trajectory: + """A time-indexed reference plus its replay descriptor. + + ``spec`` is JSON-serializable and round-trips through + :func:`trajectory_from_spec`. It is what ``run.json`` carries so + ``diagnose.py`` can rebuild ``ref_fn`` long after the run. + """ + + ref_fn: RefFn + duration_s: float + recommended_mode: ControllerMode + spec: dict[str, Any] = field(default_factory=dict) + + +# --------------------------------------------------------------------------- +# Primitives — closed-form integrals of the unicycle model +# --------------------------------------------------------------------------- + + +def straight(v: float, duration: float) -> Trajectory: + """Constant ``vx=v``, ``wz=0`` from the origin along +x for ``duration`` seconds.""" + + def ref_fn(t: float) -> TrajRefState: + t = _clip(t, 0.0, duration) + return TrajRefState(t=t, x=v * t, y=0.0, yaw=0.0, vx=v, wz=0.0) + + return Trajectory( + ref_fn=ref_fn, + duration_s=duration, + recommended_mode="openloop_ff", + spec={"kind": "straight", "v": v, "duration": duration}, + ) + + +def circle(v: float, w: float, duration: float) -> Trajectory: + """Constant ``vx=v, wz=w`` from the origin. Radius = ``v/w``.""" + if abs(w) < 1e-9: + return straight(v, duration) + + inv_w = 1.0 / w + + def ref_fn(t: float) -> TrajRefState: + t = _clip(t, 0.0, duration) + yaw = w * t + x = v * inv_w * math.sin(yaw) + y = v * inv_w * (1.0 - math.cos(yaw)) + return TrajRefState(t=t, x=x, y=y, yaw=yaw, vx=v, wz=w) + + return Trajectory( + ref_fn=ref_fn, + duration_s=duration, + recommended_mode="lowgain_p", + spec={"kind": "circle", "v": v, "w": w, "duration": duration}, + ) + + +def step_vx(v_target: float, duration: float, *, t_step: float = 0.5) -> Trajectory: + """Zero until ``t_step``, then constant ``vx=v_target``, ``wz=0``.""" + + def ref_fn(t: float) -> TrajRefState: + t = _clip(t, 0.0, duration) + if t < t_step: + return TrajRefState(t=t, x=0.0, y=0.0, yaw=0.0, vx=0.0, wz=0.0) + return TrajRefState(t=t, x=v_target * (t - t_step), y=0.0, yaw=0.0, vx=v_target, wz=0.0) + + return Trajectory( + ref_fn=ref_fn, + duration_s=duration, + recommended_mode="openloop_ff", + spec={ + "kind": "step_vx", + "v_target": v_target, + "duration": duration, + "t_step": t_step, + }, + ) + + +def step_wz( + vx: float, + w_target: float, + duration: float, + *, + t_step: float = 0.5, +) -> Trajectory: + """Straight at ``vx`` until ``t_step``, then constant ``wz=w_target`` while still moving at ``vx``.""" + + if abs(w_target) < 1e-9: + return straight(vx, duration) + + inv_w = 1.0 / w_target + + def ref_fn(t: float) -> TrajRefState: + t = _clip(t, 0.0, duration) + if t < t_step: + return TrajRefState(t=t, x=vx * t, y=0.0, yaw=0.0, vx=vx, wz=0.0) + x0 = vx * t_step + tau_active = t - t_step + yaw = w_target * tau_active + x = x0 + vx * inv_w * math.sin(yaw) + y = vx * inv_w * (1.0 - math.cos(yaw)) + return TrajRefState(t=t, x=x, y=y, yaw=yaw, vx=vx, wz=w_target) + + return Trajectory( + ref_fn=ref_fn, + duration_s=duration, + recommended_mode="openloop_ff", + spec={ + "kind": "step_wz", + "vx": vx, + "w_target": w_target, + "duration": duration, + "t_step": t_step, + }, + ) + + +def trapezoidal_vx(v_max: float, accel: float, duration: float) -> Trajectory: + """Trapezoidal ``vx`` profile: ramp 0 → v_max → 0 with magnitude ``accel`` m/s^2, ``wz=0``. + + The hold portion is whatever's left after accel + decel time. If + ``duration`` is too short to reach ``v_max``, hold time is zero and + the profile is triangular. + """ + if accel <= 0.0 or v_max <= 0.0: + raise ValueError(f"accel and v_max must be positive (got {accel=}, {v_max=})") + + t_accel = v_max / accel + if 2.0 * t_accel > duration: + # Triangular: v_peak is whatever we reach in duration/2 at accel + t_accel = duration / 2.0 + v_peak = accel * t_accel + t_hold = 0.0 + else: + v_peak = v_max + t_hold = duration - 2.0 * t_accel + t_decel_start = t_accel + t_hold + + # Accumulated x at each phase boundary + x_at_accel_end = 0.5 * accel * t_accel * t_accel + x_at_hold_end = x_at_accel_end + v_peak * t_hold + + def ref_fn(t: float) -> TrajRefState: + t = _clip(t, 0.0, duration) + if t < t_accel: + v = accel * t + x = 0.5 * accel * t * t + elif t < t_decel_start: + v = v_peak + x = x_at_accel_end + v_peak * (t - t_accel) + else: + dt_decel = t - t_decel_start + v = max(0.0, v_peak - accel * dt_decel) + x = x_at_hold_end + v_peak * dt_decel - 0.5 * accel * dt_decel * dt_decel + return TrajRefState(t=t, x=x, y=0.0, yaw=0.0, vx=v, wz=0.0) + + return Trajectory( + ref_fn=ref_fn, + duration_s=duration, + recommended_mode="openloop_ff", + spec={ + "kind": "trapezoidal_vx", + "v_max": v_max, + "accel": accel, + "duration": duration, + }, + ) + + +def sinusoidal_wz( + vx: float, + w_amp: float, + freq_hz: float, + duration: float, + *, + integration_dt: float = 0.001, +) -> Trajectory: + """Constant ``vx``, ``wz = w_amp * sin(2*pi*freq_hz*t)``. + + No closed form for the position integral (cos of an oscillating + argument). Precomputes a fine numerical integration once at + construction; ``ref_fn`` linearly interpolates that grid. + """ + if integration_dt <= 0.0: + raise ValueError(f"integration_dt must be positive (got {integration_dt=})") + + n = math.ceil(duration / integration_dt) + 1 + ts = np.linspace(0.0, duration, n) + omega = 2.0 * math.pi * freq_hz + + # yaw(t) = integral of wz(s) ds = (w_amp/omega) * (1 - cos(omega*t)) + if omega < 1e-9: + yaws = np.zeros(n) + else: + yaws = (w_amp / omega) * (1.0 - np.cos(omega * ts)) + + cos_yaw = np.cos(yaws) + sin_yaw = np.sin(yaws) + xs = np.zeros(n) + ys = np.zeros(n) + # Trapezoidal integration of (vx*cos(yaw), vx*sin(yaw)) + for i in range(1, n): + dt = ts[i] - ts[i - 1] + xs[i] = xs[i - 1] + 0.5 * vx * (cos_yaw[i] + cos_yaw[i - 1]) * dt + ys[i] = ys[i - 1] + 0.5 * vx * (sin_yaw[i] + sin_yaw[i - 1]) * dt + + def ref_fn(t: float) -> TrajRefState: + t = _clip(t, 0.0, duration) + # Linear interpolation on the precomputed grid + x = float(np.interp(t, ts, xs)) + y = float(np.interp(t, ts, ys)) + yaw = float(np.interp(t, ts, yaws)) + wz = w_amp * math.sin(omega * t) + return TrajRefState(t=t, x=x, y=y, yaw=yaw, vx=vx, wz=wz) + + return Trajectory( + ref_fn=ref_fn, + duration_s=duration, + recommended_mode="lowgain_p", + spec={ + "kind": "sinusoidal_wz", + "vx": vx, + "w_amp": w_amp, + "freq_hz": freq_hz, + "duration": duration, + }, + ) + + +# --------------------------------------------------------------------------- +# Start-pose anchoring +# --------------------------------------------------------------------------- + + +def anchor_trajectory( + ref_fn: RefFn, + start_x: float, + start_y: float, + start_yaw: float, +) -> RefFn: + """Wrap ``ref_fn`` so its output is expressed in the world frame. + + Trajectory primitives are defined in a local frame: the robot starts + at the origin facing +x. On a real robot the start pose is wherever + odom puts it. Without this transform, ``e(t) = pose - ref`` measures + the fixed start-pose offset, not plant behavior. + + Applies the SE(2) transform ``(start_x, start_y, start_yaw)`` to the + position and heading. Body-frame ``vx``/``wz`` are invariant under a + rigid transform of the path and pass through unchanged. + """ + cos_y = math.cos(start_yaw) + sin_y = math.sin(start_yaw) + + def wrapped(t: float) -> TrajRefState: + loc = ref_fn(t) + return TrajRefState( + t=loc.t, + x=start_x + cos_y * loc.x - sin_y * loc.y, + y=start_y + sin_y * loc.x + cos_y * loc.y, + yaw=start_yaw + loc.yaw, + vx=loc.vx, + wz=loc.wz, + ) + + return wrapped + + +# --------------------------------------------------------------------------- +# Replay +# --------------------------------------------------------------------------- + + +_FACTORIES: dict[str, Callable[..., Trajectory]] = { + "straight": straight, + "circle": circle, + "step_vx": step_vx, + "step_wz": step_wz, + "trapezoidal_vx": trapezoidal_vx, + "sinusoidal_wz": sinusoidal_wz, +} + + +def trajectory_from_spec(spec: dict[str, Any]) -> Trajectory: + """Reconstruct a :class:`Trajectory` from its replay ``spec`` dict. + + Used by ``diagnose.py`` to rebuild ``ref_fn`` from ``run.json``. + """ + kind = spec.get("kind") + factory = _FACTORIES.get(kind) if isinstance(kind, str) else None + if factory is None: + raise ValueError(f"unknown trajectory kind: {kind!r}") + kwargs = {k: v for k, v in spec.items() if k != "kind"} + return factory(**kwargs) + + +# --------------------------------------------------------------------------- + + +def _clip(t: float, lo: float, hi: float) -> float: + return max(lo, min(hi, t)) + + +__all__ = [ + "ControllerMode", + "RefFn", + "TrajRefState", + "Trajectory", + "circle", + "sinusoidal_wz", + "step_vx", + "step_wz", + "straight", + "trajectory_from_spec", + "trapezoidal_vx", +] From 1db91e4d9338792e7a88546196d969b9e5f01d06 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:10:02 -0700 Subject: [PATCH 04/51] dataclassses for plant modelling and tuning --- dimos/utils/benchmarking/go2_tuning.py | 445 +++++++++++++++++++++++++ 1 file changed, 445 insertions(+) create mode 100644 dimos/utils/benchmarking/go2_tuning.py diff --git a/dimos/utils/benchmarking/go2_tuning.py b/dimos/utils/benchmarking/go2_tuning.py new file mode 100644 index 0000000000..50d9326bc7 --- /dev/null +++ b/dimos/utils/benchmarking/go2_tuning.py @@ -0,0 +1,445 @@ +# 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. + +"""Go2 tuning config artifact + the DERIVE step (model -> config). + +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_go2_tuning.py``). +* :class:`Go2TuningConfig` 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/go2_tuning_README.md``): the Go2 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 Go2PlantParams +from dimos.utils.benchmarking.velocity_profile import ( + GO2_VX_MAX, + GO2_WZ_MAX, + VelocityProfileConfig, +) + +SCHEMA_VERSION = 1 + +# --- 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/go2_tuning_README.md and the " + "characterization findings." +) + + +# --- 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 = "" + + +@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] + + +@dataclass +class Go2TuningConfig: + 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 + 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) -> Go2TuningConfig: + data = json.loads(Path(path).read_text()) + return cls.from_dict(data) + + @classmethod + def from_dict(cls, data: dict) -> Go2TuningConfig: + 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"]], + ) + return cls( + provenance=Provenance(**data["provenance"]), + 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, + 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 _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 Go2 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: Go2PlantParams, + provenance: Provenance, + *, + per_amplitude: dict | None = None, +) -> Go2TuningConfig: + """Derive the full controller config from a fitted FOPDT plant model. + + Pure: model + provenance in, :class:`Go2TuningConfig` out. No I/O. + + - Feedforward gain per axis = ``1 / K`` (the compensator divides the + controller command by the plant gain so commanded == achieved). + - ``max_angular_speed`` = measured wz ceiling minus the cross-track + headroom margin. + - ``max_centripetal_accel`` = the lateral comfort constant. + - ``max_linear_accel`` ~= ``vx_ceiling / tau_vx`` (first-order rise: + a step of size v settles in ~tau, so the achievable mean accel is + ~v/tau); decel = ``DECEL_ACCEL_RATIO x`` that. + - recommended controller = baseline, hardcoded, with cited evidence. + + ``per_amplitude`` (optional) is the fitter's per-amplitude table + ``{channel: [{amplitude, K, ...}, ...]}``; when absent the Go2 + saturation envelope is used for the ceilings. + """ + # Clamp the measured ceiling to the Go2 saturation envelope: an + # un-saturated FOPDT fit extrapolates linearly past what the platform + # can physically deliver, so the envelope is a hard upper bound. + vx_ceiling = min(_channel_ceiling(per_amplitude, "vx", GO2_VX_MAX), GO2_VX_MAX) + wz_ceiling = min(_channel_ceiling(per_amplitude, "wz", GO2_WZ_MAX), GO2_WZ_MAX) + + 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 GO2_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, + ) + + caveats = [ + f"Valid only for surface={provenance.surface!r}, " + f"mode={provenance.mode!r}, {provenance.sim_or_hw}. Re-run " + f"go2-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}).", + ] + if provenance.sim_or_hw == "sim": + caveats.append( + "Derived from the FOPDT sim plant (self-test): validates the " + "pipeline, not absolute on-robot numbers — re-run --mode hw " + "for hardware-valid gains." + ) + + return Go2TuningConfig( + 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, + ) + + +# --- 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__ = [ + "SCHEMA_VERSION", + "FeedforwardDC", + "FopdtChannelDC", + "Go2TuningConfig", + "OperatingPoint", + "OperatingPointMap", + "PlantModelDC", + "Provenance", + "RecommendedControllerDC", + "ToleranceRow", + "VelocityProfileDC", + "derive_config", + "git_sha", + "invert_tolerance", +] From d3a0a8658a048303fb1d59904e1ec8f9fb0e30e0 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:10:33 -0700 Subject: [PATCH 05/51] system identification harness --- .../benchmarking/go2_characterization.py | 243 ++++++++++++++++++ 1 file changed, 243 insertions(+) create mode 100644 dimos/utils/benchmarking/go2_characterization.py diff --git a/dimos/utils/benchmarking/go2_characterization.py b/dimos/utils/benchmarking/go2_characterization.py new file mode 100644 index 0000000000..6965cb7bfb --- /dev/null +++ b/dimos/utils/benchmarking/go2_characterization.py @@ -0,0 +1,243 @@ +# 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. + +"""Tool 1 of the Go2 tuning deliverable: characterization. + +Runs a space-cheap system-ID sequence (per-channel velocity steps — no +long paths), fits FOPDT per axis (vx, vy, wz), then runs the DERIVE step +and emits the versioned config artifact. + + uv run python -m dimos.utils.benchmarking.go2_characterization \\ + --mode sim --surface mujoco + +**One harness, plant swapped by ``--mode``** — exactly the same SI loop +and fitter; only *where the robot behaves* differs: + +* ``sim``: the plant is the in-process FOPDT ``Go2PlantSim`` seeded with + the vendored ``GO2_PLANT_FITTED`` ground truth. A healthy run recovers + the injected model (printed "recovered vs injected" table) — this + self-tests the whole measure->fit->derive pipeline without a robot. +* ``hw``: the plant is the real Go2 over LCM (publish ``/cmd_vel``, + differentiate ``/go2/odom`` to body-frame velocity). Wired; not + exercised by CI/sim. + +Both modes record cmd-vs-measured per channel and fit with the same +``fit_fopdt``; there is no separate hardware data-acquisition pipeline. +The tail (sections 1-4 + 6; section 5 left ``None`` for the benchmark +tool) is the pure ``derive_config``. +""" + +from __future__ import annotations + +import argparse +from datetime import date +from pathlib import Path +import time + +import numpy as np + +from dimos.utils.benchmarking.go2_tuning import Provenance, derive_config, git_sha +from dimos.utils.benchmarking.plant import ( + GO2_PLANT_FITTED, + FopdtChannelParams, + Go2PlantParams, + Go2PlantSim, +) +from dimos.utils.characterization.modeling.fopdt import fit_fopdt + +# Space-cheap SI plan: a few amplitudes per channel. vy is a real channel +# (the Go2 base strafes) so it gets its own sweep — not a copy of vx. +_SI_AMPLITUDES: dict[str, list[float]] = { + "vx": [0.3, 0.6, 0.9], + "vy": [0.2, 0.4], + "wz": [0.4, 0.8, 1.2], +} +_DT = 0.02 # 50 Hz sample period +_PRE_ROLL_S = 1.0 +_STEP_S = 5.0 # >> max (tau + L) so the channel fully settles + +_CHANNELS = ("vx", "vy", "wz") +REPORTS_DIR = Path(__file__).parent / "reports" + + +# --- the swap point: a plant you step with a velocity command ------------ + + +class _SimPlant: + """Sim: the in-process FOPDT ``Go2PlantSim`` (vendored ground truth).""" + + is_hw = False + + def __init__(self) -> None: + self._p = Go2PlantSim(GO2_PLANT_FITTED) + + def reset(self) -> None: + self._p.reset(0.0, 0.0, 0.0, _DT) + + def step(self, cmd: dict[str, float]) -> dict[str, float]: + self._p.step(cmd["vx"], cmd["vy"], cmd["wz"], _DT) + return {"vx": self._p.vx, "vy": self._p.vy, "wz": self._p.wz} + + +class _HwPlant: + """HW: the real Go2 over LCM — publish ``/cmd_vel``, differentiate + ``/go2/odom`` to body-frame velocity. Wired; not run by CI/sim. + + Same ``reset``/``step`` surface as :class:`_SimPlant`, so the SI loop + is identical regardless of mode. + """ + + is_hw = True + + def __init__(self) -> None: + import math + + 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.geometry_msgs.Vector3 import Vector3 + + self._math = math + self._Twist, self._Vector3 = Twist, Vector3 + self._cmd_pub = LCMTransport("/cmd_vel", Twist) + self._odom_sub = LCMTransport("/go2/odom", PoseStamped) + self._pose = None + self._odom_sub.subscribe(self._on_odom) + self._prev = None + + def _on_odom(self, msg) -> None: + self._pose = msg + + def reset(self) -> None: + self._prev = None + + def step(self, cmd: dict[str, float]) -> dict[str, float]: + m = self._math + self._cmd_pub.broadcast( + None, + self._Twist( + linear=self._Vector3(cmd["vx"], cmd["vy"], 0.0), + angular=self._Vector3(0.0, 0.0, cmd["wz"]), + ), + ) + time.sleep(_DT) + p = self._pose + if p is None: + return {"vx": 0.0, "vy": 0.0, "wz": 0.0} + yaw = p.orientation.euler[2] + cur = (p.position.x, p.position.y, yaw, time.perf_counter()) + if self._prev is None: + self._prev = cur + return {"vx": 0.0, "vy": 0.0, "wz": 0.0} + dx, dy = cur[0] - self._prev[0], cur[1] - self._prev[1] + dyaw = (cur[2] - self._prev[2] + m.pi) % (2 * m.pi) - m.pi + dt = max(cur[3] - self._prev[3], 1e-3) + self._prev = cur + c, s = m.cos(yaw), m.sin(yaw) + return { + "vx": (dx * c + dy * s) / dt, + "vy": (-dx * s + dy * c) / dt, + "wz": dyaw / dt, + } + + +# --- the one SI loop (mode-independent) ---------------------------------- + + +def _run_si(plant) -> tuple[Go2PlantParams, dict]: + """Step every channel/amplitude through ``plant``, fit FOPDT per + channel, pool. Identical for sim and hw — only ``plant`` differs.""" + n_pre = int(_PRE_ROLL_S / _DT) + n_step = int(_STEP_S / _DT) + pooled: dict[str, FopdtChannelParams] = {} + per_amplitude: dict[str, list[dict]] = {} + + for channel in _CHANNELS: + fits = [] + per_amplitude[channel] = [] + for amp in _SI_AMPLITUDES[channel]: + plant.reset() + cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + for _ in range(n_pre): + plant.step(cmd) + cmd[channel] = amp + ys = [plant.step(cmd)[channel] for _ in range(n_step)] + t = np.arange(len(ys), dtype=float) * _DT # rel. to step edge + fp = fit_fopdt(t, np.asarray(ys, dtype=float), u_step=amp) + if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): + print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") + continue + fits.append(fp) + per_amplitude[channel].append( + {"amplitude": amp, "direction": "forward", "K": fp.K, "tau": fp.tau, "L": fp.L} + ) + if not fits: + raise RuntimeError(f"SI: no converged fits for channel {channel!r}") + pooled[channel] = FopdtChannelParams( + K=float(np.mean([f.K for f in fits])), + tau=float(np.mean([f.tau for f in fits])), + L=float(np.mean([f.L for f in fits])), + ) + + fitted = Go2PlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]) + if not plant.is_hw: + _print_selftest(fitted) + return fitted, per_amplitude + + +def _print_selftest(fitted: Go2PlantParams) -> None: + """sim only: recovered vs injected ground truth — should match.""" + print("\nSI self-test (recovered vs injected FOPDT ground truth):") + print(f" {'chan':4} {'K fit/true':>20} {'tau fit/true':>20} {'L fit/true':>20}") + for ch in _CHANNELS: + f, g = getattr(fitted, ch), getattr(GO2_PLANT_FITTED, ch) + print( + f" {ch:4} {f.K:8.3f}/{g.K:<8.3f} {f.tau:8.3f}/{g.tau:<8.3f} {f.L:8.3f}/{g.L:<8.3f}" + ) + + +def main() -> None: + ap = argparse.ArgumentParser(description="Go2 characterization -> tuning config artifact") + ap.add_argument("--mode", choices=["sim", "hw"], default="sim") + ap.add_argument("--out", default=str(REPORTS_DIR), help="output dir for the artifact") + ap.add_argument("--robot-id", default="go2") + ap.add_argument("--surface", default="mujoco") + ap.add_argument("--gait-mode", default="default", help="locomotion gait mode") + args = ap.parse_args() + + plant = _SimPlant() if args.mode == "sim" else _HwPlant() + fitted, per_amplitude = _run_si(plant) + + provenance = Provenance( + robot_id=args.robot_id, + surface=args.surface, + mode=args.gait_mode, + date=date.today().isoformat(), + git_sha=git_sha(), + sim_or_hw=args.mode, + characterization_session_dir="(in-process SI)" if args.mode == "sim" else "(hw LCM SI)", + ) + cfg = derive_config(fitted, provenance, per_amplitude=per_amplitude) + + out_path = ( + Path(args.out).expanduser() + / f"go2_config_{args.mode}_{args.surface}_{provenance.date}_{provenance.git_sha}.json" + ) + cfg.to_json(out_path) + print("\nWrote config artifact (sections 1-4,6; section 5 pending benchmark):") + print(out_path.resolve()) + + +if __name__ == "__main__": + main() From 0a2100095cb3629cccd9103d381475bae8b44bec Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:11:08 -0700 Subject: [PATCH 06/51] benchmark tool for go2 trajectory testing --- dimos/utils/benchmarking/go2_benchmark.py | 353 ++++++++++++++++++++++ 1 file changed, 353 insertions(+) create mode 100644 dimos/utils/benchmarking/go2_benchmark.py diff --git a/dimos/utils/benchmarking/go2_benchmark.py b/dimos/utils/benchmarking/go2_benchmark.py new file mode 100644 index 0000000000..91293c527b --- /dev/null +++ b/dimos/utils/benchmarking/go2_benchmark.py @@ -0,0 +1,353 @@ +# 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. + +"""Tool 2 of the Go2 tuning deliverable: the operating-point benchmark. + +Consumes the config artifact from ``go2_characterization``, runs the +HARDCODED baseline P-controller with the derived feedforward + velocity +profile across a speed ladder on a fixed real-space-constrained path set, +scores each (path, speed), and writes back the operating-point map + +tolerance->max-safe-speed inversion (artifact section 5). + + uv run python -m dimos.utils.benchmarking.go2_benchmark \\ + --config reports/go2_config_sim_mujoco__.json + +The sim harness (the baseline controller driven through a real +``ControlCoordinator`` + the FOPDT ``Go2SimTwistBaseAdapter``) is inlined +below — it is small, baseline-only, and used by nothing else. +""" + +from __future__ import annotations + +import argparse +from dataclasses import asdict +import json +from pathlib import Path +import time +from typing import TYPE_CHECKING, Protocol + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt + +from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.task import ControlMode, JointCommandOutput +from dimos.control.tasks.baseline_path_follower_task import ( + BaselinePathFollowerTask, + BaselinePathFollowerTaskConfig, +) +from dimos.control.tasks.feedforward_gain_compensator import FeedforwardGainConfig +from dimos.core.global_config import global_config +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.utils.benchmarking.go2_tuning import ( + Go2TuningConfig, + OperatingPoint, + OperatingPointMap, + invert_tolerance, +) +from dimos.utils.benchmarking.paths import circle, single_corner, square, straight_line +from dimos.utils.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run +from dimos.utils.benchmarking.velocity_profile import PathSpeedCap, VelocityProfileConfig + +if TYPE_CHECKING: + from dimos.hardware.drive_trains.go2_sim.adapter import Go2SimTwistBaseAdapter + +# Go2 hardware control rate. +GO2_TICK_RATE_HZ = 10.0 +_base_joints = make_twist_base_joints("base") +_ARRIVED_STATES = frozenset({"arrived", "completed"}) +_FAILED_STATES = frozenset({"aborted"}) + + +# --- inlined baseline sim harness (was runner.py + sim_blueprint.py) ----- + + +class _PathFollowerLike(Protocol): + def start_path(self, path: NavPath, current_odom: PoseStamped) -> bool: ... + def update_odom(self, odom: PoseStamped) -> None: ... + def compute(self, state) -> object: ... + def get_state(self) -> str: ... + + +class _VelocityProfileProxyTask: + """Curvature velocity-profile cap (the DERIVE consumer seam). Caps + commanded ``|vx|`` to the profile at the robot's path index, scaling + ``wz`` to preserve geometry; pure pass-through otherwise. No + control-law change.""" + + def __init__(self, inner: _PathFollowerLike, cap: PathSpeedCap) -> None: + self._inner = inner + self._cap = cap + self._xy = (0.0, 0.0) + + @property + def name(self) -> str: + return self._inner.name + + def claim(self): + return self._inner.claim() + + def is_active(self) -> bool: + return self._inner.is_active() + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + self._inner.on_preempted(by_task, joints) + + def on_buttons(self, *a, **k): + return self._inner.on_buttons(*a, **k) + + def on_cartesian_command(self, *a, **k): + return self._inner.on_cartesian_command(*a, **k) + + def set_target_by_name(self, *a, **k): + return self._inner.set_target_by_name(*a, **k) + + def set_velocities_by_name(self, *a, **k): + return self._inner.set_velocities_by_name(*a, **k) + + def get_state(self) -> str: + return self._inner.get_state() + + def update_odom(self, odom: PoseStamped) -> None: + self._xy = (float(odom.position.x), float(odom.position.y)) + self._inner.update_odom(odom) + + def start_path(self, path: NavPath, current_odom: PoseStamped) -> bool: + self._cap.for_path(path) + self._xy = (float(current_odom.position.x), float(current_odom.position.y)) + return self._inner.start_path(path, current_odom) + + def compute(self, state): + out = self._inner.compute(state) + if out is None or out.mode != ControlMode.VELOCITY or out.velocities is None: + return out + vx, vy, wz = ([*out.velocities, 0.0, 0.0, 0.0])[:3] + cx, cy, cz = self._cap.cap(self._xy[0], self._xy[1], vx, vy, wz) + return JointCommandOutput( + joint_names=out.joint_names, + velocities=[cx, cy, cz], + mode=ControlMode.VELOCITY, + ) + + +def _go2_sim_base() -> HardwareComponent: + return HardwareComponent( + hardware_id="base", + hardware_type=HardwareType.BASE, + joints=make_twist_base_joints("base"), + adapter_type="go2_sim_twist_base", + ) + + +def _odom_to_pose(odom: list[float]) -> PoseStamped: + return PoseStamped( + position=Vector3(odom[0], odom[1], 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, odom[2])), + ) + + +def _vels_to_twist(v: list[float]) -> Twist: + return Twist(linear=Vector3(v[0], v[1], 0.0), angular=Vector3(0.0, 0.0, v[2])) + + +def _run_baseline_sim( + path: NavPath, + speed: float, + k_angular: float, + ff_config: FeedforwardGainConfig, + profile_config: VelocityProfileConfig, + timeout_s: float, +) -> ExecutedTrajectory: + """Production baseline P-controller in sim against the FOPDT + ``Go2SimTwistBaseAdapter``, with the derived FF + curvature profile.""" + coord = ControlCoordinator( + tick_rate=GO2_TICK_RATE_HZ, + hardware=[_go2_sim_base()], + tasks=[ + TaskConfig(name="vel_base", type="velocity", joint_names=_base_joints, priority=10), + ], + ) + + def _make() -> _PathFollowerLike: + base = BaselinePathFollowerTask( + name="baseline_follower", + config=BaselinePathFollowerTaskConfig( + speed=speed, k_angular=k_angular, ff_config=ff_config + ), + global_config=global_config, + ) + return _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) + + coord.start() + try: + adapter: Go2SimTwistBaseAdapter = coord._hardware["base"].adapter + start = path.poses[0] + adapter.set_initial_pose(start.position.x, start.position.y, start.orientation.euler[2]) + adapter.connect() + + task = _make() + coord.add_task(task) + task.start_path(path, _odom_to_pose(adapter.read_odometry())) + + ticks: list[TrajectoryTick] = [] + period = 1.0 / GO2_TICK_RATE_HZ + t0 = time.perf_counter() + next_sample = t0 + arrived = False + while True: + now = time.perf_counter() + t_rel = now - t0 + if t_rel > timeout_s: + break + pose = _odom_to_pose(adapter.read_odometry()) + task.update_odom(pose) + ticks.append( + TrajectoryTick( + t=t_rel, + pose=pose, + cmd_twist=_vels_to_twist(adapter._cmd), + actual_twist=_vels_to_twist(adapter.read_velocities()), + ) + ) + s = task.get_state() + if s in _ARRIVED_STATES: + arrived = True + break + if s in _FAILED_STATES: + break + next_sample += period + sleep_for = next_sample - time.perf_counter() + if sleep_for > 0: + time.sleep(sleep_for) + return ExecutedTrajectory(ticks=ticks, arrived=arrived) + finally: + coord.stop() + + +# --- benchmark ---------------------------------------------------------- + + +def _path_set() -> dict: + """Real-space-constrained fixed path set (locked — do not widen).""" + return { + "straight_line": straight_line(), + "single_corner": single_corner(leg_length=2.0, angle_deg=90.0), + "square": square(side=2.0), + "circle": circle(radius=1.0), + } + + +REPORTS_DIR = Path(__file__).parent / "reports" + + +def _run_ladder( + cfg: Go2TuningConfig, speeds: list[float], timeout_s: float +) -> list[OperatingPoint]: + ff = cfg.feedforward.to_runtime() + k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) + points: list[OperatingPoint] = [] + for name, path in _path_set().items(): + for speed in speeds: + profile = cfg.velocity_profile.to_runtime(max_linear_speed=speed) + traj = _run_baseline_sim(path, speed, k_angular, ff, profile, timeout_s) + s = score_run(path, traj) + points.append( + OperatingPoint( + path=name, + speed=speed, + cte_max=s.cte_max, + cte_rms=s.cte_rms, + arrived=s.arrived, + ) + ) + print( + f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " + f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" + ) + return points + + +def _plot(points: list[OperatingPoint], out: Path) -> None: + 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("Go2 baseline tracking: 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 main() -> None: + ap = argparse.ArgumentParser(description="Go2 operating-point benchmark") + ap.add_argument("--config", required=True, help="config artifact from go2_characterization") + ap.add_argument("--speeds", default="0.3,0.5,0.7,0.9,1.0") + ap.add_argument("--tolerances", default="5,10,15", help="cm") + ap.add_argument("--timeout", type=float, default=60.0) + args = ap.parse_args() + + config_path = Path(args.config).expanduser() + cfg = Go2TuningConfig.from_json(config_path) # asserts schema_version + speeds = [float(s) for s in args.speeds.split(",")] + tolerances = [float(t) for t in args.tolerances.split(",")] + + print( + f"Speed ladder {speeds} over {len(_path_set())} paths " + f"(baseline k_angular={cfg.recommended_controller.params.get('k_angular')}):" + ) + points = _run_ladder(cfg, speeds, args.timeout) + inversion = invert_tolerance(points, tolerances) + cfg.operating_point_map = OperatingPointMap( + speeds=speeds, points=points, tolerance_inversion=inversion + ) + + cfg.to_json(config_path) # augment input artifact in place (section 5) + sha = cfg.provenance.git_sha + bench_path = REPORTS_DIR / f"go2_benchmark_{sha}.json" + bench_path.parent.mkdir(parents=True, exist_ok=True) + bench_path.write_text(json.dumps(asdict(cfg.operating_point_map), indent=2)) + plot_path = REPORTS_DIR / f"go2_benchmark_cte_vs_speed_{sha}.png" + _plot(points, plot_path) + + print(f"\nAugmented artifact: {config_path.resolve()}") + print(f"Benchmark json : {bench_path.resolve()}") + print(f"Plot : {plot_path.resolve()}") + print("\nOperating-point recommendation:") + for row in inversion: + if row.max_speed is None: + print( + f" tolerance {row.tol_cm:g} cm: NO tested speed keeps every " + f"path within tolerance — relax the tolerance or slow the fleet." + ) + else: + print( + f" For tolerance {row.tol_cm:g} cm, run at speed " + f"{row.max_speed:.2f} m/s with this profile " + f"(binding path: {row.binding_path})." + ) + + +if __name__ == "__main__": + main() From a21c45f61337b9d8f77064ce5ac1656101852ddf Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:11:29 -0700 Subject: [PATCH 07/51] tests and readme --- .../benchmarking/reports/go2_tuning_README.md | 124 +++++++++++++ dimos/utils/benchmarking/test_go2_tuning.py | 175 ++++++++++++++++++ 2 files changed, 299 insertions(+) create mode 100644 dimos/utils/benchmarking/reports/go2_tuning_README.md create mode 100644 dimos/utils/benchmarking/test_go2_tuning.py diff --git a/dimos/utils/benchmarking/reports/go2_tuning_README.md b/dimos/utils/benchmarking/reports/go2_tuning_README.md new file mode 100644 index 0000000000..e0f4cfd540 --- /dev/null +++ b/dimos/utils/benchmarking/reports/go2_tuning_README.md @@ -0,0 +1,124 @@ +# Go2 controller tuning — measure → derive → validate + +Two CLI tools that turn one real measurement of your Go2 into a single +versioned config artifact with every parameter you need to tune the base +controller — no guesswork. + +``` +go2_characterization ──(measure + derive)──▶ go2_config_*.json + │ +go2_benchmark ──(validate across speed)──▶ same file, section 5 added + │ + "for tolerance X cm, + run at speed Y m/s" +``` + +## Why these numbers (the settled findings — not re-derived here) + +The Go2 base is **FOPDT per axis** (first-order + dead-time). At a given +speed the path-tracking error is the **plant floor `(τ + L)·v`** — it +scales with speed, and no reactive control law has meaningful headroom +over it (validated controller bake-off). So: + +- The recommended controller is **hardcoded to the production baseline + P-controller**. It is not re-selected; the evidence string is embedded + in the artifact. +- The only effective levers are **feedforward gain** (so commanded == + achieved velocity) and **speed vs path curvature** (the velocity + profile). Both are *derived from the fitted plant*, not hand-tuned. + +## Tool 1 — `go2_characterization` + +``` +uv run python -m dimos.utils.benchmarking.go2_characterization \ + --mode sim --surface mujoco # sim self-test +uv run python -m dimos.utils.benchmarking.go2_characterization \ + --mode hw --surface concrete --gait-mode default # real robot +``` + +Runs a space-cheap system-ID sequence (per-channel velocity steps for +**vx, vy, wz** — vy is real, the Go2 base strafes — no long paths), fits +FOPDT per axis, runs the DERIVE step, and writes the artifact (sections +1–4 + 6; section 5 left empty for tool 2). Prints the artifact path. + +**One harness, plant swapped by `--mode`.** Identical SI loop and FOPDT +fitter both modes; only *where the robot behaves* differs: + +- **sim**: the plant is the in-process FOPDT `Go2PlantSim` seeded with + the vendored `GO2_PLANT_FITTED` ground truth. A self-test — it recovers + the injected model (printed "recovered vs injected" table should match; + small dead-time `L` is mildly under-fit — expected, not load-bearing). + Validates the whole pipeline without a robot. +- **hw**: the plant is the real Go2 over LCM (publish `/cmd_vel`, + differentiate `/go2/odom` to body-frame velocity). Same loop, same + `fit_fopdt`. Wired; not run by CI/sim. No separate data-acquisition or + statistics pipeline — point estimates per channel (mean over swept + amplitudes), not research-grade confidence intervals. + +## Tool 2 — `go2_benchmark` + +``` +uv run python -m dimos.utils.benchmarking.go2_benchmark \ + --config reports/go2_config_sim_mujoco__.json \ + --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 +``` + +Loads the artifact, runs the **hardcoded baseline** with the derived +feedforward + velocity profile across the speed ladder on a fixed, +real-space-constrained path set (`straight_line`, `single_corner` 2 m/90°, +`square` 2 m, `circle` R1.0). Scores each (path, speed), builds the +operating-point map + the tolerance→max-safe-speed inversion, and writes +**section 5 back into the same artifact**. Also emits a standalone +`go2_benchmark_.json` and one `cte_max`-vs-speed plot. + +Final line, e.g.: *"For tolerance 15 cm, run at speed 0.30 m/s with this +profile (binding path: circle)."* The binding path is the slowest path — +its limit gates the fleet. + +## Reading the artifact + +| Section | Field | Meaning | +|---|---|---| +| 1 | `provenance` | robot / surface / mode / date / git sha / sim·hw — the validity scope | +| 1 | `plant` | fitted FOPDT `{K, τ, L}` per axis | +| 2 | `feedforward` | `K_axis = 1/K_plant` + output clamps — give to `FeedforwardGainConfig` | +| 3 | `velocity_profile` | curvature speed profile — give to `VelocityProfileConfig` | +| 4 | `recommended_controller` | `baseline`, `k_angular`, and the plant-floor evidence | +| 5 | `operating_point_map` | per (path,speed) `cte_max/cte_rms/arrived` + tolerance→speed table (`null` until tool 2) | +| 6 | `caveats` | surface/mode/sim-vs-hw validity limits | + +`schema_version` is enforced on load; an artifact from an incompatible +schema is rejected rather than silently mis-read. + +## When to re-run + +Re-run **tool 1** (then tool 2) whenever the plant changes: + +- different **surface** (carpet vs concrete vs ice) — friction changes K/τ, +- different **gait mode** (e.g. `rage`), +- a firmware/locomotion change, +- moving from **sim numbers to a real robot** (`--mode hw`). + +The artifact's `caveats` state exactly what it is valid for. A sim +artifact validates the pipeline, not absolute on-robot numbers. + +## What is *not* here (by design) + +The MPC / RPP / Lyapunov / pure-pursuit bake-off, command smoothers, +oscillation & speed sweeps, and plotting R&D were the *evidence* that +justified "baseline + feedforward + curvature profile" — they are the +appendix, not the product, and were deliberately left out of this +deliverable. `velocity_profile` stays default-off for any non-blueprint +caller. + +## Tests + +``` +uv run pytest dimos/utils/benchmarking/test_go2_tuning.py -q +``` + +Covers the pure DERIVE step (1/K per axis incl. real vy, wz-ceiling +margin + envelope clamp, accel formulas, hardcoded baseline + evidence, +provenance-driven caveats), artifact JSON round-trip + schema-version +rejection, and the tolerance→max-speed inversion (binding = min across +paths, not-arrived excluded, infeasible → none). diff --git a/dimos/utils/benchmarking/test_go2_tuning.py b/dimos/utils/benchmarking/test_go2_tuning.py new file mode 100644 index 0000000000..7334491dd5 --- /dev/null +++ b/dimos/utils/benchmarking/test_go2_tuning.py @@ -0,0 +1,175 @@ +# 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. + +"""Pure, fast unit tests for the DERIVE step and the tolerance->speed +inversion — no sim, no robot. The validation that the full pipeline runs +end to end lives in the README verification steps, not here. +""" + +from __future__ import annotations + +import pytest + +from dimos.utils.benchmarking.go2_tuning import ( + SCHEMA_VERSION, + Go2TuningConfig, + OperatingPoint, + Provenance, + derive_config, + invert_tolerance, +) +from dimos.utils.benchmarking.plant import FopdtChannelParams, Go2PlantParams +from dimos.utils.benchmarking.velocity_profile import GO2_VX_MAX, GO2_WZ_MAX + + +def _plant(kvx=0.9, kvy=0.5, kwz=2.4) -> Go2PlantParams: + return Go2PlantParams( + vx=FopdtChannelParams(K=kvx, tau=0.40, L=0.06), + vy=FopdtChannelParams(K=kvy, tau=0.30, L=0.05), + wz=FopdtChannelParams(K=kwz, tau=0.60, L=0.05), + ) + + +def _prov(**kw) -> Provenance: + base = dict(robot_id="go2", surface="concrete", mode="default", sim_or_hw="hw") + base.update(kw) + return Provenance(**base) + + +# --- DERIVE --------------------------------------------------------------- + + +def test_feedforward_is_inverse_gain_per_axis_including_real_vy(): + cfg = derive_config(_plant(kvx=0.9, kvy=0.5, kwz=2.4), _prov()) + assert cfg.feedforward.K_vx == pytest.approx(1 / 0.9) + # vy is a real, independently-fitted channel — NOT a copy of vx. + assert cfg.feedforward.K_vy == pytest.approx(1 / 0.5) + assert cfg.feedforward.K_vy != pytest.approx(cfg.feedforward.K_vx) + assert cfg.feedforward.K_wz == pytest.approx(1 / 2.4) + + +def test_feedforward_guards_degenerate_zero_gain(): + cfg = derive_config(_plant(kvx=0.0), _prov()) + assert cfg.feedforward.K_vx == pytest.approx(1.0) # guard, not div-by-zero + + +def test_wz_max_uses_measured_ceiling_minus_margin(): + # per_amplitude wz ceiling = max(K*|amp|) = 1.5*0.5 = 0.75; minus 15%. + pa = {"wz": [{"amplitude": 0.5, "K": 1.5}, {"amplitude": 0.3, "K": 1.4}]} + cfg = derive_config(_plant(), _prov(), per_amplitude=pa) + assert cfg.velocity_profile.max_angular_speed == pytest.approx(0.75 * 0.85) + + +def test_wz_max_falls_back_to_envelope_without_per_amplitude(): + cfg = derive_config(_plant(), _prov(), per_amplitude=None) + assert cfg.velocity_profile.max_angular_speed == pytest.approx(GO2_WZ_MAX * 0.85) + + +def test_ceiling_clamped_to_saturation_envelope(): + # Un-saturated fit extrapolates past the platform envelope -> clamp. + pa = {"wz": [{"amplitude": 1.2, "K": 2.45}], "vx": [{"amplitude": 0.9, "K": 0.92}]} + cfg = derive_config(_plant(), _prov(), per_amplitude=pa) + assert cfg.velocity_profile.max_angular_speed == pytest.approx(GO2_WZ_MAX * 0.85) + assert cfg.velocity_profile.max_linear_speed <= GO2_VX_MAX + + +def test_linear_accel_first_order_rise_and_asymmetric_decel(): + p = _plant() + cfg = derive_config(p, _prov()) # no per_amplitude -> vx_ceiling=GO2_VX_MAX + assert cfg.velocity_profile.max_linear_accel == pytest.approx(GO2_VX_MAX / p.vx.tau) + assert cfg.velocity_profile.max_linear_decel == pytest.approx( + 2.0 * cfg.velocity_profile.max_linear_accel + ) + + +def test_recommended_controller_is_hardcoded_baseline_with_evidence(): + cfg = derive_config(_plant(), _prov()) + assert cfg.recommended_controller.name == "baseline" + assert cfg.recommended_controller.params["k_angular"] == 0.5 + assert "plant floor" in cfg.recommended_controller.evidence + assert len(cfg.recommended_controller.evidence) > 50 + + +def test_caveats_reflect_provenance(): + cfg = derive_config(_plant(), _prov(surface="ice", mode="rage", sim_or_hw="hw")) + blob = " ".join(cfg.caveats) + assert "ice" in blob and "rage" in blob + cfg_sim = derive_config(_plant(), _prov(sim_or_hw="sim")) + assert any("sim plant" in c for c in cfg_sim.caveats) + + +def test_derive_leaves_operating_point_map_none(): + assert derive_config(_plant(), _prov()).operating_point_map is None + + +# --- artifact round-trip -------------------------------------------------- + + +def test_json_round_trip_identity(tmp_path): + cfg = derive_config(_plant(), _prov()) + p = cfg.to_json(tmp_path / "c.json") + back = Go2TuningConfig.from_json(p) + assert back.feedforward == cfg.feedforward + assert back.velocity_profile == cfg.velocity_profile + assert back.plant == cfg.plant + assert back.provenance == cfg.provenance + assert back.schema_version == SCHEMA_VERSION + + +def test_loader_rejects_wrong_schema_version(tmp_path): + cfg = derive_config(_plant(), _prov()) + p = cfg.to_json(tmp_path / "c.json") + bad = p.read_text().replace(f'"schema_version": {SCHEMA_VERSION}', '"schema_version": 999') + (tmp_path / "bad.json").write_text(bad) + with pytest.raises(ValueError, match="schema_version"): + Go2TuningConfig.from_json(tmp_path / "bad.json") + + +# --- tolerance -> max-safe-speed inversion -------------------------------- + + +def _pts(rows) -> list[OperatingPoint]: + return [ + OperatingPoint(path=p, speed=s, cte_max=c, cte_rms=c * 0.6, arrived=a) + for (p, s, c, a) in rows + ] + + +def test_inversion_binding_is_min_across_paths(): + # straight tolerates fast; circle is the slow/binding path. + pts = _pts( + [ + ("straight", 0.5, 0.02, True), + ("straight", 0.9, 0.04, True), + ("circle", 0.5, 0.06, True), + ("circle", 0.9, 0.15, True), + ] + ) + (row,) = invert_tolerance(pts, [10.0]) # 10 cm = 0.10 m + # straight ok @0.9 (0.04), circle ok only @0.5 (0.06) -> binding 0.5/circle + assert row.max_speed == pytest.approx(0.5) + assert row.binding_path == "circle" + + +def test_inversion_excludes_not_arrived(): + pts = _pts([("straight", 0.5, 0.01, False), ("straight", 0.3, 0.01, True)]) + (row,) = invert_tolerance(pts, [5.0]) + assert row.max_speed == pytest.approx(0.3) # 0.5 not-arrived excluded + + +def test_inversion_none_when_no_speed_meets_tolerance(): + pts = _pts([("circle", 0.5, 0.20, True), ("circle", 0.9, 0.30, True)]) + (row,) = invert_tolerance(pts, [5.0]) + assert row.max_speed is None + assert row.binding_path is None From 197d78fd375869231c71d8fa7f73d3036dda8e50 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 18 May 2026 15:03:50 -0700 Subject: [PATCH 08/51] updated with hw testing --- dimos/utils/benchmarking/go2_benchmark.py | 426 +++++++++++++- .../benchmarking/go2_characterization.py | 543 +++++++++++++----- dimos/utils/benchmarking/go2_tuning.py | 20 +- .../benchmarking/reports/go2_tuning_README.md | 202 ++++--- dimos/utils/benchmarking/test_go2_tuning.py | 21 + 5 files changed, 957 insertions(+), 255 deletions(-) diff --git a/dimos/utils/benchmarking/go2_benchmark.py b/dimos/utils/benchmarking/go2_benchmark.py index 91293c527b..d2c0b0d385 100644 --- a/dimos/utils/benchmarking/go2_benchmark.py +++ b/dimos/utils/benchmarking/go2_benchmark.py @@ -33,7 +33,10 @@ import argparse from dataclasses import asdict import json +import math from pathlib import Path +import sys +import threading import time from typing import TYPE_CHECKING, Protocol @@ -44,7 +47,12 @@ from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.control.task import ControlMode, JointCommandOutput +from dimos.control.task import ( + ControlMode, + CoordinatorState, + JointCommandOutput, + JointStateSnapshot, +) from dimos.control.tasks.baseline_path_follower_task import ( BaselinePathFollowerTask, BaselinePathFollowerTaskConfig, @@ -174,9 +182,11 @@ def _run_baseline_sim( ff_config: FeedforwardGainConfig, profile_config: VelocityProfileConfig, timeout_s: float, -) -> ExecutedTrajectory: +) -> tuple[ExecutedTrajectory, NavPath]: """Production baseline P-controller in sim against the FOPDT - ``Go2SimTwistBaseAdapter``, with the derived FF + curvature profile.""" + ``Go2SimTwistBaseAdapter``, with the derived FF + curvature profile. + Returns the trajectory and the reference path in the executed frame + (sim runs in the path's own frame, so it is ``path`` unchanged).""" coord = ControlCoordinator( tick_rate=GO2_TICK_RATE_HZ, hardware=[_go2_sim_base()], @@ -236,11 +246,180 @@ def _make() -> _PathFollowerLike: sleep_for = next_sample - time.perf_counter() if sleep_for > 0: time.sleep(sleep_for) - return ExecutedTrajectory(ticks=ticks, arrived=arrived) + return ExecutedTrajectory(ticks=ticks, arrived=arrived), path finally: coord.stop() +# --- hw harness (real Go2 over LCM; closed-loop baseline) --------------- +# +# Ported from the R&D `_run_path_follower_hw`. Talks LCM to a separately +# running `dimos run unitree-go2-webrtc-keyboard-teleop` (publishes +# /go2/odom, consumes /cmd_vel; its teleop is publish-only-when-active, +# so between gated paths you reposition with the keyboard, release keys +# — teleop goes silent — then the tool owns /cmd_vel for the run). No +# new module — inlined here, the small estimator/anchor duplicated with +# go2_characterization by choice (no shared-module addition). + +VX_MAX = 1.0 +WZ_MAX = 1.5 +_ODOM_WARMUP_S = 10.0 # WebRTC connect + first odom (override: --odom-warmup) +_ODOM_STALE_S = 1.0 + + +def _clamp(v: float, lo: float, hi: float) -> float: + return max(lo, min(hi, v)) + + +def _twist_clamped(vx: float, wz: float) -> Twist: + return Twist( + linear=Vector3(_clamp(vx, -VX_MAX, VX_MAX), 0.0, 0.0), + angular=Vector3(0.0, 0.0, _clamp(wz, -WZ_MAX, WZ_MAX)), + ) + + +class _PoseVelocityEstimator: + """Consecutive ``PoseStamped`` -> EMA body-frame (vx,vy,wz).""" + + def __init__(self, alpha: float = 0.5) -> None: + self._pp = None + self._pt: float | None = None + self._vx = self._vy = self._wz = 0.0 + self._a = alpha + + def update(self, pose, t: float) -> tuple[float, float, float]: + if self._pp is None or self._pt is None: + self._pp, self._pt = pose, t + return 0.0, 0.0, 0.0 + dt = t - self._pt + if dt <= 0: + return self._vx, self._vy, self._wz + dx = pose.position.x - self._pp.position.x + dy = pose.position.y - self._pp.position.y + y0, y1 = self._pp.orientation.euler[2], pose.orientation.euler[2] + dyaw = (y1 - y0 + 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 + self._vx = self._a * bx + (1 - self._a) * self._vx + self._vy = self._a * by + (1 - self._a) * self._vy + self._wz = self._a * (dyaw / dt) + (1 - self._a) * self._wz + self._pp, self._pt = pose, t + return self._vx, self._vy, self._wz + + +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 it need not be positioned + precisely — only roughly aimed).""" + 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) + + +def _run_baseline_hw( + link: dict, + path: NavPath, + speed: float, + k_angular: float, + ff_config: FeedforwardGainConfig, + profile_config: VelocityProfileConfig, + timeout_s: float, + label: str, +) -> tuple[ExecutedTrajectory, NavPath]: + """Closed-loop baseline on the real Go2: anchor the path to the + robot's current pose, then track at 10 Hz off real odom. Safe: + velocity clamp, stale-odom abort, timeout, zero-Twist on exit. + Returns the trajectory and the anchored reference path (odom frame) + — score/plot must use this, not the robot-centric input path.""" + cmd_pub, get_odom = link["pub"], link["get"] + + base = BaselinePathFollowerTask( + name=f"baseline_{label}", + config=BaselinePathFollowerTaskConfig( + speed=speed, k_angular=k_angular, ff_config=ff_config + ), + global_config=global_config, + ) + task = _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) + + pose0, _ = get_odom() + path_w = _shift_path_to_start_at_pose(path, pose0) + task.start_path(path_w, pose0) + + ticks: list[TrajectoryTick] = [] + est = _PoseVelocityEstimator() + period = 1.0 / GO2_TICK_RATE_HZ + t0 = time.perf_counter() + arrived = False + try: + while True: + now = time.perf_counter() + t_rel = now - t0 + if t_rel > timeout_s: + print(f" [{label}] timeout {timeout_s:.0f}s") + break + pose, age = get_odom() + if pose is None or age > _ODOM_STALE_S: + print(f" [{label}] ABORT stale odom ({age:.2f}s)") + break + task.update_odom(pose) + ev = est.update(pose, now) + state = CoordinatorState( + joints=JointStateSnapshot( + joint_velocities={"base/vx": ev[0], "base/vy": ev[1], "base/wz": ev[2]}, + timestamp=now, + ), + t_now=now, + dt=period, + ) + out = task.compute(state) + vx, wz = ( + (out.velocities[0], out.velocities[2]) + if (out is not None and out.velocities is not None) + else (0.0, 0.0) + ) + tw = _twist_clamped(vx, wz) + cmd_pub.broadcast(None, tw) + ticks.append( + TrajectoryTick( + t=t_rel, + pose=pose, + cmd_twist=tw, + actual_twist=Twist( + linear=Vector3(ev[0], ev[1], 0.0), + angular=Vector3(0.0, 0.0, ev[2]), + ), + ) + ) + st = task.get_state() + if st in _ARRIVED_STATES: + arrived = True + print(f" [{label}] arrived in {t_rel:.1f}s") + break + if st in _FAILED_STATES: + print(f" [{label}] task aborted") + break + time.sleep(max(0.0, t0 + len(ticks) * period - time.perf_counter())) + finally: + for _ in range(3): + cmd_pub.broadcast(None, _twist_clamped(0.0, 0.0)) + time.sleep(0.05) + return ExecutedTrajectory(ticks=ticks, arrived=arrived), path_w + + # --- benchmark ---------------------------------------------------------- @@ -257,31 +436,108 @@ def _path_set() -> dict: REPORTS_DIR = Path(__file__).parent / "reports" +def _open_hw_link(warmup_s: float) -> dict: + """LCM to a running `dimos run unitree-go2-webrtc-keyboard-teleop`.""" + from dimos.core.transport import LCMTransport + + cmd_pub = LCMTransport("/cmd_vel", Twist) + odom_sub = LCMTransport("/go2/odom", PoseStamped) + lock = threading.Lock() + box: dict = {"pose": None, "t": 0.0} + + def _on(msg) -> None: + with lock: + box["pose"] = msg + box["t"] = time.perf_counter() + + odom_sub.subscribe(_on) + + def get_odom(): + with lock: + return box["pose"], time.perf_counter() - box["t"] + + print(f"[hw] waiting up to {warmup_s:.0f}s for /go2/odom ...") + deadline = time.perf_counter() + warmup_s + while time.perf_counter() < deadline and get_odom()[0] is None: + time.sleep(0.05) + if get_odom()[0] is None: + raise RuntimeError("No /go2/odom — is `dimos run unitree-go2-webrtc-keyboard-teleop` up?") + return {"pub": cmd_pub, "get": get_odom} + + def _run_ladder( - cfg: Go2TuningConfig, speeds: list[float], timeout_s: float -) -> list[OperatingPoint]: + cfg: Go2TuningConfig, speeds: list[float], timeout_s: float, mode: str, warmup_s: float +) -> tuple[list[OperatingPoint], list[dict]]: ff = cfg.feedforward.to_runtime() k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) + link = _open_hw_link(warmup_s) if mode == "hw" else None points: list[OperatingPoint] = [] - for name, path in _path_set().items(): - for speed in speeds: - profile = cfg.velocity_profile.to_runtime(max_linear_speed=speed) - traj = _run_baseline_sim(path, speed, k_angular, ff, profile, timeout_s) - s = score_run(path, traj) - points.append( - OperatingPoint( - path=name, - speed=speed, - cte_max=s.cte_max, - cte_rms=s.cte_rms, - arrived=s.arrived, + runs: list[dict] = [] # for the XY trajectory overlay + try: + for name, path in _path_set().items(): + for speed in speeds: + profile = cfg.velocity_profile.to_runtime(max_linear_speed=speed) + if mode == "hw": + for _ in range(3): + link["pub"].broadcast(None, _twist_clamped(0.0, 0.0)) + time.sleep(0.05) + resp = ( + input( + f"\n[{name} v={speed:.2f}] reposition+aim robot, " + f"ENTER=run s=skip q=quit: " + ) + .strip() + .lower() + ) + if resp == "q": + raise KeyboardInterrupt + if resp == "s": + print(" skipped") + continue + traj, ref = _run_baseline_hw( + link, + path, + speed, + k_angular, + ff, + profile, + timeout_s, + f"{name}@{speed:.2f}", + ) + else: + traj, ref = _run_baseline_sim(path, speed, k_angular, ff, profile, timeout_s) + # Score/plot against the executed-frame reference: in hw + # that's the pose-anchored path, not the robot-centric input. + s = score_run(ref, traj) + points.append( + OperatingPoint( + path=name, + speed=speed, + cte_max=s.cte_max, + cte_rms=s.cte_rms, + arrived=s.arrived, + ) ) - ) - print( - f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " - f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" - ) - return points + runs.append( + { + "path": name, + "speed": speed, + "cte_max": s.cte_max, + "arrived": s.arrived, + "ref": [(p.position.x, p.position.y) for p in ref.poses], + "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], + } + ) + print( + f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " + f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" + ) + finally: + if link is not None: + for _ in range(3): + link["pub"].broadcast(None, _twist_clamped(0.0, 0.0)) + time.sleep(0.05) + return points, runs def _plot(points: list[OperatingPoint], out: Path) -> None: @@ -300,12 +556,99 @@ def _plot(points: list[OperatingPoint], out: Path) -> None: 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. The same transform is applied + to the executed trajectory. Makes every run of a path overlay on one + identical reference sharing the origin — so speeds are comparable + regardless of where the robot physically started (hw anchors each run + to its current odom pose; sim already starts at the path origin).""" + if len(ref) < 2: + return ref, exec_ + ox, oy = ref[0] + # heading from the first reference point that is meaningfully distinct + 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): + out = [] + for x, y in pts: + dx, dy = x - ox, y - oy + out.append((dx * c - dy * s, dx * s + dy * c)) + return out + + return tf(ref), tf(exec_) + + +def _plot_xy(runs: list[dict], out: Path) -> None: + """One subplot per path: the reference path (black) overlaid with the + executed trajectory at each speed, all normalized to the canonical + path frame (common origin) so speeds are directly comparable. This is + the diagnostic view — you see exactly where/how the robot cuts + corners as speed rises.""" + 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) # common start + 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("Go2 baseline: executed trajectory vs reference path") + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) + + def main() -> None: ap = argparse.ArgumentParser(description="Go2 operating-point benchmark") ap.add_argument("--config", required=True, help="config artifact from go2_characterization") + ap.add_argument("--mode", choices=["hw", "sim"], default="hw") ap.add_argument("--speeds", default="0.3,0.5,0.7,0.9,1.0") ap.add_argument("--tolerances", default="5,10,15", help="cm") - ap.add_argument("--timeout", type=float, default=60.0) + ap.add_argument("--timeout", type=float, default=60.0, help="per (path,speed) run timeout (s)") + ap.add_argument( + "--odom-warmup", + type=float, + default=_ODOM_WARMUP_S, + help="how long to wait for first /go2/odom (s)", + ) args = ap.parse_args() config_path = Path(args.config).expanduser() @@ -313,11 +656,31 @@ def main() -> None: speeds = [float(s) for s in args.speeds.split(",")] tolerances = [float(t) for t in args.tolerances.split(",")] + if args.mode == "hw" and not cfg.valid_for_tuning: + sys.exit( + "Refusing --mode hw with a non-robot-valid config " + f"({config_path.name}, sim_or_hw={cfg.provenance.sim_or_hw!r}). Its " + "feedforward/profile were derived from the sim plant — running " + "them on the real robot is meaningless. Re-run " + "`go2_characterization --mode hw` first, or use --mode sim for a " + "pre-check." + ) + if args.mode == "sim": + print( + "[pre-check] --mode sim: validates wiring against the FOPDT sim " + "plant only; the operating-point map is NOT a real-robot result." + ) + print( - f"Speed ladder {speeds} over {len(_path_set())} paths " + f"{args.mode} speed ladder {speeds} over {len(_path_set())} paths " f"(baseline k_angular={cfg.recommended_controller.params.get('k_angular')}):" ) - points = _run_ladder(cfg, speeds, args.timeout) + try: + points, runs = _run_ladder(cfg, speeds, args.timeout, args.mode, args.odom_warmup) + except KeyboardInterrupt: + raise SystemExit( + "\n[hw] aborted by operator — robot stopped, artifact not modified." + ) from None inversion = invert_tolerance(points, tolerances) cfg.operating_point_map = OperatingPointMap( speeds=speeds, points=points, tolerance_inversion=inversion @@ -330,10 +693,13 @@ def main() -> None: bench_path.write_text(json.dumps(asdict(cfg.operating_point_map), indent=2)) plot_path = REPORTS_DIR / f"go2_benchmark_cte_vs_speed_{sha}.png" _plot(points, plot_path) + xy_path = REPORTS_DIR / f"go2_benchmark_xy_{sha}.png" + _plot_xy(runs, xy_path) - print(f"\nAugmented artifact: {config_path.resolve()}") - print(f"Benchmark json : {bench_path.resolve()}") - print(f"Plot : {plot_path.resolve()}") + print(f"\nAugmented artifact : {config_path.resolve()}") + print(f"Benchmark json : {bench_path.resolve()}") + print(f"CTE-vs-speed plot : {plot_path.resolve()}") + print(f"XY trajectory plot : {xy_path.resolve()} <-- the diagnostic view") print("\nOperating-point recommendation:") for row in inversion: if row.max_speed is None: diff --git a/dimos/utils/benchmarking/go2_characterization.py b/dimos/utils/benchmarking/go2_characterization.py index 6965cb7bfb..93f270c74f 100644 --- a/dimos/utils/benchmarking/go2_characterization.py +++ b/dimos/utils/benchmarking/go2_characterization.py @@ -14,37 +14,43 @@ """Tool 1 of the Go2 tuning deliverable: characterization. -Runs a space-cheap system-ID sequence (per-channel velocity steps — no -long paths), fits FOPDT per axis (vx, vy, wz), then runs the DERIVE step -and emits the versioned config artifact. +**This is a hardware tool.** It measures the real Go2's per-axis velocity +response (vx, vy, wz), fits FOPDT per channel, runs the DERIVE step, and +emits the versioned config artifact. + # On dimensional-gpu-0, terminal 1: + dimos run unitree-go2-webrtc-keyboard-teleop + # terminal 2 (strip /nix/store from LD_LIBRARY_PATH — see README): uv run python -m dimos.utils.benchmarking.go2_characterization \\ - --mode sim --surface mujoco - -**One harness, plant swapped by ``--mode``** — exactly the same SI loop -and fitter; only *where the robot behaves* differs: - -* ``sim``: the plant is the in-process FOPDT ``Go2PlantSim`` seeded with - the vendored ``GO2_PLANT_FITTED`` ground truth. A healthy run recovers - the injected model (printed "recovered vs injected" table) — this - self-tests the whole measure->fit->derive pipeline without a robot. -* ``hw``: the plant is the real Go2 over LCM (publish ``/cmd_vel``, - differentiate ``/go2/odom`` to body-frame velocity). Wired; not - exercised by CI/sim. - -Both modes record cmd-vs-measured per channel and fit with the same -``fit_fopdt``; there is no separate hardware data-acquisition pipeline. -The tail (sections 1-4 + 6; section 5 left ``None`` for the benchmark -tool) is the pure ``derive_config``. + --mode hw --surface concrete + +`--mode hw` (default) drives the real robot over LCM (`/cmd_vel` out, +`/go2/odom` in). It is **operator-gated**: before every step it stops the +robot and waits for you to reposition it (with the keyboard teleop from +the blueprint above) and press ENTER. This bounds drift to a single step +and is safe (velocity clamp, zero-Twist on exit/SIGINT, stale-odom +abort, timeout). + +`--mode self-test` is a **plumbing check, NOT a tuning artifact**: it +steps an in-process FOPDT `Go2PlantSim` seeded with the vendored +ground truth and recovers it. It only proves the measure->fit->derive +code runs; the artifact is stamped `valid_for_tuning=false`. Used by +pytest/CI so regressions are caught without a robot. """ from __future__ import annotations import argparse from datetime import date +import math from pathlib import Path +import threading import time +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt import numpy as np from dimos.utils.benchmarking.go2_tuning import Provenance, derive_config, git_sha @@ -54,7 +60,7 @@ Go2PlantParams, Go2PlantSim, ) -from dimos.utils.characterization.modeling.fopdt import fit_fopdt +from dimos.utils.characterization.modeling.fopdt import fit_fopdt, fopdt_step_response # Space-cheap SI plan: a few amplitudes per channel. vy is a real channel # (the Go2 base strafes) so it gets its own sweep — not a copy of vx. @@ -63,117 +69,63 @@ "vy": [0.2, 0.4], "wz": [0.4, 0.8, 1.2], } -_DT = 0.02 # 50 Hz sample period +_CHANNELS = ("vx", "vy", "wz") # velocity-tuple order (estimator output) +# Channels excited on the real robot. vy is omitted: the Go2 does not +# strafe on /cmd_vel.linear.y in the default gait, so exciting it only +# produces a degenerate fit. vy is placeholdered (= vx) post-hoc. +_HW_CHANNELS = ("vx", "wz") _PRE_ROLL_S = 1.0 -_STEP_S = 5.0 # >> max (tau + L) so the channel fully settles +# Real-robot default: gait initiation + command latency means the Go2 +# needs several seconds to ramp to and hold the commanded velocity, much +# longer than the bare FOPDT settle. Operator-overridable (--step-s). +_STEP_S = 8.0 +# Per-step travel cap (m). At high speed the time cap would run the +# robot out of the test area, so distance is the real bound; step_s is +# the safety upper bound and the terminator for wz (spins in place). +_MAX_DIST_M = 6.0 + +# Hardware safety envelope (Go2 Rung-1 saturation) + control rate. +VX_MAX = 1.0 # m/s +WZ_MAX = 1.5 # rad/s +_HW_DT = 1.0 / 10.0 # Go2 control + odom tick +_SIM_DT = 0.02 +_ODOM_WARMUP_S = 10.0 # WebRTC connect + first odom (override: --odom-warmup) +_ODOM_STALE_S = 1.0 -_CHANNELS = ("vx", "vy", "wz") REPORTS_DIR = Path(__file__).parent / "reports" -# --- the swap point: a plant you step with a velocity command ------------ - - -class _SimPlant: - """Sim: the in-process FOPDT ``Go2PlantSim`` (vendored ground truth).""" - - is_hw = False - - def __init__(self) -> None: - self._p = Go2PlantSim(GO2_PLANT_FITTED) - - def reset(self) -> None: - self._p.reset(0.0, 0.0, 0.0, _DT) - - def step(self, cmd: dict[str, float]) -> dict[str, float]: - self._p.step(cmd["vx"], cmd["vy"], cmd["wz"], _DT) - return {"vx": self._p.vx, "vy": self._p.vy, "wz": self._p.wz} - - -class _HwPlant: - """HW: the real Go2 over LCM — publish ``/cmd_vel``, differentiate - ``/go2/odom`` to body-frame velocity. Wired; not run by CI/sim. - - Same ``reset``/``step`` surface as :class:`_SimPlant`, so the SI loop - is identical regardless of mode. - """ - - is_hw = True +def _clamp(v: float, lo: float, hi: float) -> float: + return max(lo, min(hi, v)) - def __init__(self) -> None: - import math - 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.geometry_msgs.Vector3 import Vector3 +# --- self-test (in-process FOPDT plant; NOT robot-valid) ----------------- - self._math = math - self._Twist, self._Vector3 = Twist, Vector3 - self._cmd_pub = LCMTransport("/cmd_vel", Twist) - self._odom_sub = LCMTransport("/go2/odom", PoseStamped) - self._pose = None - self._odom_sub.subscribe(self._on_odom) - self._prev = None - def _on_odom(self, msg) -> None: - self._pose = msg - - def reset(self) -> None: - self._prev = None - - def step(self, cmd: dict[str, float]) -> dict[str, float]: - m = self._math - self._cmd_pub.broadcast( - None, - self._Twist( - linear=self._Vector3(cmd["vx"], cmd["vy"], 0.0), - angular=self._Vector3(0.0, 0.0, cmd["wz"]), - ), - ) - time.sleep(_DT) - p = self._pose - if p is None: - return {"vx": 0.0, "vy": 0.0, "wz": 0.0} - yaw = p.orientation.euler[2] - cur = (p.position.x, p.position.y, yaw, time.perf_counter()) - if self._prev is None: - self._prev = cur - return {"vx": 0.0, "vy": 0.0, "wz": 0.0} - dx, dy = cur[0] - self._prev[0], cur[1] - self._prev[1] - dyaw = (cur[2] - self._prev[2] + m.pi) % (2 * m.pi) - m.pi - dt = max(cur[3] - self._prev[3], 1e-3) - self._prev = cur - c, s = m.cos(yaw), m.sin(yaw) - return { - "vx": (dx * c + dy * s) / dt, - "vy": (-dx * s + dy * c) / dt, - "wz": dyaw / dt, - } - - -# --- the one SI loop (mode-independent) ---------------------------------- - - -def _run_si(plant) -> tuple[Go2PlantParams, dict]: - """Step every channel/amplitude through ``plant``, fit FOPDT per - channel, pool. Identical for sim and hw — only ``plant`` differs.""" - n_pre = int(_PRE_ROLL_S / _DT) - n_step = int(_STEP_S / _DT) +def _fit_selftest() -> tuple[Go2PlantParams, dict, list[dict]]: + """Step the vendored FOPDT sim plant and recover it. Plumbing check + only — proves the measure->fit->derive code path runs.""" + plant = Go2PlantSim(GO2_PLANT_FITTED) + n_pre = int(_PRE_ROLL_S / _SIM_DT) + n_step = int(_STEP_S / _SIM_DT) pooled: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} + traces: list[dict] = [] for channel in _CHANNELS: fits = [] per_amplitude[channel] = [] for amp in _SI_AMPLITUDES[channel]: - plant.reset() + plant.reset(0.0, 0.0, 0.0, _SIM_DT) cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} for _ in range(n_pre): - plant.step(cmd) + plant.step(cmd["vx"], cmd["vy"], cmd["wz"], _SIM_DT) cmd[channel] = amp - ys = [plant.step(cmd)[channel] for _ in range(n_step)] - t = np.arange(len(ys), dtype=float) * _DT # rel. to step edge + ys = [] + for _ in range(n_step): + plant.step(cmd["vx"], cmd["vy"], cmd["wz"], _SIM_DT) + ys.append(getattr(plant, channel)) + t = np.arange(len(ys), dtype=float) * _SIM_DT fp = fit_fopdt(t, np.asarray(ys, dtype=float), u_step=amp) if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") @@ -182,42 +134,351 @@ def _run_si(plant) -> tuple[Go2PlantParams, dict]: per_amplitude[channel].append( {"amplitude": amp, "direction": "forward", "K": fp.K, "tau": fp.tau, "L": fp.L} ) + traces.append( + { + "channel": channel, + "amp": amp, + "t": np.asarray(t, dtype=float), + "y": np.asarray(ys, dtype=float), + "K": fp.K, + "tau": fp.tau, + "L": fp.L, + "r2": fp.r_squared, + } + ) if not fits: - raise RuntimeError(f"SI: no converged fits for channel {channel!r}") + raise RuntimeError(f"self-test: no converged fits for {channel!r}") pooled[channel] = FopdtChannelParams( K=float(np.mean([f.K for f in fits])), tau=float(np.mean([f.tau for f in fits])), L=float(np.mean([f.L for f in fits])), ) - fitted = Go2PlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]) - if not plant.is_hw: - _print_selftest(fitted) - return fitted, per_amplitude - - -def _print_selftest(fitted: Go2PlantParams) -> None: - """sim only: recovered vs injected ground truth — should match.""" - print("\nSI self-test (recovered vs injected FOPDT ground truth):") + print("\nself-test (recovered vs injected FOPDT ground truth):") print(f" {'chan':4} {'K fit/true':>20} {'tau fit/true':>20} {'L fit/true':>20}") for ch in _CHANNELS: f, g = getattr(fitted, ch), getattr(GO2_PLANT_FITTED, ch) print( f" {ch:4} {f.K:8.3f}/{g.K:<8.3f} {f.tau:8.3f}/{g.tau:<8.3f} {f.L:8.3f}/{g.L:<8.3f}" ) + return fitted, per_amplitude, traces + + +# --- fit-quality graph (the human-facing deliverable) ------------------- + + +def _plot_fits(traces: list[dict], provenance: Provenance, out: Path) -> None: + """One column per channel; each step's measured velocity overlaid + with its fitted FOPDT step response. This is the artifact a human + reads to judge whether the model matches the real robot.""" + if not traces: + return + channels = list(dict.fromkeys(t["channel"] for t in traces)) + fig, axes = plt.subplots(1, len(channels), figsize=(6.0 * len(channels), 4.6), squeeze=False) + for ax, ch in zip(axes[0], channels, strict=True): + for tr in [t for t in traces if t["channel"] == ch]: + t_arr = tr["t"] + (line,) = ax.plot(t_arr, tr["y"], lw=1.4, alpha=0.85, label=f"meas @{tr['amp']:g}") + yhat = fopdt_step_response(t_arr, tr["K"], tr["tau"], tr["L"], tr["amp"]) + ax.plot(t_arr, yhat, "--", lw=1.4, color=line.get_color(), alpha=0.9) + row = list(t2["amp"] for t2 in traces if t2["channel"] == ch).index(tr["amp"]) + ax.annotate( + f"@{tr['amp']:g}: K={tr['K']:.3f} τ={tr['tau']:.3f} " + f"L={tr['L']:.3f} r²={tr['r2']:.2f}", + xy=(0.02, 0.97 - 0.06 * row), + xycoords="axes fraction", + ha="left", + va="top", + fontsize=7, + color=line.get_color(), + ) + unit = "rad/s" if ch == "wz" else "m/s" + ax.set_title(f"{ch} (solid = measured, dashed = FOPDT fit)") + ax.set_xlabel("time since step edge (s)") + ax.set_ylabel(f"{ch} ({unit})") + ax.grid(True, alpha=0.3) + ax.legend(loc="lower right", fontsize=7) + p = provenance + fig.suptitle( + f"Go2 FOPDT characterization — {p.robot_id} / {p.surface} / " + f"{p.mode} / {p.sim_or_hw} — {p.date} ({p.git_sha})" + ) + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) + + +# --- hardware SI (real Go2 over LCM, operator-gated, safe) --------------- + + +class _PoseVelocityEstimator: + """Differentiate consecutive ``PoseStamped`` to body-frame (vx,vy,wz); + EMA-smoothed (Go2 publishes pose only). Ported from the R&D hw loop.""" + + def __init__(self, alpha: float = 0.5) -> None: + self._pp = None + self._pt: float | None = None + self._vx = self._vy = self._wz = 0.0 + self._a = alpha + + def update(self, pose, t: float) -> tuple[float, float, float]: + if self._pp is None or self._pt is None: + self._pp, self._pt = pose, t + return 0.0, 0.0, 0.0 + dt = t - self._pt + if dt <= 0: + return self._vx, self._vy, self._wz + dx = pose.position.x - self._pp.position.x + dy = pose.position.y - self._pp.position.y + y0, y1 = self._pp.orientation.euler[2], pose.orientation.euler[2] + dyaw = (y1 - y0 + math.pi) % (2 * math.pi) - math.pi + yaw = y1 + c, s = math.cos(yaw), math.sin(yaw) + bx = (dx / dt) * c + (dy / dt) * s + by = -(dx / dt) * s + (dy / dt) * c + bw = dyaw / dt + self._vx = self._a * bx + (1 - self._a) * self._vx + self._vy = self._a * by + (1 - self._a) * self._vy + self._wz = self._a * bw + (1 - self._a) * self._wz + self._pp, self._pt = pose, t + return self._vx, self._vy, self._wz + + +def _prereq_banner() -> None: + print( + "\n=== HARDWARE MODE ===\n" + "Prereqs (run on dimensional-gpu-0):\n" + " 1. Another terminal: `dimos run unitree-go2-webrtc-keyboard-teleop`\n" + " (publishes /go2/odom, consumes /cmd_vel; its teleop is\n" + " publish-only-when-active so it stays silent while idle and\n" + " does NOT fight the SI commands).\n" + " 2. This process: strip /nix/store from LD_LIBRARY_PATH (see README)\n" + "Robot is STOPPED before every step. Reposition it with the keyboard\n" + "teleop (WASD/QE), then RELEASE all keys (teleop goes silent) and\n" + "press ENTER here — the tool then owns /cmd_vel for the step.\n" + "Each step ends at --max-dist travelled (default 6 m) or --step-s,\n" + "whichever first. Velocity clamped; zero-Twist on exit / Ctrl-C.\n" + ) + + +def _fit_hw( + step_s: float, pre_roll_s: float, warmup_s: float, max_dist: float +) -> tuple[Go2PlantParams, dict, list[dict]]: + 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.geometry_msgs.Vector3 import Vector3 + + _prereq_banner() + cmd_pub = LCMTransport("/cmd_vel", Twist) + odom_sub = LCMTransport("/go2/odom", PoseStamped) + lock = threading.Lock() + box: dict = {"pose": None, "t": 0.0} + + def _on_odom(msg) -> None: + with lock: + box["pose"] = msg + box["t"] = time.perf_counter() + + odom_sub.subscribe(_on_odom) + + def publish(vx: float, vy: float, wz: float) -> None: + cmd_pub.broadcast( + None, + Twist( + linear=Vector3(_clamp(vx, -VX_MAX, VX_MAX), 0.0, 0.0), + angular=Vector3(0.0, 0.0, _clamp(wz, -WZ_MAX, WZ_MAX)), + ), + ) + + def safe_stop() -> None: + for _ in range(3): + publish(0.0, 0.0, 0.0) + time.sleep(0.05) + + # No custom SIGINT handler: Ctrl-C must raise KeyboardInterrupt so it + # also breaks out of the blocking input() prompt. The try/finally + # below guarantees a zero-Twist stop on any exit (Ctrl-C, q, error). + + # odom warmup + print(f"[hw] waiting up to {warmup_s:.0f}s for /go2/odom ...") + deadline = time.perf_counter() + warmup_s + while time.perf_counter() < deadline: + with lock: + if box["pose"] is not None: + break + time.sleep(0.05) + with lock: + if box["pose"] is None: + safe_stop() + raise SystemExit("No /go2/odom — is `dimos run unitree-go2-webrtc-keyboard-teleop` up?") + + pooled: dict[str, FopdtChannelParams] = {} + per_amplitude: dict[str, list[dict]] = {} + traces: list[dict] = [] + try: + for channel in _HW_CHANNELS: + fits = [] + per_amplitude[channel] = [] + for amp in _SI_AMPLITUDES[channel]: + safe_stop() + resp = ( + input( + f"\n[{channel}@{amp}] reposition robot into clear space, " + f"ENTER=run s=skip q=quit: " + ) + .strip() + .lower() + ) + if resp == "q": + raise KeyboardInterrupt("operator quit") + if resp == "s": + print(" skipped") + continue + + est = _PoseVelocityEstimator() + # pre-roll zeros (settle + prime estimator) + t_end = time.perf_counter() + pre_roll_s + while time.perf_counter() < t_end: + publish(0.0, 0.0, 0.0) + with lock: + p = box["pose"] + est.update(p, time.perf_counter()) + time.sleep(_HW_DT) + + # step. Ends on whichever comes first: travelled distance + # >= max_dist (the real-space bound — at high speed the + # time cap would run the robot out of the test area), or + # t_rel > step_s (time safety cap; also the terminator for + # wz, which spins in place and never accumulates distance). + cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + cmd[channel] = amp + ts: list[float] = [] + ys: list[float] = [] + with lock: + sp = box["pose"] + x0, y0 = sp.position.x, sp.position.y + t0 = time.perf_counter() + end_reason = "time" + while True: + now = time.perf_counter() + t_rel = now - t0 + if t_rel > step_s: + break + publish(cmd["vx"], cmd["vy"], cmd["wz"]) + with lock: + p, pt = box["pose"], box["t"] + if p is None or now - pt > _ODOM_STALE_S: + print(f" [abort] stale odom ({now - pt:.2f}s)") + end_reason = "stale" + break + dist = math.hypot(p.position.x - x0, p.position.y - y0) + if dist >= max_dist: + end_reason = "dist" + break + v = est.update(p, now) + ts.append(t_rel) + ys.append(v[_CHANNELS.index(channel)]) + time.sleep(_HW_DT) + safe_stop() + + if len(ys) < 5: + print(f" [warn] {channel}@{amp}: too few samples, skip") + continue + fp = fit_fopdt(np.asarray(ts), np.asarray(ys), u_step=amp) + if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): + print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") + continue + print( + f" {channel}@{amp}: K={fp.K:.3f} tau={fp.tau:.3f} " + f"L={fp.L:.3f} ({len(ys)} samples, ended on {end_reason})" + ) + fits.append(fp) + per_amplitude[channel].append( + {"amplitude": amp, "direction": "forward", "K": fp.K, "tau": fp.tau, "L": fp.L} + ) + traces.append( + { + "channel": channel, + "amp": amp, + "t": np.asarray(ts, dtype=float), + "y": np.asarray(ys, dtype=float), + "K": fp.K, + "tau": fp.tau, + "L": fp.L, + "r2": fp.r_squared, + } + ) + if not fits: + raise RuntimeError(f"hw SI: no converged fits for {channel!r}") + pooled[channel] = FopdtChannelParams( + K=float(np.mean([f.K for f in fits])), + tau=float(np.mean([f.tau for f in fits])), + L=float(np.mean([f.L for f in fits])), + ) + except KeyboardInterrupt: + safe_stop() + raise SystemExit( + "\n[hw] aborted by operator — robot stopped, no artifact written." + ) from None + finally: + safe_stop() + + # vy is NOT excited on hardware: the real Go2 (default gait) does not + # strafe on /cmd_vel.linear.y, so a vy step yields a degenerate K≈0 + # fit that would corrupt the model. Placeholder vy = vx (sane FF / + # profile); flagged in the artifact caveats. + pooled["vy"] = pooled["vx"] + per_amplitude["vy"] = [] + print(" [note] vy not excited on hw (no lateral motion) — placeholder vy = vx") + return ( + Go2PlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]), + per_amplitude, + traces, + ) def main() -> None: ap = argparse.ArgumentParser(description="Go2 characterization -> tuning config artifact") - ap.add_argument("--mode", choices=["sim", "hw"], default="sim") - ap.add_argument("--out", default=str(REPORTS_DIR), help="output dir for the artifact") + ap.add_argument("--mode", choices=["hw", "self-test"], default="hw") + ap.add_argument("--out", default=str(REPORTS_DIR)) ap.add_argument("--robot-id", default="go2") - ap.add_argument("--surface", default="mujoco") - ap.add_argument("--gait-mode", default="default", help="locomotion gait mode") + ap.add_argument("--surface", default="concrete") + ap.add_argument("--gait-mode", default="default") + ap.add_argument( + "--step-s", + type=float, + default=_STEP_S, + help="per-step excitation duration (s); the robot must reach " + "and hold the commanded speed within this window", + ) + ap.add_argument( + "--pre-roll-s", + type=float, + default=_PRE_ROLL_S, + help="zero-command settle before each step (s)", + ) + ap.add_argument( + "--odom-warmup", + type=float, + default=_ODOM_WARMUP_S, + help="how long to wait for first /go2/odom (s)", + ) + ap.add_argument( + "--max-dist", + type=float, + default=_MAX_DIST_M, + help="per-step travel cap (m); ends the step early at high speed " + "so the robot stays in the test area", + ) args = ap.parse_args() - plant = _SimPlant() if args.mode == "sim" else _HwPlant() - fitted, per_amplitude = _run_si(plant) + if args.mode == "hw": + fitted, per_amplitude, traces = _fit_hw( + args.step_s, args.pre_roll_s, args.odom_warmup, args.max_dist + ) + else: + fitted, per_amplitude, traces = _fit_selftest() provenance = Provenance( robot_id=args.robot_id, @@ -225,18 +486,34 @@ def main() -> None: mode=args.gait_mode, date=date.today().isoformat(), git_sha=git_sha(), - sim_or_hw=args.mode, - characterization_session_dir="(in-process SI)" if args.mode == "sim" else "(hw LCM SI)", + sim_or_hw="hw" if args.mode == "hw" else "self-test", + characterization_session_dir=( + "(real Go2, LCM SI)" if args.mode == "hw" else "(in-process self-test)" + ), ) cfg = derive_config(fitted, provenance, per_amplitude=per_amplitude) + if args.mode == "hw": + cfg.caveats.append( + "vy was NOT characterized on hardware (the Go2 does not strafe " + "on /cmd_vel.linear.y in this gait); plant.vy / feedforward.K_vy " + "are a placeholder copy of vx. The benchmark paths are vx+wz " + "only, so this does not affect tuning; re-characterize vy if a " + "lateral-capable gait is used." + ) out_path = ( Path(args.out).expanduser() / f"go2_config_{args.mode}_{args.surface}_{provenance.date}_{provenance.git_sha}.json" ) cfg.to_json(out_path) - print("\nWrote config artifact (sections 1-4,6; section 5 pending benchmark):") - print(out_path.resolve()) + plot_path = out_path.with_suffix(".png") + _plot_fits(traces, provenance, plot_path) + + tag = "ROBOT-VALID" if cfg.valid_for_tuning else "NOT robot-valid (plumbing check)" + print("\nFOPDT fit graph (the deliverable — model vs real data):") + print(f" {plot_path.resolve()}") + print(f"Config artifact [{tag}] (machine handoff for the benchmark):") + print(f" {out_path.resolve()}") if __name__ == "__main__": diff --git a/dimos/utils/benchmarking/go2_tuning.py b/dimos/utils/benchmarking/go2_tuning.py index 50d9326bc7..6145b77a8c 100644 --- a/dimos/utils/benchmarking/go2_tuning.py +++ b/dimos/utils/benchmarking/go2_tuning.py @@ -206,6 +206,9 @@ class Go2TuningConfig: recommended_controller: RecommendedControllerDC caveats: list[str] = field(default_factory=list) operating_point_map: OperatingPointMap | 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 --- @@ -253,6 +256,7 @@ def _chan(d: dict) -> FopdtChannelDC: recommended_controller=RecommendedControllerDC(**data["recommended_controller"]), caveats=list(data.get("caveats", [])), operating_point_map=opm, + valid_for_tuning=bool(data.get("valid_for_tuning", True)), schema_version=sv, ) @@ -361,11 +365,16 @@ def derive_config( f"Plant fitted from {provenance.characterization_session_dir or 'n/a'} " f"on {provenance.date} (git {provenance.git_sha}).", ] - if provenance.sim_or_hw == "sim": - caveats.append( - "Derived from the FOPDT sim plant (self-test): validates the " - "pipeline, not absolute on-robot numbers — re-run --mode hw " - "for hardware-valid gains." + 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 " + "`go2_characterization --mode hw` on the real Go2 for a " + "tuning-valid artifact.", ) return Go2TuningConfig( @@ -380,6 +389,7 @@ def derive_config( recommended_controller=RecommendedControllerDC(), caveats=caveats, operating_point_map=None, + valid_for_tuning=valid_for_tuning, ) diff --git a/dimos/utils/benchmarking/reports/go2_tuning_README.md b/dimos/utils/benchmarking/reports/go2_tuning_README.md index e0f4cfd540..71c374b9f9 100644 --- a/dimos/utils/benchmarking/reports/go2_tuning_README.md +++ b/dimos/utils/benchmarking/reports/go2_tuning_README.md @@ -1,115 +1,136 @@ -# Go2 controller tuning — measure → derive → validate +# Go2 controller tuning — measure → derive → validate (HARDWARE) Two CLI tools that turn one real measurement of your Go2 into a single versioned config artifact with every parameter you need to tune the base -controller — no guesswork. +controller, then validate it on the real robot. ``` -go2_characterization ──(measure + derive)──▶ go2_config_*.json - │ -go2_benchmark ──(validate across speed)──▶ same file, section 5 added - │ - "for tolerance X cm, - run at speed Y m/s" +go2_characterization --mode hw ──▶ go2_config_hw_*.json (robot-valid) +go2_benchmark --mode hw --config … ──▶ same file + section 5 + "for tolerance X cm, run Y m/s" ``` -## Why these numbers (the settled findings — not re-derived here) - -The Go2 base is **FOPDT per axis** (first-order + dead-time). At a given -speed the path-tracking error is the **plant floor `(τ + L)·v`** — it -scales with speed, and no reactive control law has meaningful headroom -over it (validated controller bake-off). So: - -- The recommended controller is **hardcoded to the production baseline - P-controller**. It is not re-selected; the evidence string is embedded - in the artifact. -- The only effective levers are **feedforward gain** (so commanded == - achieved velocity) and **speed vs path curvature** (the velocity - profile). Both are *derived from the fitted plant*, not hand-tuned. +**This is a hardware deliverable.** Sim exists only as a plumbing +self-test / pre-check and is explicitly stamped not-robot-valid — never +tune from it. + +## Why these numbers (settled findings, not re-derived) + +Go2 base = FOPDT per axis. At a given speed the tracking error is the +plant floor `(τ+L)·v`; no reactive control law beats it. So the +recommended controller is hardcoded to the production baseline +P-controller, and the only real levers — feedforward gain (`1/K`) and a +curvature velocity profile — are *derived from the measured plant*, not +hand-tuned. + +## Prerequisites (real robot) + +1. Run on **`dimensional-gpu-0`** (the only host that reaches the Go2). +2. Terminal 1: `dimos run unitree-go2-webrtc-keyboard-teleop` + — brings up the Go2 connection (publishes `/go2/odom`, consumes + `/cmd_vel`) **and** the keyboard teleop for repositioning. This + blueprint runs the teleop **publish-only-when-active**: it stays + silent while no movement key is held (one zero Twist on release, + then nothing), so it does not flood `/cmd_vel` and coexists with the + tool. (Plain `KeyboardTeleop` defaults to streaming every loop — + only this blueprint enables the silent mode.) +3. Terminal 2: strip nix from the linker path or `.venv` numpy breaks + (`GLIBC_2.38`), then run the tool: + ``` + export LD_LIBRARY_PATH="$(echo "$LD_LIBRARY_PATH" | tr ':' '\n' \ + | grep -v /nix/store | paste -sd:)" + ``` +4. Repositioning: the robot is **stopped** at every prompt. Reposition + with the keyboard teleop (WASD move/turn, QE strafe), then **release + all keys** — the teleop goes silent — and press ENTER. The tool then + owns `/cmd_vel` for that run. Do not hold keys while a run is going. +5. Timings are operator-tunable when the robot needs more time to reach + the commanded speed or the connection is slow to come up: + `--step-s` (default 8 s, time safety cap), `--max-dist` (default 6 m, + the real-space bound — each step ends at whichever of distance/time + comes first; `wz` spins in place so it ends on time), `--pre-roll-s` + (1 s), `--odom-warmup` (default 10 s). ## Tool 1 — `go2_characterization` ``` uv run python -m dimos.utils.benchmarking.go2_characterization \ - --mode sim --surface mujoco # sim self-test -uv run python -m dimos.utils.benchmarking.go2_characterization \ - --mode hw --surface concrete --gait-mode default # real robot + --mode hw --surface concrete --gait-mode default ``` -Runs a space-cheap system-ID sequence (per-channel velocity steps for -**vx, vy, wz** — vy is real, the Go2 base strafes — no long paths), fits -FOPDT per axis, runs the DERIVE step, and writes the artifact (sections -1–4 + 6; section 5 left empty for tool 2). Prints the artifact path. - -**One harness, plant swapped by `--mode`.** Identical SI loop and FOPDT -fitter both modes; only *where the robot behaves* differs: - -- **sim**: the plant is the in-process FOPDT `Go2PlantSim` seeded with - the vendored `GO2_PLANT_FITTED` ground truth. A self-test — it recovers - the injected model (printed "recovered vs injected" table should match; - small dead-time `L` is mildly under-fit — expected, not load-bearing). - Validates the whole pipeline without a robot. -- **hw**: the plant is the real Go2 over LCM (publish `/cmd_vel`, - differentiate `/go2/odom` to body-frame velocity). Same loop, same - `fit_fopdt`. Wired; not run by CI/sim. No separate data-acquisition or - statistics pipeline — point estimates per channel (mean over swept - amplitudes), not research-grade confidence intervals. +Per channel (vx, vy, wz — vy is real, the Go2 strafes) × a few +amplitudes: + +1. Robot is **stopped**; prompt: `reposition robot … ENTER=run s=skip + q=quit`. Reposition with the keyboard teleop, release keys, ENTER. +2. Pre-roll zeros (settle), then a velocity step for `--step-s` (default + 8 s) at 10 Hz — long enough for the real Go2 to ramp to and hold the + commanded speed — recording commanded vs body-frame velocity + differentiated from `/go2/odom`. +3. `safe_stop`, fit FOPDT. + +Drift is bounded to one step (operator gate before each). Safety +throughout: velocity clamp (`VX_MAX=1.0`, `WZ_MAX=1.5`), stale-odom +abort, timeout, zero-Twist on exit and on Ctrl-C. + +**Primary output is a graph** — `go2_config_<…>.png`, one column per +channel (vx, wz) overlaying every step's *measured* velocity (solid) +with its *fitted FOPDT* step response (dashed), annotated with +K/τ/L/r² per amplitude. This is what you read to judge whether the +model matches the real robot. The `.json` is written alongside only as +the machine handoff the benchmark consumes (sections 1–4 + 6; section 5 +pending; `valid_for_tuning=true`). vx has 3 amplitudes, wz 3; vy is the +vx placeholder (not strafe-capable in this gait). + +`--mode self-test` (no robot): steps an in-process FOPDT plant seeded +with the vendored ground truth and recovers it. Proves the +measure→fit→derive code runs; artifact stamped +`valid_for_tuning=false`. This is the pytest/CI path — **not a tuning +artifact**. ## Tool 2 — `go2_benchmark` ``` uv run python -m dimos.utils.benchmarking.go2_benchmark \ - --config reports/go2_config_sim_mujoco__.json \ - --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 + --config reports/go2_config_hw_concrete__.json \ + --mode hw --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 ``` -Loads the artifact, runs the **hardcoded baseline** with the derived -feedforward + velocity profile across the speed ladder on a fixed, -real-space-constrained path set (`straight_line`, `single_corner` 2 m/90°, -`square` 2 m, `circle` R1.0). Scores each (path, speed), builds the -operating-point map + the tolerance→max-safe-speed inversion, and writes -**section 5 back into the same artifact**. Also emits a standalone -`go2_benchmark_.json` and one `cte_max`-vs-speed plot. +Loads the artifact, runs the hardcoded baseline + derived FF + velocity +profile across the speed ladder on a fixed path set (`straight_line`, +`single_corner` 2 m/90°, `square` 2 m, `circle` R1.0). For each +(path, speed): operator gate (reposition+aim, ENTER), the path is +**anchored to the robot's current pose** (so it need not be placed +precisely), then tracked closed-loop at 10 Hz off real odom; CTE scored +from the real trajectory. Writes section 5 (operating-point map + +tolerance→max-safe-speed inversion) back into the artifact, plus a +standalone json and one `cte_max`-vs-speed plot. Same safety as Tool 1. -Final line, e.g.: *"For tolerance 15 cm, run at speed 0.30 m/s with this -profile (binding path: circle)."* The binding path is the slowest path — -its limit gates the fleet. +`--mode hw` **refuses a non-robot-valid config** (a self-test artifact) — +its gains are sim-derived and meaningless on the robot. + +`--mode sim`: optional fast pre-check against the FOPDT sim plant. Loudly +labelled a pre-check; the map is not a real-robot result. Useful to +sanity-check wiring before committing the robot. ## Reading the artifact | Section | Field | Meaning | |---|---|---| -| 1 | `provenance` | robot / surface / mode / date / git sha / sim·hw — the validity scope | -| 1 | `plant` | fitted FOPDT `{K, τ, L}` per axis | -| 2 | `feedforward` | `K_axis = 1/K_plant` + output clamps — give to `FeedforwardGainConfig` | -| 3 | `velocity_profile` | curvature speed profile — give to `VelocityProfileConfig` | -| 4 | `recommended_controller` | `baseline`, `k_angular`, and the plant-floor evidence | -| 5 | `operating_point_map` | per (path,speed) `cte_max/cte_rms/arrived` + tolerance→speed table (`null` until tool 2) | -| 6 | `caveats` | surface/mode/sim-vs-hw validity limits | - -`schema_version` is enforced on load; an artifact from an incompatible -schema is rejected rather than silently mis-read. +| 1 | `provenance` | robot/surface/mode/date/sha, `sim_or_hw` | +| 1 | `valid_for_tuning` | **false ⇒ do not tune from this** (self-test) | +| 1 | `plant` | fitted FOPDT `{K,τ,L}` per axis | +| 2 | `feedforward` | `1/K` per axis + clamps | +| 3 | `velocity_profile` | curvature speed profile | +| 4 | `recommended_controller` | baseline + plant-floor evidence | +| 5 | `operating_point_map` | per (path,speed) CTE + tolerance→speed (null until Tool 2) | +| 6 | `caveats` | validity scope; self-test artifacts lead with a loud DO-NOT-TUNE banner | ## When to re-run -Re-run **tool 1** (then tool 2) whenever the plant changes: - -- different **surface** (carpet vs concrete vs ice) — friction changes K/τ, -- different **gait mode** (e.g. `rage`), -- a firmware/locomotion change, -- moving from **sim numbers to a real robot** (`--mode hw`). - -The artifact's `caveats` state exactly what it is valid for. A sim -artifact validates the pipeline, not absolute on-robot numbers. - -## What is *not* here (by design) - -The MPC / RPP / Lyapunov / pure-pursuit bake-off, command smoothers, -oscillation & speed sweeps, and plotting R&D were the *evidence* that -justified "baseline + feedforward + curvature profile" — they are the -appendix, not the product, and were deliberately left out of this -deliverable. `velocity_profile` stays default-off for any non-blueprint -caller. +Re-run Tool 1 (then Tool 2) on any plant change: different surface +(friction → K/τ), gait mode (e.g. `rage`), firmware/locomotion change. +The `caveats` state exactly what the artifact is valid for. ## Tests @@ -117,8 +138,15 @@ caller. uv run pytest dimos/utils/benchmarking/test_go2_tuning.py -q ``` -Covers the pure DERIVE step (1/K per axis incl. real vy, wz-ceiling -margin + envelope clamp, accel formulas, hardcoded baseline + evidence, -provenance-driven caveats), artifact JSON round-trip + schema-version -rejection, and the tolerance→max-speed inversion (binding = min across -paths, not-arrived excluded, infeasible → none). +Pure DERIVE (1/K incl. real vy, wz-ceiling margin + envelope clamp, +accel formulas, hardcoded baseline + evidence), `valid_for_tuning` +(true only for hw; self-test false + leading DO-NOT-TUNE caveat; +survives round-trip), artifact round-trip + schema rejection, and the +tolerance→max-speed inversion. HW loops require a robot — covered by the +manual prerequisites above, not pytest. + +## Not here (by design) + +The MPC/RPP/Lyapunov bake-off, command smoothers, sweeps, and plotting +R&D were the evidence for "baseline + FF + curvature profile"; they are +the appendix, archived off-repo, not the product. diff --git a/dimos/utils/benchmarking/test_go2_tuning.py b/dimos/utils/benchmarking/test_go2_tuning.py index 7334491dd5..48d6d8916d 100644 --- a/dimos/utils/benchmarking/test_go2_tuning.py +++ b/dimos/utils/benchmarking/test_go2_tuning.py @@ -113,6 +113,27 @@ def test_derive_leaves_operating_point_map_none(): assert derive_config(_plant(), _prov()).operating_point_map is None +def test_valid_for_tuning_only_when_hw(): + hw = derive_config(_plant(), _prov(sim_or_hw="hw")) + assert hw.valid_for_tuning is True + assert not any("DO NOT TUNE" in c for c in hw.caveats) + + st = derive_config(_plant(), _prov(sim_or_hw="self-test")) + assert st.valid_for_tuning is False + assert any("DO NOT TUNE FROM THIS" in c for c in st.caveats) + # the loud warning must be first so it can't be missed + assert "PIPELINE CHECK ONLY" in st.caveats[0] + + +def test_valid_for_tuning_survives_round_trip(tmp_path): + st = derive_config(_plant(), _prov(sim_or_hw="self-test")) + back = Go2TuningConfig.from_json(st.to_json(tmp_path / "st.json")) + assert back.valid_for_tuning is False + hw = derive_config(_plant(), _prov(sim_or_hw="hw")) + back_hw = Go2TuningConfig.from_json(hw.to_json(tmp_path / "hw.json")) + assert back_hw.valid_for_tuning is True + + # --- artifact round-trip -------------------------------------------------- From f78ddfc82318e085f82bdc573b35edc90f09b18b Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 18 May 2026 16:10:37 -0700 Subject: [PATCH 09/51] seperated ff and velocity profiler from benchmark --- dimos/utils/benchmarking/go2_benchmark.py | 143 +++++++++++++----- .../benchmarking/reports/go2_tuning_README.md | 35 +++-- 2 files changed, 125 insertions(+), 53 deletions(-) diff --git a/dimos/utils/benchmarking/go2_benchmark.py b/dimos/utils/benchmarking/go2_benchmark.py index d2c0b0d385..71ee345984 100644 --- a/dimos/utils/benchmarking/go2_benchmark.py +++ b/dimos/utils/benchmarking/go2_benchmark.py @@ -179,14 +179,16 @@ def _run_baseline_sim( path: NavPath, speed: float, k_angular: float, - ff_config: FeedforwardGainConfig, - profile_config: VelocityProfileConfig, + ff_config: FeedforwardGainConfig | None, + profile_config: VelocityProfileConfig | None, timeout_s: float, ) -> tuple[ExecutedTrajectory, NavPath]: - """Production baseline P-controller in sim against the FOPDT - ``Go2SimTwistBaseAdapter``, with the derived FF + curvature profile. - Returns the trajectory and the reference path in the executed frame - (sim runs in the path's own frame, so it is ``path`` unchanged).""" + """Stock baseline P-controller in sim against the FOPDT + ``Go2SimTwistBaseAdapter``. ``ff_config``/``profile_config`` are + OPTIONAL comparison arms (``None`` = bare controller — the + physical-limit measurement). Returns the trajectory and the + reference path in the executed frame (sim runs in the path's own + frame, so it is ``path`` unchanged).""" coord = ControlCoordinator( tick_rate=GO2_TICK_RATE_HZ, hardware=[_go2_sim_base()], @@ -203,6 +205,8 @@ def _make() -> _PathFollowerLike: ), global_config=global_config, ) + if profile_config is None: + return base return _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) coord.start() @@ -334,16 +338,18 @@ def _run_baseline_hw( path: NavPath, speed: float, k_angular: float, - ff_config: FeedforwardGainConfig, - profile_config: VelocityProfileConfig, + ff_config: FeedforwardGainConfig | None, + profile_config: VelocityProfileConfig | None, timeout_s: float, label: str, ) -> tuple[ExecutedTrajectory, NavPath]: - """Closed-loop baseline on the real Go2: anchor the path to the - robot's current pose, then track at 10 Hz off real odom. Safe: - velocity clamp, stale-odom abort, timeout, zero-Twist on exit. - Returns the trajectory and the anchored reference path (odom frame) - — score/plot must use this, not the robot-centric input path.""" + """Closed-loop stock baseline on the real Go2: anchor the path to + the robot's current pose, then track at 10 Hz off real odom. + ``ff_config``/``profile_config`` are OPTIONAL arms (``None`` = bare + controller = the physical-limit measurement). Safe: velocity clamp, + stale-odom abort, timeout, zero-Twist on exit. Returns the trajectory + and the anchored reference path (odom frame) — score/plot must use + this, not the robot-centric input path.""" cmd_pub, get_odom = link["pub"], link["get"] base = BaselinePathFollowerTask( @@ -353,7 +359,11 @@ def _run_baseline_hw( ), global_config=global_config, ) - task = _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) + task = ( + base + if profile_config is None + else _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) + ) pose0, _ = get_odom() path_w = _shift_path_to_start_at_pose(path, pose0) @@ -466,9 +476,17 @@ def get_odom(): def _run_ladder( - cfg: Go2TuningConfig, speeds: list[float], timeout_s: float, mode: str, warmup_s: float + cfg: Go2TuningConfig, + speeds: list[float], + timeout_s: float, + mode: str, + warmup_s: float, + use_ff: bool, + use_profile: bool, ) -> tuple[list[OperatingPoint], list[dict]]: - ff = cfg.feedforward.to_runtime() + # Bare stock baseline by default: this is the physical-limit + # measurement. FF / velocity profile are opt-in comparison arms. + ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) link = _open_hw_link(warmup_s) if mode == "hw" else None points: list[OperatingPoint] = [] @@ -476,7 +494,9 @@ def _run_ladder( try: for name, path in _path_set().items(): for speed in speeds: - profile = cfg.velocity_profile.to_runtime(max_linear_speed=speed) + profile = ( + cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None + ) if mode == "hw": for _ in range(3): link["pub"].broadcast(None, _twist_clamped(0.0, 0.0)) @@ -540,7 +560,7 @@ def _run_ladder( return points, runs -def _plot(points: list[OperatingPoint], out: Path) -> None: +def _plot(points: list[OperatingPoint], out: Path, arm: str = "bare") -> None: 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] @@ -548,7 +568,8 @@ def _plot(points: list[OperatingPoint], out: Path) -> None: ax.plot(xs, ys, marker="o", label=name) ax.set_xlabel("commanded speed (m/s)") ax.set_ylabel("cte_max (cm)") - ax.set_title("Go2 baseline tracking: cross-track error vs speed") + label = "BARE baseline (physical limit)" if arm == "bare" else f"baseline+{arm}" + ax.set_title(f"Go2 {label}: cross-track error vs speed") ax.grid(True, alpha=0.3) ax.legend() fig.tight_layout() @@ -584,7 +605,7 @@ def tf(pts): return tf(ref), tf(exec_) -def _plot_xy(runs: list[dict], out: Path) -> None: +def _plot_xy(runs: list[dict], out: Path, arm: str = "bare") -> None: """One subplot per path: the reference path (black) overlaid with the executed trajectory at each speed, all normalized to the canonical path frame (common origin) so speeds are directly comparable. This is @@ -630,7 +651,8 @@ def _plot_xy(runs: list[dict], out: Path) -> None: ax.legend(fontsize=7) for ax in flat[n:]: ax.set_visible(False) - fig.suptitle("Go2 baseline: executed trajectory vs reference path") + label = "BARE baseline (physical limit)" if arm == "bare" else f"baseline+{arm}" + fig.suptitle(f"Go2 {label}: executed trajectory vs reference path") fig.tight_layout() fig.savefig(out, dpi=120) plt.close(fig) @@ -649,21 +671,35 @@ def main() -> None: default=_ODOM_WARMUP_S, help="how long to wait for first /go2/odom (s)", ) + ap.add_argument( + "--ff", + action="store_true", + help="OPT-IN arm: apply the artifact's derived feedforward " + "(default OFF — bare stock baseline = the physical-limit measurement)", + ) + ap.add_argument( + "--profile", + action="store_true", + help="OPT-IN arm: apply the artifact's derived curvature velocity " + "profile (default OFF — bare stock baseline)", + ) args = ap.parse_args() config_path = Path(args.config).expanduser() cfg = Go2TuningConfig.from_json(config_path) # asserts schema_version speeds = [float(s) for s in args.speeds.split(",")] tolerances = [float(t) for t in args.tolerances.split(",")] + arm = "+".join(x for x, on in (("ff", args.ff), ("profile", args.profile)) if on) or "bare" - if args.mode == "hw" and not cfg.valid_for_tuning: + # The sim-derived ff/profile are only meaningless on the real robot + # if you actually apply them; the bare baseline doesn't use them. + if args.mode == "hw" and (args.ff or args.profile) and not cfg.valid_for_tuning: sys.exit( - "Refusing --mode hw with a non-robot-valid config " - f"({config_path.name}, sim_or_hw={cfg.provenance.sim_or_hw!r}). Its " - "feedforward/profile were derived from the sim plant — running " - "them on the real robot is meaningless. Re-run " - "`go2_characterization --mode hw` first, or use --mode sim for a " - "pre-check." + f"Refusing --mode hw with --{arm} and a non-robot-valid config " + f"({config_path.name}, sim_or_hw={cfg.provenance.sim_or_hw!r}): its " + "feedforward/profile were derived from the sim plant. Re-run " + "`go2_characterization --mode hw` first, drop --ff/--profile for " + "the bare physical-limit run, or use --mode sim." ) if args.mode == "sim": print( @@ -671,32 +707,55 @@ def main() -> None: "plant only; the operating-point map is NOT a real-robot result." ) + arm_desc = ( + "BARE stock baseline (no FF, no profile) — the plant's physical tracking limit" + if arm == "bare" + else f"baseline + {arm} (comparison arm, vs the bare physical limit)" + ) print( - f"{args.mode} speed ladder {speeds} over {len(_path_set())} paths " - f"(baseline k_angular={cfg.recommended_controller.params.get('k_angular')}):" + f"{args.mode} speed ladder {speeds} over {len(_path_set())} paths\n" + f" controller: {arm_desc}\n" + f" k_angular={cfg.recommended_controller.params.get('k_angular')}" ) try: - points, runs = _run_ladder(cfg, speeds, args.timeout, args.mode, args.odom_warmup) + points, runs = _run_ladder( + cfg, + speeds, + args.timeout, + args.mode, + args.odom_warmup, + use_ff=args.ff, + use_profile=args.profile, + ) except KeyboardInterrupt: raise SystemExit( "\n[hw] aborted by operator — robot stopped, artifact not modified." ) from None inversion = invert_tolerance(points, tolerances) - cfg.operating_point_map = OperatingPointMap( - speeds=speeds, points=points, tolerance_inversion=inversion - ) + opm = OperatingPointMap(speeds=speeds, points=points, tolerance_inversion=inversion) - cfg.to_json(config_path) # augment input artifact in place (section 5) sha = cfg.provenance.git_sha - bench_path = REPORTS_DIR / f"go2_benchmark_{sha}.json" + # Only the BARE run defines section 5 (the canonical physical-limit + # operating-point map). Comparison arms emit standalone artifacts so + # they never clobber the physical-limit map in the config. + if arm == "bare": + cfg.operating_point_map = opm + cfg.to_json(config_path) + artifact_msg = f"Augmented artifact (section 5 = physical limit): {config_path.resolve()}" + else: + artifact_msg = ( + f"Config NOT modified (arm '{arm}' is a comparison, not the " + f"physical-limit map). See standalone outputs below." + ) + bench_path = REPORTS_DIR / f"go2_benchmark_{arm}_{sha}.json" bench_path.parent.mkdir(parents=True, exist_ok=True) - bench_path.write_text(json.dumps(asdict(cfg.operating_point_map), indent=2)) - plot_path = REPORTS_DIR / f"go2_benchmark_cte_vs_speed_{sha}.png" - _plot(points, plot_path) - xy_path = REPORTS_DIR / f"go2_benchmark_xy_{sha}.png" - _plot_xy(runs, xy_path) + bench_path.write_text(json.dumps(asdict(opm), indent=2)) + plot_path = REPORTS_DIR / f"go2_benchmark_cte_vs_speed_{arm}_{sha}.png" + _plot(points, plot_path, arm) + xy_path = REPORTS_DIR / f"go2_benchmark_xy_{arm}_{sha}.png" + _plot_xy(runs, xy_path, arm) - print(f"\nAugmented artifact : {config_path.resolve()}") + print(f"\n{artifact_msg}") print(f"Benchmark json : {bench_path.resolve()}") print(f"CTE-vs-speed plot : {plot_path.resolve()}") print(f"XY trajectory plot : {xy_path.resolve()} <-- the diagnostic view") diff --git a/dimos/utils/benchmarking/reports/go2_tuning_README.md b/dimos/utils/benchmarking/reports/go2_tuning_README.md index 71c374b9f9..6e738e03e5 100644 --- a/dimos/utils/benchmarking/reports/go2_tuning_README.md +++ b/dimos/utils/benchmarking/reports/go2_tuning_README.md @@ -96,18 +96,31 @@ uv run python -m dimos.utils.benchmarking.go2_benchmark \ --mode hw --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 ``` -Loads the artifact, runs the hardcoded baseline + derived FF + velocity -profile across the speed ladder on a fixed path set (`straight_line`, -`single_corner` 2 m/90°, `square` 2 m, `circle` R1.0). For each -(path, speed): operator gate (reposition+aim, ENTER), the path is -**anchored to the robot's current pose** (so it need not be placed +**By default it runs the BARE stock baseline P-controller — no +feedforward, no velocity profile.** That is the point: this run measures +the **plant's physical tracking limit** with the existing production +controller, the number you compare everything against and check against +the `(τ+L)·v` floor from characterization. Path set is fixed +(`straight_line`, `single_corner` 2 m/90°, `square` 2 m, `circle` R1.0). +For each (path, speed): operator gate (reposition+aim, ENTER), the path +is **anchored to the robot's current pose** (so it need not be placed precisely), then tracked closed-loop at 10 Hz off real odom; CTE scored -from the real trajectory. Writes section 5 (operating-point map + -tolerance→max-safe-speed inversion) back into the artifact, plus a -standalone json and one `cte_max`-vs-speed plot. Same safety as Tool 1. - -`--mode hw` **refuses a non-robot-valid config** (a self-test artifact) — -its gains are sim-derived and meaningless on the robot. +from the real trajectory. The **bare** run writes section 5 +(operating-point map + tolerance→max-safe-speed inversion) back into the +artifact — that is the canonical physical-limit map. Same safety as Tool 1. + +Optional comparison arms (off by default), each measured *against* the +bare physical limit, written to standalone `__` files that never +clobber section 5: + +- `--ff` — apply the artifact's derived feedforward. +- `--profile` — apply the artifact's derived curvature velocity profile. +- `--ff --profile` — both (the fully-derived config). + +`--mode hw` only **refuses a non-robot-valid config when `--ff`/`--profile` +is set** (sim-derived gains are meaningless on the real robot). The bare +physical-limit run accepts any config (it doesn't use the derived +params). `--mode sim`: optional fast pre-check against the FOPDT sim plant. Loudly labelled a pre-check; the map is not a real-robot result. Useful to From 3afa747212e18b0ef0ed4508c511811d7fe51e98 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Mon, 18 May 2026 17:07:30 -0700 Subject: [PATCH 10/51] refactored to make stack robot agnostic --- .../{go2_sim => fopdt_sim_base}/adapter.py | 42 +-- .../{go2_benchmark.py => benchmark.py} | 212 ++++++++------- ...haracterization.py => characterization.py} | 253 +++++++++--------- dimos/utils/benchmarking/plant.py | 105 ++++++-- .../benchmarking/reports/go2_tuning_README.md | 165 ------------ .../benchmarking/reports/tuning_README.md | 171 ++++++++++++ .../{test_go2_tuning.py => test_tuning.py} | 18 +- .../benchmarking/{go2_tuning.py => tuning.py} | 58 ++-- 8 files changed, 562 insertions(+), 462 deletions(-) rename dimos/hardware/drive_trains/{go2_sim => fopdt_sim_base}/adapter.py (78%) rename dimos/utils/benchmarking/{go2_benchmark.py => benchmark.py} (80%) rename dimos/utils/benchmarking/{go2_characterization.py => characterization.py} (66%) delete mode 100644 dimos/utils/benchmarking/reports/go2_tuning_README.md create mode 100644 dimos/utils/benchmarking/reports/tuning_README.md rename dimos/utils/benchmarking/{test_go2_tuning.py => test_tuning.py} (93%) rename dimos/utils/benchmarking/{go2_tuning.py => tuning.py} (89%) diff --git a/dimos/hardware/drive_trains/go2_sim/adapter.py b/dimos/hardware/drive_trains/fopdt_sim_base/adapter.py similarity index 78% rename from dimos/hardware/drive_trains/go2_sim/adapter.py rename to dimos/hardware/drive_trains/fopdt_sim_base/adapter.py index 77e62f7c70..a726edf8c9 100644 --- a/dimos/hardware/drive_trains/go2_sim/adapter.py +++ b/dimos/hardware/drive_trains/fopdt_sim_base/adapter.py @@ -12,12 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Layer 1 sim TwistBase adapter for the Unitree Go2. +"""Layer 1 FOPDT sim TwistBase adapter (robot-agnostic). -Wraps :class:`~dimos.utils.benchmarking.plant.Go2PlantSim` and presents -the standard :class:`TwistBaseAdapter` protocol so any controller / task / -coordinator that talks to a real Go2 base can be exercised in pure-Python -sim with no hardware. +Wraps :class:`~dimos.utils.benchmarking.plant.TwistBasePlantSim` and +presents the standard :class:`TwistBaseAdapter` protocol so any +controller / task / coordinator that talks to a real velocity-commanded +base can be exercised in pure-Python sim with no hardware. The plant +params are supplied per robot (``params=`` kwarg, via the robot +profile's ``sim_plant``); the Go2 fit is only the default fallback. Plant integration is wall-clock driven: each :meth:`write_velocities` call advances the plant by ``time.perf_counter()`` delta since the last @@ -30,34 +32,38 @@ import time from typing import TYPE_CHECKING -from dimos.utils.benchmarking.plant import GO2_PLANT_FITTED, Go2PlantParams, Go2PlantSim +from dimos.utils.benchmarking.plant import ( + GO2_PLANT_FITTED, + TwistBasePlantParams, + TwistBasePlantSim, +) if TYPE_CHECKING: from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry -class Go2SimTwistBaseAdapter: - """FOPDT + unicycle sim posing as a real Go2 twist base. +class FopdtTwistBaseAdapter: + """FOPDT + unicycle sim posing as a real twist base. Implements :class:`TwistBaseAdapter`. ``dof`` is fixed at 3 for the - holonomic Go2 model (vx, vy, wz); commanding non-zero vy is a sim - artifact in the sim ground truth (the real Go2 strafes — characterize - real vy via ``go2_characterization --mode hw``). + twist-base model (vx, vy, wz). For a non-strafing robot, vy is simply + never commanded (the characterization tool excludes it); the model + itself is robot-agnostic — pass the robot's fitted ``params``. """ def __init__( self, dof: int = 3, - params: Go2PlantParams | None = None, + params: TwistBasePlantParams | None = None, initial_pose: tuple[float, float, float] = (0.0, 0.0, 0.0), nominal_dt: float = 0.1, **_: object, ) -> None: if dof != 3: - raise ValueError(f"Go2SimTwistBaseAdapter requires dof=3, got {dof}") + raise ValueError(f"FopdtTwistBaseAdapter requires dof=3, got {dof}") self._dof = dof self._params = params if params is not None else GO2_PLANT_FITTED - self._plant = Go2PlantSim(self._params) + self._plant = TwistBasePlantSim(self._params) self._initial_pose = initial_pose self._nominal_dt = nominal_dt @@ -137,7 +143,7 @@ def read_enabled(self) -> bool: # ========================================================================= @property - def plant(self) -> Go2PlantSim: + def plant(self) -> TwistBasePlantSim: """Direct access to the underlying plant (for inspection / tests).""" return self._plant @@ -147,8 +153,8 @@ def set_initial_pose(self, x: float, y: float, yaw: float) -> None: def register(registry: TwistBaseAdapterRegistry) -> None: - """Register this adapter with the registry under ``go2_sim_twist_base``.""" - registry.register("go2_sim_twist_base", Go2SimTwistBaseAdapter) + """Register this adapter under ``fopdt_sim_twist_base``.""" + registry.register("fopdt_sim_twist_base", FopdtTwistBaseAdapter) -__all__ = ["Go2SimTwistBaseAdapter"] +__all__ = ["FopdtTwistBaseAdapter"] diff --git a/dimos/utils/benchmarking/go2_benchmark.py b/dimos/utils/benchmarking/benchmark.py similarity index 80% rename from dimos/utils/benchmarking/go2_benchmark.py rename to dimos/utils/benchmarking/benchmark.py index 71ee345984..39c7e9f742 100644 --- a/dimos/utils/benchmarking/go2_benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -12,20 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Tool 2 of the Go2 tuning deliverable: the operating-point benchmark. +"""Tool 2 of the twist-base tuning deliverable: operating-point benchmark. -Consumes the config artifact from ``go2_characterization``, runs the -HARDCODED baseline P-controller with the derived feedforward + velocity -profile across a speed ladder on a fixed real-space-constrained path set, -scores each (path, speed), and writes back the operating-point map + -tolerance->max-safe-speed inversion (artifact section 5). +Consumes the config artifact from ``characterization``, runs the stock +baseline P-controller (bare by default = the plant's physical tracking +limit; ``--ff`` / ``--profile`` are opt-in comparison arms) across a +speed ladder on a fixed real-space-constrained path set, scores each +(path, speed), and writes back the operating-point map + +tolerance->max-safe-speed inversion (artifact section 5). Robot-agnostic: +everything robot-specific comes from the ``RobotProfile`` (``--robot``). - uv run python -m dimos.utils.benchmarking.go2_benchmark \\ - --config reports/go2_config_sim_mujoco__.json + uv run python -m dimos.utils.benchmarking.benchmark \\ + --robot go2 --config reports/go2_config_hw_<...>.json --mode hw -The sim harness (the baseline controller driven through a real -``ControlCoordinator`` + the FOPDT ``Go2SimTwistBaseAdapter``) is inlined -below — it is small, baseline-only, and used by nothing else. +The sim harness (the baseline driven through a real ``ControlCoordinator`` ++ the FOPDT sim adapter) is inlined below — small, baseline-only. """ from __future__ import annotations @@ -64,25 +65,44 @@ 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.utils.benchmarking.go2_tuning import ( - Go2TuningConfig, +from dimos.utils.benchmarking.paths import circle, single_corner, square, straight_line +from dimos.utils.benchmarking.plant import ROBOT_PROFILES, RobotProfile +from dimos.utils.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run +from dimos.utils.benchmarking.tuning import ( OperatingPoint, OperatingPointMap, + TuningConfig, invert_tolerance, ) -from dimos.utils.benchmarking.paths import circle, single_corner, square, straight_line -from dimos.utils.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run from dimos.utils.benchmarking.velocity_profile import PathSpeedCap, VelocityProfileConfig if TYPE_CHECKING: - from dimos.hardware.drive_trains.go2_sim.adapter import Go2SimTwistBaseAdapter + from dimos.hardware.drive_trains.fopdt_sim_base.adapter import FopdtTwistBaseAdapter -# Go2 hardware control rate. -GO2_TICK_RATE_HZ = 10.0 _base_joints = make_twist_base_joints("base") _ARRIVED_STATES = frozenset({"arrived", "completed"}) _FAILED_STATES = frozenset({"aborted"}) +REPORTS_DIR = Path(__file__).parent / "reports" + + +def _resolve_profile(name: str) -> RobotProfile: + try: + return ROBOT_PROFILES[name] + except KeyError: + raise SystemExit(f"unknown --robot {name!r}; known: {sorted(ROBOT_PROFILES)}") from None + + +def _clamp(v: float, lo: float, hi: float) -> float: + return max(lo, min(hi, v)) + + +def _twist_clamped(vx: float, wz: float, vx_max: float, wz_max: float) -> Twist: + return Twist( + linear=Vector3(_clamp(vx, -vx_max, vx_max), 0.0, 0.0), + angular=Vector3(0.0, 0.0, _clamp(wz, -wz_max, wz_max)), + ) + # --- inlined baseline sim harness (was runner.py + sim_blueprint.py) ----- @@ -155,12 +175,13 @@ def compute(self, state): ) -def _go2_sim_base() -> HardwareComponent: +def _sim_base(profile: RobotProfile) -> HardwareComponent: return HardwareComponent( hardware_id="base", hardware_type=HardwareType.BASE, joints=make_twist_base_joints("base"), - adapter_type="go2_sim_twist_base", + adapter_type=profile.sim_adapter_key, + adapter_kwargs={"params": profile.sim_plant}, ) @@ -176,6 +197,7 @@ def _vels_to_twist(v: list[float]) -> Twist: def _run_baseline_sim( + profile: RobotProfile, path: NavPath, speed: float, k_angular: float, @@ -183,15 +205,14 @@ def _run_baseline_sim( profile_config: VelocityProfileConfig | None, timeout_s: float, ) -> tuple[ExecutedTrajectory, NavPath]: - """Stock baseline P-controller in sim against the FOPDT - ``Go2SimTwistBaseAdapter``. ``ff_config``/``profile_config`` are - OPTIONAL comparison arms (``None`` = bare controller — the - physical-limit measurement). Returns the trajectory and the - reference path in the executed frame (sim runs in the path's own - frame, so it is ``path`` unchanged).""" + """Stock baseline P-controller in sim against the profile's FOPDT + sim adapter. ``ff_config``/``profile_config`` are OPTIONAL comparison + arms (``None`` = bare controller — the physical-limit measurement). + Returns the trajectory and the reference path in the executed frame + (sim runs in the path's own frame, so it is ``path`` unchanged).""" coord = ControlCoordinator( - tick_rate=GO2_TICK_RATE_HZ, - hardware=[_go2_sim_base()], + tick_rate=profile.tick_rate_hz, + hardware=[_sim_base(profile)], tasks=[ TaskConfig(name="vel_base", type="velocity", joint_names=_base_joints, priority=10), ], @@ -211,7 +232,7 @@ def _make() -> _PathFollowerLike: coord.start() try: - adapter: Go2SimTwistBaseAdapter = coord._hardware["base"].adapter + adapter: FopdtTwistBaseAdapter = coord._hardware["base"].adapter start = path.poses[0] adapter.set_initial_pose(start.position.x, start.position.y, start.orientation.euler[2]) adapter.connect() @@ -221,7 +242,7 @@ def _make() -> _PathFollowerLike: task.start_path(path, _odom_to_pose(adapter.read_odometry())) ticks: list[TrajectoryTick] = [] - period = 1.0 / GO2_TICK_RATE_HZ + period = 1.0 / profile.tick_rate_hz t0 = time.perf_counter() next_sample = t0 arrived = False @@ -255,31 +276,14 @@ def _make() -> _PathFollowerLike: coord.stop() -# --- hw harness (real Go2 over LCM; closed-loop baseline) --------------- +# --- hw harness (real robot over LCM; closed-loop baseline) ------------- # # Ported from the R&D `_run_path_follower_hw`. Talks LCM to a separately -# running `dimos run unitree-go2-webrtc-keyboard-teleop` (publishes -# /go2/odom, consumes /cmd_vel; its teleop is publish-only-when-active, -# so between gated paths you reposition with the keyboard, release keys -# — teleop goes silent — then the tool owns /cmd_vel for the run). No -# new module — inlined here, the small estimator/anchor duplicated with -# go2_characterization by choice (no shared-module addition). - -VX_MAX = 1.0 -WZ_MAX = 1.5 -_ODOM_WARMUP_S = 10.0 # WebRTC connect + first odom (override: --odom-warmup) -_ODOM_STALE_S = 1.0 - - -def _clamp(v: float, lo: float, hi: float) -> float: - return max(lo, min(hi, v)) - - -def _twist_clamped(vx: float, wz: float) -> Twist: - return Twist( - linear=Vector3(_clamp(vx, -VX_MAX, VX_MAX), 0.0, 0.0), - angular=Vector3(0.0, 0.0, _clamp(wz, -WZ_MAX, WZ_MAX)), - ) +# running `dimos run ` (publishes the odom topic, +# consumes the cmd topic; if that blueprint includes a keyboard teleop it +# must be publish-only-when-active so it does not fight the run). No new +# module — the small estimator/anchor are duplicated with +# characterization by choice (no shared-module addition). class _PoseVelocityEstimator: @@ -334,6 +338,7 @@ def _shift_path_to_start_at_pose(path: NavPath, start_pose: PoseStamped) -> NavP def _run_baseline_hw( + profile: RobotProfile, link: dict, path: NavPath, speed: float, @@ -343,15 +348,18 @@ def _run_baseline_hw( timeout_s: float, label: str, ) -> tuple[ExecutedTrajectory, NavPath]: - """Closed-loop stock baseline on the real Go2: anchor the path to - the robot's current pose, then track at 10 Hz off real odom. - ``ff_config``/``profile_config`` are OPTIONAL arms (``None`` = bare - controller = the physical-limit measurement). Safe: velocity clamp, - stale-odom abort, timeout, zero-Twist on exit. Returns the trajectory - and the anchored reference path (odom frame) — score/plot must use - this, not the robot-centric input path.""" + """Closed-loop stock baseline on the real robot: anchor the path to + the robot's current pose, then track at the profile tick rate off + real odom. ``ff_config``/``profile_config`` are OPTIONAL arms + (``None`` = bare = the physical-limit measurement). Safe: velocity + clamp, stale-odom abort, timeout, zero-Twist on exit. Returns the + trajectory and the anchored reference path (odom frame) — score/plot + must use this, not the robot-centric input path.""" cmd_pub, get_odom = link["pub"], link["get"] + def stop_twist() -> Twist: + return _twist_clamped(0.0, 0.0, profile.vx_max, profile.wz_max) + base = BaselinePathFollowerTask( name=f"baseline_{label}", config=BaselinePathFollowerTaskConfig( @@ -371,7 +379,7 @@ def _run_baseline_hw( ticks: list[TrajectoryTick] = [] est = _PoseVelocityEstimator() - period = 1.0 / GO2_TICK_RATE_HZ + period = 1.0 / profile.tick_rate_hz t0 = time.perf_counter() arrived = False try: @@ -382,7 +390,7 @@ def _run_baseline_hw( print(f" [{label}] timeout {timeout_s:.0f}s") break pose, age = get_odom() - if pose is None or age > _ODOM_STALE_S: + if pose is None or age > profile.odom_stale_s: print(f" [{label}] ABORT stale odom ({age:.2f}s)") break task.update_odom(pose) @@ -401,7 +409,7 @@ def _run_baseline_hw( if (out is not None and out.velocities is not None) else (0.0, 0.0) ) - tw = _twist_clamped(vx, wz) + tw = _twist_clamped(vx, wz, profile.vx_max, profile.wz_max) cmd_pub.broadcast(None, tw) ticks.append( TrajectoryTick( @@ -425,7 +433,7 @@ def _run_baseline_hw( time.sleep(max(0.0, t0 + len(ticks) * period - time.perf_counter())) finally: for _ in range(3): - cmd_pub.broadcast(None, _twist_clamped(0.0, 0.0)) + cmd_pub.broadcast(None, stop_twist()) time.sleep(0.05) return ExecutedTrajectory(ticks=ticks, arrived=arrived), path_w @@ -443,15 +451,12 @@ def _path_set() -> dict: } -REPORTS_DIR = Path(__file__).parent / "reports" - - -def _open_hw_link(warmup_s: float) -> dict: - """LCM to a running `dimos run unitree-go2-webrtc-keyboard-teleop`.""" +def _open_hw_link(profile: RobotProfile, warmup_s: float) -> dict: + """LCM to a running `dimos run `.""" from dimos.core.transport import LCMTransport - cmd_pub = LCMTransport("/cmd_vel", Twist) - odom_sub = LCMTransport("/go2/odom", PoseStamped) + cmd_pub = LCMTransport(profile.cmd_topic, Twist) + odom_sub = LCMTransport(profile.odom_topic, PoseStamped) lock = threading.Lock() box: dict = {"pose": None, "t": 0.0} @@ -466,17 +471,18 @@ def get_odom(): with lock: return box["pose"], time.perf_counter() - box["t"] - print(f"[hw] waiting up to {warmup_s:.0f}s for /go2/odom ...") + print(f"[hw] waiting up to {warmup_s:.0f}s for {profile.odom_topic} ...") deadline = time.perf_counter() + warmup_s while time.perf_counter() < deadline and get_odom()[0] is None: time.sleep(0.05) if get_odom()[0] is None: - raise RuntimeError("No /go2/odom — is `dimos run unitree-go2-webrtc-keyboard-teleop` up?") + raise RuntimeError(f"No {profile.odom_topic} — is `dimos run {profile.blueprint}` up?") return {"pub": cmd_pub, "get": get_odom} def _run_ladder( - cfg: Go2TuningConfig, + cfg: TuningConfig, + profile: RobotProfile, speeds: list[float], timeout_s: float, mode: str, @@ -488,18 +494,20 @@ def _run_ladder( # measurement. FF / velocity profile are opt-in comparison arms. ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) - link = _open_hw_link(warmup_s) if mode == "hw" else None + link = _open_hw_link(profile, warmup_s) if mode == "hw" else None points: list[OperatingPoint] = [] runs: list[dict] = [] # for the XY trajectory overlay try: for name, path in _path_set().items(): for speed in speeds: - profile = ( + prof_cfg = ( cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None ) if mode == "hw": for _ in range(3): - link["pub"].broadcast(None, _twist_clamped(0.0, 0.0)) + link["pub"].broadcast( + None, _twist_clamped(0.0, 0.0, profile.vx_max, profile.wz_max) + ) time.sleep(0.05) resp = ( input( @@ -515,17 +523,20 @@ def _run_ladder( print(" skipped") continue traj, ref = _run_baseline_hw( + profile, link, path, speed, k_angular, ff, - profile, + prof_cfg, timeout_s, f"{name}@{speed:.2f}", ) else: - traj, ref = _run_baseline_sim(path, speed, k_angular, ff, profile, timeout_s) + traj, ref = _run_baseline_sim( + profile, path, speed, k_angular, ff, prof_cfg, timeout_s + ) # Score/plot against the executed-frame reference: in hw # that's the pose-anchored path, not the robot-centric input. s = score_run(ref, traj) @@ -555,12 +566,14 @@ def _run_ladder( finally: if link is not None: for _ in range(3): - link["pub"].broadcast(None, _twist_clamped(0.0, 0.0)) + link["pub"].broadcast( + None, _twist_clamped(0.0, 0.0, profile.vx_max, profile.wz_max) + ) time.sleep(0.05) return points, runs -def _plot(points: list[OperatingPoint], out: Path, arm: str = "bare") -> None: +def _plot(points: list[OperatingPoint], out: Path, robot_name: str, arm: str) -> None: 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] @@ -569,7 +582,7 @@ def _plot(points: list[OperatingPoint], out: Path, arm: str = "bare") -> None: ax.set_xlabel("commanded speed (m/s)") ax.set_ylabel("cte_max (cm)") label = "BARE baseline (physical limit)" if arm == "bare" else f"baseline+{arm}" - ax.set_title(f"Go2 {label}: cross-track error vs speed") + ax.set_title(f"{robot_name} {label}: cross-track error vs speed") ax.grid(True, alpha=0.3) ax.legend() fig.tight_layout() @@ -605,7 +618,7 @@ def tf(pts): return tf(ref), tf(exec_) -def _plot_xy(runs: list[dict], out: Path, arm: str = "bare") -> None: +def _plot_xy(runs: list[dict], out: Path, robot_name: str, arm: str) -> None: """One subplot per path: the reference path (black) overlaid with the executed trajectory at each speed, all normalized to the canonical path frame (common origin) so speeds are directly comparable. This is @@ -652,15 +665,16 @@ def _plot_xy(runs: list[dict], out: Path, arm: str = "bare") -> None: for ax in flat[n:]: ax.set_visible(False) label = "BARE baseline (physical limit)" if arm == "bare" else f"baseline+{arm}" - fig.suptitle(f"Go2 {label}: executed trajectory vs reference path") + fig.suptitle(f"{robot_name} {label}: executed trajectory vs reference path") fig.tight_layout() fig.savefig(out, dpi=120) plt.close(fig) def main() -> None: - ap = argparse.ArgumentParser(description="Go2 operating-point benchmark") - ap.add_argument("--config", required=True, help="config artifact from go2_characterization") + ap = argparse.ArgumentParser(description="Twist-base operating-point benchmark") + ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PROFILES)}") + ap.add_argument("--config", required=True, help="config artifact from characterization") ap.add_argument("--mode", choices=["hw", "sim"], default="hw") ap.add_argument("--speeds", default="0.3,0.5,0.7,0.9,1.0") ap.add_argument("--tolerances", default="5,10,15", help="cm") @@ -668,8 +682,8 @@ def main() -> None: ap.add_argument( "--odom-warmup", type=float, - default=_ODOM_WARMUP_S, - help="how long to wait for first /go2/odom (s)", + default=None, + help="how long to wait for first odom (s); default from profile", ) ap.add_argument( "--ff", @@ -685,8 +699,10 @@ def main() -> None: ) args = ap.parse_args() + profile = _resolve_profile(args.robot) + warmup_s = args.odom_warmup if args.odom_warmup is not None else profile.odom_warmup_s config_path = Path(args.config).expanduser() - cfg = Go2TuningConfig.from_json(config_path) # asserts schema_version + cfg = TuningConfig.from_json(config_path) # asserts schema_version speeds = [float(s) for s in args.speeds.split(",")] tolerances = [float(t) for t in args.tolerances.split(",")] arm = "+".join(x for x, on in (("ff", args.ff), ("profile", args.profile)) if on) or "bare" @@ -698,7 +714,7 @@ def main() -> None: f"Refusing --mode hw with --{arm} and a non-robot-valid config " f"({config_path.name}, sim_or_hw={cfg.provenance.sim_or_hw!r}): its " "feedforward/profile were derived from the sim plant. Re-run " - "`go2_characterization --mode hw` first, drop --ff/--profile for " + "`characterization --mode hw` first, drop --ff/--profile for " "the bare physical-limit run, or use --mode sim." ) if args.mode == "sim": @@ -713,17 +729,18 @@ def main() -> None: else f"baseline + {arm} (comparison arm, vs the bare physical limit)" ) print( - f"{args.mode} speed ladder {speeds} over {len(_path_set())} paths\n" + f"{profile.name} {args.mode} speed ladder {speeds} over {len(_path_set())} paths\n" f" controller: {arm_desc}\n" f" k_angular={cfg.recommended_controller.params.get('k_angular')}" ) try: points, runs = _run_ladder( cfg, + profile, speeds, args.timeout, args.mode, - args.odom_warmup, + warmup_s, use_ff=args.ff, use_profile=args.profile, ) @@ -735,6 +752,7 @@ def main() -> None: opm = OperatingPointMap(speeds=speeds, points=points, tolerance_inversion=inversion) sha = cfg.provenance.git_sha + rid = cfg.provenance.robot_id # Only the BARE run defines section 5 (the canonical physical-limit # operating-point map). Comparison arms emit standalone artifacts so # they never clobber the physical-limit map in the config. @@ -747,13 +765,13 @@ def main() -> None: f"Config NOT modified (arm '{arm}' is a comparison, not the " f"physical-limit map). See standalone outputs below." ) - bench_path = REPORTS_DIR / f"go2_benchmark_{arm}_{sha}.json" + bench_path = REPORTS_DIR / f"{rid}_benchmark_{arm}_{sha}.json" bench_path.parent.mkdir(parents=True, exist_ok=True) bench_path.write_text(json.dumps(asdict(opm), indent=2)) - plot_path = REPORTS_DIR / f"go2_benchmark_cte_vs_speed_{arm}_{sha}.png" - _plot(points, plot_path, arm) - xy_path = REPORTS_DIR / f"go2_benchmark_xy_{arm}_{sha}.png" - _plot_xy(runs, xy_path, arm) + plot_path = REPORTS_DIR / f"{rid}_benchmark_cte_vs_speed_{arm}_{sha}.png" + _plot(points, plot_path, profile.name, arm) + xy_path = REPORTS_DIR / f"{rid}_benchmark_xy_{arm}_{sha}.png" + _plot_xy(runs, xy_path, profile.name, arm) print(f"\n{artifact_msg}") print(f"Benchmark json : {bench_path.resolve()}") diff --git a/dimos/utils/benchmarking/go2_characterization.py b/dimos/utils/benchmarking/characterization.py similarity index 66% rename from dimos/utils/benchmarking/go2_characterization.py rename to dimos/utils/benchmarking/characterization.py index 93f270c74f..102002e4e1 100644 --- a/dimos/utils/benchmarking/go2_characterization.py +++ b/dimos/utils/benchmarking/characterization.py @@ -12,30 +12,30 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Tool 1 of the Go2 tuning deliverable: characterization. +"""Tool 1 of the twist-base tuning deliverable: characterization. -**This is a hardware tool.** It measures the real Go2's per-axis velocity -response (vx, vy, wz), fits FOPDT per channel, runs the DERIVE step, and -emits the versioned config artifact. +**This is a hardware tool.** It measures a real velocity-commanded +base's per-axis response (vx, vy, wz), fits FOPDT per channel, runs the +DERIVE step, and emits the versioned config artifact. Robot-agnostic: +everything robot-specific comes from the selected ``RobotProfile`` +(``--robot``, default ``go2``). - # On dimensional-gpu-0, terminal 1: - dimos run unitree-go2-webrtc-keyboard-teleop + # terminal 1 (the robot's bring-up blueprint, see the profile): + dimos run # terminal 2 (strip /nix/store from LD_LIBRARY_PATH — see README): - uv run python -m dimos.utils.benchmarking.go2_characterization \\ - --mode hw --surface concrete + uv run python -m dimos.utils.benchmarking.characterization \\ + --robot go2 --mode hw --surface concrete -`--mode hw` (default) drives the real robot over LCM (`/cmd_vel` out, -`/go2/odom` in). It is **operator-gated**: before every step it stops the -robot and waits for you to reposition it (with the keyboard teleop from -the blueprint above) and press ENTER. This bounds drift to a single step -and is safe (velocity clamp, zero-Twist on exit/SIGINT, stale-odom -abort, timeout). +`--mode hw` (default) drives the real robot over LCM (profile cmd topic +out, odom topic in). It is **operator-gated**: before every step it +stops the robot and waits for you to reposition it and press ENTER. +Safe (velocity clamp, zero-Twist on exit/SIGINT, stale-odom abort, +distance + time caps). `--mode self-test` is a **plumbing check, NOT a tuning artifact**: it -steps an in-process FOPDT `Go2PlantSim` seeded with the vendored -ground truth and recovers it. It only proves the measure->fit->derive -code runs; the artifact is stamped `valid_for_tuning=false`. Used by -pytest/CI so regressions are caught without a robot. +steps the profile's in-process FOPDT sim plant and recovers it. It only +proves the measure->fit->derive code runs; the artifact is stamped +`valid_for_tuning=false`. Used by pytest/CI without a robot. """ from __future__ import annotations @@ -53,44 +53,21 @@ import matplotlib.pyplot as plt import numpy as np -from dimos.utils.benchmarking.go2_tuning import Provenance, derive_config, git_sha from dimos.utils.benchmarking.plant import ( - GO2_PLANT_FITTED, + ROBOT_PROFILES, FopdtChannelParams, - Go2PlantParams, - Go2PlantSim, + RobotProfile, + TwistBasePlantParams, + TwistBasePlantSim, ) +from dimos.utils.benchmarking.tuning import Provenance, derive_config, git_sha from dimos.utils.characterization.modeling.fopdt import fit_fopdt, fopdt_step_response -# Space-cheap SI plan: a few amplitudes per channel. vy is a real channel -# (the Go2 base strafes) so it gets its own sweep — not a copy of vx. -_SI_AMPLITUDES: dict[str, list[float]] = { - "vx": [0.3, 0.6, 0.9], - "vy": [0.2, 0.4], - "wz": [0.4, 0.8, 1.2], -} -_CHANNELS = ("vx", "vy", "wz") # velocity-tuple order (estimator output) -# Channels excited on the real robot. vy is omitted: the Go2 does not -# strafe on /cmd_vel.linear.y in the default gait, so exciting it only -# produces a degenerate fit. vy is placeholdered (= vx) post-hoc. -_HW_CHANNELS = ("vx", "wz") -_PRE_ROLL_S = 1.0 -# Real-robot default: gait initiation + command latency means the Go2 -# needs several seconds to ramp to and hold the commanded velocity, much -# longer than the bare FOPDT settle. Operator-overridable (--step-s). -_STEP_S = 8.0 -# Per-step travel cap (m). At high speed the time cap would run the -# robot out of the test area, so distance is the real bound; step_s is -# the safety upper bound and the terminator for wz (spins in place). -_MAX_DIST_M = 6.0 - -# Hardware safety envelope (Go2 Rung-1 saturation) + control rate. -VX_MAX = 1.0 # m/s -WZ_MAX = 1.5 # rad/s -_HW_DT = 1.0 / 10.0 # Go2 control + odom tick -_SIM_DT = 0.02 -_ODOM_WARMUP_S = 10.0 # WebRTC connect + first odom (override: --odom-warmup) -_ODOM_STALE_S = 1.0 +# Fixed twist-base velocity-tuple order (estimator output / channel +# index). NOT robot-specific — the per-robot excited subset is +# profile.excited_channels. +_CHANNELS = ("vx", "vy", "wz") +_SIM_DT = 0.02 # in-process self-test integration step (not robot-specific) REPORTS_DIR = Path(__file__).parent / "reports" @@ -99,15 +76,23 @@ def _clamp(v: float, lo: float, hi: float) -> float: return max(lo, min(hi, v)) +def _resolve_profile(name: str) -> RobotProfile: + try: + return ROBOT_PROFILES[name] + except KeyError: + raise SystemExit(f"unknown --robot {name!r}; known: {sorted(ROBOT_PROFILES)}") from None + + # --- self-test (in-process FOPDT plant; NOT robot-valid) ----------------- -def _fit_selftest() -> tuple[Go2PlantParams, dict, list[dict]]: - """Step the vendored FOPDT sim plant and recover it. Plumbing check +def _fit_selftest(profile: RobotProfile) -> tuple[TwistBasePlantParams, dict, list[dict]]: + """Step the profile's FOPDT sim plant and recover it. Plumbing check only — proves the measure->fit->derive code path runs.""" - plant = Go2PlantSim(GO2_PLANT_FITTED) - n_pre = int(_PRE_ROLL_S / _SIM_DT) - n_step = int(_STEP_S / _SIM_DT) + truth = profile.sim_plant + plant = TwistBasePlantSim(truth) + n_pre = int(profile.pre_roll_s / _SIM_DT) + n_step = int(profile.step_s / _SIM_DT) pooled: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} traces: list[dict] = [] @@ -115,7 +100,7 @@ def _fit_selftest() -> tuple[Go2PlantParams, dict, list[dict]]: for channel in _CHANNELS: fits = [] per_amplitude[channel] = [] - for amp in _SI_AMPLITUDES[channel]: + for amp in profile.si_amplitudes[channel]: plant.reset(0.0, 0.0, 0.0, _SIM_DT) cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} for _ in range(n_pre): @@ -153,11 +138,11 @@ def _fit_selftest() -> tuple[Go2PlantParams, dict, list[dict]]: tau=float(np.mean([f.tau for f in fits])), L=float(np.mean([f.L for f in fits])), ) - fitted = Go2PlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]) + fitted = TwistBasePlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]) print("\nself-test (recovered vs injected FOPDT ground truth):") print(f" {'chan':4} {'K fit/true':>20} {'tau fit/true':>20} {'L fit/true':>20}") for ch in _CHANNELS: - f, g = getattr(fitted, ch), getattr(GO2_PLANT_FITTED, ch) + f, g = getattr(fitted, ch), getattr(truth, ch) print( f" {ch:4} {f.K:8.3f}/{g.K:<8.3f} {f.tau:8.3f}/{g.tau:<8.3f} {f.L:8.3f}/{g.L:<8.3f}" ) @@ -167,7 +152,9 @@ def _fit_selftest() -> tuple[Go2PlantParams, dict, list[dict]]: # --- fit-quality graph (the human-facing deliverable) ------------------- -def _plot_fits(traces: list[dict], provenance: Provenance, out: Path) -> None: +def _plot_fits( + traces: list[dict], provenance: Provenance, profile: RobotProfile, out: Path +) -> None: """One column per channel; each step's measured velocity overlaid with its fitted FOPDT step response. This is the artifact a human reads to judge whether the model matches the real robot.""" @@ -200,7 +187,7 @@ def _plot_fits(traces: list[dict], provenance: Provenance, out: Path) -> None: ax.legend(loc="lower right", fontsize=7) p = provenance fig.suptitle( - f"Go2 FOPDT characterization — {p.robot_id} / {p.surface} / " + f"{profile.name} FOPDT characterization — {p.robot_id} / {p.surface} / " f"{p.mode} / {p.sim_or_hw} — {p.date} ({p.git_sha})" ) fig.tight_layout() @@ -208,12 +195,12 @@ def _plot_fits(traces: list[dict], provenance: Provenance, out: Path) -> None: plt.close(fig) -# --- hardware SI (real Go2 over LCM, operator-gated, safe) --------------- +# --- hardware SI (real robot over LCM, operator-gated, safe) ------------- class _PoseVelocityEstimator: """Differentiate consecutive ``PoseStamped`` to body-frame (vx,vy,wz); - EMA-smoothed (Go2 publishes pose only). Ported from the R&D hw loop.""" + EMA-smoothed (pose-only odom). Ported from the R&D hw loop.""" def __init__(self, alpha: float = 0.5) -> None: self._pp = None @@ -244,34 +231,39 @@ def update(self, pose, t: float) -> tuple[float, float, float]: return self._vx, self._vy, self._wz -def _prereq_banner() -> None: +def _prereq_banner(profile: RobotProfile) -> None: print( - "\n=== HARDWARE MODE ===\n" - "Prereqs (run on dimensional-gpu-0):\n" - " 1. Another terminal: `dimos run unitree-go2-webrtc-keyboard-teleop`\n" - " (publishes /go2/odom, consumes /cmd_vel; its teleop is\n" - " publish-only-when-active so it stays silent while idle and\n" - " does NOT fight the SI commands).\n" - " 2. This process: strip /nix/store from LD_LIBRARY_PATH (see README)\n" - "Robot is STOPPED before every step. Reposition it with the keyboard\n" - "teleop (WASD/QE), then RELEASE all keys (teleop goes silent) and\n" - "press ENTER here — the tool then owns /cmd_vel for the step.\n" - "Each step ends at --max-dist travelled (default 6 m) or --step-s,\n" - "whichever first. Velocity clamped; zero-Twist on exit / Ctrl-C.\n" + f"\n=== HARDWARE MODE ({profile.name}) ===\n" + "Prereqs:\n" + f" 1. Another terminal: `dimos run {profile.blueprint}`\n" + f" (publishes {profile.odom_topic}, consumes " + f"{profile.cmd_topic}; if it includes a keyboard teleop it must\n" + " be publish-only-when-active so it does not fight the SI\n" + " commands — wrong topic => runtime 'no odom', not an error).\n" + " 2. This process: strip /nix/store from LD_LIBRARY_PATH (README)\n" + "Robot is STOPPED before every step. Reposition it, then press\n" + "ENTER here — the tool then owns the cmd topic for the step.\n" + "Each step ends at --max-dist travelled or --step-s, whichever\n" + "first. Velocity clamped; zero-Twist on exit / Ctrl-C.\n" ) def _fit_hw( - step_s: float, pre_roll_s: float, warmup_s: float, max_dist: float -) -> tuple[Go2PlantParams, dict, list[dict]]: + profile: RobotProfile, + step_s: float, + pre_roll_s: float, + warmup_s: float, + max_dist: float, +) -> tuple[TwistBasePlantParams, dict, list[dict]]: 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.geometry_msgs.Vector3 import Vector3 - _prereq_banner() - cmd_pub = LCMTransport("/cmd_vel", Twist) - odom_sub = LCMTransport("/go2/odom", PoseStamped) + _prereq_banner(profile) + hw_dt = 1.0 / profile.tick_rate_hz + cmd_pub = LCMTransport(profile.cmd_topic, Twist) + odom_sub = LCMTransport(profile.odom_topic, PoseStamped) lock = threading.Lock() box: dict = {"pose": None, "t": 0.0} @@ -286,8 +278,8 @@ def publish(vx: float, vy: float, wz: float) -> None: cmd_pub.broadcast( None, Twist( - linear=Vector3(_clamp(vx, -VX_MAX, VX_MAX), 0.0, 0.0), - angular=Vector3(0.0, 0.0, _clamp(wz, -WZ_MAX, WZ_MAX)), + linear=Vector3(_clamp(vx, -profile.vx_max, profile.vx_max), 0.0, 0.0), + angular=Vector3(0.0, 0.0, _clamp(wz, -profile.wz_max, profile.wz_max)), ), ) @@ -300,8 +292,7 @@ def safe_stop() -> None: # also breaks out of the blocking input() prompt. The try/finally # below guarantees a zero-Twist stop on any exit (Ctrl-C, q, error). - # odom warmup - print(f"[hw] waiting up to {warmup_s:.0f}s for /go2/odom ...") + print(f"[hw] waiting up to {warmup_s:.0f}s for {profile.odom_topic} ...") deadline = time.perf_counter() + warmup_s while time.perf_counter() < deadline: with lock: @@ -311,16 +302,16 @@ def safe_stop() -> None: with lock: if box["pose"] is None: safe_stop() - raise SystemExit("No /go2/odom — is `dimos run unitree-go2-webrtc-keyboard-teleop` up?") + raise SystemExit(f"No {profile.odom_topic} — is `dimos run {profile.blueprint}` up?") pooled: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} traces: list[dict] = [] try: - for channel in _HW_CHANNELS: + for channel in profile.excited_channels: fits = [] per_amplitude[channel] = [] - for amp in _SI_AMPLITUDES[channel]: + for amp in profile.si_amplitudes[channel]: safe_stop() resp = ( input( @@ -344,7 +335,7 @@ def safe_stop() -> None: with lock: p = box["pose"] est.update(p, time.perf_counter()) - time.sleep(_HW_DT) + time.sleep(hw_dt) # step. Ends on whichever comes first: travelled distance # >= max_dist (the real-space bound — at high speed the @@ -368,7 +359,7 @@ def safe_stop() -> None: publish(cmd["vx"], cmd["vy"], cmd["wz"]) with lock: p, pt = box["pose"], box["t"] - if p is None or now - pt > _ODOM_STALE_S: + if p is None or now - pt > profile.odom_stale_s: print(f" [abort] stale odom ({now - pt:.2f}s)") end_reason = "stale" break @@ -379,7 +370,7 @@ def safe_stop() -> None: v = est.update(p, now) ts.append(t_rel) ys.append(v[_CHANNELS.index(channel)]) - time.sleep(_HW_DT) + time.sleep(hw_dt) safe_stop() if len(ys) < 5: @@ -424,90 +415,94 @@ def safe_stop() -> None: finally: safe_stop() - # vy is NOT excited on hardware: the real Go2 (default gait) does not - # strafe on /cmd_vel.linear.y, so a vy step yields a degenerate K≈0 - # fit that would corrupt the model. Placeholder vy = vx (sane FF / - # profile); flagged in the artifact caveats. - pooled["vy"] = pooled["vx"] - per_amplitude["vy"] = [] - print(" [note] vy not excited on hw (no lateral motion) — placeholder vy = vx") + # Channels not excited (e.g. vy on a non-strafing robot) are + # placeholdered = vx so FF / profile stay sane; flagged in caveats. + for ch in _CHANNELS: + if ch not in pooled: + pooled[ch] = pooled["vx"] + per_amplitude[ch] = [] + print(f" [note] {ch} not excited on hw — placeholder {ch} = vx") return ( - Go2PlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]), + TwistBasePlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]), per_amplitude, traces, ) def main() -> None: - ap = argparse.ArgumentParser(description="Go2 characterization -> tuning config artifact") + ap = argparse.ArgumentParser(description="Twist-base characterization -> tuning artifact") + ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PROFILES)}") ap.add_argument("--mode", choices=["hw", "self-test"], default="hw") ap.add_argument("--out", default=str(REPORTS_DIR)) - ap.add_argument("--robot-id", default="go2") + ap.add_argument("--robot-id", default=None, help="provenance id (default: profile.robot_id)") ap.add_argument("--surface", default="concrete") ap.add_argument("--gait-mode", default="default") ap.add_argument( "--step-s", type=float, - default=_STEP_S, - help="per-step excitation duration (s); the robot must reach " - "and hold the commanded speed within this window", + default=None, + help="per-step excitation duration (s); default from profile", ) ap.add_argument( - "--pre-roll-s", - type=float, - default=_PRE_ROLL_S, - help="zero-command settle before each step (s)", + "--pre-roll-s", type=float, default=None, help="zero-command settle before each step (s)" ) ap.add_argument( - "--odom-warmup", - type=float, - default=_ODOM_WARMUP_S, - help="how long to wait for first /go2/odom (s)", + "--odom-warmup", type=float, default=None, help="how long to wait for first odom (s)" ) ap.add_argument( "--max-dist", type=float, - default=_MAX_DIST_M, - help="per-step travel cap (m); ends the step early at high speed " - "so the robot stays in the test area", + default=None, + help="per-step travel cap (m); ends the step early at speed", ) args = ap.parse_args() + profile = _resolve_profile(args.robot) + step_s = args.step_s if args.step_s is not None else profile.step_s + pre_roll_s = args.pre_roll_s if args.pre_roll_s is not None else profile.pre_roll_s + warmup_s = args.odom_warmup if args.odom_warmup is not None else profile.odom_warmup_s + max_dist = args.max_dist if args.max_dist is not None else profile.max_dist_m + robot_id = args.robot_id if args.robot_id is not None else profile.robot_id + if args.mode == "hw": - fitted, per_amplitude, traces = _fit_hw( - args.step_s, args.pre_roll_s, args.odom_warmup, args.max_dist - ) + fitted, per_amplitude, traces = _fit_hw(profile, step_s, pre_roll_s, warmup_s, max_dist) else: - fitted, per_amplitude, traces = _fit_selftest() + fitted, per_amplitude, traces = _fit_selftest(profile) provenance = Provenance( - robot_id=args.robot_id, + robot_id=robot_id, surface=args.surface, mode=args.gait_mode, date=date.today().isoformat(), git_sha=git_sha(), sim_or_hw="hw" if args.mode == "hw" else "self-test", characterization_session_dir=( - "(real Go2, LCM SI)" if args.mode == "hw" else "(in-process self-test)" + f"(real {profile.name}, LCM SI)" if args.mode == "hw" else "(in-process self-test)" ), ) - cfg = derive_config(fitted, provenance, per_amplitude=per_amplitude) - if args.mode == "hw": + cfg = derive_config( + fitted, + provenance, + per_amplitude=per_amplitude, + vx_max=profile.vx_max, + wz_max=profile.wz_max, + ) + if args.mode == "hw" and "vy" not in profile.excited_channels: cfg.caveats.append( - "vy was NOT characterized on hardware (the Go2 does not strafe " - "on /cmd_vel.linear.y in this gait); plant.vy / feedforward.K_vy " - "are a placeholder copy of vx. The benchmark paths are vx+wz " - "only, so this does not affect tuning; re-characterize vy if a " + f"vy was NOT characterized on hardware ({profile.name} does not " + "strafe in this gait); plant.vy / feedforward.K_vy are a " + "placeholder copy of vx. The benchmark paths are vx+wz only, so " + "this does not affect tuning; re-characterize vy if a " "lateral-capable gait is used." ) out_path = ( Path(args.out).expanduser() - / f"go2_config_{args.mode}_{args.surface}_{provenance.date}_{provenance.git_sha}.json" + / f"{robot_id}_config_{args.mode}_{args.surface}_{provenance.date}_{provenance.git_sha}.json" ) cfg.to_json(out_path) plot_path = out_path.with_suffix(".png") - _plot_fits(traces, provenance, plot_path) + _plot_fits(traces, provenance, profile, plot_path) tag = "ROBOT-VALID" if cfg.valid_for_tuning else "NOT robot-valid (plumbing check)" print("\nFOPDT fit graph (the deliverable — model vs real data):") diff --git a/dimos/utils/benchmarking/plant.py b/dimos/utils/benchmarking/plant.py index d68d2e63b8..70a7e90d3c 100644 --- a/dimos/utils/benchmarking/plant.py +++ b/dimos/utils/benchmarking/plant.py @@ -12,13 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Layer 1 sim plant for the Go2 base. +"""Sim plant + per-robot profile for the twist-base tuning tools. -Per-channel FOPDT velocity tracking + unicycle kinematics. Tick-based: -each call to :meth:`Go2PlantSim.step` advances one control period. +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 vendored fitted parameters (``GO2_PLANT_FITTED``) live at the bottom -of this module — types, simulator, and the measured values in one place. +The bottom of this module holds the per-robot config (``RobotProfile`` + +``ROBOT_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 @@ -70,15 +73,15 @@ def step(self, u: float, dt: float) -> float: @dataclass -class Go2PlantParams: - """FOPDT params for all three velocity channels.""" +class TwistBasePlantParams: + """FOPDT params for the three twist-base velocity channels.""" vx: FopdtChannelParams vy: FopdtChannelParams wz: FopdtChannelParams -class Go2PlantSim: +class TwistBasePlantSim: """Unicycle kinematic sim with FOPDT velocity response per channel. Body-frame velocities `(vx, vy, wz)` are commanded; the plant produces @@ -86,7 +89,7 @@ class Go2PlantSim: in the world frame. """ - def __init__(self, params: Go2PlantParams) -> None: + def __init__(self, params: TwistBasePlantParams) -> None: self.params = params self.ch_vx = FOPDTChannel(params.vx) self.ch_vy = FOPDTChannel(params.vy) @@ -125,17 +128,87 @@ def step(self, cmd_vx: float, cmd_vy: float, cmd_wz: float, dt: float) -> None: # 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 ground truth the sim self-test injects and recovers, and -# the documented rationale for the derived feedforward gains. vy on the -# real robot strafes and should be characterized for real (--mode hw); -# the sim ground truth copies vx into vy only because the sim FOPDT has -# no independent lateral model. +# 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 — +# the Go2 does not strafe in the default gait (so vy is not excited on +# hardware) and the sim FOPDT has no independent lateral model. 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 = Go2PlantParams( +GO2_PLANT_FITTED = TwistBasePlantParams( vx=GO2_VX_RISE, - vy=GO2_VX_RISE, # sim ground-truth placeholder; real vy via --mode hw + vy=GO2_VX_RISE, # placeholder; Go2 doesn't strafe in default gait wz=GO2_WZ_RISE, ) + + +# --- Per-robot profile (single source of truth for robot specifics) ----- + + +@dataclass(frozen=True) +class RobotProfile: + """Everything the characterization + benchmark tools need to know + about a specific velocity-commanded twist base. Add a robot by + appending one instance to ``ROBOT_PROFILES``.""" + + # identity / cosmetic + name: str + robot_id: str + # transport / bring-up + cmd_topic: str + odom_topic: str + blueprint: str # the `dimos run ` the operator starts + sim_adapter_key: str + # physical envelope + vx_max: float + wz_max: float + tick_rate_hz: float + odom_warmup_s: float + odom_stale_s: float + # SI plan / kinematics + excited_channels: tuple[str, ...] # subset of (vx,vy,wz); omit vy => non-strafing + si_amplitudes: dict[str, list[float]] + step_s: float + pre_roll_s: float + max_dist_m: float + # sim ground truth: self-test + sim adapter + DERIVE ceiling fallback + sim_plant: TwistBasePlantParams + + +GO2_PROFILE = RobotProfile( + name="Go2", + robot_id="go2", + cmd_topic="/cmd_vel", + odom_topic="/go2/odom", + blueprint="unitree-go2-webrtc-keyboard-teleop", + sim_adapter_key="fopdt_sim_twist_base", + 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", "wz"), # Go2 does not strafe in default gait + si_amplitudes={"vx": [0.3, 0.6, 0.9], "vy": [0.2, 0.4], "wz": [0.4, 0.8, 1.2]}, + step_s=8.0, + pre_roll_s=1.0, + max_dist_m=6.0, + sim_plant=GO2_PLANT_FITTED, +) + +ROBOT_PROFILES: dict[str, RobotProfile] = {"go2": GO2_PROFILE} + + +__all__ = [ + "GO2_PLANT_FITTED", + "GO2_PROFILE", + "GO2_VX_RISE", + "GO2_WZ_RISE", + "ROBOT_PROFILES", + "FOPDTChannel", + "FopdtChannelParams", + "RobotProfile", + "TwistBasePlantParams", + "TwistBasePlantSim", +] diff --git a/dimos/utils/benchmarking/reports/go2_tuning_README.md b/dimos/utils/benchmarking/reports/go2_tuning_README.md deleted file mode 100644 index 6e738e03e5..0000000000 --- a/dimos/utils/benchmarking/reports/go2_tuning_README.md +++ /dev/null @@ -1,165 +0,0 @@ -# Go2 controller tuning — measure → derive → validate (HARDWARE) - -Two CLI tools that turn one real measurement of your Go2 into a single -versioned config artifact with every parameter you need to tune the base -controller, then validate it on the real robot. - -``` -go2_characterization --mode hw ──▶ go2_config_hw_*.json (robot-valid) -go2_benchmark --mode hw --config … ──▶ same file + section 5 - "for tolerance X cm, run Y m/s" -``` - -**This is a hardware deliverable.** Sim exists only as a plumbing -self-test / pre-check and is explicitly stamped not-robot-valid — never -tune from it. - -## Why these numbers (settled findings, not re-derived) - -Go2 base = FOPDT per axis. At a given speed the tracking error is the -plant floor `(τ+L)·v`; no reactive control law beats it. So the -recommended controller is hardcoded to the production baseline -P-controller, and the only real levers — feedforward gain (`1/K`) and a -curvature velocity profile — are *derived from the measured plant*, not -hand-tuned. - -## Prerequisites (real robot) - -1. Run on **`dimensional-gpu-0`** (the only host that reaches the Go2). -2. Terminal 1: `dimos run unitree-go2-webrtc-keyboard-teleop` - — brings up the Go2 connection (publishes `/go2/odom`, consumes - `/cmd_vel`) **and** the keyboard teleop for repositioning. This - blueprint runs the teleop **publish-only-when-active**: it stays - silent while no movement key is held (one zero Twist on release, - then nothing), so it does not flood `/cmd_vel` and coexists with the - tool. (Plain `KeyboardTeleop` defaults to streaming every loop — - only this blueprint enables the silent mode.) -3. Terminal 2: strip nix from the linker path or `.venv` numpy breaks - (`GLIBC_2.38`), then run the tool: - ``` - export LD_LIBRARY_PATH="$(echo "$LD_LIBRARY_PATH" | tr ':' '\n' \ - | grep -v /nix/store | paste -sd:)" - ``` -4. Repositioning: the robot is **stopped** at every prompt. Reposition - with the keyboard teleop (WASD move/turn, QE strafe), then **release - all keys** — the teleop goes silent — and press ENTER. The tool then - owns `/cmd_vel` for that run. Do not hold keys while a run is going. -5. Timings are operator-tunable when the robot needs more time to reach - the commanded speed or the connection is slow to come up: - `--step-s` (default 8 s, time safety cap), `--max-dist` (default 6 m, - the real-space bound — each step ends at whichever of distance/time - comes first; `wz` spins in place so it ends on time), `--pre-roll-s` - (1 s), `--odom-warmup` (default 10 s). - -## Tool 1 — `go2_characterization` - -``` -uv run python -m dimos.utils.benchmarking.go2_characterization \ - --mode hw --surface concrete --gait-mode default -``` - -Per channel (vx, vy, wz — vy is real, the Go2 strafes) × a few -amplitudes: - -1. Robot is **stopped**; prompt: `reposition robot … ENTER=run s=skip - q=quit`. Reposition with the keyboard teleop, release keys, ENTER. -2. Pre-roll zeros (settle), then a velocity step for `--step-s` (default - 8 s) at 10 Hz — long enough for the real Go2 to ramp to and hold the - commanded speed — recording commanded vs body-frame velocity - differentiated from `/go2/odom`. -3. `safe_stop`, fit FOPDT. - -Drift is bounded to one step (operator gate before each). Safety -throughout: velocity clamp (`VX_MAX=1.0`, `WZ_MAX=1.5`), stale-odom -abort, timeout, zero-Twist on exit and on Ctrl-C. - -**Primary output is a graph** — `go2_config_<…>.png`, one column per -channel (vx, wz) overlaying every step's *measured* velocity (solid) -with its *fitted FOPDT* step response (dashed), annotated with -K/τ/L/r² per amplitude. This is what you read to judge whether the -model matches the real robot. The `.json` is written alongside only as -the machine handoff the benchmark consumes (sections 1–4 + 6; section 5 -pending; `valid_for_tuning=true`). vx has 3 amplitudes, wz 3; vy is the -vx placeholder (not strafe-capable in this gait). - -`--mode self-test` (no robot): steps an in-process FOPDT plant seeded -with the vendored ground truth and recovers it. Proves the -measure→fit→derive code runs; artifact stamped -`valid_for_tuning=false`. This is the pytest/CI path — **not a tuning -artifact**. - -## Tool 2 — `go2_benchmark` - -``` -uv run python -m dimos.utils.benchmarking.go2_benchmark \ - --config reports/go2_config_hw_concrete__.json \ - --mode hw --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 -``` - -**By default it runs the BARE stock baseline P-controller — no -feedforward, no velocity profile.** That is the point: this run measures -the **plant's physical tracking limit** with the existing production -controller, the number you compare everything against and check against -the `(τ+L)·v` floor from characterization. Path set is fixed -(`straight_line`, `single_corner` 2 m/90°, `square` 2 m, `circle` R1.0). -For each (path, speed): operator gate (reposition+aim, ENTER), the path -is **anchored to the robot's current pose** (so it need not be placed -precisely), then tracked closed-loop at 10 Hz off real odom; CTE scored -from the real trajectory. The **bare** run writes section 5 -(operating-point map + tolerance→max-safe-speed inversion) back into the -artifact — that is the canonical physical-limit map. Same safety as Tool 1. - -Optional comparison arms (off by default), each measured *against* the -bare physical limit, written to standalone `__` files that never -clobber section 5: - -- `--ff` — apply the artifact's derived feedforward. -- `--profile` — apply the artifact's derived curvature velocity profile. -- `--ff --profile` — both (the fully-derived config). - -`--mode hw` only **refuses a non-robot-valid config when `--ff`/`--profile` -is set** (sim-derived gains are meaningless on the real robot). The bare -physical-limit run accepts any config (it doesn't use the derived -params). - -`--mode sim`: optional fast pre-check against the FOPDT sim plant. Loudly -labelled a pre-check; the map is not a real-robot result. Useful to -sanity-check wiring before committing the robot. - -## Reading the artifact - -| Section | Field | Meaning | -|---|---|---| -| 1 | `provenance` | robot/surface/mode/date/sha, `sim_or_hw` | -| 1 | `valid_for_tuning` | **false ⇒ do not tune from this** (self-test) | -| 1 | `plant` | fitted FOPDT `{K,τ,L}` per axis | -| 2 | `feedforward` | `1/K` per axis + clamps | -| 3 | `velocity_profile` | curvature speed profile | -| 4 | `recommended_controller` | baseline + plant-floor evidence | -| 5 | `operating_point_map` | per (path,speed) CTE + tolerance→speed (null until Tool 2) | -| 6 | `caveats` | validity scope; self-test artifacts lead with a loud DO-NOT-TUNE banner | - -## When to re-run - -Re-run Tool 1 (then Tool 2) on any plant change: different surface -(friction → K/τ), gait mode (e.g. `rage`), firmware/locomotion change. -The `caveats` state exactly what the artifact is valid for. - -## Tests - -``` -uv run pytest dimos/utils/benchmarking/test_go2_tuning.py -q -``` - -Pure DERIVE (1/K incl. real vy, wz-ceiling margin + envelope clamp, -accel formulas, hardcoded baseline + evidence), `valid_for_tuning` -(true only for hw; self-test false + leading DO-NOT-TUNE caveat; -survives round-trip), artifact round-trip + schema rejection, and the -tolerance→max-speed inversion. HW loops require a robot — covered by the -manual prerequisites above, not pytest. - -## Not here (by design) - -The MPC/RPP/Lyapunov bake-off, command smoothers, sweeps, and plotting -R&D were the evidence for "baseline + FF + curvature profile"; they are -the appendix, archived off-repo, not the product. diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md new file mode 100644 index 0000000000..7b107ddaef --- /dev/null +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -0,0 +1,171 @@ +# Twist-base controller tuning — measure → derive → validate (HARDWARE) + +Two CLI tools that turn one real measurement of a velocity-commanded +mobile base into a single versioned config artifact with every parameter +needed to tune its path controller, then validate it on the real robot. +**Robot-agnostic**: everything robot-specific lives in a `RobotProfile` +(`--robot`, default `go2`). Adding a robot = one profile entry (see +*Adding a robot* below); the two commands are otherwise identical. + +``` +characterization --robot R --mode hw ──▶ R_config_hw_*.json (robot-valid) +benchmark --robot R --mode hw --config … ──▶ same file + section 5 + "for tolerance X cm, run Y m/s" +``` + +**This is a hardware deliverable.** Sim exists only as a plumbing +self-test / pre-check and is explicitly stamped not-robot-valid — never +tune from it. + +## Why these numbers (settled findings, not re-derived) + +A velocity-commanded base is FOPDT per axis. At a given speed the +tracking error is the plant floor `(τ+L)·v`; no reactive control law +beats it. So the recommended controller is hardcoded to the production +baseline P-controller, and the only real levers — feedforward gain +(`1/K`) and a curvature velocity profile — are *derived from the measured +plant*, not hand-tuned. (The embedded evidence string cites the Go2 +result; a different robot's headroom is TBD until characterized.) + +## Prerequisites (real robot) + +1. The host that reaches the robot (for the Go2 profile: + **`dimensional-gpu-0`**). +2. Terminal 1: `dimos run ` — for `--robot go2` that + is `unitree-go2-webrtc-keyboard-teleop`, which brings up the Go2 + connection (publishes the odom topic, consumes the cmd topic) **and** + a keyboard teleop for repositioning, run **publish-only-when-active** + (silent while idle, so it does not flood the cmd topic / fight the + tool). A different robot needs an equivalent bring-up blueprint that + speaks Twist on the profile's cmd topic + `PoseStamped` odom. +3. Terminal 2: strip nix from the linker path or `.venv` numpy breaks + (`GLIBC_2.38`): + ``` + export LD_LIBRARY_PATH="$(echo "$LD_LIBRARY_PATH" | tr ':' '\n' \ + | grep -v /nix/store | paste -sd:)" + ``` +4. Repositioning: the robot is **stopped** at every prompt. Reposition + (Go2: keyboard teleop WASD/QE, then **release all keys** so it goes + silent), then press ENTER. The tool then owns the cmd topic for that + run. Do not hold teleop keys while a run is going. +5. Operator-tunable timings (defaults come from the profile): + `--step-s` (time safety cap), `--max-dist` (real-space bound — each + step ends at whichever of distance/time comes first; `wz` spins in + place so it ends on time), `--pre-roll-s`, `--odom-warmup`. + +## Tool 1 — `characterization` + +``` +uv run python -m dimos.utils.benchmarking.characterization \ + --robot go2 --mode hw --surface concrete --gait-mode default +``` + +Per excited channel (`profile.excited_channels`; Go2 = vx, wz — it does +not strafe in the default gait) × a few amplitudes: + +1. Robot **stopped**; prompt `ENTER=run s=skip q=quit`. Reposition, ENTER. +2. Pre-roll zeros (settle), then a velocity step (`--step-s`) at the + profile tick rate, recording commanded vs body-frame velocity + differentiated from the odom topic. Ends at `--max-dist` or `--step-s`. +3. `safe_stop`, fit FOPDT. + +Drift is bounded to one step (operator gate before each). Safety: clamp +to the profile envelope, stale-odom abort, distance + time caps, +zero-Twist on exit / Ctrl-C / `q`. + +**Primary output is a graph** — `_config_<…>.png`, one column +per channel overlaying every step's *measured* velocity (solid) with its +*fitted FOPDT* step response (dashed), annotated K/τ/L/r² per amplitude +— this is what you read to judge whether the model matches the real +robot. The `.json` alongside is the machine handoff the benchmark +consumes (sections 1–4 + 6; section 5 pending; `valid_for_tuning=true`). +Channels not excited (e.g. vy on a non-strafing robot) are placeholdered += vx and flagged in the caveats. + +`--mode self-test` (no robot): steps the profile's in-process FOPDT sim +plant and recovers it. Proves the measure→fit→derive code runs; artifact +stamped `valid_for_tuning=false`. The pytest/CI path — **not a tuning +artifact**. + +## Tool 2 — `benchmark` + +``` +uv run python -m dimos.utils.benchmarking.benchmark \ + --robot go2 --config reports/go2_config_hw_concrete__.json \ + --mode hw --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 +``` + +**By default it runs the BARE stock baseline P-controller — no +feedforward, no velocity profile.** That is the point: this measures the +**plant's physical tracking limit** with the existing production +controller, the number you compare everything against and check against +the `(τ+L)·v` floor. Path set is fixed (`straight_line`, `single_corner` +2 m/90°, `square` 2 m, `circle` R1.0). For each (path, speed): operator +gate, the path is **anchored to the robot's current pose**, then tracked +closed-loop at the profile tick rate off real odom; CTE scored from the +real trajectory. The **bare** run writes section 5 (operating-point map ++ tolerance→max-safe-speed inversion) back into the artifact — the +canonical physical-limit map. Same safety as Tool 1. + +Optional comparison arms (off by default), each measured *against* the +bare physical limit, written to standalone `__` files that never +clobber section 5: + +- `--ff` — apply the artifact's derived feedforward. +- `--profile` — apply the artifact's derived curvature velocity profile. +- `--ff --profile` — both (the fully-derived config). + +`--mode hw` only **refuses a non-robot-valid config when `--ff`/`--profile` +is set** (sim-derived gains are meaningless on the real robot). The bare +physical-limit run accepts any config. + +`--mode sim`: optional fast pre-check against the profile's FOPDT sim +plant. Loudly labelled a pre-check; the map is not a real-robot result. + +## Reading the artifact + +| Section | Field | Meaning | +|---|---|---| +| 1 | `provenance` | robot/surface/mode/date/sha, `sim_or_hw` | +| 1 | `valid_for_tuning` | **false ⇒ do not tune from this** (self-test) | +| 1 | `plant` | fitted FOPDT `{K,τ,L}` per axis | +| 2 | `feedforward` | `1/K` per axis + clamps | +| 3 | `velocity_profile` | curvature speed profile | +| 4 | `recommended_controller` | baseline + plant-floor evidence | +| 5 | `operating_point_map` | per (path,speed) CTE + tolerance→speed (null until Tool 2 bare run) | +| 6 | `caveats` | validity scope; self-test artifacts lead with a loud DO-NOT-TUNE banner | + +## Adding a robot + +Append one `RobotProfile` to `ROBOT_PROFILES` in +`dimos/utils/benchmarking/plant.py`: its `cmd_topic` / `odom_topic` / +`blueprint`, `sim_adapter_key` (`fopdt_sim_twist_base`), saturation +envelope (`vx_max`, `wz_max`), `tick_rate_hz`, `excited_channels` +(omit `vy` if it doesn't strafe), `si_amplitudes`, and a `sim_plant` +(`TwistBasePlantParams`) used as the self-test ground truth. Then the +identical two commands with `--robot `. No other code changes. + +## When to re-run + +Re-run Tool 1 (then Tool 2) on any plant change: different surface +(friction → K/τ), gait mode, firmware/locomotion change. The `caveats` +state exactly what the artifact is valid for. + +## Tests + +``` +uv run pytest dimos/utils/benchmarking/test_tuning.py -q +``` + +Pure DERIVE (1/K per axis, wz-ceiling margin + envelope clamp, accel +formulas, hardcoded baseline + evidence), `valid_for_tuning` (true only +for hw; self-test false + leading DO-NOT-TUNE caveat; survives +round-trip), artifact round-trip + schema rejection, tolerance→max-speed +inversion. HW loops require a robot — covered by the manual prerequisites +above, not pytest. + +## Not here (by design) + +The MPC/RPP/Lyapunov bake-off, command smoothers, sweeps, and plotting +R&D were the evidence for "baseline + FF + curvature profile"; they are +the appendix, archived off-repo, not the product. diff --git a/dimos/utils/benchmarking/test_go2_tuning.py b/dimos/utils/benchmarking/test_tuning.py similarity index 93% rename from dimos/utils/benchmarking/test_go2_tuning.py rename to dimos/utils/benchmarking/test_tuning.py index 48d6d8916d..7d05e18ff5 100644 --- a/dimos/utils/benchmarking/test_go2_tuning.py +++ b/dimos/utils/benchmarking/test_tuning.py @@ -21,20 +21,20 @@ import pytest -from dimos.utils.benchmarking.go2_tuning import ( +from dimos.utils.benchmarking.plant import FopdtChannelParams, TwistBasePlantParams +from dimos.utils.benchmarking.tuning import ( SCHEMA_VERSION, - Go2TuningConfig, OperatingPoint, Provenance, + TuningConfig, derive_config, invert_tolerance, ) -from dimos.utils.benchmarking.plant import FopdtChannelParams, Go2PlantParams from dimos.utils.benchmarking.velocity_profile import GO2_VX_MAX, GO2_WZ_MAX -def _plant(kvx=0.9, kvy=0.5, kwz=2.4) -> Go2PlantParams: - return Go2PlantParams( +def _plant(kvx=0.9, kvy=0.5, kwz=2.4) -> TwistBasePlantParams: + return TwistBasePlantParams( vx=FopdtChannelParams(K=kvx, tau=0.40, L=0.06), vy=FopdtChannelParams(K=kvy, tau=0.30, L=0.05), wz=FopdtChannelParams(K=kwz, tau=0.60, L=0.05), @@ -127,10 +127,10 @@ def test_valid_for_tuning_only_when_hw(): def test_valid_for_tuning_survives_round_trip(tmp_path): st = derive_config(_plant(), _prov(sim_or_hw="self-test")) - back = Go2TuningConfig.from_json(st.to_json(tmp_path / "st.json")) + back = TuningConfig.from_json(st.to_json(tmp_path / "st.json")) assert back.valid_for_tuning is False hw = derive_config(_plant(), _prov(sim_or_hw="hw")) - back_hw = Go2TuningConfig.from_json(hw.to_json(tmp_path / "hw.json")) + back_hw = TuningConfig.from_json(hw.to_json(tmp_path / "hw.json")) assert back_hw.valid_for_tuning is True @@ -140,7 +140,7 @@ def test_valid_for_tuning_survives_round_trip(tmp_path): def test_json_round_trip_identity(tmp_path): cfg = derive_config(_plant(), _prov()) p = cfg.to_json(tmp_path / "c.json") - back = Go2TuningConfig.from_json(p) + back = TuningConfig.from_json(p) assert back.feedforward == cfg.feedforward assert back.velocity_profile == cfg.velocity_profile assert back.plant == cfg.plant @@ -154,7 +154,7 @@ def test_loader_rejects_wrong_schema_version(tmp_path): bad = p.read_text().replace(f'"schema_version": {SCHEMA_VERSION}', '"schema_version": 999') (tmp_path / "bad.json").write_text(bad) with pytest.raises(ValueError, match="schema_version"): - Go2TuningConfig.from_json(tmp_path / "bad.json") + TuningConfig.from_json(tmp_path / "bad.json") # --- tolerance -> max-safe-speed inversion -------------------------------- diff --git a/dimos/utils/benchmarking/go2_tuning.py b/dimos/utils/benchmarking/tuning.py similarity index 89% rename from dimos/utils/benchmarking/go2_tuning.py rename to dimos/utils/benchmarking/tuning.py index 6145b77a8c..fb07abe5e4 100644 --- a/dimos/utils/benchmarking/go2_tuning.py +++ b/dimos/utils/benchmarking/tuning.py @@ -12,23 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Go2 tuning config artifact + the DERIVE step (model -> config). +"""Twist-base tuning config artifact + the DERIVE step (model -> config). -This is the contract the two tuning tools share: +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_go2_tuning.py``). -* :class:`Go2TuningConfig` is the versioned artifact. It owns the JSON + 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/go2_tuning_README.md``): the Go2 base is FOPDT per -axis; at a given speed the tracking error is the plant floor +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. @@ -42,7 +41,7 @@ import subprocess from dimos.control.tasks.feedforward_gain_compensator import FeedforwardGainConfig -from dimos.utils.benchmarking.plant import Go2PlantParams +from dimos.utils.benchmarking.plant import TwistBasePlantParams from dimos.utils.benchmarking.velocity_profile import ( GO2_VX_MAX, GO2_WZ_MAX, @@ -78,8 +77,9 @@ "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/go2_tuning_README.md and the " - "characterization findings." + "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)." ) @@ -198,7 +198,7 @@ class OperatingPointMap: @dataclass -class Go2TuningConfig: +class TuningConfig: provenance: Provenance plant: PlantModelDC feedforward: FeedforwardDC @@ -220,12 +220,12 @@ def to_json(self, path: str | Path) -> Path: return path @classmethod - def from_json(cls, path: str | Path) -> Go2TuningConfig: + 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) -> Go2TuningConfig: + def from_dict(cls, data: dict) -> TuningConfig: sv = data.get("schema_version") if sv != SCHEMA_VERSION: raise ValueError( @@ -290,8 +290,8 @@ def _safe_inv_gain(K: float) -> float: 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 Go2 saturation envelope when per-amplitude data is missing or - too sparse to be trustworthy.""" + 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 [] @@ -314,14 +314,16 @@ def _channel_ceiling(per_amplitude: dict | None, channel: str, fallback: float) def derive_config( - plant: Go2PlantParams, + plant: TwistBasePlantParams, provenance: Provenance, *, per_amplitude: dict | None = None, -) -> Go2TuningConfig: + vx_max: float = GO2_VX_MAX, + wz_max: float = GO2_WZ_MAX, +) -> TuningConfig: """Derive the full controller config from a fitted FOPDT plant model. - Pure: model + provenance in, :class:`Go2TuningConfig` out. No I/O. + 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). @@ -334,14 +336,14 @@ def derive_config( - recommended controller = baseline, hardcoded, with cited evidence. ``per_amplitude`` (optional) is the fitter's per-amplitude table - ``{channel: [{amplitude, K, ...}, ...]}``; when absent the Go2 - saturation envelope is used for the ceilings. + ``{channel: [{amplitude, K, ...}, ...]}``; when absent the robot's + saturation envelope (``vx_max``/``wz_max``) is used for the ceilings. """ - # Clamp the measured ceiling to the Go2 saturation envelope: an + # Clamp the measured ceiling to the robot's saturation envelope: an # un-saturated FOPDT fit extrapolates linearly past what the platform # can physically deliver, so the envelope is a hard upper bound. - vx_ceiling = min(_channel_ceiling(per_amplitude, "vx", GO2_VX_MAX), GO2_VX_MAX) - wz_ceiling = min(_channel_ceiling(per_amplitude, "wz", GO2_WZ_MAX), GO2_WZ_MAX) + vx_ceiling = min(_channel_ceiling(per_amplitude, "vx", vx_max), vx_max) + wz_ceiling = min(_channel_ceiling(per_amplitude, "wz", wz_max), wz_max) feedforward = FeedforwardDC( K_vx=_safe_inv_gain(plant.vx.K), @@ -349,7 +351,7 @@ def derive_config( K_wz=_safe_inv_gain(plant.wz.K), ) - max_linear_accel = vx_ceiling / plant.vx.tau if plant.vx.tau > 1e-6 else GO2_VX_MAX + 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), @@ -361,7 +363,7 @@ def derive_config( caveats = [ f"Valid only for surface={provenance.surface!r}, " f"mode={provenance.mode!r}, {provenance.sim_or_hw}. Re-run " - f"go2-characterization on any surface or gait-mode change.", + 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}).", ] @@ -373,11 +375,11 @@ def derive_config( "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 " - "`go2_characterization --mode hw` on the real Go2 for a " + "`characterization --mode hw` on the real robot for a " "tuning-valid artifact.", ) - return Go2TuningConfig( + return TuningConfig( provenance=provenance, plant=PlantModelDC( vx=FopdtChannelDC(plant.vx.K, plant.vx.tau, plant.vx.L), @@ -441,13 +443,13 @@ def invert_tolerance( "SCHEMA_VERSION", "FeedforwardDC", "FopdtChannelDC", - "Go2TuningConfig", "OperatingPoint", "OperatingPointMap", "PlantModelDC", "Provenance", "RecommendedControllerDC", "ToleranceRow", + "TuningConfig", "VelocityProfileDC", "derive_config", "git_sha", From f159edbccbd721c77d31a27fd9774aa864db58e1 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 16:34:03 -0700 Subject: [PATCH 11/51] coordinator updates for velocity task specific config --- dimos/control/coordinator.py | 5 +++ .../tasks/baseline_path_follower_task.py | 41 ++++++++++++++++++- 2 files changed, 44 insertions(+), 2 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 13c959cd89..4b90c58f62 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -90,6 +90,9 @@ class TaskConfig: type: str = "trajectory" joint_names: list[str] = field(default_factory=lambda: []) priority: int = 10 + # Velocity-task specific. zero_on_timeout=True keeps the task streaming zeros + velocity_timeout: float = 0.2 + velocity_zero_on_timeout: bool = True # Cartesian IK / Teleop IK specific model_path: str | Path | None = None ee_joint_id: int = 6 @@ -320,6 +323,8 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: JointVelocityTaskConfig( joint_names=cfg.joint_names, priority=cfg.priority, + timeout=cfg.velocity_timeout, + zero_on_timeout=cfg.velocity_zero_on_timeout, ), ) diff --git a/dimos/control/tasks/baseline_path_follower_task.py b/dimos/control/tasks/baseline_path_follower_task.py index 42bf48de76..7deda21556 100644 --- a/dimos/control/tasks/baseline_path_follower_task.py +++ b/dimos/control/tasks/baseline_path_follower_task.py @@ -46,14 +46,17 @@ VelocityTrackingConfig, VelocityTrackingPID, ) +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.utils.benchmarking.velocity_profile import PathSpeedCap, 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.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.Path import Path logger = setup_logger() @@ -80,6 +83,8 @@ class BaselinePathFollowerTaskConfig: # 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 BaselinePathFollowerTask(BaseControlTask): @@ -111,6 +116,11 @@ def __init__( self._ff: FeedforwardGainCompensator | None = ( FeedforwardGainCompensator(config.ff_config) if config.ff_config else None ) + self._profile_cap: PathSpeedCap | None = ( + PathSpeedCap(config.velocity_profile_config) + if config.velocity_profile_config is not None + else None + ) self._state: BaselineState = "idle" self._path: Path | None = None @@ -141,7 +151,22 @@ def is_active(self) -> bool: def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if not self.is_active(): return None - if self._path is None or self._distancer is None or self._current_odom is 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: @@ -164,6 +189,12 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: # Static gain compensation: cmd_to_robot = controller_cmd / K_plant vx, vy, wz = self._ff.compute(vx, vy, wz) + # Curvature velocity-profile cap (preserves commanded turn radius). + if 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 + ) + return JointCommandOutput( joint_names=self._joint_names_list, velocities=[vx, vy, wz], @@ -268,6 +299,8 @@ def start_path(self, path: Path, current_odom: PoseStamped) -> bool: 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) first_yaw = path.poses[0].orientation.euler[2] robot_yaw = current_odom.orientation.euler[2] @@ -290,6 +323,10 @@ def start_path(self, path: Path, current_odom: PoseStamped) -> bool: 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 cancel(self) -> bool: From d8f56345a39c750b012a6506791525ad6714b1d4 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 16:36:37 -0700 Subject: [PATCH 12/51] create a fopdt plant connection module to pretend as hardware for sim evals --- .../drive_trains/fopdt_sim_base/adapter.py | 160 ------------------ dimos/robot/sim/fopdt_plant_connection.py | 152 +++++++++++++++++ 2 files changed, 152 insertions(+), 160 deletions(-) delete mode 100644 dimos/hardware/drive_trains/fopdt_sim_base/adapter.py create mode 100644 dimos/robot/sim/fopdt_plant_connection.py diff --git a/dimos/hardware/drive_trains/fopdt_sim_base/adapter.py b/dimos/hardware/drive_trains/fopdt_sim_base/adapter.py deleted file mode 100644 index a726edf8c9..0000000000 --- a/dimos/hardware/drive_trains/fopdt_sim_base/adapter.py +++ /dev/null @@ -1,160 +0,0 @@ -# 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. - -"""Layer 1 FOPDT sim TwistBase adapter (robot-agnostic). - -Wraps :class:`~dimos.utils.benchmarking.plant.TwistBasePlantSim` and -presents the standard :class:`TwistBaseAdapter` protocol so any -controller / task / coordinator that talks to a real velocity-commanded -base can be exercised in pure-Python sim with no hardware. The plant -params are supplied per robot (``params=`` kwarg, via the robot -profile's ``sim_plant``); the Go2 fit is only the default fallback. - -Plant integration is wall-clock driven: each :meth:`write_velocities` -call advances the plant by ``time.perf_counter()`` delta since the last -write. The ControlCoordinator's tick loop calls write_velocities once -per tick, so the plant ticks at the coordinator's tick_rate. -""" - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING - -from dimos.utils.benchmarking.plant import ( - GO2_PLANT_FITTED, - TwistBasePlantParams, - TwistBasePlantSim, -) - -if TYPE_CHECKING: - from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry - - -class FopdtTwistBaseAdapter: - """FOPDT + unicycle sim posing as a real twist base. - - Implements :class:`TwistBaseAdapter`. ``dof`` is fixed at 3 for the - twist-base model (vx, vy, wz). For a non-strafing robot, vy is simply - never commanded (the characterization tool excludes it); the model - itself is robot-agnostic — pass the robot's fitted ``params``. - """ - - def __init__( - self, - dof: int = 3, - params: TwistBasePlantParams | None = None, - initial_pose: tuple[float, float, float] = (0.0, 0.0, 0.0), - nominal_dt: float = 0.1, - **_: object, - ) -> None: - if dof != 3: - raise ValueError(f"FopdtTwistBaseAdapter requires dof=3, got {dof}") - self._dof = dof - self._params = params if params is not None else GO2_PLANT_FITTED - self._plant = TwistBasePlantSim(self._params) - self._initial_pose = initial_pose - self._nominal_dt = nominal_dt - - self._cmd: list[float] = [0.0, 0.0, 0.0] - self._last_step_time: float | None = None - self._enabled = False - self._connected = False - - # ========================================================================= - # Connection - # ========================================================================= - - def connect(self) -> bool: - self._plant.reset(*self._initial_pose, dt=self._nominal_dt) - self._last_step_time = None - self._connected = True - return True - - def disconnect(self) -> None: - self._connected = False - - def is_connected(self) -> bool: - return self._connected - - # ========================================================================= - # Info - # ========================================================================= - - def get_dof(self) -> int: - return self._dof - - # ========================================================================= - # State Reading - # ========================================================================= - - def read_velocities(self) -> list[float]: - """Return plant's actual filtered velocities (m/s, m/s, rad/s).""" - return [self._plant.vx, self._plant.vy, self._plant.wz] - - def read_odometry(self) -> list[float] | None: - """Return plant's integrated pose [x (m), y (m), yaw (rad)].""" - return [self._plant.x, self._plant.y, self._plant.yaw] - - # ========================================================================= - # Control - # ========================================================================= - - def write_velocities(self, velocities: list[float]) -> bool: - """Advance plant under ZOH of the prior cmd, then latch new cmd.""" - if len(velocities) != self._dof: - return False - now = time.perf_counter() - if self._last_step_time is not None: - dt = now - self._last_step_time - if dt > 0: - self._plant.step(self._cmd[0], self._cmd[1], self._cmd[2], dt) - self._cmd = list(velocities) - self._last_step_time = now - return True - - def write_stop(self) -> bool: - return self.write_velocities([0.0, 0.0, 0.0]) - - # ========================================================================= - # Enable/Disable - # ========================================================================= - - def write_enable(self, enable: bool) -> bool: - self._enabled = enable - return True - - def read_enabled(self) -> bool: - return self._enabled - - # ========================================================================= - # Sim helpers (not part of Protocol) - # ========================================================================= - - @property - def plant(self) -> TwistBasePlantSim: - """Direct access to the underlying plant (for inspection / tests).""" - return self._plant - - def set_initial_pose(self, x: float, y: float, yaw: float) -> None: - """Override the start pose. Takes effect on next :meth:`connect`.""" - self._initial_pose = (x, y, yaw) - - -def register(registry: TwistBaseAdapterRegistry) -> None: - """Register this adapter under ``fopdt_sim_twist_base``.""" - registry.register("fopdt_sim_twist_base", FopdtTwistBaseAdapter) - - -__all__ = ["FopdtTwistBaseAdapter"] diff --git a/dimos/robot/sim/fopdt_plant_connection.py b/dimos/robot/sim/fopdt_plant_connection.py new file mode 100644 index 0000000000..e1635015b2 --- /dev/null +++ b/dimos/robot/sim/fopdt_plant_connection.py @@ -0,0 +1,152 @@ +# 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. + +"""Robot-side connection module for the FOPDT twist-base sim plant. + +Mirrors :class:`dimos.robot.unitree.go2.connection.GO2Connection`'s shape +so the rest of the stack (ControlCoordinator + ``transport_lcm`` adapter ++ tasks) is identical between sim and hw. The only thing that differs is +which connection module the operator brings up: + + sim: dimos run coordinator-sim-fopdt + hw: dimos run unitree-go2-webrtc-keyboard-teleop + +This module: +- subscribes ``cmd_vel: In[Twist]`` from the bus, +- integrates a :class:`TwistBasePlantSim` under the latest command at a + fixed tick rate (ZOH between callbacks), +- publishes ``odom: Out[PoseStamped]`` from the integrated unicycle pose. +""" + +from __future__ import annotations + +from threading import Event, Thread +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +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.utils.benchmarking.plant import ( + GO2_PLANT_FITTED, + TwistBasePlantParams, + TwistBasePlantSim, +) +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class FopdtPlantConnectionConfig(ModuleConfig): + """Sim plant runtime config. + + ``plant_params`` defaults to the vendored Go2 fit so a bare blueprint + works out of the box. ``tick_rate_hz`` controls how often the plant + integrates and republishes odom — matches the coordinator's tick + rate by convention so the sim ticks at the same cadence as control. + ``frame_id`` is stamped on published PoseStamped messages. + """ + + plant_params: TwistBasePlantParams = GO2_PLANT_FITTED + tick_rate_hz: float = 10.0 + initial_x: float = 0.0 + initial_y: float = 0.0 + initial_yaw: float = 0.0 + frame_id: str = "odom" + + +class FopdtPlantConnection(Module): + """In-process FOPDT twist-base sim posing as a real robot connection. + + Wire shape (LCM topics) is identical to a real twist-base bring-up: + consume Twist on ``cmd_vel``, publish PoseStamped on ``odom``. The + coordinator on the other side uses ``transport_lcm`` exactly as it + does against hw — there is no per-mode adapter. + """ + + config: FopdtPlantConnectionConfig + cmd_vel: In[Twist] + odom: Out[PoseStamped] + + _plant: TwistBasePlantSim + _cmd: tuple[float, float, float] = (0.0, 0.0, 0.0) + _stop_event: Event + _thread: Thread | None = None + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._plant = TwistBasePlantSim(self.config.plant_params) + self._stop_event = Event() + + @rpc + def start(self) -> None: + super().start() + dt = 1.0 / self.config.tick_rate_hz + self._plant.reset(self.config.initial_x, self.config.initial_y, self.config.initial_yaw, dt) + self._cmd = (0.0, 0.0, 0.0) + self._stop_event.clear() + + unsub = self.cmd_vel.subscribe(self._on_cmd_vel) + self.register_disposable(Disposable(unsub)) + + self._thread = Thread(target=self._run, daemon=True) + self._thread.start() + logger.info( + f"FopdtPlantConnection started @ {self.config.tick_rate_hz:g} Hz " + f"(initial pose=({self.config.initial_x:.2f}, {self.config.initial_y:.2f}, " + f"{self.config.initial_yaw:.2f}))" + ) + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._thread is not None and self._thread.is_alive(): + self._thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + self._thread = None + super().stop() + + def _on_cmd_vel(self, msg: Twist) -> None: + self._cmd = (float(msg.linear.x), float(msg.linear.y), float(msg.angular.z)) + + def _run(self) -> None: + period = 1.0 / self.config.tick_rate_hz + next_tick = time.perf_counter() + while not self._stop_event.is_set(): + vx, vy, wz = self._cmd + self._plant.step(vx, vy, wz, period) + + pose = PoseStamped( + ts=time.time(), + frame_id=self.config.frame_id, + position=Vector3(self._plant.x, self._plant.y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, self._plant.yaw)), + ) + self.odom.publish(pose) + + next_tick += period + sleep_for = next_tick - time.perf_counter() + if sleep_for > 0: + self._stop_event.wait(sleep_for) + else: + next_tick = time.perf_counter() + + +__all__ = ["FopdtPlantConnection", "FopdtPlantConnectionConfig"] From 263e48ff2f6695519bcc9b44675f5c7c378633cc Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 16:37:16 -0700 Subject: [PATCH 13/51] refactor and clean si and benchmark scripts --- dimos/utils/benchmarking/benchmark.py | 731 ++++++++----------- dimos/utils/benchmarking/characterization.py | 233 +++--- dimos/utils/benchmarking/plant.py | 55 +- 3 files changed, 481 insertions(+), 538 deletions(-) diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py index 39c7e9f742..f8368a0260 100644 --- a/dimos/utils/benchmarking/benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -20,13 +20,18 @@ speed ladder on a fixed real-space-constrained path set, scores each (path, speed), and writes back the operating-point map + tolerance->max-safe-speed inversion (artifact section 5). Robot-agnostic: -everything robot-specific comes from the ``RobotProfile`` (``--robot``). +everything robot-specific comes from the ``RobotPlantProfile`` (``--robot``). + +Architecturally sim and hw are identical here. The benchmark always +runs the baseline INSIDE a real ``ControlCoordinator`` tick loop driving +the ``transport_lcm`` twist-base adapter. The only thing that changes +between modes is which connection module is on the robot side of the +LCM topics — sim: ``coordinator-sim-fopdt`` (FopdtPlantConnection), hw: +``unitree-go2-webrtc-keyboard-teleop`` (GO2Connection). The operator +brings that up in another terminal; the prereq banner reminds them. uv run python -m dimos.utils.benchmarking.benchmark \\ --robot go2 --config reports/go2_config_hw_<...>.json --mode hw - -The sim harness (the baseline driven through a real ``ControlCoordinator`` -+ the FOPDT sim adapter) is inlined below — small, baseline-only. """ from __future__ import annotations @@ -39,7 +44,6 @@ import sys import threading import time -from typing import TYPE_CHECKING, Protocol import matplotlib @@ -47,13 +51,7 @@ import matplotlib.pyplot as plt from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints -from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.control.task import ( - ControlMode, - CoordinatorState, - JointCommandOutput, - JointStateSnapshot, -) +from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.baseline_path_follower_task import ( BaselinePathFollowerTask, BaselinePathFollowerTaskConfig, @@ -65,8 +63,9 @@ 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.sensor_msgs.JointState import JointState from dimos.utils.benchmarking.paths import circle, single_corner, square, straight_line -from dimos.utils.benchmarking.plant import ROBOT_PROFILES, RobotProfile +from dimos.utils.benchmarking.plant import ROBOT_PLANT_PROFILES, RobotPlantProfile from dimos.utils.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run from dimos.utils.benchmarking.tuning import ( OperatingPoint, @@ -74,252 +73,28 @@ TuningConfig, invert_tolerance, ) -from dimos.utils.benchmarking.velocity_profile import PathSpeedCap, VelocityProfileConfig +from dimos.utils.benchmarking.velocity_profile import VelocityProfileConfig -if TYPE_CHECKING: - from dimos.hardware.drive_trains.fopdt_sim_base.adapter import FopdtTwistBaseAdapter - -_base_joints = make_twist_base_joints("base") _ARRIVED_STATES = frozenset({"arrived", "completed"}) _FAILED_STATES = frozenset({"aborted"}) REPORTS_DIR = Path(__file__).parent / "reports" -def _resolve_profile(name: str) -> RobotProfile: +def _resolve_profile(name: str) -> RobotPlantProfile: try: - return ROBOT_PROFILES[name] + return ROBOT_PLANT_PROFILES[name] except KeyError: - raise SystemExit(f"unknown --robot {name!r}; known: {sorted(ROBOT_PROFILES)}") from None - - -def _clamp(v: float, lo: float, hi: float) -> float: - return max(lo, min(hi, v)) - - -def _twist_clamped(vx: float, wz: float, vx_max: float, wz_max: float) -> Twist: - return Twist( - linear=Vector3(_clamp(vx, -vx_max, vx_max), 0.0, 0.0), - angular=Vector3(0.0, 0.0, _clamp(wz, -wz_max, wz_max)), - ) - - -# --- inlined baseline sim harness (was runner.py + sim_blueprint.py) ----- - - -class _PathFollowerLike(Protocol): - def start_path(self, path: NavPath, current_odom: PoseStamped) -> bool: ... - def update_odom(self, odom: PoseStamped) -> None: ... - def compute(self, state) -> object: ... - def get_state(self) -> str: ... - - -class _VelocityProfileProxyTask: - """Curvature velocity-profile cap (the DERIVE consumer seam). Caps - commanded ``|vx|`` to the profile at the robot's path index, scaling - ``wz`` to preserve geometry; pure pass-through otherwise. No - control-law change.""" - - def __init__(self, inner: _PathFollowerLike, cap: PathSpeedCap) -> None: - self._inner = inner - self._cap = cap - self._xy = (0.0, 0.0) - - @property - def name(self) -> str: - return self._inner.name - - def claim(self): - return self._inner.claim() - - def is_active(self) -> bool: - return self._inner.is_active() - - def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: - self._inner.on_preempted(by_task, joints) - - def on_buttons(self, *a, **k): - return self._inner.on_buttons(*a, **k) - - def on_cartesian_command(self, *a, **k): - return self._inner.on_cartesian_command(*a, **k) - - def set_target_by_name(self, *a, **k): - return self._inner.set_target_by_name(*a, **k) - - def set_velocities_by_name(self, *a, **k): - return self._inner.set_velocities_by_name(*a, **k) - - def get_state(self) -> str: - return self._inner.get_state() - - def update_odom(self, odom: PoseStamped) -> None: - self._xy = (float(odom.position.x), float(odom.position.y)) - self._inner.update_odom(odom) - - def start_path(self, path: NavPath, current_odom: PoseStamped) -> bool: - self._cap.for_path(path) - self._xy = (float(current_odom.position.x), float(current_odom.position.y)) - return self._inner.start_path(path, current_odom) - - def compute(self, state): - out = self._inner.compute(state) - if out is None or out.mode != ControlMode.VELOCITY or out.velocities is None: - return out - vx, vy, wz = ([*out.velocities, 0.0, 0.0, 0.0])[:3] - cx, cy, cz = self._cap.cap(self._xy[0], self._xy[1], vx, vy, wz) - return JointCommandOutput( - joint_names=out.joint_names, - velocities=[cx, cy, cz], - mode=ControlMode.VELOCITY, - ) - - -def _sim_base(profile: RobotProfile) -> HardwareComponent: - return HardwareComponent( - hardware_id="base", - hardware_type=HardwareType.BASE, - joints=make_twist_base_joints("base"), - adapter_type=profile.sim_adapter_key, - adapter_kwargs={"params": profile.sim_plant}, - ) - - -def _odom_to_pose(odom: list[float]) -> PoseStamped: - return PoseStamped( - position=Vector3(odom[0], odom[1], 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, odom[2])), - ) - - -def _vels_to_twist(v: list[float]) -> Twist: - return Twist(linear=Vector3(v[0], v[1], 0.0), angular=Vector3(0.0, 0.0, v[2])) - - -def _run_baseline_sim( - profile: RobotProfile, - path: NavPath, - speed: float, - k_angular: float, - ff_config: FeedforwardGainConfig | None, - profile_config: VelocityProfileConfig | None, - timeout_s: float, -) -> tuple[ExecutedTrajectory, NavPath]: - """Stock baseline P-controller in sim against the profile's FOPDT - sim adapter. ``ff_config``/``profile_config`` are OPTIONAL comparison - arms (``None`` = bare controller — the physical-limit measurement). - Returns the trajectory and the reference path in the executed frame - (sim runs in the path's own frame, so it is ``path`` unchanged).""" - coord = ControlCoordinator( - tick_rate=profile.tick_rate_hz, - hardware=[_sim_base(profile)], - tasks=[ - TaskConfig(name="vel_base", type="velocity", joint_names=_base_joints, priority=10), - ], - ) - - def _make() -> _PathFollowerLike: - base = BaselinePathFollowerTask( - name="baseline_follower", - config=BaselinePathFollowerTaskConfig( - speed=speed, k_angular=k_angular, ff_config=ff_config - ), - global_config=global_config, - ) - if profile_config is None: - return base - return _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) - - coord.start() - try: - adapter: FopdtTwistBaseAdapter = coord._hardware["base"].adapter - start = path.poses[0] - adapter.set_initial_pose(start.position.x, start.position.y, start.orientation.euler[2]) - adapter.connect() - - task = _make() - coord.add_task(task) - task.start_path(path, _odom_to_pose(adapter.read_odometry())) - - ticks: list[TrajectoryTick] = [] - period = 1.0 / profile.tick_rate_hz - t0 = time.perf_counter() - next_sample = t0 - arrived = False - while True: - now = time.perf_counter() - t_rel = now - t0 - if t_rel > timeout_s: - break - pose = _odom_to_pose(adapter.read_odometry()) - task.update_odom(pose) - ticks.append( - TrajectoryTick( - t=t_rel, - pose=pose, - cmd_twist=_vels_to_twist(adapter._cmd), - actual_twist=_vels_to_twist(adapter.read_velocities()), - ) - ) - s = task.get_state() - if s in _ARRIVED_STATES: - arrived = True - break - if s in _FAILED_STATES: - break - next_sample += period - sleep_for = next_sample - time.perf_counter() - if sleep_for > 0: - time.sleep(sleep_for) - return ExecutedTrajectory(ticks=ticks, arrived=arrived), path - finally: - coord.stop() - - -# --- hw harness (real robot over LCM; closed-loop baseline) ------------- -# -# Ported from the R&D `_run_path_follower_hw`. Talks LCM to a separately -# running `dimos run ` (publishes the odom topic, -# consumes the cmd topic; if that blueprint includes a keyboard teleop it -# must be publish-only-when-active so it does not fight the run). No new -# module — the small estimator/anchor are duplicated with -# characterization by choice (no shared-module addition). - - -class _PoseVelocityEstimator: - """Consecutive ``PoseStamped`` -> EMA body-frame (vx,vy,wz).""" - - def __init__(self, alpha: float = 0.5) -> None: - self._pp = None - self._pt: float | None = None - self._vx = self._vy = self._wz = 0.0 - self._a = alpha - - def update(self, pose, t: float) -> tuple[float, float, float]: - if self._pp is None or self._pt is None: - self._pp, self._pt = pose, t - return 0.0, 0.0, 0.0 - dt = t - self._pt - if dt <= 0: - return self._vx, self._vy, self._wz - dx = pose.position.x - self._pp.position.x - dy = pose.position.y - self._pp.position.y - y0, y1 = self._pp.orientation.euler[2], pose.orientation.euler[2] - dyaw = (y1 - y0 + 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 - self._vx = self._a * bx + (1 - self._a) * self._vx - self._vy = self._a * by + (1 - self._a) * self._vy - self._wz = self._a * (dyaw / dt) + (1 - self._a) * self._wz - self._pp, self._pt = pose, t - return self._vx, self._vy, self._wz + raise SystemExit( + f"unknown --robot {name!r}; known: {sorted(ROBOT_PLANT_PROFILES)}" + ) from None 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 it need not be positioned - precisely — only roughly aimed).""" + precisely — only roughly aimed). Used in BOTH sim and hw so scoring + is 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 @@ -337,9 +112,158 @@ def _shift_path_to_start_at_pose(path: NavPath, start_pose: PoseStamped) -> NavP return NavPath(poses=new) -def _run_baseline_hw( - profile: RobotProfile, - link: dict, +class _JointStateRecorder: + """Subscribes to a coordinator's ``joint_state`` Out and turns each + tick into a ``TrajectoryTick``. Recovers body-frame velocity by + pose differentiation (``read_velocities`` returns last-commanded for + ``transport_lcm``, not measured — same for hw GO2Connection and the + sim FopdtPlantConnection). EMA-smoothed (alpha=0.5).""" + + def __init__(self, joint_names: list[str], alpha: float = 0.5) -> None: + self._jx, self._jy, self._jyaw = joint_names + self._alpha = alpha + self._lock = threading.Lock() + self._ticks: list[TrajectoryTick] = [] + self._first_pose: PoseStamped | None = None + self._t0: float | None = None + # diff state + self._prev_pose: PoseStamped | None = None + self._prev_t: float | None = None + self._vx = self._vy = self._wz = 0.0 + # commanded telemetry: most recent JointState.velocity (the adapter's + # last write) for this hardware's joints + self._cmd_vx = self._cmd_vy = self._cmd_wz = 0.0 + + def on_joint_state(self, msg: JointState) -> None: + # ConnectedTwistBase publishes positions = odometry [x, y, yaw] + # and velocities = last commanded (transport_lcm convention). + # Caller waits a grace period after coord.start before sampling + # the latest pose so the first /odom has time to propagate + # through the adapter and one tick — that avoids latching onto + # the [0,0,0] placeholder ConnectedTwistBase emits before the + # adapter has seen any odom. + if not msg.name: + return + idx = {n: i for i, n in enumerate(msg.name)} + try: + x = float(msg.position[idx[self._jx]]) + y = float(msg.position[idx[self._jy]]) + yaw = float(msg.position[idx[self._jyaw]]) + except (KeyError, IndexError): + return + + now = time.perf_counter() + pose = PoseStamped( + ts=now, + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + + # commanded telemetry (optional — used only to colour the recorded + # cmd_twist column; behaviour is identical with or without it) + if msg.velocity: + try: + self._cmd_vx = float(msg.velocity[idx[self._jx]]) + self._cmd_vy = float(msg.velocity[idx[self._jy]]) + self._cmd_wz = float(msg.velocity[idx[self._jyaw]]) + except (KeyError, IndexError): + pass + + with self._lock: + if self._first_pose is None: + self._first_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 + self._ticks.append( + TrajectoryTick( + t=t_rel, + pose=pose, + cmd_twist=Twist( + linear=Vector3(self._cmd_vx, self._cmd_vy, 0.0), + angular=Vector3(0.0, 0.0, self._cmd_wz), + ), + actual_twist=Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ), + ) + ) + return + + 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 + y0 = self._prev_pose.orientation.euler[2] + y1 = pose.orientation.euler[2] + dyaw = (y1 - y0 + 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( + TrajectoryTick( + t=t_rel, + pose=pose, + cmd_twist=Twist( + linear=Vector3(self._cmd_vx, self._cmd_vy, 0.0), + angular=Vector3(0.0, 0.0, self._cmd_wz), + ), + actual_twist=Twist( + linear=Vector3(self._vx, self._vy, 0.0), + angular=Vector3(0.0, 0.0, self._wz), + ), + ) + ) + + def first_pose(self, timeout_s: float, grace_s: float = 0.5) -> PoseStamped: + # Wait at minimum until coord+adapter have had time to receive a + # first /odom and propagate it through one tick (otherwise we + # latch onto the ConnectedTwistBase [0,0,0] placeholder). After + # the grace period the latest pose is the real current one. + time.sleep(grace_s) + deadline = time.perf_counter() + timeout_s + while time.perf_counter() < deadline: + with self._lock: + if self._prev_pose is not None: + return self._prev_pose + time.sleep(0.02) + raise RuntimeError(f"no odom within {timeout_s:.1f}s") + + def snapshot(self) -> list[TrajectoryTick]: + with self._lock: + return list(self._ticks) + + +def _make_base_component(profile: RobotPlantProfile) -> HardwareComponent: + """In-process transport_lcm base — pubs Twist on /{robot_id}/cmd_vel, + subs PoseStamped on /{robot_id}/odom. Identical in sim and hw; the + only thing that differs is which connection module is the other end + of those topics (the operator's running blueprint).""" + return HardwareComponent( + hardware_id=profile.robot_id, + hardware_type=HardwareType.BASE, + joints=make_twist_base_joints(profile.robot_id), + adapter_type="transport_lcm", + # READ-ONLY: we observe /{robot_id}/odom via this adapter, but the + # tool publishes its own Twist on /cmd_vel into the operator's + # coord. If we let this adapter write, it would also publish on + # /{robot_id}/cmd_vel and race the operator's coord. + auto_enable=False, + ) + + +def _run_baseline( + profile: RobotPlantProfile, path: NavPath, speed: float, k_angular: float, @@ -348,94 +272,71 @@ def _run_baseline_hw( timeout_s: float, label: str, ) -> tuple[ExecutedTrajectory, NavPath]: - """Closed-loop stock baseline on the real robot: anchor the path to - the robot's current pose, then track at the profile tick rate off - real odom. ``ff_config``/``profile_config`` are OPTIONAL arms - (``None`` = bare = the physical-limit measurement). Safe: velocity - clamp, stale-odom abort, timeout, zero-Twist on exit. Returns the - trajectory and the anchored reference path (odom frame) — score/plot - must use this, not the robot-centric input path.""" - cmd_pub, get_odom = link["pub"], link["get"] - - def stop_twist() -> Twist: - return _twist_clamped(0.0, 0.0, profile.vx_max, profile.wz_max) - - base = BaselinePathFollowerTask( + """Stock baseline P-controller inside a real ControlCoordinator, + talking ``transport_lcm`` to whichever connection module the operator + brought up. ``ff_config``/``profile_config`` are OPTIONAL arms + (``None`` = bare = the physical-limit measurement). + + Path is anchored to the robot's first observed pose so the operator + doesn't have to position the robot precisely — only roughly aim it. + Returns the executed trajectory and the anchored reference path + (scoring + plotting must use this, not the robot-centric input).""" + joints = make_twist_base_joints(profile.robot_id) + coord = ControlCoordinator( + tick_rate=profile.tick_rate_hz, + hardware=[_make_base_component(profile)], + ) + task = BaselinePathFollowerTask( name=f"baseline_{label}", config=BaselinePathFollowerTaskConfig( - speed=speed, k_angular=k_angular, ff_config=ff_config + joint_names=joints, + speed=speed, + k_angular=k_angular, + control_frequency=profile.tick_rate_hz, + ff_config=ff_config, + velocity_profile_config=profile_config, ), global_config=global_config, ) - task = ( - base - if profile_config is None - else _VelocityProfileProxyTask(base, PathSpeedCap(profile_config)) - ) - - pose0, _ = get_odom() - path_w = _shift_path_to_start_at_pose(path, pose0) - task.start_path(path_w, pose0) + recorder = _JointStateRecorder(joint_names=joints) + unsub = coord.joint_state.subscribe(recorder.on_joint_state) - ticks: list[TrajectoryTick] = [] - est = _PoseVelocityEstimator() - period = 1.0 / profile.tick_rate_hz - t0 = time.perf_counter() + coord.start() arrived = False + path_w = path try: - while True: - now = time.perf_counter() - t_rel = now - t0 - if t_rel > timeout_s: - print(f" [{label}] timeout {timeout_s:.0f}s") - break - pose, age = get_odom() - if pose is None or age > profile.odom_stale_s: - print(f" [{label}] ABORT stale odom ({age:.2f}s)") - break - task.update_odom(pose) - ev = est.update(pose, now) - state = CoordinatorState( - joints=JointStateSnapshot( - joint_velocities={"base/vx": ev[0], "base/vy": ev[1], "base/wz": ev[2]}, - timestamp=now, - ), - t_now=now, - dt=period, - ) - out = task.compute(state) - vx, wz = ( - (out.velocities[0], out.velocities[2]) - if (out is not None and out.velocities is not None) - else (0.0, 0.0) - ) - tw = _twist_clamped(vx, wz, profile.vx_max, profile.wz_max) - cmd_pub.broadcast(None, tw) - ticks.append( - TrajectoryTick( - t=t_rel, - pose=pose, - cmd_twist=tw, - actual_twist=Twist( - linear=Vector3(ev[0], ev[1], 0.0), - angular=Vector3(0.0, 0.0, ev[2]), - ), - ) - ) + pose0 = recorder.first_pose(timeout_s=profile.odom_warmup_s) + path_w = _shift_path_to_start_at_pose(path, pose0) + coord.add_task(task) + if not task.start_path(path_w, pose0): + print(f" [{label}] start_path rejected; aborting run") + return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w + + t_start = time.perf_counter() + deadline = t_start + timeout_s + terminated = False + while time.perf_counter() < deadline: st = task.get_state() if st in _ARRIVED_STATES: arrived = True - print(f" [{label}] arrived in {t_rel:.1f}s") + terminated = True + print(f" [{label}] arrived in {time.perf_counter() - t_start:.1f}s") break if st in _FAILED_STATES: - print(f" [{label}] task aborted") + terminated = True + print(f" [{label}] task aborted (state={st})") break - time.sleep(max(0.0, t0 + len(ticks) * period - time.perf_counter())) - finally: - for _ in range(3): - cmd_pub.broadcast(None, stop_twist()) time.sleep(0.05) - return ExecutedTrajectory(ticks=ticks, arrived=arrived), path_w + if not terminated: + print(f" [{label}] timeout {timeout_s:.0f}s") + finally: + try: + task.cancel() + except Exception: + pass + unsub() + coord.stop() + return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=arrived), path_w # --- benchmark ---------------------------------------------------------- @@ -451,42 +352,12 @@ def _path_set() -> dict: } -def _open_hw_link(profile: RobotProfile, warmup_s: float) -> dict: - """LCM to a running `dimos run `.""" - from dimos.core.transport import LCMTransport - - cmd_pub = LCMTransport(profile.cmd_topic, Twist) - odom_sub = LCMTransport(profile.odom_topic, PoseStamped) - lock = threading.Lock() - box: dict = {"pose": None, "t": 0.0} - - def _on(msg) -> None: - with lock: - box["pose"] = msg - box["t"] = time.perf_counter() - - odom_sub.subscribe(_on) - - def get_odom(): - with lock: - return box["pose"], time.perf_counter() - box["t"] - - print(f"[hw] waiting up to {warmup_s:.0f}s for {profile.odom_topic} ...") - deadline = time.perf_counter() + warmup_s - while time.perf_counter() < deadline and get_odom()[0] is None: - time.sleep(0.05) - if get_odom()[0] is None: - raise RuntimeError(f"No {profile.odom_topic} — is `dimos run {profile.blueprint}` up?") - return {"pub": cmd_pub, "get": get_odom} - - def _run_ladder( cfg: TuningConfig, - profile: RobotProfile, + profile: RobotPlantProfile, speeds: list[float], timeout_s: float, mode: str, - warmup_s: float, use_ff: bool, use_profile: bool, ) -> tuple[list[OperatingPoint], list[dict]]: @@ -494,82 +365,62 @@ def _run_ladder( # measurement. FF / velocity profile are opt-in comparison arms. ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) - link = _open_hw_link(profile, warmup_s) if mode == "hw" else None points: list[OperatingPoint] = [] runs: list[dict] = [] # for the XY trajectory overlay - try: - for name, path in _path_set().items(): - for speed in speeds: - prof_cfg = ( - cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None - ) - if mode == "hw": - for _ in range(3): - link["pub"].broadcast( - None, _twist_clamped(0.0, 0.0, profile.vx_max, profile.wz_max) - ) - time.sleep(0.05) - resp = ( - input( - f"\n[{name} v={speed:.2f}] reposition+aim robot, " - f"ENTER=run s=skip q=quit: " - ) - .strip() - .lower() - ) - if resp == "q": - raise KeyboardInterrupt - if resp == "s": - print(" skipped") - continue - traj, ref = _run_baseline_hw( - profile, - link, - path, - speed, - k_angular, - ff, - prof_cfg, - timeout_s, - f"{name}@{speed:.2f}", - ) - else: - traj, ref = _run_baseline_sim( - profile, path, speed, k_angular, ff, prof_cfg, timeout_s - ) - # Score/plot against the executed-frame reference: in hw - # that's the pose-anchored path, not the robot-centric input. - s = score_run(ref, traj) - points.append( - OperatingPoint( - path=name, - speed=speed, - cte_max=s.cte_max, - cte_rms=s.cte_rms, - arrived=s.arrived, + for name, path in _path_set().items(): + for speed in speeds: + prof_cfg = ( + cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None + ) + if mode == "hw": + resp = ( + input( + f"\n[{name} v={speed:.2f}] reposition+aim robot, " + f"ENTER=run s=skip q=quit: " ) + .strip() + .lower() ) - runs.append( - { - "path": name, - "speed": speed, - "cte_max": s.cte_max, - "arrived": s.arrived, - "ref": [(p.position.x, p.position.y) for p in ref.poses], - "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], - } - ) - print( - f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " - f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" - ) - finally: - if link is not None: - for _ in range(3): - link["pub"].broadcast( - None, _twist_clamped(0.0, 0.0, profile.vx_max, profile.wz_max) + if resp == "q": + raise KeyboardInterrupt + if resp == "s": + print(" skipped") + continue + traj, ref = _run_baseline( + profile, + path, + speed, + k_angular, + ff, + prof_cfg, + timeout_s, + f"{name}@{speed:.2f}", + ) + # Score/plot against the executed-frame reference (the anchored path). + s = score_run(ref, traj) + points.append( + OperatingPoint( + path=name, + speed=speed, + cte_max=s.cte_max, + cte_rms=s.cte_rms, + arrived=s.arrived, ) - time.sleep(0.05) + ) + runs.append( + { + "path": name, + "speed": speed, + "cte_max": s.cte_max, + "arrived": s.arrived, + "ref": [(p.position.x, p.position.y) for p in ref.poses], + "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], + } + ) + print( + f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " + f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" + ) return points, runs @@ -595,8 +446,8 @@ def _canonicalize(ref: list, exec_: list) -> tuple[list, list]: start -> (0,0), initial heading -> +x. The same transform is applied to the executed trajectory. Makes every run of a path overlay on one identical reference sharing the origin — so speeds are comparable - regardless of where the robot physically started (hw anchors each run - to its current odom pose; sim already starts at the path origin).""" + regardless of where the robot physically started (paths are anchored + to the robot's first odom pose, which differs between runs).""" if len(ref) < 2: return ref, exec_ ox, oy = ref[0] @@ -671,20 +522,32 @@ def _plot_xy(runs: list[dict], out: Path, robot_name: str, arm: str) -> None: plt.close(fig) +def _prereq_banner(profile: RobotPlantProfile, mode: str) -> None: + if mode == "hw": + bp = profile.blueprint + kind = "HARDWARE" + else: + bp = profile.sim_blueprint + kind = "SIM" + print( + f"\n=== {kind} MODE ({profile.name}) ===\n" + f"Prereqs:\n" + f" 1. Another terminal: `dimos run {bp}`\n" + f" (publishes {profile.odom_topic}, consumes {profile.cmd_topic}).\n" + f" 2. This process: strip /nix/store from LD_LIBRARY_PATH (README).\n" + f"Each (path,speed): reposition+aim, then ENTER. Velocity-commanded\n" + f"baseline runs inside our ControlCoordinator; ticks at {profile.tick_rate_hz:g}Hz.\n" + ) + + def main() -> None: ap = argparse.ArgumentParser(description="Twist-base operating-point benchmark") - ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PROFILES)}") + ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PLANT_PROFILES)}") ap.add_argument("--config", required=True, help="config artifact from characterization") ap.add_argument("--mode", choices=["hw", "sim"], default="hw") ap.add_argument("--speeds", default="0.3,0.5,0.7,0.9,1.0") ap.add_argument("--tolerances", default="5,10,15", help="cm") ap.add_argument("--timeout", type=float, default=60.0, help="per (path,speed) run timeout (s)") - ap.add_argument( - "--odom-warmup", - type=float, - default=None, - help="how long to wait for first odom (s); default from profile", - ) ap.add_argument( "--ff", action="store_true", @@ -700,7 +563,6 @@ def main() -> None: args = ap.parse_args() profile = _resolve_profile(args.robot) - warmup_s = args.odom_warmup if args.odom_warmup is not None else profile.odom_warmup_s config_path = Path(args.config).expanduser() cfg = TuningConfig.from_json(config_path) # asserts schema_version speeds = [float(s) for s in args.speeds.split(",")] @@ -723,6 +585,8 @@ def main() -> None: "plant only; the operating-point map is NOT a real-robot result." ) + _prereq_banner(profile, args.mode) + arm_desc = ( "BARE stock baseline (no FF, no profile) — the plant's physical tracking limit" if arm == "bare" @@ -740,7 +604,6 @@ def main() -> None: speeds, args.timeout, args.mode, - warmup_s, use_ff=args.ff, use_profile=args.profile, ) diff --git a/dimos/utils/benchmarking/characterization.py b/dimos/utils/benchmarking/characterization.py index 102002e4e1..5600a3a64f 100644 --- a/dimos/utils/benchmarking/characterization.py +++ b/dimos/utils/benchmarking/characterization.py @@ -17,7 +17,7 @@ **This is a hardware tool.** It measures a real velocity-commanded base's per-axis response (vx, vy, wz), fits FOPDT per channel, runs the DERIVE step, and emits the versioned config artifact. Robot-agnostic: -everything robot-specific comes from the selected ``RobotProfile`` +everything robot-specific comes from the selected ``RobotPlantProfile`` (``--robot``, default ``go2``). # terminal 1 (the robot's bring-up blueprint, see the profile): @@ -26,11 +26,14 @@ uv run python -m dimos.utils.benchmarking.characterization \\ --robot go2 --mode hw --surface concrete -`--mode hw` (default) drives the real robot over LCM (profile cmd topic -out, odom topic in). It is **operator-gated**: before every step it -stops the robot and waits for you to reposition it and press ENTER. -Safe (velocity clamp, zero-Twist on exit/SIGINT, stale-odom abort, -distance + time caps). +`--mode hw` (default) drives the real robot via the same path the +benchmark does: an in-process ``ControlCoordinator`` with the +``transport_lcm`` twist-base adapter spins up to give us a ``joint_state`` +Out stream sourced from the adapter's odometry. Signal-injection itself +stays a standalone Twist publisher (SI is open-loop by nature). Each +step is **operator-gated**: before every step the robot is stopped and +we wait for ENTER. Safe (velocity clamp, zero-Twist on exit/SIGINT, +stale-odom abort, distance + time caps). `--mode self-test` is a **plumbing check, NOT a tuning artifact**: it steps the profile's in-process FOPDT sim plant and recovers it. It only @@ -53,10 +56,17 @@ import matplotlib.pyplot as plt import numpy as np +from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints +from dimos.control.coordinator import ControlCoordinator +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.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.benchmarking.plant import ( - ROBOT_PROFILES, + ROBOT_PLANT_PROFILES, FopdtChannelParams, - RobotProfile, + RobotPlantProfile, TwistBasePlantParams, TwistBasePlantSim, ) @@ -76,17 +86,19 @@ def _clamp(v: float, lo: float, hi: float) -> float: return max(lo, min(hi, v)) -def _resolve_profile(name: str) -> RobotProfile: +def _resolve_profile(name: str) -> RobotPlantProfile: try: - return ROBOT_PROFILES[name] + return ROBOT_PLANT_PROFILES[name] except KeyError: - raise SystemExit(f"unknown --robot {name!r}; known: {sorted(ROBOT_PROFILES)}") from None + raise SystemExit( + f"unknown --robot {name!r}; known: {sorted(ROBOT_PLANT_PROFILES)}" + ) from None # --- self-test (in-process FOPDT plant; NOT robot-valid) ----------------- -def _fit_selftest(profile: RobotProfile) -> tuple[TwistBasePlantParams, dict, list[dict]]: +def _fit_selftest(profile: RobotPlantProfile) -> tuple[TwistBasePlantParams, dict, list[dict]]: """Step the profile's FOPDT sim plant and recover it. Plumbing check only — proves the measure->fit->derive code path runs.""" truth = profile.sim_plant @@ -153,7 +165,7 @@ def _fit_selftest(profile: RobotProfile) -> tuple[TwistBasePlantParams, dict, li def _plot_fits( - traces: list[dict], provenance: Provenance, profile: RobotProfile, out: Path + traces: list[dict], provenance: Provenance, profile: RobotPlantProfile, out: Path ) -> None: """One column per channel; each step's measured velocity overlaid with its fitted FOPDT step response. This is the artifact a human @@ -198,40 +210,78 @@ def _plot_fits( # --- hardware SI (real robot over LCM, operator-gated, safe) ------------- -class _PoseVelocityEstimator: - """Differentiate consecutive ``PoseStamped`` to body-frame (vx,vy,wz); - EMA-smoothed (pose-only odom). Ported from the R&D hw loop.""" - - def __init__(self, alpha: float = 0.5) -> None: - self._pp = None - self._pt: float | None = None +class _JointStatePoseStream: + """Pose + body-velocity stream sourced from a coordinator's + ``joint_state`` Out. Reuses the benchmark observer's math: positions + are [x, y, yaw] (twist-base adapter convention); body-frame velocity + is recovered by EMA-smoothed pose differentiation. Drop-in + replacement for the old standalone /odom LCM subscriber + + in-house ``_PoseVelocityEstimator``.""" + + def __init__(self, joint_names: list[str], alpha: float = 0.5) -> None: + self._jx, self._jy, self._jyaw = joint_names + self._alpha = alpha + self._lock = threading.Lock() + self._pose: PoseStamped | None = None + self._pose_t: float = 0.0 + self._prev_pose: PoseStamped | None = None + self._prev_t: float | None = None self._vx = self._vy = self._wz = 0.0 - self._a = alpha - - def update(self, pose, t: float) -> tuple[float, float, float]: - if self._pp is None or self._pt is None: - self._pp, self._pt = pose, t - return 0.0, 0.0, 0.0 - dt = t - self._pt - if dt <= 0: - return self._vx, self._vy, self._wz - dx = pose.position.x - self._pp.position.x - dy = pose.position.y - self._pp.position.y - y0, y1 = self._pp.orientation.euler[2], pose.orientation.euler[2] - dyaw = (y1 - y0 + math.pi) % (2 * math.pi) - math.pi - yaw = y1 - c, s = math.cos(yaw), math.sin(yaw) - bx = (dx / dt) * c + (dy / dt) * s - by = -(dx / dt) * s + (dy / dt) * c - bw = dyaw / dt - self._vx = self._a * bx + (1 - self._a) * self._vx - self._vy = self._a * by + (1 - self._a) * self._vy - self._wz = self._a * bw + (1 - self._a) * self._wz - self._pp, self._pt = pose, t - return self._vx, self._vy, self._wz - - -def _prereq_banner(profile: RobotProfile) -> None: + + def on_joint_state(self, msg: JointState) -> None: + if not msg.name: + return + idx = {n: i for i, n in enumerate(msg.name)} + try: + x = float(msg.position[idx[self._jx]]) + y = float(msg.position[idx[self._jy]]) + yaw = float(msg.position[idx[self._jyaw]]) + except (KeyError, IndexError): + return + # The caller waits a grace period after coord.start before + # sampling, so the (0,0,0) placeholder from ConnectedTwistBase + # (emitted before the adapter receives its first /odom) does + # not get latched as the start pose. + now = time.perf_counter() + from dimos.msgs.geometry_msgs.Quaternion import Quaternion + + pose = PoseStamped( + ts=now, + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + with self._lock: + if self._prev_pose is not None and self._prev_t is not None: + 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 + y0 = self._prev_pose.orientation.euler[2] + y1 = pose.orientation.euler[2] + dyaw = (y1 - y0 + 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._pose, self._pose_t = pose, now + + def latest(self) -> tuple[PoseStamped | None, float, tuple[float, float, float]]: + with self._lock: + return self._pose, self._pose_t, (self._vx, self._vy, self._wz) + + def reset_velocity(self) -> None: + """Drop EMA state — called at pre-roll so each step starts clean.""" + with self._lock: + self._vx = self._vy = self._wz = 0.0 + self._prev_pose = None + self._prev_t = None + + +def _prereq_banner(profile: RobotPlantProfile) -> None: print( f"\n=== HARDWARE MODE ({profile.name}) ===\n" "Prereqs:\n" @@ -249,30 +299,19 @@ def _prereq_banner(profile: RobotProfile) -> None: def _fit_hw( - profile: RobotProfile, + profile: RobotPlantProfile, step_s: float, pre_roll_s: float, warmup_s: float, max_dist: float, ) -> tuple[TwistBasePlantParams, dict, list[dict]]: - 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.geometry_msgs.Vector3 import Vector3 - _prereq_banner(profile) hw_dt = 1.0 / profile.tick_rate_hz - cmd_pub = LCMTransport(profile.cmd_topic, Twist) - odom_sub = LCMTransport(profile.odom_topic, PoseStamped) - lock = threading.Lock() - box: dict = {"pose": None, "t": 0.0} - - def _on_odom(msg) -> None: - with lock: - box["pose"] = msg - box["t"] = time.perf_counter() - odom_sub.subscribe(_on_odom) + # Signal-injection is open-loop and naturally external — we publish + # Twist directly onto the LCM cmd topic without going through the + # coordinator's task graph (the SI is not a task). + cmd_pub = LCMTransport(profile.cmd_topic, Twist) def publish(vx: float, vy: float, wz: float) -> None: cmd_pub.broadcast( @@ -288,21 +327,48 @@ def safe_stop() -> None: publish(0.0, 0.0, 0.0) time.sleep(0.05) - # No custom SIGINT handler: Ctrl-C must raise KeyboardInterrupt so it - # also breaks out of the blocking input() prompt. The try/finally - # below guarantees a zero-Twist stop on any exit (Ctrl-C, q, error). + # Observation goes through an in-process ControlCoordinator with the + # transport_lcm adapter — same path the benchmark uses. JointState + # positions = [x, y, yaw]; body velocity is recovered by pose-diff + # in the observer (transport_lcm.read_velocities returns last-cmd, + # not measured, so we always differentiate pose). + joints = make_twist_base_joints(profile.robot_id) + coord = ControlCoordinator( + tick_rate=profile.tick_rate_hz, + hardware=[ + HardwareComponent( + hardware_id=profile.robot_id, + hardware_type=HardwareType.BASE, + joints=joints, + adapter_type="transport_lcm", + # READ-ONLY: we observe /{robot_id}/odom via this adapter, + # but the SI loop publishes its own Twist on /cmd_vel into + # the operator's coord. If we let this adapter write, it + # would also publish on /{robot_id}/cmd_vel and race the + # operator's coord. + auto_enable=False, + ) + ], + ) + stream = _JointStatePoseStream(joint_names=joints) + unsub = coord.joint_state.subscribe(stream.on_joint_state) + coord.start() - print(f"[hw] waiting up to {warmup_s:.0f}s for {profile.odom_topic} ...") + print( + f"[hw] waiting up to {warmup_s:.0f}s for {profile.odom_topic} (via coord.joint_state) ..." + ) + time.sleep(0.5) # grace: let adapter receive first /odom + one tick deadline = time.perf_counter() + warmup_s while time.perf_counter() < deadline: - with lock: - if box["pose"] is not None: - break + p, _, _ = stream.latest() + if p is not None: + break time.sleep(0.05) - with lock: - if box["pose"] is None: - safe_stop() - raise SystemExit(f"No {profile.odom_topic} — is `dimos run {profile.blueprint}` up?") + if stream.latest()[0] is None: + safe_stop() + unsub() + coord.stop() + raise SystemExit(f"No {profile.odom_topic} — is `dimos run {profile.blueprint}` up?") pooled: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} @@ -327,14 +393,11 @@ def safe_stop() -> None: print(" skipped") continue - est = _PoseVelocityEstimator() # pre-roll zeros (settle + prime estimator) + stream.reset_velocity() t_end = time.perf_counter() + pre_roll_s while time.perf_counter() < t_end: publish(0.0, 0.0, 0.0) - with lock: - p = box["pose"] - est.update(p, time.perf_counter()) time.sleep(hw_dt) # step. Ends on whichever comes first: travelled distance @@ -346,8 +409,10 @@ def safe_stop() -> None: cmd[channel] = amp ts: list[float] = [] ys: list[float] = [] - with lock: - sp = box["pose"] + sp, _, _ = stream.latest() + if sp is None: + print(" [abort] lost odom before step") + continue x0, y0 = sp.position.x, sp.position.y t0 = time.perf_counter() end_reason = "time" @@ -357,8 +422,7 @@ def safe_stop() -> None: if t_rel > step_s: break publish(cmd["vx"], cmd["vy"], cmd["wz"]) - with lock: - p, pt = box["pose"], box["t"] + p, pt, v = stream.latest() if p is None or now - pt > profile.odom_stale_s: print(f" [abort] stale odom ({now - pt:.2f}s)") end_reason = "stale" @@ -367,7 +431,6 @@ def safe_stop() -> None: if dist >= max_dist: end_reason = "dist" break - v = est.update(p, now) ts.append(t_rel) ys.append(v[_CHANNELS.index(channel)]) time.sleep(hw_dt) @@ -408,12 +471,14 @@ def safe_stop() -> None: L=float(np.mean([f.L for f in fits])), ) except KeyboardInterrupt: - safe_stop() + # finally below does safe_stop + unsub + coord.stop — don't duplicate raise SystemExit( "\n[hw] aborted by operator — robot stopped, no artifact written." ) from None finally: safe_stop() + unsub() + coord.stop() # Channels not excited (e.g. vy on a non-strafing robot) are # placeholdered = vx so FF / profile stay sane; flagged in caveats. @@ -431,7 +496,7 @@ def safe_stop() -> None: def main() -> None: ap = argparse.ArgumentParser(description="Twist-base characterization -> tuning artifact") - ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PROFILES)}") + ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PLANT_PROFILES)}") ap.add_argument("--mode", choices=["hw", "self-test"], default="hw") ap.add_argument("--out", default=str(REPORTS_DIR)) ap.add_argument("--robot-id", default=None, help="provenance id (default: profile.robot_id)") diff --git a/dimos/utils/benchmarking/plant.py b/dimos/utils/benchmarking/plant.py index 70a7e90d3c..a8b2e566de 100644 --- a/dimos/utils/benchmarking/plant.py +++ b/dimos/utils/benchmarking/plant.py @@ -18,10 +18,10 @@ 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 config (``RobotProfile`` + -``ROBOT_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. +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 @@ -148,19 +148,25 @@ def step(self, cmd_vx: float, cmd_vy: float, cmd_wz: float, dt: float) -> None: @dataclass(frozen=True) -class RobotProfile: +class RobotPlantProfile: """Everything the characterization + benchmark tools need to know - about a specific velocity-commanded twist base. Add a robot by - appending one instance to ``ROBOT_PROFILES``.""" + 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``. + + ``robot_id`` doubles as the ``hardware_id`` on the ``transport_lcm`` + adapter that the tools use to drive the robot (so the LCM topic + prefix is ``/{robot_id}/{cmd_vel|odom}``). The adapter is always + ``transport_lcm`` — sim/hw differ only in which connection module + the operator brings up on the robot side (the ``blueprint``). + """ # identity / cosmetic name: str robot_id: str # transport / bring-up - cmd_topic: str - odom_topic: str - blueprint: str # the `dimos run ` the operator starts - sim_adapter_key: str + blueprint: str # the `dimos run ` the operator starts (hw) + sim_blueprint: str # the `dimos run ` for sim (FOPDT plant) # physical envelope vx_max: float wz_max: float @@ -173,17 +179,26 @@ class RobotProfile: step_s: float pre_roll_s: float max_dist_m: float - # sim ground truth: self-test + sim adapter + DERIVE ceiling fallback + # Sim ground truth: drives FopdtPlantConnection (sim blueprint) + + # self-test path + DERIVE ceiling fallback. sim_plant: TwistBasePlantParams + @property + def cmd_topic(self) -> str: + """LCM topic the transport_lcm adapter publishes Twist on.""" + return f"/{self.robot_id}/cmd_vel" + + @property + def odom_topic(self) -> str: + """LCM topic the transport_lcm adapter subscribes PoseStamped on.""" + return f"/{self.robot_id}/odom" + -GO2_PROFILE = RobotProfile( +GO2_PLANT_PROFILE = RobotPlantProfile( name="Go2", robot_id="go2", - cmd_topic="/cmd_vel", - odom_topic="/go2/odom", blueprint="unitree-go2-webrtc-keyboard-teleop", - sim_adapter_key="fopdt_sim_twist_base", + sim_blueprint="coordinator-sim-fopdt", vx_max=1.0, wz_max=1.5, tick_rate_hz=10.0, @@ -197,18 +212,18 @@ class RobotProfile: sim_plant=GO2_PLANT_FITTED, ) -ROBOT_PROFILES: dict[str, RobotProfile] = {"go2": GO2_PROFILE} +ROBOT_PLANT_PROFILES: dict[str, RobotPlantProfile] = {"go2": GO2_PLANT_PROFILE} __all__ = [ "GO2_PLANT_FITTED", - "GO2_PROFILE", + "GO2_PLANT_PROFILE", "GO2_VX_RISE", "GO2_WZ_RISE", - "ROBOT_PROFILES", + "ROBOT_PLANT_PROFILES", "FOPDTChannel", "FopdtChannelParams", - "RobotProfile", + "RobotPlantProfile", "TwistBasePlantParams", "TwistBasePlantSim", ] From a675d5b6abb6e44daf27a6a7ab7b99a23bd9daa2 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 16:37:38 -0700 Subject: [PATCH 14/51] update blueprints --- dimos/control/blueprints/mobile.py | 17 +++++++++++++++++ dimos/robot/all_blueprints.py | 2 ++ .../blueprints/basic/unitree_go2_coordinator.py | 1 + 3 files changed, 20 insertions(+) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index c5065ea8d4..9a957e940c 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -20,6 +20,7 @@ dimos run coordinator-flowbase # FlowBase holonomic base (Portal RPC) dimos run coordinator-flowbase-keyboard-teleop # FlowBase + WASD pygame teleop dimos run coordinator-flowbase-nav # FlowBase + FastLio2 + nav stack (click-to-drive) + dimos run coordinator-sim-fopdt # FOPDT sim plant on /go2/cmd_vel|odom (Go2-shaped) """ from __future__ import annotations @@ -36,6 +37,7 @@ from dimos.core.transport import LCMTransport from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.msgs.geometry_msgs.Pose import Pose +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 @@ -43,6 +45,7 @@ from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config from dimos.robot.catalog.ufactory import xarm7 as _catalog_xarm7 +from dimos.robot.sim.fopdt_plant_connection import FopdtPlantConnection from dimos.robot.unitree.g1.config import G1_LOCAL_PLANNER_PRECOMPUTED_PATHS from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.visualization.rerun.bridge import RerunBridgeModule @@ -239,10 +242,24 @@ def _flowbase_twist_base( ) +# FOPDT in-process sim plant exposed on the same LCM topic shape as the +# real Go2 bring-up (/go2/cmd_vel + /go2/odom). Pair with the benchmark / +# characterization tools (sim mode) — they drive transport_lcm with +# hardware_id="go2", so this blueprint is the drop-in stand-in for +# `unitree-go2-webrtc-keyboard-teleop` when no robot is present. +coordinator_sim_fopdt = FopdtPlantConnection.blueprint().transports( + { + ("cmd_vel", Twist): LCMTransport("/go2/cmd_vel", Twist), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } +) + + __all__ = [ "coordinator_flowbase", "coordinator_flowbase_keyboard_teleop", "coordinator_flowbase_nav", "coordinator_mobile_manip_mock", "coordinator_mock_twist_base", + "coordinator_sim_fopdt", ] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 658063f642..dd43cda2fd 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -36,6 +36,7 @@ "coordinator-piper": "dimos.control.blueprints.basic:coordinator_piper", "coordinator-piper-xarm": "dimos.control.blueprints.dual:coordinator_piper_xarm", "coordinator-servo-xarm6": "dimos.control.blueprints.teleop:coordinator_servo_xarm6", + "coordinator-sim-fopdt": "dimos.control.blueprints.mobile:coordinator_sim_fopdt", "coordinator-teleop-dual": "dimos.control.blueprints.teleop:coordinator_teleop_dual", "coordinator-teleop-piper": "dimos.control.blueprints.teleop:coordinator_teleop_piper", "coordinator-teleop-xarm6": "dimos.control.blueprints.teleop:coordinator_teleop_xarm6", @@ -142,6 +143,7 @@ "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", "far-planner": "dimos.navigation.nav_stack.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", + "fopdt-plant-connection": "dimos.robot.sim.fopdt_plant_connection.FopdtPlantConnection", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", "g1-high-level-dds-sdk": "dimos.robot.unitree.g1.effectors.high_level.dds_sdk.G1HighLevelDdsSdk", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index ef8263fbf3..742f601f83 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -51,6 +51,7 @@ type="velocity", joint_names=_go2_joints, priority=10, + velocity_zero_on_timeout=False, ), ], ), From e8db36ee6c5ff140e69105c5f1ba0d4e1052ffed Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 16:39:44 -0700 Subject: [PATCH 15/51] updated readmes --- .../benchmarking/reports/tuning_README.md | 52 ++++++++++++++----- 1 file changed, 38 insertions(+), 14 deletions(-) diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index 7b107ddaef..5ed47320ad 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -3,9 +3,10 @@ Two CLI tools that turn one real measurement of a velocity-commanded mobile base into a single versioned config artifact with every parameter needed to tune its path controller, then validate it on the real robot. -**Robot-agnostic**: everything robot-specific lives in a `RobotProfile` -(`--robot`, default `go2`). Adding a robot = one profile entry (see -*Adding a robot* below); the two commands are otherwise identical. +**Robot-agnostic**: everything robot-specific lives in a +`RobotPlantProfile` (`--robot`, default `go2`). Adding a robot = one +profile entry (see *Adding a robot* below); the two commands are +otherwise identical. ``` characterization --robot R --mode hw ──▶ R_config_hw_*.json (robot-valid) @@ -13,6 +14,16 @@ benchmark --robot R --mode hw --config … ──▶ same file + section 5 "for tolerance X cm, run Y m/s" ``` +Both tools run the baseline path follower **inside a real +`ControlCoordinator`** in this process, driving the existing +`transport_lcm` twist-base adapter. The operator brings up whichever +connection module owns the robot side of the LCM topics in another +terminal — `unitree-go2-webrtc-keyboard-teleop` for hw, the new +`coordinator-sim-fopdt` (an in-process FOPDT plant exposed on the same +`/{robot_id}/cmd_vel|odom`) for sim. **The two modes are architecturally +identical**: same coordinator, same adapter, same task; only the robot +on the other side differs. + **This is a hardware deliverable.** Sim exists only as a plumbing self-test / pre-check and is explicitly stamped not-robot-valid — never tune from it. @@ -82,10 +93,16 @@ consumes (sections 1–4 + 6; section 5 pending; `valid_for_tuning=true`). Channels not excited (e.g. vy on a non-strafing robot) are placeholdered = vx and flagged in the caveats. -`--mode self-test` (no robot): steps the profile's in-process FOPDT sim -plant and recovers it. Proves the measure→fit→derive code runs; artifact -stamped `valid_for_tuning=false`. The pytest/CI path — **not a tuning -artifact**. +`--mode self-test` (no robot, no blueprint, no coord): steps the +profile's in-process FOPDT sim plant and recovers it. Proves the +measure→fit→derive code runs; artifact stamped `valid_for_tuning=false`. +The pytest/CI path — **not a tuning artifact**. + +For a `--mode hw` *style* run against the sim plant (full coordinator + +transport_lcm path, no robot): bring up `coordinator-sim-fopdt` in +terminal 1, then run the benchmark/characterization with `--mode sim` +in terminal 2. Same architectural shape as hw — only the LCM peer +differs. ## Tool 2 — `benchmark` @@ -119,8 +136,11 @@ clobber section 5: is set** (sim-derived gains are meaningless on the real robot). The bare physical-limit run accepts any config. -`--mode sim`: optional fast pre-check against the profile's FOPDT sim -plant. Loudly labelled a pre-check; the map is not a real-robot result. +`--mode sim`: optional fast pre-check. Same baseline + coordinator + +`transport_lcm` path as hw, but the LCM peer is the +`coordinator-sim-fopdt` blueprint (FOPDT plant + odom integrator) +instead of the real Go2 bring-up. Loudly labelled a pre-check; the map +is not a real-robot result. ## Reading the artifact @@ -137,13 +157,17 @@ plant. Loudly labelled a pre-check; the map is not a real-robot result. ## Adding a robot -Append one `RobotProfile` to `ROBOT_PROFILES` in -`dimos/utils/benchmarking/plant.py`: its `cmd_topic` / `odom_topic` / -`blueprint`, `sim_adapter_key` (`fopdt_sim_twist_base`), saturation +Append one `RobotPlantProfile` to `ROBOT_PLANT_PROFILES` in +`dimos/utils/benchmarking/plant.py`: its `robot_id` (= LCM topic prefix += `transport_lcm` adapter `hardware_id`), the hw `blueprint` and the +sim `sim_blueprint` the operator runs in the other terminal, saturation envelope (`vx_max`, `wz_max`), `tick_rate_hz`, `excited_channels` (omit `vy` if it doesn't strafe), `si_amplitudes`, and a `sim_plant` -(`TwistBasePlantParams`) used as the self-test ground truth. Then the -identical two commands with `--robot `. No other code changes. +(`TwistBasePlantParams`) used as the self-test ground truth and by +`FopdtPlantConnection` when the sim blueprint is composed. Then the +identical two commands with `--robot `. For a brand-new sim plant +shape (different topic prefix), add a tiny blueprint mirroring +`coordinator_sim_fopdt` in `dimos/control/blueprints/mobile.py`. ## When to re-run From ed9407a4e33bb072eec078b10bc5233d60bd73e6 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 18:17:46 -0700 Subject: [PATCH 16/51] added a baseline path follower task to the coordinator --- dimos/control/coordinator.py | 18 ++++++++ .../tasks/baseline_path_follower_task.py | 42 ++++++++++++++++++- 2 files changed, 59 insertions(+), 1 deletion(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 4b90c58f62..772269ac35 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -328,6 +328,24 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: ), ) + elif task_type == "baseline_path_follower": + from dimos.control.tasks.baseline_path_follower_task import ( + BaselinePathFollowerTask, + BaselinePathFollowerTaskConfig, + ) + from dimos.core.global_config import global_config as _gc + + # Idle defaults; per-run params come from the tool via the + # task's `configure()` RPC immediately before `start_path()`. + return BaselinePathFollowerTask( + cfg.name, + BaselinePathFollowerTaskConfig( + joint_names=cfg.joint_names, + priority=cfg.priority, + ), + global_config=_gc, + ) + elif task_type == "cartesian_ik": from dimos.control.tasks.cartesian_ik_task import CartesianIKTask, CartesianIKTaskConfig diff --git a/dimos/control/tasks/baseline_path_follower_task.py b/dimos/control/tasks/baseline_path_follower_task.py index 7deda21556..0703e4bcb3 100644 --- a/dimos/control/tasks/baseline_path_follower_task.py +++ b/dimos/control/tasks/baseline_path_follower_task.py @@ -65,6 +65,10 @@ "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 BaselinePathFollowerTaskConfig: @@ -283,9 +287,45 @@ def _step_final_rotation(self) -> tuple[float, float, float]: return float(twist.linear.x), float(twist.linear.y), float(twist.angular.z) # ------------------------------------------------------------------ - # Public API (called by runner) + # Public API (called by runner — typically over RPC from a tool) # ------------------------------------------------------------------ + def configure( + self, + speed: float | None = None, + k_angular: float | None = None, + ff_config: FeedforwardGainConfig | None | object = _UNSET, + velocity_profile_config: VelocityProfileConfig | None | object = _UNSET, + ) -> bool: + """Override per-run knobs before start_path. ``ff_config`` and + ``velocity_profile_config`` 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. + """ + if self.is_active(): + logger.warning( + f"BaselinePathFollowerTask '{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 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 + if 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 + ) + return True + def start_path(self, path: Path, current_odom: PoseStamped) -> bool: if path is None or len(path.poses) < 2: logger.warning(f"BaselinePathFollowerTask '{self._name}': invalid path") From f778684f6c2e3d0d3a574a7e0253cd4ba220215d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 18:19:26 -0700 Subject: [PATCH 17/51] update dbenchmarking tools with new plant profile --- dimos/utils/benchmarking/benchmark.py | 269 ++++++++++--------- dimos/utils/benchmarking/characterization.py | 88 +++--- dimos/utils/benchmarking/plant.py | 103 ++++--- 3 files changed, 254 insertions(+), 206 deletions(-) diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py index f8368a0260..5a7ac55563 100644 --- a/dimos/utils/benchmarking/benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -22,13 +22,12 @@ tolerance->max-safe-speed inversion (artifact section 5). Robot-agnostic: everything robot-specific comes from the ``RobotPlantProfile`` (``--robot``). -Architecturally sim and hw are identical here. The benchmark always -runs the baseline INSIDE a real ``ControlCoordinator`` tick loop driving -the ``transport_lcm`` twist-base adapter. The only thing that changes -between modes is which connection module is on the robot side of the -LCM topics — sim: ``coordinator-sim-fopdt`` (FopdtPlantConnection), hw: -``unitree-go2-webrtc-keyboard-teleop`` (GO2Connection). The operator -brings that up in another terminal; the prereq banner reminds them. +The tool talks to whichever operator coord is up via two well-known +topics: publishes Twist on ``/cmd_vel`` (the coord's ``twist_command`` +In) and subscribes to ``/coordinator/joint_state`` (positions = +[x,y,yaw] from the adapter's read_odometry). Adding a robot = adding a +``RobotPlantProfile`` — no in-process coord, no new blueprint, no +per-robot topic dance. uv run python -m dimos.utils.benchmarking.benchmark \\ --robot go2 --config reports/go2_config_hw_<...>.json --mode hw @@ -50,14 +49,11 @@ matplotlib.use("Agg") import matplotlib.pyplot as plt -from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints +from dimos.control.components import make_twist_base_joints from dimos.control.coordinator import ControlCoordinator -from dimos.control.tasks.baseline_path_follower_task import ( - BaselinePathFollowerTask, - BaselinePathFollowerTaskConfig, -) from dimos.control.tasks.feedforward_gain_compensator import FeedforwardGainConfig -from dimos.core.global_config import global_config +from dimos.core.rpc_client import RPCClient +from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Twist import Twist @@ -75,6 +71,15 @@ ) from dimos.utils.benchmarking.velocity_profile import VelocityProfileConfig +# Well-known topic the operator coord publishes its JointState Out on. +# Positions carry [x,y,yaw] (ConnectedTwistBase populates them from +# adapter.read_odometry). The tool subscribes to this for trajectory +# recording; the baseline task — which actually drives the robot — lives +# inside the operator coord (see TaskConfig type="baseline_path_follower" +# wired into each robot's coord blueprint). +_JOINT_STATE_TOPIC = "/coordinator/joint_state" +_BASELINE_TASK_NAME = "baseline_follower" # task name in operator coord blueprint + _ARRIVED_STATES = frozenset({"arrived", "completed"}) _FAILED_STATES = frozenset({"aborted"}) @@ -243,27 +248,29 @@ def snapshot(self) -> list[TrajectoryTick]: with self._lock: return list(self._ticks) - -def _make_base_component(profile: RobotPlantProfile) -> HardwareComponent: - """In-process transport_lcm base — pubs Twist on /{robot_id}/cmd_vel, - subs PoseStamped on /{robot_id}/odom. Identical in sim and hw; the - only thing that differs is which connection module is the other end - of those topics (the operator's running blueprint).""" - return HardwareComponent( - hardware_id=profile.robot_id, - hardware_type=HardwareType.BASE, - joints=make_twist_base_joints(profile.robot_id), - adapter_type="transport_lcm", - # READ-ONLY: we observe /{robot_id}/odom via this adapter, but the - # tool publishes its own Twist on /cmd_vel into the operator's - # coord. If we let this adapter write, it would also publish on - # /{robot_id}/cmd_vel and race the operator's coord. - auto_enable=False, + def reset_trajectory(self) -> None: + """Clear recorded ticks and the t=0 anchor — called before each + run so each (path, speed) is scored on its own time axis.""" + with self._lock: + self._ticks.clear() + self._t0 = None + + +def _invoke(coord: RPCClient, method: str, **kwargs: object) -> object: + """RPC `task_invoke(_BASELINE_TASK_NAME, method, kwargs)` on the operator + coord. Centralises the .task_invoke wrapping so the run loop reads as + plain method calls on a remote object.""" + return coord.task_invoke( + task_name=_BASELINE_TASK_NAME, + method=method, + kwargs=dict(kwargs), ) def _run_baseline( profile: RobotPlantProfile, + coord: RPCClient, + recorder: _JointStateRecorder, path: NavPath, speed: float, k_angular: float, @@ -272,51 +279,45 @@ def _run_baseline( timeout_s: float, label: str, ) -> tuple[ExecutedTrajectory, NavPath]: - """Stock baseline P-controller inside a real ControlCoordinator, - talking ``transport_lcm`` to whichever connection module the operator - brought up. ``ff_config``/``profile_config`` are OPTIONAL arms - (``None`` = bare = the physical-limit measurement). + """Send a path to the operator coord's ``baseline_follower`` task and + wait for it to terminate. The task is pre-added by the operator's + blueprint (priority 20, claims base/{vx,vy,wz}) so it preempts the + operator's teleop velocity task while a run is active. We only RPC + configure/start/cancel; the coord owns the tick-loop compute and the + adapter that drives the robot. ``ff_config``/``profile_config`` are + optional arms (``None`` = bare = the physical-limit measurement). Path is anchored to the robot's first observed pose so the operator - doesn't have to position the robot precisely — only roughly aim it. - Returns the executed trajectory and the anchored reference path - (scoring + plotting must use this, not the robot-centric input).""" - joints = make_twist_base_joints(profile.robot_id) - coord = ControlCoordinator( - tick_rate=profile.tick_rate_hz, - hardware=[_make_base_component(profile)], - ) - task = BaselinePathFollowerTask( - name=f"baseline_{label}", - config=BaselinePathFollowerTaskConfig( - joint_names=joints, - speed=speed, - k_angular=k_angular, - control_frequency=profile.tick_rate_hz, - ff_config=ff_config, - velocity_profile_config=profile_config, - ), - global_config=global_config, - ) - recorder = _JointStateRecorder(joint_names=joints) - unsub = coord.joint_state.subscribe(recorder.on_joint_state) + only has to roughly aim the robot. Returns the executed trajectory + (recorded from /coordinator/joint_state) and the anchored reference.""" + pose0 = recorder.first_pose(timeout_s=profile.odom_warmup_s) + path_w = _shift_path_to_start_at_pose(path, pose0) + + # Reset accumulated trajectory so each run starts at t=0. + recorder.reset_trajectory() + + if not _invoke( + coord, + "configure", + speed=speed, + k_angular=k_angular, + ff_config=ff_config, + velocity_profile_config=profile_config, + ): + print(f" [{label}] configure rejected — task still active from prior run?") + return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w + + if not _invoke(coord, "start_path", path=path_w, current_odom=pose0): + print(f" [{label}] start_path rejected") + return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w - coord.start() arrived = False - path_w = path + t_start = time.perf_counter() + deadline = t_start + timeout_s + terminated = False try: - pose0 = recorder.first_pose(timeout_s=profile.odom_warmup_s) - path_w = _shift_path_to_start_at_pose(path, pose0) - coord.add_task(task) - if not task.start_path(path_w, pose0): - print(f" [{label}] start_path rejected; aborting run") - return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w - - t_start = time.perf_counter() - deadline = t_start + timeout_s - terminated = False while time.perf_counter() < deadline: - st = task.get_state() + st = _invoke(coord, "get_state") if st in _ARRIVED_STATES: arrived = True terminated = True @@ -328,14 +329,13 @@ def _run_baseline( break time.sleep(0.05) if not terminated: - print(f" [{label}] timeout {timeout_s:.0f}s") + print(f" [{label}] timeout {timeout_s:.0f}s — cancelling") finally: + # Best-effort cancel; safe to ignore if already terminal. try: - task.cancel() + _invoke(coord, "cancel") except Exception: pass - unsub() - coord.stop() return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=arrived), path_w @@ -365,62 +365,77 @@ def _run_ladder( # measurement. FF / velocity profile are opt-in comparison arms. ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) + + # Long-lived: RPC client to the operator's coord + LCM subscription + # to /coordinator/joint_state. Shared across all (path, speed) runs. + coord_rpc: RPCClient = RPCClient(None, ControlCoordinator) + joints = make_twist_base_joints(profile.joints_prefix) + recorder = _JointStateRecorder(joint_names=joints) + js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) + js_unsub = js_sub.subscribe(recorder.on_joint_state) + points: list[OperatingPoint] = [] runs: list[dict] = [] # for the XY trajectory overlay - for name, path in _path_set().items(): - for speed in speeds: - prof_cfg = ( - cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None - ) - if mode == "hw": - resp = ( - input( - f"\n[{name} v={speed:.2f}] reposition+aim robot, " - f"ENTER=run s=skip q=quit: " + try: + for name, path in _path_set().items(): + for speed in speeds: + prof_cfg = ( + cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None + ) + if mode == "hw": + resp = ( + input( + f"\n[{name} v={speed:.2f}] reposition+aim robot, " + f"ENTER=run s=skip q=quit: " + ) + .strip() + .lower() ) - .strip() - .lower() + if resp == "q": + raise KeyboardInterrupt + if resp == "s": + print(" skipped") + continue + traj, ref = _run_baseline( + profile, + coord_rpc, + recorder, + path, + speed, + k_angular, + ff, + prof_cfg, + timeout_s, + f"{name}@{speed:.2f}", ) - if resp == "q": - raise KeyboardInterrupt - if resp == "s": - print(" skipped") - continue - traj, ref = _run_baseline( - profile, - path, - speed, - k_angular, - ff, - prof_cfg, - timeout_s, - f"{name}@{speed:.2f}", - ) - # Score/plot against the executed-frame reference (the anchored path). - s = score_run(ref, traj) - points.append( - OperatingPoint( - path=name, - speed=speed, - cte_max=s.cte_max, - cte_rms=s.cte_rms, - arrived=s.arrived, + # Score/plot against the executed-frame reference (anchored path). + s = score_run(ref, traj) + points.append( + OperatingPoint( + path=name, + speed=speed, + cte_max=s.cte_max, + cte_rms=s.cte_rms, + arrived=s.arrived, + ) ) - ) - runs.append( - { - "path": name, - "speed": speed, - "cte_max": s.cte_max, - "arrived": s.arrived, - "ref": [(p.position.x, p.position.y) for p in ref.poses], - "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], - } - ) - print( - f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " - f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" - ) + runs.append( + { + "path": name, + "speed": speed, + "cte_max": s.cte_max, + "arrived": s.arrived, + "ref": [(p.position.x, p.position.y) for p in ref.poses], + "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], + } + ) + print( + f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " + f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" + ) + finally: + js_unsub() + coord_rpc.stop_rpc_client() return points, runs @@ -533,10 +548,12 @@ def _prereq_banner(profile: RobotPlantProfile, mode: str) -> None: f"\n=== {kind} MODE ({profile.name}) ===\n" f"Prereqs:\n" f" 1. Another terminal: `dimos run {bp}`\n" - f" (publishes {profile.odom_topic}, consumes {profile.cmd_topic}).\n" + f" (its ControlCoordinator publishes {_JOINT_STATE_TOPIC} and\n" + f" hosts the `{_BASELINE_TASK_NAME}` task at priority 20).\n" f" 2. This process: strip /nix/store from LD_LIBRARY_PATH (README).\n" - f"Each (path,speed): reposition+aim, then ENTER. Velocity-commanded\n" - f"baseline runs inside our ControlCoordinator; ticks at {profile.tick_rate_hz:g}Hz.\n" + f"Each (path,speed): reposition+aim, then ENTER. We RPC the operator\n" + f"coord to configure + start_path; the task drives the robot from\n" + f"inside its tick loop. Coord ticks at {profile.tick_rate_hz:g}Hz.\n" ) diff --git a/dimos/utils/benchmarking/characterization.py b/dimos/utils/benchmarking/characterization.py index 5600a3a64f..b74e5ff584 100644 --- a/dimos/utils/benchmarking/characterization.py +++ b/dimos/utils/benchmarking/characterization.py @@ -56,13 +56,20 @@ import matplotlib.pyplot as plt import numpy as np -from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints -from dimos.control.coordinator import ControlCoordinator +from dimos.control.components import make_twist_base_joints from dimos.core.transport import LCMTransport 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.sensor_msgs.JointState import JointState + +# Well-known LCM topics — every ControlCoordinator blueprint honors this +# contract (twist_command In = /cmd_vel, joint_state Out = +# /coordinator/joint_state). Adding a robot to the tools means adding a +# RobotPlantProfile, not a new topic / module / adapter. +_CMD_VEL_TOPIC = "/cmd_vel" +_JOINT_STATE_TOPIC = "/coordinator/joint_state" from dimos.utils.benchmarking.plant import ( ROBOT_PLANT_PROFILES, FopdtChannelParams, @@ -243,8 +250,6 @@ def on_joint_state(self, msg: JointState) -> None: # (emitted before the adapter receives its first /odom) does # not get latched as the start pose. now = time.perf_counter() - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - pose = PoseStamped( ts=now, position=Vector3(x, y, 0.0), @@ -286,15 +291,15 @@ def _prereq_banner(profile: RobotPlantProfile) -> None: f"\n=== HARDWARE MODE ({profile.name}) ===\n" "Prereqs:\n" f" 1. Another terminal: `dimos run {profile.blueprint}`\n" - f" (publishes {profile.odom_topic}, consumes " - f"{profile.cmd_topic}; if it includes a keyboard teleop it must\n" - " be publish-only-when-active so it does not fight the SI\n" - " commands — wrong topic => runtime 'no odom', not an error).\n" - " 2. This process: strip /nix/store from LD_LIBRARY_PATH (README)\n" + f" (its ControlCoordinator listens on {_CMD_VEL_TOPIC} and\n" + f" publishes {_JOINT_STATE_TOPIC} with positions=[x,y,yaw]).\n" + " If it includes a keyboard teleop it must be\n" + " publish-only-when-active so it does not fight the SI cmds.\n" + " 2. This process: strip /nix/store from LD_LIBRARY_PATH (README).\n" "Robot is STOPPED before every step. Reposition it, then press\n" - "ENTER here — the tool then owns the cmd topic for the step.\n" - "Each step ends at --max-dist travelled or --step-s, whichever\n" - "first. Velocity clamped; zero-Twist on exit / Ctrl-C.\n" + "ENTER here — the tool owns the cmd topic for the step. Each step\n" + "ends at --max-dist travelled or --step-s, whichever first.\n" + "Velocity clamped; zero-Twist on exit / Ctrl-C.\n" ) @@ -308,16 +313,21 @@ def _fit_hw( _prereq_banner(profile) hw_dt = 1.0 / profile.tick_rate_hz - # Signal-injection is open-loop and naturally external — we publish - # Twist directly onto the LCM cmd topic without going through the - # coordinator's task graph (the SI is not a task). - cmd_pub = LCMTransport(profile.cmd_topic, Twist) + # Signal-injection is open-loop — publish Twist directly on the coord's + # twist_command topic (/cmd_vel). The operator coord's velocity task + # picks it up and routes through whichever adapter (transport_lcm for + # Go2, flowbase Portal RPC for FlowBase, ...) the operator brought up. + cmd_pub = LCMTransport(_CMD_VEL_TOPIC, Twist) def publish(vx: float, vy: float, wz: float) -> None: cmd_pub.broadcast( None, Twist( - linear=Vector3(_clamp(vx, -profile.vx_max, profile.vx_max), 0.0, 0.0), + linear=Vector3( + _clamp(vx, -profile.vx_max, profile.vx_max), + _clamp(vy, -profile.vx_max, profile.vx_max), + 0.0, + ), angular=Vector3(0.0, 0.0, _clamp(wz, -profile.wz_max, profile.wz_max)), ), ) @@ -327,37 +337,17 @@ def safe_stop() -> None: publish(0.0, 0.0, 0.0) time.sleep(0.05) - # Observation goes through an in-process ControlCoordinator with the - # transport_lcm adapter — same path the benchmark uses. JointState - # positions = [x, y, yaw]; body velocity is recovered by pose-diff - # in the observer (transport_lcm.read_velocities returns last-cmd, - # not measured, so we always differentiate pose). - joints = make_twist_base_joints(profile.robot_id) - coord = ControlCoordinator( - tick_rate=profile.tick_rate_hz, - hardware=[ - HardwareComponent( - hardware_id=profile.robot_id, - hardware_type=HardwareType.BASE, - joints=joints, - adapter_type="transport_lcm", - # READ-ONLY: we observe /{robot_id}/odom via this adapter, - # but the SI loop publishes its own Twist on /cmd_vel into - # the operator's coord. If we let this adapter write, it - # would also publish on /{robot_id}/cmd_vel and race the - # operator's coord. - auto_enable=False, - ) - ], - ) + # Observation: subscribe to the operator coord's /coordinator/joint_state + # Out directly over LCM. JointState positions carry [x, y, yaw] because + # ConnectedTwistBase populates them from adapter.read_odometry(). Body + # velocity recovered by pose-differentiation in the stream (commanded + # velocities in the JointState are not measured). + joints = make_twist_base_joints(profile.joints_prefix) stream = _JointStatePoseStream(joint_names=joints) - unsub = coord.joint_state.subscribe(stream.on_joint_state) - coord.start() + js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) + unsub = js_sub.subscribe(stream.on_joint_state) - print( - f"[hw] waiting up to {warmup_s:.0f}s for {profile.odom_topic} (via coord.joint_state) ..." - ) - time.sleep(0.5) # grace: let adapter receive first /odom + one tick + print(f"[hw] waiting up to {warmup_s:.0f}s for {_JOINT_STATE_TOPIC} ...") deadline = time.perf_counter() + warmup_s while time.perf_counter() < deadline: p, _, _ = stream.latest() @@ -367,8 +357,7 @@ def safe_stop() -> None: if stream.latest()[0] is None: safe_stop() unsub() - coord.stop() - raise SystemExit(f"No {profile.odom_topic} — is `dimos run {profile.blueprint}` up?") + raise SystemExit(f"No {_JOINT_STATE_TOPIC} — is `dimos run {profile.blueprint}` up?") pooled: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} @@ -471,14 +460,13 @@ def safe_stop() -> None: L=float(np.mean([f.L for f in fits])), ) except KeyboardInterrupt: - # finally below does safe_stop + unsub + coord.stop — don't duplicate + # finally below does safe_stop + unsub — don't duplicate raise SystemExit( "\n[hw] aborted by operator — robot stopped, no artifact written." ) from None finally: safe_stop() unsub() - coord.stop() # Channels not excited (e.g. vy on a non-strafing robot) are # placeholdered = vx so FF / profile stay sane; flagged in caveats. diff --git a/dimos/utils/benchmarking/plant.py b/dimos/utils/benchmarking/plant.py index a8b2e566de..408d95f34b 100644 --- a/dimos/utils/benchmarking/plant.py +++ b/dimos/utils/benchmarking/plant.py @@ -27,7 +27,7 @@ from __future__ import annotations from collections import deque -from dataclasses import dataclass +from dataclasses import dataclass, field import math @@ -154,44 +154,53 @@ class RobotPlantProfile: the control-loop knobs that surround it. Add a robot by appending one instance to ``ROBOT_PLANT_PROFILES``. - ``robot_id`` doubles as the ``hardware_id`` on the ``transport_lcm`` - adapter that the tools use to drive the robot (so the LCM topic - prefix is ``/{robot_id}/{cmd_vel|odom}``). The adapter is always - ``transport_lcm`` — sim/hw differ only in which connection module - the operator brings up on the robot side (the ``blueprint``). + 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 + 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 (FOPDT plant) + 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 - wz_max: float - tick_rate_hz: float - odom_warmup_s: float - odom_stale_s: float + 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, ...] # subset of (vx,vy,wz); omit vy => non-strafing - si_amplitudes: dict[str, list[float]] - step_s: float - pre_roll_s: float - max_dist_m: float - # Sim ground truth: drives FopdtPlantConnection (sim blueprint) + - # self-test path + DERIVE ceiling fallback. - sim_plant: TwistBasePlantParams - - @property - def cmd_topic(self) -> str: - """LCM topic the transport_lcm adapter publishes Twist on.""" - return f"/{self.robot_id}/cmd_vel" + 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]} + ) + step_s: float = 8.0 + pre_roll_s: float = 1.0 + max_dist_m: float = 6.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 odom_topic(self) -> str: - """LCM topic the transport_lcm adapter subscribes PoseStamped on.""" - return f"/{self.robot_id}/odom" + def joints_prefix(self) -> str: + return self.joint_prefix if self.joint_prefix is not None else self.robot_id GO2_PLANT_PROFILE = RobotPlantProfile( @@ -199,6 +208,7 @@ def odom_topic(self) -> str: 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, @@ -212,10 +222,43 @@ def odom_topic(self) -> str: sim_plant=GO2_PLANT_FITTED, ) -ROBOT_PLANT_PROFILES: dict[str, RobotPlantProfile] = {"go2": GO2_PLANT_PROFILE} +# 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. +# +# Envelope values are placeholders pending real characterization. The +# vy channel is excited (FlowBase strafes natively) so vy is in the +# excited_channels tuple. sim_plant reuses the Go2 FOPDT shape until a +# FlowBase-specific fit lands — the values are noise for `--mode hw`. +FLOWBASE_PLANT_PROFILE = RobotPlantProfile( + name="FlowBase", + robot_id="flowbase", + blueprint="coordinator-flowbase-keyboard-teleop", + sim_blueprint="coordinator-sim-fopdt", + 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 + si_amplitudes={"vx": [0.2, 0.4, 0.6], "vy": [0.2, 0.4], "wz": [0.3, 0.6, 1.0]}, + step_s=6.0, + pre_roll_s=1.0, + max_dist_m=4.0, + sim_plant=GO2_PLANT_FITTED, # placeholder until FlowBase has its own fit +) + +ROBOT_PLANT_PROFILES: dict[str, RobotPlantProfile] = { + "go2": GO2_PLANT_PROFILE, + "flowbase": FLOWBASE_PLANT_PROFILE, +} __all__ = [ + "FLOWBASE_PLANT_PROFILE", "GO2_PLANT_FITTED", "GO2_PLANT_PROFILE", "GO2_VX_RISE", From 2f7b1ff0e92beed6e88bb4658da30fba87145c32 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 18:19:59 -0700 Subject: [PATCH 18/51] blueprint modification --- dimos/control/blueprints/mobile.py | 89 ++++++++++++++++--- .../basic/unitree_go2_coordinator.py | 9 ++ 2 files changed, 88 insertions(+), 10 deletions(-) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 9a957e940c..c3d0e1f862 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -90,6 +90,15 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=10, + velocity_zero_on_timeout=False, + ), + # Closed-loop path follower used by the benchmark tool. + # Inactive until the tool RPCs configure(...) + start_path(...). + TaskConfig( + name="baseline_follower", + type="baseline_path_follower", + joint_names=_base_joints, + priority=20, ), ], ).transports( @@ -108,6 +117,15 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=10, + velocity_zero_on_timeout=False, + ), + # Closed-loop path follower used by the benchmark tool. + # Inactive until the tool RPCs configure(...) + start_path(...). + TaskConfig( + name="baseline_follower", + type="baseline_path_follower", + joint_names=_base_joints, + priority=20, ), ], ).transports( @@ -232,6 +250,15 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=10, + velocity_zero_on_timeout=False, + ), + # Closed-loop path follower used by the benchmark tool. + # Inactive until the tool RPCs configure(...) + start_path(...). + TaskConfig( + name="baseline_follower", + type="baseline_path_follower", + joint_names=_base_joints, + priority=20, ), ], ).transports( @@ -242,16 +269,58 @@ def _flowbase_twist_base( ) -# FOPDT in-process sim plant exposed on the same LCM topic shape as the -# real Go2 bring-up (/go2/cmd_vel + /go2/odom). Pair with the benchmark / -# characterization tools (sim mode) — they drive transport_lcm with -# hardware_id="go2", so this blueprint is the drop-in stand-in for -# `unitree-go2-webrtc-keyboard-teleop` when no robot is present. -coordinator_sim_fopdt = FopdtPlantConnection.blueprint().transports( - { - ("cmd_vel", Twist): LCMTransport("/go2/cmd_vel", Twist), - ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - } +# FOPDT in-process sim plant + a ControlCoordinator on top, so the +# tuning tools see exactly the same /cmd_vel + /coordinator/joint_state +# contract sim and hw. FopdtPlantConnection exposes /sim/cmd_vel (In) +# and /sim/odom (Out); the coord drives /sim/cmd_vel via its +# transport_lcm adapter (hardware_id="sim"), reads pose back via the +# same adapter's /sim/odom subscription, and publishes JointState + +# hosts the baseline_follower task. Drop-in stand-in for a real robot. +_sim_joints = make_twist_base_joints("sim") + +coordinator_sim_fopdt = ( + autoconnect( + FopdtPlantConnection.blueprint(), + ControlCoordinator.blueprint( + hardware=[ + HardwareComponent( + hardware_id="sim", + hardware_type=HardwareType.BASE, + joints=_sim_joints, + adapter_type="transport_lcm", + ), + ], + tasks=[ + TaskConfig( + name="vel_sim", + type="velocity", + joint_names=_sim_joints, + priority=10, + velocity_zero_on_timeout=False, + ), + TaskConfig( + name="baseline_follower", + type="baseline_path_follower", + joint_names=_sim_joints, + priority=20, + ), + ], + ), + ) + .remappings( + [ + (FopdtPlantConnection, "cmd_vel", "sim_cmd_vel"), + (FopdtPlantConnection, "odom", "sim_odom"), + ] + ) + .transports( + { + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + ("sim_cmd_vel", Twist): LCMTransport("/sim/cmd_vel", Twist), + ("sim_odom", PoseStamped): LCMTransport("/sim/odom", PoseStamped), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } + ) ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index 742f601f83..b821fe8730 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -53,6 +53,15 @@ priority=10, velocity_zero_on_timeout=False, ), + # Closed-loop path follower used by the benchmark tool. + # Inactive until the tool RPCs configure(...) + start_path(...). + # Higher priority than vel_go2 so it preempts teleop during a run. + TaskConfig( + name="baseline_follower", + type="baseline_path_follower", + joint_names=_go2_joints, + priority=20, + ), ], ), ) From 1192d5474f88db1d6f37500a8c9ea18e528a25e4 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 20 May 2026 18:55:08 -0700 Subject: [PATCH 19/51] added vy channel test --- dimos/utils/benchmarking/plant.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/utils/benchmarking/plant.py b/dimos/utils/benchmarking/plant.py index 408d95f34b..16837f21ee 100644 --- a/dimos/utils/benchmarking/plant.py +++ b/dimos/utils/benchmarking/plant.py @@ -214,7 +214,7 @@ def joints_prefix(self) -> str: tick_rate_hz=10.0, odom_warmup_s=10.0, odom_stale_s=1.0, - excited_channels=("vx", "wz"), # Go2 does not strafe in default gait + excited_channels=("vx", "vy", "wz"), si_amplitudes={"vx": [0.3, 0.6, 0.9], "vy": [0.2, 0.4], "wz": [0.4, 0.8, 1.2]}, step_s=8.0, pre_roll_s=1.0, From 0aa09b1e2a7ce6ec9fa128012e80a8ab807eeba7 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 22 May 2026 10:00:34 -0700 Subject: [PATCH 20/51] fix: read_odometry did not convert data from flowbase frame to rep103 --- dimos/hardware/drive_trains/flowbase/adapter.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/drive_trains/flowbase/adapter.py b/dimos/hardware/drive_trains/flowbase/adapter.py index 29a9fdee71..0109ae6c57 100644 --- a/dimos/hardware/drive_trains/flowbase/adapter.py +++ b/dimos/hardware/drive_trains/flowbase/adapter.py @@ -107,7 +107,7 @@ def read_velocities(self) -> list[float]: return self._last_velocities.copy() def read_odometry(self) -> list[float] | None: - """Read odometry from FlowBase as [x, y, theta].""" + """Read odometry from FlowBase as [x, y, theta] in standard frame.""" if not self._connected or not self._client: return None @@ -118,9 +118,13 @@ def read_odometry(self) -> list[float] | None: if odom is None: return None - translation = odom["translation"] # [x, y] - rotation = odom["rotation"] # theta in radians - return [float(translation[0]), float(translation[1]), float(rotation)] + translation = odom["translation"] # [x, y] in FlowBase frame + rotation = odom["rotation"] # theta (rad) in FlowBase frame + return [ + float(translation[0]), + -float(translation[1]), + -float(rotation), + ] except Exception as e: logger.error(f"Error reading FlowBase odometry: {e}") return None From d76adf3c4c667920cd5af20b905c22c2273d5a8b Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 22 May 2026 10:51:13 -0700 Subject: [PATCH 21/51] updated task priority to fix teleop safety override --- dimos/control/blueprints/mobile.py | 12 +++++++++++- .../go2/blueprints/basic/unitree_go2_coordinator.py | 5 ++--- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index c3d0e1f862..7be3503f88 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -140,15 +140,25 @@ def _flowbase_twist_base( ControlCoordinator.blueprint( hardware=[_flowbase_twist_base()], tasks=[ + # task go inactive so control hands back to the path follower. TaskConfig( name="vel_base", type="velocity", joint_names=_base_joints, + priority=20, + velocity_zero_on_timeout=False, + ), + # Closed-loop path follower used by the benchmark tool. Inactive + # until the tool RPCs configure(...) + start_path(...). + TaskConfig( + name="baseline_follower", + type="baseline_path_follower", + joint_names=_base_joints, priority=10, ), ], ), - KeyboardTeleop.blueprint(), + KeyboardTeleop.blueprint(publish_only_when_active=True), ).transports( { ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index b821fe8730..a12c62441f 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -50,17 +50,16 @@ name="vel_go2", type="velocity", joint_names=_go2_joints, - priority=10, + priority=20, velocity_zero_on_timeout=False, ), # Closed-loop path follower used by the benchmark tool. # Inactive until the tool RPCs configure(...) + start_path(...). - # Higher priority than vel_go2 so it preempts teleop during a run. TaskConfig( name="baseline_follower", type="baseline_path_follower", joint_names=_go2_joints, - priority=20, + priority=10, ), ], ), From 90fdffb29fe270ddf41920d1cafbe8bfdd81cc23 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 22 May 2026 14:55:44 -0700 Subject: [PATCH 22/51] fix segmentation fault --- dimos/utils/benchmarking/benchmark.py | 150 ++++++++++++++------------ 1 file changed, 82 insertions(+), 68 deletions(-) diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py index 5a7ac55563..139b8ae5e3 100644 --- a/dimos/utils/benchmarking/benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -39,6 +39,7 @@ from dataclasses import asdict import json import math +import os from pathlib import Path import sys import threading @@ -360,82 +361,72 @@ def _run_ladder( mode: str, use_ff: bool, use_profile: bool, + coord_rpc: RPCClient, + recorder: _JointStateRecorder, ) -> tuple[list[OperatingPoint], list[dict]]: # Bare stock baseline by default: this is the physical-limit # measurement. FF / velocity profile are opt-in comparison arms. ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) - # Long-lived: RPC client to the operator's coord + LCM subscription - # to /coordinator/joint_state. Shared across all (path, speed) runs. - coord_rpc: RPCClient = RPCClient(None, ControlCoordinator) - joints = make_twist_base_joints(profile.joints_prefix) - recorder = _JointStateRecorder(joint_names=joints) - js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) - js_unsub = js_sub.subscribe(recorder.on_joint_state) - points: list[OperatingPoint] = [] runs: list[dict] = [] # for the XY trajectory overlay - try: - for name, path in _path_set().items(): - for speed in speeds: - prof_cfg = ( - cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None - ) - if mode == "hw": - resp = ( - input( - f"\n[{name} v={speed:.2f}] reposition+aim robot, " - f"ENTER=run s=skip q=quit: " - ) - .strip() - .lower() - ) - if resp == "q": - raise KeyboardInterrupt - if resp == "s": - print(" skipped") - continue - traj, ref = _run_baseline( - profile, - coord_rpc, - recorder, - path, - speed, - k_angular, - ff, - prof_cfg, - timeout_s, - f"{name}@{speed:.2f}", - ) - # Score/plot against the executed-frame reference (anchored path). - s = score_run(ref, traj) - points.append( - OperatingPoint( - path=name, - speed=speed, - cte_max=s.cte_max, - cte_rms=s.cte_rms, - arrived=s.arrived, + for name, path in _path_set().items(): + for speed in speeds: + prof_cfg = ( + cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None + ) + if mode == "hw": + resp = ( + input( + f"\n[{name} v={speed:.2f}] reposition+aim robot, " + f"ENTER=run s=skip q=quit: " ) + .strip() + .lower() ) - runs.append( - { - "path": name, - "speed": speed, - "cte_max": s.cte_max, - "arrived": s.arrived, - "ref": [(p.position.x, p.position.y) for p in ref.poses], - "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], - } - ) - print( - f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " - f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" + if resp == "q": + raise KeyboardInterrupt + if resp == "s": + print(" skipped") + continue + traj, ref = _run_baseline( + profile, + coord_rpc, + recorder, + path, + speed, + k_angular, + ff, + prof_cfg, + timeout_s, + f"{name}@{speed:.2f}", + ) + # Score/plot against the executed-frame reference (anchored path). + s = score_run(ref, traj) + points.append( + OperatingPoint( + path=name, + speed=speed, + cte_max=s.cte_max, + cte_rms=s.cte_rms, + arrived=s.arrived, ) - finally: - js_unsub() - coord_rpc.stop_rpc_client() + ) + runs.append( + { + "path": name, + "speed": speed, + "cte_max": s.cte_max, + "arrived": s.arrived, + "ref": [(p.position.x, p.position.y) for p in ref.poses], + "exec": [(tk.pose.position.x, tk.pose.position.y) for tk in traj.ticks], + } + ) + print( + f" {name:14} v={speed:.2f} cte_max={s.cte_max * 100:6.1f}cm " + f"cte_rms={s.cte_rms * 100:6.1f}cm arrived={s.arrived}" + ) return points, runs @@ -614,6 +605,12 @@ def main() -> None: f" controller: {arm_desc}\n" f" k_angular={cfg.recommended_controller.params.get('k_angular')}" ) + coord_rpc: RPCClient = RPCClient(None, ControlCoordinator) + joints = make_twist_base_joints(profile.joints_prefix) + recorder = _JointStateRecorder(joint_names=joints) + js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) + js_unsub = js_sub.subscribe(recorder.on_joint_state) + try: points, runs = _run_ladder( cfg, @@ -623,11 +620,16 @@ def main() -> None: args.mode, use_ff=args.ff, use_profile=args.profile, + coord_rpc=coord_rpc, + recorder=recorder, ) except KeyboardInterrupt: - raise SystemExit( - "\n[hw] aborted by operator — robot stopped, artifact not modified." - ) from None + # Try not to lose accumulated data even on operator quit. + points, runs = [], [] + print("\n[hw] aborted by operator — robot stopped, no artifact written.") + os._exit(1) + + # === ARTIFACTS FIRST (before any teardown that might segfault) === inversion = invert_tolerance(points, tolerances) opm = OperatingPointMap(speeds=speeds, points=points, tolerance_inversion=inversion) @@ -671,6 +673,18 @@ def main() -> None: f"(binding path: {row.binding_path})." ) + # === BEST-EFFORT TEARDOWN (artifacts already on disk) === + for label, cleanup in (("unsubscribe", js_unsub), ("rpc client", coord_rpc.stop_rpc_client)): + try: + cleanup() + except Exception as e: + print(f" [cleanup warning] {label}: {e}") + + # Skip Python interp shutdown to avoid LCM/portal C-library teardown + # segfaults. Artifacts are already saved; nothing useful happens after + # this point. Exit code 0 (success). + os._exit(0) + if __name__ == "__main__": main() From bcff4354dadf1d45871e5dc6e2c635ac7980d246 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 13:50:44 -0700 Subject: [PATCH 23/51] reference governor added --- .../tasks/baseline_path_follower_task.py | 36 +- .../reference_governor/reference_governor.py | 584 +++++++++++++++++ .../test_reference_governor.py | 561 ++++++++++++++++ .../benchmarking/reports/tuning_README.md | 177 ++++++ dimos/utils/benchmarking/scoring.py | 4 +- examples/go2_reference_governor_demo.py | 600 ++++++++++++++++++ 6 files changed, 1952 insertions(+), 10 deletions(-) create mode 100644 dimos/navigation/reference_governor/reference_governor.py create mode 100644 dimos/navigation/reference_governor/test_reference_governor.py create mode 100644 examples/go2_reference_governor_demo.py diff --git a/dimos/control/tasks/baseline_path_follower_task.py b/dimos/control/tasks/baseline_path_follower_task.py index 0703e4bcb3..0c037c9929 100644 --- a/dimos/control/tasks/baseline_path_follower_task.py +++ b/dimos/control/tasks/baseline_path_follower_task.py @@ -58,6 +58,7 @@ if TYPE_CHECKING: from dimos.core.global_config import GlobalConfig from dimos.msgs.nav_msgs.Path import Path + from dimos.navigation.reference_governor import PathSpeedCapProtocol logger = setup_logger() @@ -99,6 +100,7 @@ def __init__( name: str, config: BaselinePathFollowerTaskConfig, global_config: GlobalConfig, + external_profile_cap: PathSpeedCapProtocol | None = None, ) -> None: if len(config.joint_names) != 3: raise ValueError( @@ -120,8 +122,14 @@ def __init__( self._ff: FeedforwardGainCompensator | None = ( FeedforwardGainCompensator(config.ff_config) if config.ff_config else None ) - self._profile_cap: PathSpeedCap | None = ( - PathSpeedCap(config.velocity_profile_config) + # 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 ) @@ -296,12 +304,18 @@ def configure( k_angular: float | None = None, ff_config: FeedforwardGainConfig | None | object = _UNSET, velocity_profile_config: VelocityProfileConfig | None | object = _UNSET, + external_profile_cap: PathSpeedCapProtocol | None | object = _UNSET, ) -> bool: - """Override per-run knobs before start_path. ``ff_config`` and - ``velocity_profile_config`` 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. + """Override per-run knobs before start_path. ``ff_config``, + ``velocity_profile_config`` and ``external_profile_cap`` 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. """ if self.is_active(): logger.warning( @@ -317,7 +331,13 @@ def configure( 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 - if velocity_profile_config is not _UNSET: + # 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) diff --git a/dimos/navigation/reference_governor/reference_governor.py b/dimos/navigation/reference_governor/reference_governor.py new file mode 100644 index 0000000000..bff3bf58bc --- /dev/null +++ b/dimos/navigation/reference_governor/reference_governor.py @@ -0,0 +1,584 @@ +# Copyright 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. + +"""Reference governor: model-based per-waypoint velocity profile. + +Wraps the existing curvature MVC + forward/backward accel passes from +:class:`dimos.control.tasks.velocity_profiler.VelocityProfiler` with one +NEW constraint — a precision cap derived from the FOPDT plant floor: + + v <= e_max / max(tau_vx + L_vx, tau_wz + L_wz) + +(the empirical straight-line CTE floor is ~(tau+L)*v below ω-saturation; +on curved paths the wz-channel lag dominates instead, so the constraint +takes the worse channel — see PrecisionMVC docstring). The artifact's +v_max / ω_max / a_lat caps are unchanged and remain in the constraint +set; the governor composes ``min(all_constraints)`` per waypoint and +runs the existing accel passes. + +An optional closed-loop alpha-feedback variant (config flag ``closed_loop``) +observes per-tick CTE and multiplicatively scales the open-loop output +by alpha ∈ [alpha_min, 1.0]. Shipped opt-in; empirically does NOT converge on +continuously-curving paths (see ``tuning_README.md`` "Closed-loop +alpha-feedback variant — NEGATIVE result" for the data and the structural +follow-up options). + +The governor is a dimos Module (not a ControlTask) — the coordinator's +tick loop has no upstream-non-actuating slot. It exposes the +:class:`PathSpeedCapProtocol` method shape (``for_path``, +``speed_limit_at``, ``cap``) so it can be injected as the baseline +follower's ``_profile_cap`` (the existing per-tick consumption seam). +""" + +from __future__ import annotations + +from collections.abc import Callable, Sequence +from dataclasses import dataclass +import threading +from typing import Any, Protocol, runtime_checkable + +import numpy as np +from numpy.typing import NDArray +from reactivex.disposable import Disposable + +from dimos.control.tasks.velocity_profiler import VelocityProfiler +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Path import Path +from dimos.utils.benchmarking.scoring import nearest_segment +from dimos.utils.benchmarking.tuning import PlantModelDC, TuningConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +# --------------------------------------------------------------------------- +# PathSpeedCap method contract — the consumption seam in BaselinePathFollowerTask. +# --------------------------------------------------------------------------- + + +@runtime_checkable +class PathSpeedCapProtocol(Protocol): + """Duck-type contract for objects that can be installed as the + follower's ``_profile_cap``. Mirrors the shape of + :class:`dimos.utils.benchmarking.velocity_profile.PathSpeedCap`. + """ + + 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]: ... + + +# --------------------------------------------------------------------------- +# Velocity constraint generators — per-waypoint pure-function upper bounds. +# +# Architecturally the solver applies min() over the constraint set then runs +# the existing accel passes. To add a new constraint: drop in one more class, +# include it in the governor's constraint list. No solver change needed. +# --------------------------------------------------------------------------- + + +@dataclass +class ConstraintContext: + """Path-derived context passed to each constraint's upper_bound. + + Bundling curvatures (computed once per path) avoids each curvature- + dependent constraint recomputing the same numbers. + """ + + path: Path + curvatures: NDArray[np.float64] # length len(path.poses); abs curvature in 1/m + plant: PlantModelDC + + +class VelocityConstraint(Protocol): + name: str + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: ... + + +_INF = float("inf") +_KAPPA_EPS = 1e-6 + + +@dataclass +class GeometricMVC: + """Static linear-speed cap (artifact's ``velocity_profile.max_linear_speed``).""" + + v_max: float + name: str = "geometric" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + return float(self.v_max) + + +@dataclass +class SaturationMVC: + """Turn-rate saturation: v <= omega_max / |kappa|. HARD cap — above + this the controller can't track the geometry at all.""" + + omega_max: float + name: str = "saturation" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + kappa = float(ctx.curvatures[s_idx]) + if kappa < _KAPPA_EPS: + return _INF + return self.omega_max / kappa + + +@dataclass +class LateralMVC: + """Lateral-comfort cap: v <= sqrt(a_lat_max / |kappa|).""" + + a_lat_max: float + name: str = "lateral" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + kappa = float(ctx.curvatures[s_idx]) + if kappa < _KAPPA_EPS: + return _INF + return float(np.sqrt(self.a_lat_max / kappa)) + + +@dataclass +class PrecisionMVC: + """Precision cap derived from the FOPDT plant CTE floor: + + v <= e_max / max(tau_vx + L_vx, tau_wz + L_wz) + + The straight-line characterization fits only `(tau_vx + L_vx) * v` as + the CTE floor; that holds on straight segments. On curved segments + the heading-tracking lag (a function of `tau_wz + L_wz`) dominates + instead. Empirically (slalom/smooth_corner/figure_eight sim runs) + using only the vx channel under-predicts CTE on curved paths by ~2x; + taking max(vx_chan, wz_chan) halves the residual at the cost of a + proportionally lower v. + + e_max read via a callable so the governor can hot-update the runtime + corridor half-width without rebuilding the constraint. The bound is + constant across waypoints (κ-independent); the min() in the solver + handles composition with the κ-dependent caps. + """ + + e_max_provider: Callable[[], float] + min_e_max: float = 0.005 # 5 mm floor: keeps v from collapsing to 0 + name: str = "precision" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + e_max = max(float(self.e_max_provider()), self.min_e_max) + tau_plus_L = max( + float(ctx.plant.vx.tau + ctx.plant.vx.L), + float(ctx.plant.wz.tau + ctx.plant.wz.L), + ) + if tau_plus_L < 1e-9: + return _INF + return e_max / tau_plus_L + + +# --------------------------------------------------------------------------- +# Solver: compose constraints, then reuse existing accel passes. +# --------------------------------------------------------------------------- + + +def _path_pts(path: Path) -> NDArray[np.float64]: + return np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) + + +def solve_profile( + path: Path, + plant: PlantModelDC, + constraints: Sequence[VelocityConstraint], + *, + accel_max: float, + decel_max: float, + min_speed: float, + curvatures: NDArray[np.float64] | None = None, + pts: NDArray[np.float64] | None = None, +) -> NDArray[np.float64]: + """Per-waypoint MVC = min(constraints), then forward/backward accel + passes via the existing :class:`VelocityProfiler` helpers. ``pts`` + and ``curvatures`` can be passed in to avoid recomputing on hot + paths (e_max updates with a fixed path).""" + n = len(path.poses) + if n < 2: + return np.array([min_speed], dtype=float) + + if pts is None: + pts = _path_pts(path) + if curvatures is None: + # _compute_curvatures depends only on pts (it's stateless w.r.t. profiler config). + curvatures = VelocityProfiler()._compute_curvatures(pts) + + ctx = ConstraintContext(path=path, curvatures=curvatures, plant=plant) + mvc = np.empty(n, dtype=float) + for i in range(n): + mvc[i] = min(c.upper_bound(ctx, i) for c in constraints) + + # Reuse the forward/backward accel passes verbatim. + profiler = VelocityProfiler( + max_linear_accel=accel_max, + max_linear_decel=decel_max, + ) + v = profiler._acceleration_pass(pts, mvc, forward=True) + v = profiler._acceleration_pass(pts, v, forward=False) + return np.maximum(v, min_speed) + + +# --------------------------------------------------------------------------- +# Module: lifecycle, In streams, atomic-snapshot state. +# --------------------------------------------------------------------------- + + +class ReferenceGovernorConfig(ModuleConfig): + plant_artifact_path: str + e_max_default: float = 0.1 + min_e_max: float = 0.005 + lookahead_pts: int = 8 + + # --- Closed-loop alpha-feedback (OPT-IN; default off = open-loop) ------- + # Background: the open-loop precision bound v ≤ e_max / max(τ+L per + # channel) under-promises CTE on continuously-curving paths by ~2× + # because the FOPDT lag model doesn't see the controller's + # heading-chase dynamics. Closing the loop on measured CTE corrects + # this via a multiplicative scaling factor alpha ∈ [alpha_min, 1.0] applied + # to the open-loop profile output. + closed_loop: bool = False + kp_alpha: float = 4.0 # P gain on (cte - e_max) → -Δalpha + ki_alpha: float = 0.5 # I gain (1/s); produces zero steady-state error + alpha_min: float = 0.2 # floor: never starve v below 20% of open-loop + max_integral: float = 0.5 # anti-windup clamp on the integral state + cte_ema_alpha: float = 0.3 # EMA factor on raw CTE; 0.3 ≈ 3-tick window @ 10Hz + tick_dt_s: float = 0.05 # nominal tick period for integrating error + + +class ReferenceGovernor(Module): + """Per-waypoint velocity-profile producer for the baseline path + follower. Consumes (path, e_max). Outputs are read via the + :class:`PathSpeedCapProtocol` methods on each follower tick. + """ + + config: ReferenceGovernorConfig + + path: In[Path] + corridor_half_width: In[float] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + cfg = self.config + + artifact = TuningConfig.from_json(cfg.plant_artifact_path) + self._tuning_config = artifact + self._plant = artifact.plant + vp = artifact.velocity_profile + self._accel_max = vp.max_linear_accel + self._decel_max = vp.max_linear_decel + self._min_speed = vp.min_speed + self._lookahead_pts = cfg.lookahead_pts + + # State (all guarded by _state_lock). + self._state_lock = threading.Lock() + self._path: Path | None = None + self._pts: NDArray[np.float64] | None = None + self._curvatures: NDArray[np.float64] | None = None + self._profile: NDArray[np.float64] | None = None + if cfg.e_max_default <= 0: + raise ValueError(f"e_max_default must be > 0, got {cfg.e_max_default}") + self._e_max: float = max(cfg.e_max_default, cfg.min_e_max) + + # Constraints — PrecisionMVC reads _e_max via _current_e_max + # under the lock so the constraint always sees a consistent value + # even while recompute is running on another thread. + self._constraints: list[VelocityConstraint] = [ + GeometricMVC(v_max=vp.max_linear_speed), + SaturationMVC(omega_max=vp.max_angular_speed), + LateralMVC(a_lat_max=vp.max_centripetal_accel), + PrecisionMVC(e_max_provider=self._current_e_max, min_e_max=cfg.min_e_max), + ] + + # Closed-loop alpha-feedback state (also guarded by _state_lock). + # When closed_loop=False these stay at their initial values and + # speed_limit_at() short-circuits the feedback path entirely. + self._alpha: float = 1.0 + self._alpha_integral: float = 0.0 + self._cte_filtered: float = 0.0 + + # ----- lifecycle ------------------------------------------------------ + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.path.subscribe(self._on_path))) + self.register_disposable(Disposable(self.corridor_half_width.subscribe(self._on_e_max))) + + @rpc + def stop(self) -> None: + super().stop() + + # ----- imperative API (also RPC-callable) ----------------------------- + + @rpc + def set_path(self, path: Path) -> None: + """Install a new path and compute its profile.""" + self._on_path(path) + + @rpc + def update_e_max(self, value: float) -> None: + """Update the corridor half-width and recompute (if a path is set).""" + self._on_e_max(value) + + # ----- PathSpeedCap interface (duck-typed; called per-tick from the + # follower's compute() on the coordinator tick thread) ---------------- + + def for_path(self, path: Path) -> None: + """Alias of set_path. Lets the governor drop into the follower's + existing PathSpeedCap injection seam (the follower calls + ``cap.for_path(path)`` inside ``start_path``).""" + self._on_path(path) + + def speed_limit_at(self, x: float, y: float) -> float: + # Atomic snapshot under the lock; lock-free work afterwards. + with self._state_lock: + profile = self._profile + pts = self._pts + min_speed = self._min_speed + alpha = self._alpha + if profile is None or pts is None or len(profile) == 0: + # No path installed yet — fall back to the artifact's v_max + # so the follower's cap is a no-op until set_path is called. + return float(self._tuning_config.velocity_profile.max_linear_speed) + + # Closed-loop alpha-feedback: measure CTE from current pose, update alpha + # via PI law, write back atomically. Open-loop short-circuit keeps + # alpha at its initial 1.0 so this becomes a no-op multiplier. + if self.config.closed_loop: + cte_raw = self._measure_cte(x, y, pts) + alpha, integral, filtered = self._update_alpha(cte_raw) + with self._state_lock: + self._alpha = alpha + self._alpha_integral = integral + self._cte_filtered = filtered + + i = int(np.argmin(np.sum((pts - np.array([x, y])) ** 2, axis=1))) + j = min(len(profile), i + max(1, self._lookahead_pts)) + return float(max(alpha * float(np.min(profile[i:j])), min_speed)) + + def cap( + self, x: float, y: float, vx: float, vy: float, wz: float + ) -> tuple[float, float, float]: + 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 + + def speed_at(self, s_idx: int, fallback: float | None = None) -> float: + """Direct index-based lookup (debug/test seam).""" + with self._state_lock: + profile = self._profile + if profile is None or len(profile) == 0: + if fallback is None: + raise RuntimeError("ReferenceGovernor.speed_at: no path installed") + return float(fallback) + idx = max(0, min(s_idx, len(profile) - 1)) + return float(profile[idx]) + + # ----- internals ------------------------------------------------------ + + def _current_e_max(self) -> float: + # No lock here — the constraint is called from within a recompute + # that already holds the snapshot of e_max it was triggered with. + # (Holding the lock here would self-deadlock.) + return self._e_max + + # ----- closed-loop alpha-feedback helpers --------------------------------- + + def _measure_cte(self, x: float, y: float, pts: NDArray[np.float64]) -> float: + """Perpendicular distance from (x, y) to the nearest path segment. + Thin wrapper over scoring.nearest_segment so the governor's + closed-loop math is decoupled from the scoring module's call shape.""" + _, perp_dist, _ = nearest_segment(np.array([x, y], dtype=float), pts) + return float(perp_dist) + + def _update_alpha(self, cte_raw: float) -> tuple[float, float, float]: + """PI feedback on (cte - e_max) with anti-windup + EMA pre-filter. + + Pure function of inputs + the current alpha/integral/filtered_cte + snapshot — caller writes back under _state_lock. Returns + (new_alpha, new_integral, new_filtered_cte). + + Convention: positive error (cte > e_max) shrinks alpha toward + alpha_min; negative error (cte < e_max) grows alpha back toward 1.0. + The 1-sided clamp at alpha=1.0 means the closed-loop never asks for v + ABOVE the open-loop bound — only at-or-below. + """ + cfg = self.config + + # Snapshot current state (atomic; cheap). + with self._state_lock: + integral = self._alpha_integral + filtered_prev = self._cte_filtered + + # 1. EMA-smooth the raw CTE to reject single-tick noise spikes. + ema = float(cfg.cte_ema_alpha) + cte = ema * float(cte_raw) + (1.0 - ema) * filtered_prev + + # 2. Error in metres; positive means we're outside the corridor. + error = cte - self._e_max + dt = float(cfg.tick_dt_s) + + # 3. Tentative integral update + clamp (anti-windup). + integral_candidate = integral + error * dt + max_int = float(cfg.max_integral) + integral_candidate = max(-max_int, min(max_int, integral_candidate)) + + # 4. PI output; clamp alpha into [alpha_min, 1.0]. + alpha_min = float(cfg.alpha_min) + kp = float(cfg.kp_alpha) + ki = float(cfg.ki_alpha) + alpha_raw = 1.0 - kp * error - ki * integral_candidate + alpha_new = max(alpha_min, min(1.0, alpha_raw)) + + # 5. Anti-windup: if alpha saturated against a clamp this tick AND + # the integral move is in the saturating direction, freeze the + # integral. Prevents the integral from winding up while alpha can't + # respond. + if alpha_new == alpha_min and alpha_raw < alpha_min: + # alpha pegged low; integral would push alpha even lower → freeze. + integral_new = integral + elif alpha_new == 1.0 and alpha_raw > 1.0: + integral_new = integral + else: + integral_new = integral_candidate + + return alpha_new, integral_new, cte + + def _on_path(self, path: Path) -> None: + if path is None or len(path.poses) < 2: + logger.warning( + "ReferenceGovernor: ignored invalid path " + f"(poses={0 if path is None else len(path.poses)})" + ) + return + # Compute pts + curvatures outside the lock (pure numpy). + pts = _path_pts(path) + curvatures = VelocityProfiler()._compute_curvatures(pts) + # Lock briefly to snapshot e_max for the recompute. + with self._state_lock: + e_max_snapshot = self._e_max + profile = self._compute_profile(path, pts, curvatures, e_max_snapshot) + with self._state_lock: + self._path = path + self._pts = pts + self._curvatures = curvatures + self._profile = profile + # Reset closed-loop state on every new path so feedback from + # the prior path doesn't carry over (different geometry, fresh + # convergence). + self._alpha = 1.0 + self._alpha_integral = 0.0 + self._cte_filtered = 0.0 + self._log_provenance("path", profile, e_max_snapshot) + + def _on_e_max(self, value: float) -> None: + if not np.isfinite(value) or value <= 0: + logger.warning(f"ReferenceGovernor: ignored non-positive e_max={value}") + return + clamped = max(float(value), self.config.min_e_max) + # Need pts/curvatures/path under the lock; recompute outside. + with self._state_lock: + path = self._path + pts = self._pts + curvatures = self._curvatures + if path is None or pts is None or curvatures is None: + # No path yet — just stash the value; recompute will pick it up + # when set_path is called. + with self._state_lock: + self._e_max = clamped + return + # Set e_max BEFORE solving so PrecisionMVC reads the new value. + with self._state_lock: + self._e_max = clamped + profile = self._compute_profile(path, pts, curvatures, clamped) + with self._state_lock: + self._profile = profile + self._log_provenance("e_max", profile, clamped) + + def _compute_profile( + self, + path: Path, + pts: NDArray[np.float64], + curvatures: NDArray[np.float64], + e_max: float, # unused directly; PrecisionMVC reads via provider + ) -> NDArray[np.float64]: + del e_max # documented above; kept for log signature parity + return solve_profile( + path, + self._plant, + self._constraints, + accel_max=self._accel_max, + decel_max=self._decel_max, + min_speed=self._min_speed, + curvatures=curvatures, + pts=pts, + ) + + def _log_provenance( + self, + trigger: str, + profile: NDArray[np.float64], + e_max: float, + ) -> None: + # Which constraint binds at each waypoint? (Debug surface — count by name.) + with self._state_lock: + curvatures = self._curvatures + path = self._path + if curvatures is None or path is None: + return + ctx = ConstraintContext(path=path, curvatures=curvatures, plant=self._plant) + n = len(profile) + binding_counts: dict[str, int] = {c.name: 0 for c in self._constraints} + for i in range(n): + bounds = [(c.name, c.upper_bound(ctx, i)) for c in self._constraints] + binding = min(bounds, key=lambda t: t[1])[0] + binding_counts[binding] = binding_counts.get(binding, 0) + 1 + logger.info( + "ReferenceGovernor recomputed", + trigger=trigger, + e_max=round(e_max, 4), + v_min=round(float(np.min(profile)), 3), + v_max=round(float(np.max(profile)), 3), + n_waypoints=n, + binding=binding_counts, + ) + + +__all__ = [ + "ConstraintContext", + "GeometricMVC", + "LateralMVC", + "PathSpeedCapProtocol", + "PrecisionMVC", + "ReferenceGovernor", + "ReferenceGovernorConfig", + "SaturationMVC", + "VelocityConstraint", + "solve_profile", +] diff --git a/dimos/navigation/reference_governor/test_reference_governor.py b/dimos/navigation/reference_governor/test_reference_governor.py new file mode 100644 index 0000000000..c8145066ba --- /dev/null +++ b/dimos/navigation/reference_governor/test_reference_governor.py @@ -0,0 +1,561 @@ +# Copyright 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. + +from __future__ import annotations + +from collections.abc import Generator +import math +from pathlib import Path as FsPath +import threading +import time + +import numpy as np +import pytest + +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.navigation.reference_governor.reference_governor import ( + ConstraintContext, + GeometricMVC, + LateralMVC, + PrecisionMVC, + ReferenceGovernor, + SaturationMVC, + solve_profile, +) +from dimos.utils.benchmarking.paths import circle, single_corner, straight_line +from dimos.utils.benchmarking.tuning import ( + FeedforwardDC, + FopdtChannelDC, + PlantModelDC, + Provenance, + RecommendedControllerDC, + TuningConfig, + VelocityProfileDC, +) + +# --------------------------------------------------------------------------- +# Fixtures +# --------------------------------------------------------------------------- + + +def _plant() -> PlantModelDC: + """Go2-class FOPDT plant. + vx: tau+L = 0.46s; wz: tau+L = 0.65s. + PrecisionMVC takes max() across channels, so 0.65 is the divisor.""" + return PlantModelDC( + vx=FopdtChannelDC(K=0.922, tau=0.4, L=0.06), + vy=FopdtChannelDC(K=1.0, tau=0.4, L=0.06), + wz=FopdtChannelDC(K=2.45, tau=0.6, L=0.05), + ) + + +def _ctx(path: Path) -> ConstraintContext: + from dimos.control.tasks.velocity_profiler import VelocityProfiler + + pts = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) + curvatures = VelocityProfiler()._compute_curvatures(pts) + return ConstraintContext(path=path, curvatures=curvatures, plant=_plant()) + + +@pytest.fixture() +def artifact_path(tmp_path: FsPath) -> FsPath: + """Write a minimal valid TuningConfig artifact to tmp_path.""" + cfg = TuningConfig( + provenance=Provenance( + robot_id="go2-test", + surface="test", + mode="default", + date="2026-05-25", + git_sha="testtest", + sim_or_hw="hw", + ), + plant=_plant(), + feedforward=FeedforwardDC(K_vx=1.085, K_vy=1.0, K_wz=0.408), + velocity_profile=VelocityProfileDC( + max_linear_speed=1.0, + max_angular_speed=1.275, + max_centripetal_accel=1.0, + max_linear_accel=2.5, + max_linear_decel=5.0, + ), + recommended_controller=RecommendedControllerDC(), + caveats=[], + ) + p = tmp_path / "test_config.json" + cfg.to_json(p) + return p + + +@pytest.fixture() +def governor(artifact_path: FsPath) -> Generator[ReferenceGovernor, None, None]: + gov = ReferenceGovernor( + plant_artifact_path=str(artifact_path), + e_max_default=0.05, + ) + try: + yield gov + finally: + gov._close_module() + + +# --------------------------------------------------------------------------- +# Constraint generators — pure-function tests +# --------------------------------------------------------------------------- + + +class TestGeometricMVC: + def test_returns_v_max_everywhere(self) -> None: + ctx = _ctx(straight_line(length=2.0)) + c = GeometricMVC(v_max=0.7) + assert c.upper_bound(ctx, 0) == pytest.approx(0.7) + assert c.upper_bound(ctx, len(ctx.curvatures) // 2) == pytest.approx(0.7) + assert c.upper_bound(ctx, len(ctx.curvatures) - 1) == pytest.approx(0.7) + + +class TestSaturationMVC: + def test_infinite_at_zero_curvature(self) -> None: + ctx = _ctx(straight_line(length=2.0)) + c = SaturationMVC(omega_max=1.5) + # All curvatures should be ~0 on a straight line. + for i in range(len(ctx.curvatures)): + assert c.upper_bound(ctx, i) == float("inf") + + def test_omega_over_kappa_on_circle(self) -> None: + # Circle of radius R: the existing _compute_curvatures uses a + # (d1+d2) arc-length denominator (2-step), so reported kappa is + # ~1/(2R) = 0.5 for R=1 — half the textbook 1/R. The whole + # tuning pipeline is calibrated against this convention; our + # constraints follow it consistently. + ctx = _ctx(circle(radius=1.0, n_points=200)) + c = SaturationMVC(omega_max=1.275) + v = c.upper_bound(ctx, len(ctx.curvatures) // 2) + # omega/kappa = 1.275 / 0.5 = 2.55 + assert v == pytest.approx(2.55, rel=0.05) + + +class TestLateralMVC: + def test_infinite_at_zero_curvature(self) -> None: + ctx = _ctx(straight_line(length=2.0)) + c = LateralMVC(a_lat_max=1.0) + for i in range(len(ctx.curvatures)): + assert c.upper_bound(ctx, i) == float("inf") + + def test_sqrt_a_over_kappa_on_circle(self) -> None: + # Same kappa convention as SaturationMVC: kappa ~ 1/(2R) = 0.5 for R=1. + ctx = _ctx(circle(radius=1.0, n_points=200)) + c = LateralMVC(a_lat_max=1.0) + v = c.upper_bound(ctx, len(ctx.curvatures) // 2) + # sqrt(a_lat / kappa) = sqrt(1.0 / 0.5) = sqrt(2) ≈ 1.414 + assert v == pytest.approx(math.sqrt(2.0), rel=0.05) + + +class TestPrecisionMVC: + def test_constant_across_waypoints(self) -> None: + ctx = _ctx(single_corner(leg_length=2.0, angle_deg=90.0)) + c = PrecisionMVC(e_max_provider=lambda: 0.05) + # max(vx tau+L, wz tau+L) = max(0.46, 0.65) = 0.65 + # → 0.05/0.65 ≈ 0.0769 + expected = 0.05 / 0.65 + for i in (0, len(ctx.curvatures) // 2, len(ctx.curvatures) - 1): + assert c.upper_bound(ctx, i) == pytest.approx(expected, rel=1e-6) + + def test_min_floor_when_e_max_is_tiny(self) -> None: + ctx = _ctx(straight_line(length=1.0)) + c = PrecisionMVC(e_max_provider=lambda: 0.0001, min_e_max=0.005) + # 0.005/0.65 ≈ 0.0077 — the e_max floor wins. + assert c.upper_bound(ctx, 0) == pytest.approx(0.005 / 0.65, rel=1e-6) + + def test_reads_provider_at_call_time(self) -> None: + ctx = _ctx(straight_line(length=1.0)) + state = {"e": 0.05} + c = PrecisionMVC(e_max_provider=lambda: state["e"]) + v0 = c.upper_bound(ctx, 0) + state["e"] = 0.10 + v1 = c.upper_bound(ctx, 0) + assert v1 == pytest.approx(v0 * 2.0, rel=1e-6) + + +# --------------------------------------------------------------------------- +# Solver +# --------------------------------------------------------------------------- + + +class TestSolveProfile: + def test_straight_line_clamped_by_precision(self) -> None: + path = straight_line(length=2.0) + plant = _plant() + constraints = [ + GeometricMVC(v_max=1.0), + SaturationMVC(omega_max=1.275), + LateralMVC(a_lat_max=1.0), + PrecisionMVC(e_max_provider=lambda: 0.05), + ] + v = solve_profile( + path, + plant, + constraints, + accel_max=2.5, + decel_max=5.0, + min_speed=0.05, + ) + # On a straight line, only GeometricMVC and PrecisionMVC are + # finite; precision is tighter (0.109 < 1.0). + assert np.all(v == pytest.approx(0.05 / 0.65, rel=1e-4)) + + def test_short_path_returns_min_speed(self) -> None: + # Path with a single pose — degenerate, solver returns [min_speed]. + path = Path( + poses=[ + PoseStamped( + position=Vector3(0.0, 0.0, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), + ) + ] + ) + plant = _plant() + constraints: list = [GeometricMVC(v_max=1.0)] + v = solve_profile( + path, + plant, + constraints, + accel_max=2.5, + decel_max=5.0, + min_speed=0.05, + ) + assert len(v) == 1 + assert v[0] == pytest.approx(0.05) + + def test_corner_slows_at_curvature(self) -> None: + path = single_corner(leg_length=2.0, angle_deg=90.0) + plant = _plant() + constraints = [ + GeometricMVC(v_max=1.0), + SaturationMVC(omega_max=1.275), + LateralMVC(a_lat_max=1.0), + PrecisionMVC(e_max_provider=lambda: 0.5), # loose enough not to bind on legs + ] + v = solve_profile( + path, + plant, + constraints, + accel_max=2.5, + decel_max=5.0, + min_speed=0.05, + ) + # Far from corner v should ~ geometric cap, at corner v should drop. + assert v.min() < v.max(), "expected corner to slow profile" + + def test_min_speed_floor(self) -> None: + # Sharp corner + impossibly tight e_max should still floor at min_speed. + path = single_corner(leg_length=1.0, angle_deg=170.0) + constraints = [ + GeometricMVC(v_max=1.0), + SaturationMVC(omega_max=1.275), + LateralMVC(a_lat_max=1.0), + PrecisionMVC(e_max_provider=lambda: 0.001, min_e_max=0.001), + ] + v = solve_profile( + path, + _plant(), + constraints, + accel_max=2.5, + decel_max=5.0, + min_speed=0.05, + ) + assert np.all(v >= 0.05 - 1e-9) + + +# --------------------------------------------------------------------------- +# ReferenceGovernor — Module integration +# --------------------------------------------------------------------------- + + +class TestReferenceGovernor: + def test_no_path_returns_v_max_fallback(self, governor: ReferenceGovernor) -> None: + # Before any path is set, speed_limit_at falls back to artifact v_max. + assert governor.speed_limit_at(0.0, 0.0) == pytest.approx(1.0) + + def test_set_path_then_speed_limit_drops_to_precision( + self, governor: ReferenceGovernor + ) -> None: + governor.set_path(straight_line(length=2.0)) + # e_max_default=0.05 → 0.05/0.46 ≈ 0.10870 + v = governor.speed_limit_at(1.0, 0.0) + assert v == pytest.approx(0.05 / 0.65, rel=1e-3) + + def test_update_e_max_recomputes(self, governor: ReferenceGovernor) -> None: + governor.set_path(straight_line(length=2.0)) + v0 = governor.speed_limit_at(1.0, 0.0) + governor.update_e_max(0.20) + v1 = governor.speed_limit_at(1.0, 0.0) + # e_max quadrupled → v_cap should quadruple (still below geometric cap of 1.0) + assert v1 == pytest.approx(min(4 * v0, 1.0), rel=1e-3) + + def test_update_e_max_clamps_below_floor(self, governor: ReferenceGovernor) -> None: + # Two floors stack: min_e_max (governor, 5 mm) AND the artifact's + # velocity_profile.min_speed (solver, 0.05 m/s here). For a Go2 + # plant with max(vx,wz)tau+L=0.65s, 0.005/0.65 ≈ 0.008 m/s — below the + # solver's min_speed, so the output floors at min_speed. + governor.set_path(straight_line(length=2.0)) + governor.update_e_max(0.0001) # below min_e_max=0.005 + v = governor.speed_limit_at(1.0, 0.0) + assert v == pytest.approx(0.05, rel=1e-6) # solver min_speed floor + # Sanity: not zero, not NaN. + assert v > 0.0 + + def test_update_e_max_rejects_non_positive(self, governor: ReferenceGovernor) -> None: + governor.set_path(straight_line(length=2.0)) + before = governor.speed_limit_at(1.0, 0.0) + governor.update_e_max(-1.0) + governor.update_e_max(0.0) + governor.update_e_max(float("nan")) + after = governor.speed_limit_at(1.0, 0.0) + assert before == pytest.approx(after, rel=1e-6) + + def test_cap_preserves_direction_when_under_limit(self, governor: ReferenceGovernor) -> None: + governor.set_path(straight_line(length=2.0)) + # Commanded vx already below vlim → pass-through. + vlim = governor.speed_limit_at(1.0, 0.0) + vx, vy, wz = governor.cap(1.0, 0.0, vlim * 0.5, 0.0, 0.0) + assert vx == pytest.approx(vlim * 0.5) + assert vy == pytest.approx(0.0) + assert wz == pytest.approx(0.0) + + def test_cap_scales_overcommanded_geometry_preserved(self, governor: ReferenceGovernor) -> None: + governor.set_path(straight_line(length=2.0)) + vlim = governor.speed_limit_at(1.0, 0.0) + # Command 2x vlim with some wz → output should be vlim with wz scaled + # by the same factor so turn radius is preserved. + out_vx, out_vy, out_wz = governor.cap(1.0, 0.0, 2 * vlim, 0.0, 1.0) + assert out_vx == pytest.approx(vlim, rel=1e-3) + assert out_wz == pytest.approx(0.5, rel=1e-3) # scaled by vlim / (2*vlim) = 0.5 + + def test_speed_at_index_lookup(self, governor: ReferenceGovernor) -> None: + path = straight_line(length=2.0) + governor.set_path(path) + v_mid = governor.speed_at(len(path.poses) // 2) + assert v_mid == pytest.approx(0.05 / 0.65, rel=1e-3) + + def test_speed_at_fallback_when_no_path(self, governor: ReferenceGovernor) -> None: + assert governor.speed_at(5, fallback=0.42) == pytest.approx(0.42) + with pytest.raises(RuntimeError): + governor.speed_at(0) + + def test_invalid_path_ignored(self, governor: ReferenceGovernor) -> None: + # Path with 0 or 1 poses is rejected; pre-state preserved. + before = governor.speed_limit_at(1.0, 0.0) + governor.set_path(Path(poses=[])) + governor.set_path(Path(poses=[straight_line(length=0.1).poses[0]])) + after = governor.speed_limit_at(1.0, 0.0) + assert before == pytest.approx(after, rel=1e-6) + + +class TestClosedLoopAlpha: + """alpha-feedback unit tests. The PI law is exercised by directly poking + ``_update_alpha`` (pure function) and by driving ``speed_limit_at`` + with synthetic poses that land at known perpendicular distances from + a straight reference path.""" + + @pytest.fixture() + def closed_loop_governor( + self, artifact_path: FsPath + ) -> Generator[ReferenceGovernor, None, None]: + gov = ReferenceGovernor( + plant_artifact_path=str(artifact_path), + e_max_default=0.05, + closed_loop=True, + # Tighter gains than defaults so unit tests converge in + # 10s-100s of synthetic ticks rather than seconds. + kp_alpha=4.0, + ki_alpha=0.5, + alpha_min=0.2, + max_integral=0.5, + cte_ema_alpha=1.0, # disable filter to make tests deterministic + tick_dt_s=0.05, + ) + try: + yield gov + finally: + gov._close_module() + + def test_closed_loop_off_alpha_stays_one(self, governor: ReferenceGovernor) -> None: + # Sanity: with closed_loop=False, speed_limit_at never touches alpha. + governor.set_path(straight_line(length=2.0)) + # Query at a series of poses — including off-path ones that would + # produce nonzero CTE if measured. + v_a = governor.speed_limit_at(1.0, 0.0) + v_b = governor.speed_limit_at(1.0, 0.2) # 20cm off path + v_c = governor.speed_limit_at(1.0, -0.5) + # All identical (open-loop is pose-independent past nearest-idx). + assert v_a == pytest.approx(v_b, rel=1e-6) + assert v_b == pytest.approx(v_c, rel=1e-6) + # And alpha never moved. + assert governor._alpha == pytest.approx(1.0) + + def test_alpha_decreases_on_excess_cte(self, closed_loop_governor: ReferenceGovernor) -> None: + gov = closed_loop_governor + gov.set_path(straight_line(length=2.0)) + # Feed cte = 2 * e_max = 0.10m for 5 ticks. + prev_alpha = 1.0 + for _ in range(5): + alpha, integral, filtered = gov._update_alpha(cte_raw=0.10) + with gov._state_lock: + gov._alpha = alpha + gov._alpha_integral = integral + gov._cte_filtered = filtered + assert alpha <= prev_alpha + 1e-9 # monotonic decrease + prev_alpha = alpha + assert gov._alpha < 1.0 + assert gov._alpha >= gov.config.alpha_min + + def test_alpha_recovers_when_cte_low(self, closed_loop_governor: ReferenceGovernor) -> None: + gov = closed_loop_governor + gov.set_path(straight_line(length=2.0)) + # Drive alpha down with high CTE. + for _ in range(10): + a, i, f = gov._update_alpha(cte_raw=0.15) + with gov._state_lock: + gov._alpha = a + gov._alpha_integral = i + gov._cte_filtered = f + depressed = gov._alpha + assert depressed < 0.9, f"setup failed: alpha={depressed}" + # Now feed cte well below e_max → alpha should rise back. + for _ in range(20): + a, i, f = gov._update_alpha(cte_raw=0.01) + with gov._state_lock: + gov._alpha = a + gov._alpha_integral = i + gov._cte_filtered = f + assert gov._alpha > depressed + + def test_anti_windup_at_saturation(self, closed_loop_governor: ReferenceGovernor) -> None: + gov = closed_loop_governor + gov.set_path(straight_line(length=2.0)) + # Feed a catastrophic CTE (10x e_max) for many ticks. alpha will peg + # at alpha_min; the integral must NOT keep accumulating past the + # max_integral clamp. + for _ in range(500): + a, i, f = gov._update_alpha(cte_raw=0.50) + with gov._state_lock: + gov._alpha = a + gov._alpha_integral = i + gov._cte_filtered = f + assert gov._alpha == pytest.approx(gov.config.alpha_min) + assert abs(gov._alpha_integral) <= gov.config.max_integral + 1e-9 + + def test_alpha_resets_on_set_path(self, closed_loop_governor: ReferenceGovernor) -> None: + gov = closed_loop_governor + gov.set_path(straight_line(length=2.0)) + # Drive alpha down. + for _ in range(20): + a, i, f = gov._update_alpha(cte_raw=0.20) + with gov._state_lock: + gov._alpha = a + gov._alpha_integral = i + gov._cte_filtered = f + assert gov._alpha < 1.0 + # New path → fresh alpha, integral, filter. + gov.set_path(circle(radius=1.0, n_points=120)) + assert gov._alpha == pytest.approx(1.0) + assert gov._alpha_integral == pytest.approx(0.0) + assert gov._cte_filtered == pytest.approx(0.0) + + def test_speed_limit_at_uses_alpha(self, closed_loop_governor: ReferenceGovernor) -> None: + gov = closed_loop_governor + gov.set_path(straight_line(length=2.0)) + # On-path query (alpha==1.0 at start; alpha update pushes back to 1.0). + v_on_path = gov.speed_limit_at(1.0, 0.0) + # Set both alpha=0.5 AND a consistent integral state so the alpha update + # at error=0 keeps alpha at 0.5 (the integral cancels the +Δalpha the P + # term would otherwise inject toward 1.0). With kp=4, ki=0.5, + # error=0 ⟹ alpha_raw = 1.0 - 0 - 0.5·I = 0.5 ⇒ I = 1.0. Then + # max_integral=0.5 clamps to 0.5, giving alpha_raw = 0.75 → alpha=0.75. + # Just assert that v scales with alpha after one tick (not exactly + # 0.5x, but strictly less than the on-path alpha=1.0 case). + with gov._state_lock: + gov._alpha = 0.5 + gov._alpha_integral = 1.0 # gets clamped to max_integral=0.5 + v_with_alpha_low = gov.speed_limit_at(1.0, 0.0) + assert v_with_alpha_low < v_on_path + + def test_ema_filter_dampens_spikes(self, artifact_path: FsPath) -> None: + # Use the public ctor with EMA enabled (cte_ema_alpha < 1). + gov = ReferenceGovernor( + plant_artifact_path=str(artifact_path), + e_max_default=0.05, + closed_loop=True, + cte_ema_alpha=0.2, # heavy smoothing + ) + try: + gov.set_path(straight_line(length=2.0)) + # Single CTE spike at 0.3m followed by zeros. + inputs = [0.30] + [0.0] * 10 + filtered_history = [] + for cte_raw in inputs: + a, i, f = gov._update_alpha(cte_raw=cte_raw) + with gov._state_lock: + gov._alpha = a + gov._alpha_integral = i + gov._cte_filtered = f + filtered_history.append(f) + # First filtered value < raw spike (low-pass). + assert filtered_history[0] < 0.30 + # Filter decays back toward zero (subsequent zeros). + assert filtered_history[-1] < filtered_history[0] + finally: + gov._close_module() + + +class TestConcurrentUpdates: + """Atomic-snapshot test: hammer update_e_max from one thread while + cap() reads from another. Should never crash or return torn state. + """ + + def test_concurrent_update_and_read(self, governor: ReferenceGovernor) -> None: + governor.set_path(circle(radius=1.0, n_points=200)) + stop = threading.Event() + errors: list[BaseException] = [] + + def writer() -> None: + try: + e_values = [0.02, 0.05, 0.10, 0.20, 0.50] + i = 0 + while not stop.is_set(): + governor.update_e_max(e_values[i % len(e_values)]) + i += 1 + except BaseException as e: + errors.append(e) + + def reader() -> None: + try: + while not stop.is_set(): + out = governor.cap(0.5, 0.5, 1.0, 0.0, 0.5) + # All outputs must be finite and bounded. + assert all(math.isfinite(v) for v in out) + assert abs(out[0]) <= 1.0 + 1e-6 + except BaseException as e: + errors.append(e) + + threads = [threading.Thread(target=writer), threading.Thread(target=reader)] + for t in threads: + t.start() + time.sleep(0.5) + stop.set() + for t in threads: + t.join(timeout=2.0) + assert not errors, f"Concurrent execution raised: {errors}" diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index 5ed47320ad..a7810581cd 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -193,3 +193,180 @@ above, not pytest. The MPC/RPP/Lyapunov bake-off, command smoothers, sweeps, and plotting R&D were the evidence for "baseline + FF + curvature profile"; they are the appendix, archived off-repo, not the product. + +## Reference governor (precision → per-waypoint v) + +The artifact is the *static* tuning. The reference governor is the +*runtime* layer that consumes a corridor half-width `e_max` (metres) +and produces a per-waypoint velocity profile. It is intentionally +**open-loop, model-based** — no measured-CTE feedback, no new +controller, no toppra; just one new closed-form constraint composed +with the existing curvature MVC + accel passes. + +Data flow: + +``` + ┌──────────────────────────────────────────────┐ +nav path ──▶│ ReferenceGovernor (Module) │ + │ • Loads TuningConfig artifact (plant + caps)│ +e_max ─────▶│ • Composes 4 constraints (one is NEW): │ + │ v ≤ v_max (geometric) │ + │ v ≤ ω_max / κ (saturation) │ + │ v ≤ √(a_lat / κ) (lateral) │ + │ v ≤ e_max / max(τ+L per channel) │ + │ ── precision; NEW │ + │ • Forward/backward accel passes (reused) │ + │ • Atomic-snapshot read API │ + └──────────────────────────────────────────────┘ + │ + ▼ installed as external_profile_cap + ┌──────────────────────────────────────────────┐ + │ BaselinePathFollowerTask │ + │ (control law unchanged; only cap source │ + │ swaps from VelocityProfileConfig→governor) │ + └──────────────────────────────────────────────┘ +``` + +### The precision constraint + +The straight-line CTE floor of a velocity-commanded FOPDT base is +`(τ_vx + L_vx) · v` (issue #921 characterization, see +`project_go2_plant_and_diagnostic`). On *curved* segments the dominant +lag is the wz channel rather than vx — the heading-tracking lag of +`τ_wz + L_wz` produces a comparable CTE term. Empirically (sim runs +on `smooth_corner`, `slalom`, `figure_eight`), using only the vx +channel under-predicts CTE on curved paths by ~2×. The shipped +constraint takes the worse channel: + + v ≤ e_max / max(τ_vx + L_vx, τ_wz + L_wz) + +For the vendored Go2 plant this gives `max(0.46, 0.65) = 0.65 s`, so +e_max=5 cm caps v at 0.077 m/s (was 0.109 m/s under the vx-only +formula). This is the *only* new math. The other three caps already +lived in `velocity_profiler.py` (geometric/lateral) or in the artifact +(saturation envelope). The constraint set composes with `min()` per +waypoint; the existing accel passes run on top. + +The remaining residual on continuously-curving paths (~30% above the +predicted floor) reflects closed-loop heading-chase dynamics that no +single-segment FOPDT model captures. Closing the loop on measured CTE +is the next layer — see the demo's `cte_max` print for the residual on +each path. + +### Where it lives + +| Path | What | +|---|---| +| `dimos/navigation/reference_governor/reference_governor.py` | Module + 4 constraint classes + `solve_profile` | +| `dimos/control/tasks/baseline_path_follower_task.py` | Added `external_profile_cap` injection seam (control law unchanged) | +| `examples/go2_reference_governor_demo.py` | End-to-end demo (in-process FOPDT sim) | + +### Demo + +``` +# Static e_max (5 cm corridor on the canonical 90° corner) +uv run python examples/go2_reference_governor_demo.py --path single_corner --e-max 0.05 + +# Time-varying e_max (loose → tight every 2s) — exercises the +# atomic-snapshot recompute path on a hot stream +uv run python examples/go2_reference_governor_demo.py --path circle --mode square-wave \ + --e-max-high 0.10 --e-max-low 0.02 --period 4.0 +``` + +A `*.png` plot is written to `/tmp/reference_governor_demo.png` by +default: reference path + executed trajectory, plus an e_max(t) vs +commanded `|vx|(t)` overlay in square-wave mode. + +### Open-loop assumption — what the calibration plot would tell you + +The model predicts `cte ≈ max(τ_vx+L_vx, τ_wz+L_wz) · v_binding`. The +demo prints actual `cte_max` from `score_run` so you can compare +against the predicted floor for each `e_max`. On the vendored Go2 +plant (max τ+L = 0.65 s), `--e-max 0.05` predicts ~5 cm CTE; sim runs +on `single_corner` measure ~8 cm and on `smooth_corner` measure ~7 cm. +The ~30–50% residual is the heading-chase term the open-loop model +doesn't see. + +For a systematic precision-vs-speed sweep, lean on the existing +benchmark harness: + +``` +uv run python -m dimos.utils.benchmarking.benchmark \ + --robot go2 --config --mode sim --profile +``` + +(the `--profile` arm exercises the static curvature profiler — directly +analogous to the governor in the `e_max → ∞` limit). For the +governor's precision axis specifically, drive the demo script across a +grid of `--e-max` values and read off the per-run `cte_max` it prints. + +### Scope honesty + +The constraint set is validated against the same path battery used by +the static tuning (`straight_line`, `single_corner`, `square`, `circle`, +`smooth_corner` etc.). For geometries with materially different local +curvature distributions (cusps, near-zero-radius corners no arc-radius +blend smooths), the characterization → derive → governor chain must be +re-checked. The governor is open-loop by design: it does *not* see the +live CTE, so a model gap manifests as silently wider-than-predicted +tracking error. The demo's `cte_max` print is your sanity check. + +### Closed-loop α-feedback variant — NEGATIVE result (shipped, opt-in, default OFF) + +A closed-loop variant is wired and shipped behind the `--closed-loop` +flag on the demo (and the `closed_loop=True` config field on +`ReferenceGovernor`). It observes per-tick measured CTE via +`scoring.nearest_segment`, runs a PI law on `(cte - e_max)` with +anti-windup + EMA filter, and applies a multiplicative α ∈ [α_min, +1.0] scaling to the open-loop profile output. The architecture and +unit tests pass cleanly. **The empirical result on the canonical path +battery, however, does not show convergence:** + +| Path | OL (option A) | CL default | Final α | Bare follower | +|---|---|---|---|---| +| `single_corner` | 8.8 cm | **8.8 cm** | 1.000 | 10.3 cm | +| `smooth_corner` | 9.9 cm | **10.0 cm** | 1.000 | 7.7 cm | +| `slalom` | 9.8 cm | **10.4 cm** | 0.722 | 5.0 cm | +| `figure_eight` | 11.9 cm | **12.1 cm** | 0.852 | 5.2 cm | + +Two failure modes: + +1. **Spike-CTE paths** (`single_corner`, `smooth_corner`): α stays at + 1.0 because the high-CTE excursion at the corner is too brief — the + EMA filter dampens it before the PI integrator can wind up. Could + be addressed with more aggressive gains (`kp_alpha ≈ 20`, + `cte_ema_alpha ≈ 0.7`), but the corner is anyway a transient + feature. +2. **Sustained-CTE paths** (`slalom`, `figure_eight`): α DOES drop + (0.72–0.85, the loop is actuating) — but `cte_max` gets slightly + *worse*, not better. Slowing further does not help because the + bottleneck on these paths is **wz authority**, and the cap's + geometry-preserving scaling reduces `wz` proportionally with `vx`. + The controller loses turning authority at exactly the moment it + needs more. The closed-loop is doing the algebraically correct + thing for the wrong physics model. + +The fix is structural, not a gain tune. Three options, all larger +than the V1 closed-loop scope: + +- **Decouple α** into `α_vx` and `α_wz`. The cap stops preserving + turn radius when wz is the bottleneck. Simplest concept; adds a + second feedback channel. +- **Per-waypoint α gating** — apply α only at waypoints where + precision binds (not saturation/lateral). Requires the solver to + track binding constraints. +- **Adapt e_max** — when wz is the binding bottleneck, widen the + corridor instead of slowing the robot. Concedes the precision + promise but stays inside the open-loop framework. + +The V1 closed-loop is left in place (opt-in, default off) so the +machinery exists for the structural follow-up. The demo prints a +"converged?" line and an α(t) overlay to make this state visible. + +### Non-goals + +- No toppra dependency. The constraint-generator architecture is + future-ready for a toppra swap but does not pull it in. +- No new control law in the follower. +- The shipped closed-loop variant is wired but does not converge on + the canonical battery — see "Closed-loop α-feedback variant" above. diff --git a/dimos/utils/benchmarking/scoring.py b/dimos/utils/benchmarking/scoring.py index 698c19f33b..3002e15658 100644 --- a/dimos/utils/benchmarking/scoring.py +++ b/dimos/utils/benchmarking/scoring.py @@ -88,7 +88,7 @@ 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( +def nearest_segment( pt: NDArray[np.float64], path_xy: NDArray[np.float64] ) -> tuple[int, float, float]: """Find nearest path segment to ``pt``. @@ -165,7 +165,7 @@ def score_run(reference_path: Path, executed: ExecutedTrajectory) -> ScoreResult 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) + seg_idx, d, _ = nearest_segment(pt, path_xy) cte_abs.append(d) cte_sq.append(d * d) diff --git a/examples/go2_reference_governor_demo.py b/examples/go2_reference_governor_demo.py new file mode 100644 index 0000000000..cb979a00ad --- /dev/null +++ b/examples/go2_reference_governor_demo.py @@ -0,0 +1,600 @@ +#!/usr/bin/env python +# Copyright 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. + +"""End-to-end ReferenceGovernor demo (in-process sim). + +Wires the governor as the BaselinePathFollowerTask's `external_profile_cap`, +drives a manual tick loop against a TwistBasePlantSim, and prints the CTE +summary using the existing benchmark scoring. + +Two modes: + --mode static fixed e_max throughout the run + --mode square-wave e_max alternates high/low in a background thread, + exercising the atomic-snapshot recompute path + +The same coordinator + follower + plant code path runs against real +hardware when the operator coord brings up a DDS adapter instead of the +in-process FOPDT plant — this script is the simplest reproducible variant. + +Usage examples (from repo root): + uv run python examples/go2_reference_governor_demo.py --path single_corner --e-max 0.05 + uv run python examples/go2_reference_governor_demo.py --path circle --mode square-wave \\ + --e-max-high 0.10 --e-max-low 0.02 --period 4.0 +""" + +from __future__ import annotations + +import argparse +from dataclasses import dataclass +from pathlib import Path as FsPath +import sys +import tempfile +import threading +import time + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt + +from dimos.control.task import ( + CoordinatorState, + JointStateSnapshot, +) +from dimos.control.tasks.baseline_path_follower_task import ( + BaselinePathFollowerTask, + BaselinePathFollowerTaskConfig, +) +from dimos.core.global_config import global_config +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.navigation.reference_governor import ReferenceGovernor +from dimos.utils.benchmarking import paths as path_battery +from dimos.utils.benchmarking.plant import ( + GO2_PLANT_FITTED, + TwistBasePlantSim, +) +from dimos.utils.benchmarking.scoring import ( + ExecutedTrajectory, + TrajectoryTick, + score_run, +) +from dimos.utils.benchmarking.tuning import ( + FopdtChannelDC, + PlantModelDC, + Provenance, + derive_config, + git_sha, +) + +# --- Joints used by the BaselinePathFollowerTask (twist-base trio) --------- +_JX, _JY, _JYAW = "base/vx", "base/vy", "base/wz" +_JOINTS = [_JX, _JY, _JYAW] + + +def _write_self_test_artifact(tmpdir: FsPath) -> FsPath: + """Emit a minimal TuningConfig artifact at tmpdir/config.json built + from the vendored Go2 plant. Avoids requiring a real characterization + artifact on disk just to run the demo.""" + plant = GO2_PLANT_FITTED + prov = Provenance( + robot_id="go2-demo", + surface="demo", + mode="default", + date="demo", + git_sha=git_sha(), + sim_or_hw="hw", # hw so valid_for_tuning=True (we want the gates open) + characterization_session_dir="self-test (demo script)", + ) + cfg = derive_config(plant, prov) + # Substitute the artifact's plant DC for explicit clarity in JSON. + cfg.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), + ) + p = tmpdir / "go2_demo_config.json" + cfg.to_json(p) + return p + + +def _resolve_path(name: str) -> NavPath: + table = { + "straight_line": lambda: path_battery.straight_line(length=2.0), + "single_corner": lambda: path_battery.single_corner(leg_length=2.0, angle_deg=90.0), + "circle": lambda: path_battery.circle(radius=1.0, n_points=120), + "square": lambda: path_battery.square(side=2.0), + "figure_eight": lambda: path_battery.figure_eight(loop_radius=1.0, n_points=200), + "slalom": lambda: path_battery.slalom(), + "smooth_corner": lambda: path_battery.smooth_corner( + leg_length=2.0, angle_deg=90.0, arc_radius=0.5 + ), + } + if name not in table: + raise SystemExit(f"unknown --path {name!r}; pick one of {sorted(table)}") + return table[name]() + + +def _state(plant: TwistBasePlantSim, t_now: float, dt: float) -> CoordinatorState: + return CoordinatorState( + joints=JointStateSnapshot( + joint_positions={_JX: plant.x, _JY: plant.y, _JYAW: plant.yaw}, + joint_velocities={_JX: plant.vx, _JY: plant.vy, _JYAW: plant.wz}, + joint_efforts={_JX: 0.0, _JY: 0.0, _JYAW: 0.0}, + timestamp=t_now, + ), + t_now=t_now, + dt=dt, + ) + + +def _start_pose(plant: TwistBasePlantSim, t_now: float) -> PoseStamped: + return PoseStamped( + ts=t_now, + position=Vector3(plant.x, plant.y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, plant.yaw)), + ) + + +def _square_wave_e_max( + governor: ReferenceGovernor, + e_high: float, + e_low: float, + period: float, + stop: threading.Event, + log: list[tuple[float, float]], +) -> None: + t0 = time.perf_counter() + half = period / 2.0 + while not stop.is_set(): + for value in (e_high, e_low): + if stop.is_set(): + break + governor.update_e_max(value) + log.append((time.perf_counter() - t0, value)) + stop.wait(half) + + +@dataclass +class RunResult: + traj: ExecutedTrajectory + e_log: list[tuple[float, float]] + alpha_log: list[tuple[float, float]] # (t, alpha) — empty unless closed_loop + cte_log: list[tuple[float, float]] # (t, instantaneous CTE m) — empty unless closed_loop + + +def _run_once( + path: NavPath, + args: argparse.Namespace, + artifact_path: FsPath, + use_governor: bool, + label: str, +) -> RunResult: + """Single pass: build follower (+ optional governor), drive plant, return + trajectory + telemetry. Used once normally, twice in --compare mode.""" + governor: ReferenceGovernor | None = None + if use_governor: + governor = ReferenceGovernor( + plant_artifact_path=str(artifact_path), + e_max_default=args.e_max, + closed_loop=bool(args.closed_loop), + ) + governor.set_path(path) + + follower_cfg = BaselinePathFollowerTaskConfig( + joint_names=_JOINTS, + priority=20, + speed=args.speed, + control_frequency=float(args.tick_hz), + goal_tolerance=0.2, + orientation_tolerance=0.35, + k_angular=0.5, + ) + follower = BaselinePathFollowerTask( + name="baseline_follower", + config=follower_cfg, + global_config=global_config, + external_profile_cap=governor, + ) + + dt = 1.0 / args.tick_hz + plant = TwistBasePlantSim(GO2_PLANT_FITTED) + plant.reset(0.0, 0.0, 0.0, dt) + + start_pose = _start_pose(plant, time.perf_counter()) + if not follower.start_path(path, start_pose): + print(f"[{label}] start_path rejected", file=sys.stderr) + if governor is not None: + governor._close_module() + return RunResult( + traj=ExecutedTrajectory(ticks=[], arrived=False), + e_log=[], + alpha_log=[], + cte_log=[], + ) + + e_log: list[tuple[float, float]] = [] + alpha_log: list[tuple[float, float]] = [] + cte_log: list[tuple[float, float]] = [] + stop_ev = threading.Event() + sq_thread: threading.Thread | None = None + if use_governor and args.mode == "square-wave" and governor is not None: + sq_thread = threading.Thread( + target=_square_wave_e_max, + args=( + governor, + args.e_max_high, + args.e_max_low, + args.period, + stop_ev, + e_log, + ), + daemon=True, + ) + sq_thread.start() + + ticks: list[TrajectoryTick] = [] + t0 = time.perf_counter() + deadline = t0 + args.timeout + prev_t = t0 + arrived = False + + while time.perf_counter() < deadline: + now = time.perf_counter() + dt_tick = max(dt, now - prev_t) + state = _state(plant, now, dt_tick) + output = follower.compute(state) + if output is None or output.velocities is None: + if follower.get_state() == "arrived": + arrived = True + break + vx, vy, wz = output.velocities + plant.step(vx, vy, wz, dt_tick) + prev_t = now + ticks.append( + TrajectoryTick( + t=now - t0, + pose=_start_pose(plant, now), + cmd_twist=Twist( + linear=Vector3(vx, vy, 0.0), + angular=Vector3(0.0, 0.0, wz), + ), + actual_twist=Twist( + linear=Vector3(plant.vx, plant.vy, 0.0), + angular=Vector3(0.0, 0.0, plant.wz), + ), + ) + ) + # Closed-loop telemetry: lock-safe snapshot of alpha + filtered CTE + # (these are updated inside speed_limit_at on every follower tick). + if governor is not None and args.closed_loop: + with governor._state_lock: + alpha = governor._alpha + cte_filt = governor._cte_filtered + alpha_log.append((now - t0, alpha)) + cte_log.append((now - t0, cte_filt)) + + sleep_for = (prev_t + dt) - time.perf_counter() + if sleep_for > 0: + time.sleep(sleep_for) + + if sq_thread is not None: + stop_ev.set() + sq_thread.join(timeout=1.0) + if governor is not None: + governor._close_module() + + return RunResult( + traj=ExecutedTrajectory(ticks=ticks, arrived=arrived), + e_log=e_log, + alpha_log=alpha_log, + cte_log=cte_log, + ) + + +def _report( + label: str, + path: NavPath, + result: RunResult, + e_max: float, +) -> None: + score = score_run(path, result.traj) + duration = result.traj.ticks[-1].t if result.traj.ticks else 0.0 + lines = [ + f"\n=== {label} ===", + f" arrived : {result.traj.arrived}", + f" cte_max : {score.cte_max * 100:.1f} cm", + f" cte_rms : {score.cte_rms * 100:.1f} cm", + f" duration : {duration:.1f} s ({len(result.traj.ticks)} ticks)", + ] + if result.alpha_log: + final_alpha = result.alpha_log[-1][1] + within = "YES" if score.cte_max <= e_max else "NO" + lines.extend( + [ + f" final alpha : {final_alpha:.3f}", + f" converged? : {within} (cte_max {score.cte_max * 100:.1f}cm vs target {e_max * 100:.1f}cm)", + ] + ) + print("\n".join(lines)) + + +def run(args: argparse.Namespace) -> int: + with tempfile.TemporaryDirectory() as raw_tmp: + tmpdir = FsPath(raw_tmp) + artifact_path = FsPath(args.config) if args.config else _write_self_test_artifact(tmpdir) + path = _resolve_path(args.path) + out = FsPath(args.out) + out.parent.mkdir(parents=True, exist_ok=True) + + if args.compare: + r_g = _run_once( + path, + args, + artifact_path, + use_governor=True, + label=f"{args.path} WITH governor", + ) + r_n = _run_once( + path, + args, + artifact_path, + use_governor=False, + label=f"{args.path} WITHOUT governor", + ) + _report(f"{args.path} (mode={args.mode}) WITH governor", path, r_g, args.e_max) + _report(f"{args.path} (mode={args.mode}) WITHOUT governor", path, r_n, args.e_max) + _plot_compare(path, r_g, r_n, args, out) + else: + r = _run_once( + path, + args, + artifact_path, + use_governor=not args.no_governor, + label=args.path, + ) + if args.no_governor: + tag = "WITHOUT governor" + elif args.closed_loop: + tag = f"CLOSED-LOOP governor e_max={args.e_max:g}m" + else: + tag = f"open-loop governor e_max={args.e_max:g}m" + _report(f"{args.path} (mode={args.mode}) {tag}", path, r, args.e_max) + _plot(path, r, args, out) + + print(f"plot : {out.resolve()}") + # Single-run mode returns 0 only if arrived; compare always returns 0 + # (it's diagnostic, not a pass/fail gate). + return 0 + + +def _plot_compare( + path: NavPath, + r_g: RunResult, + r_n: RunResult, + args: argparse.Namespace, + out: FsPath, +) -> None: + """Side-by-side: XY overlay (left) + |cmd vx|(t) overlay (right), with + an optional third subplot showing alpha(t) + measured CTE(t) when the + closed-loop variant ran. The closed-loop subplot is the diagnostic + surface for "did alpha converge to a steady value that meets the + corridor".""" + have_cl = bool(r_g.alpha_log) + ncols = 3 if have_cl else 2 + fig, axes = plt.subplots(1, ncols, figsize=(6.5 * ncols, 5.0)) + ax_xy = axes[0] + ax_v = axes[1] + + ref_x = [p.position.x for p in path.poses] + ref_y = [p.position.y for p in path.poses] + ax_xy.plot(ref_x, ref_y, "k--", lw=2, label="reference") + + def _draw(ax, ticks, color, label): + xs = [tk.pose.position.x for tk in ticks] + ys = [tk.pose.position.y for tk in ticks] + if not xs: + return + ax.plot(xs, ys, color=color, lw=1.4, label=label) + ax.plot(xs[0], ys[0], "o", color=color, ms=5) + ax.plot(xs[-1], ys[-1], "s", color=color, ms=5) + + sg = score_run(path, r_g.traj) + sn = score_run(path, r_n.traj) + _draw(ax_xy, r_g.traj.ticks, "tab:blue", f"WITH governor (cte_max={sg.cte_max * 100:.1f}cm)") + _draw( + ax_xy, r_n.traj.ticks, "tab:orange", f"WITHOUT governor (cte_max={sn.cte_max * 100:.1f}cm)" + ) + ax_xy.set_aspect("equal", adjustable="datalim") + ax_xy.set_xlabel("x (m)") + ax_xy.set_ylabel("y (m)") + ax_xy.grid(True, alpha=0.3) + ax_xy.legend(fontsize=8) + ax_xy.set_title(f"{args.path} — XY tracking") + + tg = [tk.t for tk in r_g.traj.ticks] + vg = [abs(tk.cmd_twist.linear.x) for tk in r_g.traj.ticks] + tn = [tk.t for tk in r_n.traj.ticks] + vn = [abs(tk.cmd_twist.linear.x) for tk in r_n.traj.ticks] + ax_v.plot(tg, vg, "tab:blue", lw=1.0, label="WITH governor") + ax_v.plot(tn, vn, "tab:orange", lw=1.0, label="WITHOUT governor") + if r_g.e_log: + ax2 = ax_v.twinx() + ts = [t for t, _ in r_g.e_log] + es = [e for _, e in r_g.e_log] + ax2.step(ts, es, "r-", where="post", lw=0.8, alpha=0.6, label="e_max") + ax2.set_ylabel("e_max (m)", color="r") + ax_v.set_xlabel("t (s)") + ax_v.set_ylabel("|cmd vx| (m/s)") + ax_v.set_title("Commanded forward speed") + ax_v.grid(True, alpha=0.3) + ax_v.legend(fontsize=8, loc="upper right") + + if have_cl: + _plot_closed_loop_panel(axes[2], r_g, args.e_max) + + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) + + +def _plot( + path: NavPath, + r: RunResult, + args: argparse.Namespace, + out: FsPath, +) -> None: + have_cl = bool(r.alpha_log) + have_e = bool(r.e_log) + ncols = 1 + int(have_e) + int(have_cl) + fig, axes = plt.subplots(1, ncols, figsize=(6.0 * ncols, 5.0), squeeze=False) + panel = list(axes[0]) + ax_xy = panel.pop(0) + + ref_x = [p.position.x for p in path.poses] + ref_y = [p.position.y for p in path.poses] + exec_x = [tk.pose.position.x for tk in r.traj.ticks] + exec_y = [tk.pose.position.y for tk in r.traj.ticks] + ax_xy.plot(ref_x, ref_y, "k--", lw=2, label="reference") + ax_xy.plot(exec_x, exec_y, "b-", lw=1.3, label="executed") + if exec_x: + ax_xy.plot(exec_x[0], exec_y[0], "go", ms=6, label="start") + ax_xy.plot(exec_x[-1], exec_y[-1], "rs", ms=6, label="end") + ax_xy.set_aspect("equal", adjustable="datalim") + ax_xy.set_xlabel("x (m)") + ax_xy.set_ylabel("y (m)") + ax_xy.grid(True, alpha=0.3) + ax_xy.legend(fontsize=8) + mode_tag = "CL" if args.closed_loop else "OL" + ax_xy.set_title(f"{args.path} — {mode_tag} governor e_max={args.e_max:g}m") + + if have_e: + ax_e = panel.pop(0) + ts = [t for t, _ in r.e_log] + es = [e for _, e in r.e_log] + cmd_t = [tk.t for tk in r.traj.ticks] + cmd_v = [abs(tk.cmd_twist.linear.x) for tk in r.traj.ticks] + ax_e.step(ts, es, "r-", where="post", label="e_max (m)") + ax2 = ax_e.twinx() + ax2.plot(cmd_t, cmd_v, "b-", lw=0.8, alpha=0.7, label="|cmd vx|") + ax_e.set_xlabel("t (s)") + ax_e.set_ylabel("e_max (m)", color="r") + ax2.set_ylabel("|cmd vx| (m/s)", color="b") + ax_e.set_title("e_max vs commanded speed") + ax_e.grid(True, alpha=0.3) + + if have_cl: + _plot_closed_loop_panel(panel.pop(0), r, args.e_max) + + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) + + +def _plot_closed_loop_panel(ax, r: RunResult, e_max: float) -> None: + """alpha(t) on the left axis, instantaneous CTE(t) on the right axis, + with a horizontal line at e_max so convergence is visible at a glance.""" + if not r.alpha_log: + return + t_a = [t for t, _ in r.alpha_log] + a = [v for _, v in r.alpha_log] + t_c = [t for t, _ in r.cte_log] + c = [v * 100 for _, v in r.cte_log] # cm + ax.plot(t_a, a, "tab:blue", lw=1.0, label="alpha(t)") + ax.set_ylabel("alpha", color="tab:blue") + ax.set_ylim(0.0, 1.05) + ax2 = ax.twinx() + ax2.plot(t_c, c, "tab:red", lw=0.9, alpha=0.7, label="|CTE| (cm)") + ax2.axhline( + e_max * 100, color="tab:red", ls=":", lw=0.8, alpha=0.6, label=f"e_max={e_max * 100:.0f}cm" + ) + ax2.set_ylabel("|CTE| (cm)", color="tab:red") + ax.set_xlabel("t (s)") + ax.set_title("Closed-loop alpha vs measured CTE") + ax.grid(True, alpha=0.3) + ax.legend(loc="upper left", fontsize=8) + ax2.legend(loc="upper right", fontsize=8) + + +def main() -> int: + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument( + "--config", + help="TuningConfig JSON. If omitted, a self-test artifact " + "is generated from the vendored Go2 plant.", + ) + ap.add_argument( + "--path", + default="single_corner", + help="straight_line | single_corner | circle | square | figure_eight | " + "slalom | smooth_corner", + ) + ap.add_argument( + "--speed", + type=float, + default=0.55, + help="follower base speed (m/s). Governor caps it per-waypoint.", + ) + ap.add_argument("--tick-hz", type=float, default=20.0) + ap.add_argument("--timeout", type=float, default=60.0) + ap.add_argument("--mode", choices=["static", "square-wave"], default="static") + # static mode + ap.add_argument("--e-max", type=float, default=0.05, help="static corridor half-width (m)") + # square-wave mode + ap.add_argument("--e-max-high", type=float, default=0.10) + ap.add_argument("--e-max-low", type=float, default=0.02) + ap.add_argument( + "--period", + type=float, + default=4.0, + help="square-wave period (s); half-period at each level", + ) + ap.add_argument("--out", default="/tmp/reference_governor_demo.png") + # --- comparison knobs --- + ap.add_argument( + "--no-governor", + action="store_true", + help="Run a single pass WITHOUT the governor (bare follower at " + "--speed). Use as a baseline reference.", + ) + ap.add_argument( + "--compare", + action="store_true", + help="Run the same path TWICE: once with the governor, once " + "without. Overlays both trajectories on the output plot.", + ) + # --- closed-loop knob (only meaningful when the governor is in use) --- + ap.add_argument( + "--closed-loop", + action="store_true", + help="Enable the closed-loop alpha-feedback variant: the governor " + "observes per-tick CTE and dynamically scales the open-loop " + "bound to drive actual tracking error toward e_max. Default " + "off (open-loop). Implies the governor is in use; ignored " + "when --no-governor is set.", + ) + args = ap.parse_args() + if args.no_governor and args.compare: + ap.error("--no-governor and --compare are mutually exclusive") + if args.no_governor and args.closed_loop: + ap.error("--closed-loop has no effect with --no-governor") + return run(args) + + +if __name__ == "__main__": + sys.exit(main()) From 516d8babc8785007ec7a0f4e2cd940daed9644c7 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 17:45:11 -0700 Subject: [PATCH 24/51] created characterization module --- dimos/robot/unitree/keyboard_teleop.py | 26 +- dimos/utils/benchmarking/characterization.py | 475 ++++++++++++++---- .../utils/characterization/modeling/fopdt.py | 99 +++- 3 files changed, 493 insertions(+), 107 deletions(-) diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index 07af844c60..f69eb7d5e0 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -25,8 +25,15 @@ 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.Int8 import Int8 from dimos.utils.logging_config import setup_logger +# Gate event codes published on KeyboardTeleop.gate for tools that need +# operator-confirmation per step (e.g. unitree-go2-characterization). +GATE_ADVANCE = 0 +GATE_SKIP = 1 +GATE_QUIT = 2 + logger = setup_logger() # Force X11 driver to avoid OpenGL threading issues @@ -47,9 +54,19 @@ 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] _stop_event: threading.Event _keys_held: set[int] | None = None @@ -133,6 +150,12 @@ 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 event.type == pygame.KEYUP: self._keys_held.discard(event.key) @@ -233,6 +256,7 @@ def _update_display(self, twist: Twist) -> None: "WS: Move | AD: Turn | QE: Strafe", "Shift: Boost | Ctrl: Slow", "Space: E-Stop | ESC: Quit", + "Enter: Advance | K: Skip | Backspace: Quit (tools)", ] for text in help_texts: surf = self._font.render(text, True, _HELP_TEXT_COLOR) diff --git a/dimos/utils/benchmarking/characterization.py b/dimos/utils/benchmarking/characterization.py index b74e5ff584..152220918d 100644 --- a/dimos/utils/benchmarking/characterization.py +++ b/dimos/utils/benchmarking/characterization.py @@ -44,25 +44,35 @@ from __future__ import annotations import argparse +from collections.abc import Callable from datetime import date import math from pathlib import Path +import queue import threading import time +from typing import Any, Literal import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt import numpy as np +from reactivex.disposable import Disposable from dimos.control.components import make_twist_base_joints +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In from dimos.core.transport import LCMTransport 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.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.robot.unitree.keyboard_teleop import GATE_ADVANCE, GATE_QUIT, GATE_SKIP +from dimos.utils.path_utils import get_project_root # Well-known LCM topics — every ControlCoordinator blueprint honors this # contract (twist_command In = /cmd_vel, joint_state Out = @@ -87,6 +97,79 @@ _SIM_DT = 0.02 # in-process self-test integration step (not robot-specific) REPORTS_DIR = Path(__file__).parent / "reports" +# New default landing dir for tuning artifacts: /data/characterization//. +# REPORTS_DIR is retained as the package-local location for the README / docs, +# not as the artifact output default. +DEFAULT_OUT_DIR = get_project_root() / "data" / "characterization" + + +def reconstruct_body_velocities( + ts: np.ndarray, + x: np.ndarray, + y: np.ndarray, + yaw: np.ndarray, + window: int = 5, + order: int = 2, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Recover body-frame (vx, vy, dyaw) from a buffered pose trace. + + The pipeline is intentionally smooth-position-then-differentiate + (not differentiate-then-smooth): odom measures position directly, + and numerical differentiation amplifies high-frequency noise that no + amount of post-hoc smoothing fully recovers from. On a legged base + the dominant "noise" is body bob at gait frequency — buffering raw + pose, applying Savitzky-Golay to the position trace, then + central-differencing the smoothed series gives a much cleaner + velocity signal for the FOPDT fit. Matches the historical + ``analyze.py`` pipeline. + + ``window`` must be odd and > ``order``; falls back to no smoothing + if the buffer is too short.""" + from scipy.signal import savgol_filter + + yaw_u = np.unwrap(yaw) + if len(ts) >= window and window % 2 == 1 and order < window: + xf = savgol_filter(x, window, order) + yf = savgol_filter(y, window, order) + yawf = savgol_filter(yaw_u, window, order) + else: + xf, yf, yawf = x, y, yaw_u + dx = np.gradient(xf, ts) + dy = np.gradient(yf, ts) + dyaw = np.gradient(yawf, ts) + c, s = np.cos(yawf), np.sin(yawf) + vx = c * dx + s * dy + vy = -s * dx + c * dy + return vx, vy, dyaw + + +def _hampel(ys: np.ndarray, window: int = 11, n_sigma: float = 3.0) -> tuple[np.ndarray, int]: + """Hampel outlier filter — replace points >n_sigma·MAD from the local + median with the local median. Window-centered; preserves trace length. + + Returns (filtered_array, n_replaced). MAD is scaled by 1.4826 so + n_sigma is in Gaussian-sigma units. Spikes from leg impacts / odom + estimator glitches get surgically removed; the rest of the trace is + untouched (the goal is to keep FOPDT step shape readable, not + smooth-out the dynamics).""" + if window <= 0 or len(ys) < window: + return ys.copy(), 0 + half = window // 2 + n = len(ys) + out = ys.copy() + replaced = 0 + for i in range(n): + lo = max(0, i - half) + hi = min(n, i + half + 1) + w = ys[lo:hi] + med = float(np.median(w)) + mad = float(np.median(np.abs(w - med))) + if mad == 0.0: + continue + if abs(ys[i] - med) > n_sigma * 1.4826 * mad: + out[i] = med + replaced += 1 + return out, replaced def _clamp(v: float, lo: float, hi: float) -> float: @@ -185,12 +268,20 @@ def _plot_fits( for tr in [t for t in traces if t["channel"] == ch]: t_arr = tr["t"] (line,) = ax.plot(t_arr, tr["y"], lw=1.4, alpha=0.85, label=f"meas @{tr['amp']:g}") + y_raw = tr.get("y_raw") + if y_raw is not None and tr.get("n_replaced", 0) > 0: + ax.plot(t_arr, y_raw, ":", lw=0.9, color=line.get_color(), alpha=0.5) yhat = fopdt_step_response(t_arr, tr["K"], tr["tau"], tr["L"], tr["amp"]) ax.plot(t_arr, yhat, "--", lw=1.4, color=line.get_color(), alpha=0.9) row = list(t2["amp"] for t2 in traces if t2["channel"] == ch).index(tr["amp"]) - ax.annotate( + ann = ( f"@{tr['amp']:g}: K={tr['K']:.3f} τ={tr['tau']:.3f} " - f"L={tr['L']:.3f} r²={tr['r2']:.2f}", + f"L={tr['L']:.3f} r²={tr['r2']:.2f}" + ) + if tr.get("n_replaced", 0) > 0: + ann += f" (hampel: {tr['n_replaced']})" + ax.annotate( + ann, xy=(0.02, 0.97 - 0.06 * row), xycoords="axes fraction", ha="left", @@ -199,7 +290,7 @@ def _plot_fits( color=line.get_color(), ) unit = "rad/s" if ch == "wz" else "m/s" - ax.set_title(f"{ch} (solid = measured, dashed = FOPDT fit)") + ax.set_title(f"{ch} (solid = filtered, dotted = raw, dashed = FOPDT fit)") ax.set_xlabel("time since step edge (s)") ax.set_ylabel(f"{ch} ({unit})") ax.grid(True, alpha=0.3) @@ -218,22 +309,22 @@ def _plot_fits( class _JointStatePoseStream: - """Pose + body-velocity stream sourced from a coordinator's - ``joint_state`` Out. Reuses the benchmark observer's math: positions - are [x, y, yaw] (twist-base adapter convention); body-frame velocity - is recovered by EMA-smoothed pose differentiation. Drop-in - replacement for the old standalone /odom LCM subscriber + - in-house ``_PoseVelocityEstimator``.""" - - def __init__(self, joint_names: list[str], alpha: float = 0.5) -> None: + """Pose stream sourced from a coordinator's ``joint_state`` Out. + Positions are [x, y, yaw] (twist-base adapter convention). Buffers + raw pose samples between :meth:`start_buffering` and + :meth:`stop_and_pop` so callers can reconstruct body-frame + velocity at step end (via :func:`reconstruct_body_velocities`) + rather than per-sample. Smooth-then-differentiate is much cleaner + than differentiate-then-smooth on legged-robot data — see that + function's docstring.""" + + def __init__(self, joint_names: list[str]) -> None: self._jx, self._jy, self._jyaw = joint_names - self._alpha = alpha self._lock = threading.Lock() self._pose: PoseStamped | None = None self._pose_t: float = 0.0 - self._prev_pose: PoseStamped | None = None - self._prev_t: float | None = None - self._vx = self._vy = self._wz = 0.0 + self._buffer: list[tuple[float, float, float, float]] = [] + self._buffering: bool = False def on_joint_state(self, msg: JointState) -> None: if not msg.name: @@ -256,34 +347,33 @@ def on_joint_state(self, msg: JointState) -> None: orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), ) with self._lock: - if self._prev_pose is not None and self._prev_t is not None: - 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 - y0 = self._prev_pose.orientation.euler[2] - y1 = pose.orientation.euler[2] - dyaw = (y1 - y0 + 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._pose, self._pose_t = pose, now + if self._buffering: + self._buffer.append((now, x, y, yaw)) - def latest(self) -> tuple[PoseStamped | None, float, tuple[float, float, float]]: + def latest(self) -> tuple[PoseStamped | None, float]: with self._lock: - return self._pose, self._pose_t, (self._vx, self._vy, self._wz) + return self._pose, self._pose_t - def reset_velocity(self) -> None: - """Drop EMA state — called at pre-roll so each step starts clean.""" + def start_buffering(self) -> None: + """Begin recording raw pose samples — called at step start.""" with self._lock: - self._vx = self._vy = self._wz = 0.0 - self._prev_pose = None - self._prev_t = None + self._buffer = [] + self._buffering = True + + def stop_and_pop( + self, + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Stop buffering and return (ts, x, y, yaw) arrays for the step.""" + with self._lock: + self._buffering = False + buf = self._buffer + self._buffer = [] + if not buf: + empty = np.array([], dtype=float) + return empty, empty, empty, empty + arr = np.asarray(buf, dtype=float) + return arr[:, 0], arr[:, 1], arr[:, 2], arr[:, 3] def _prereq_banner(profile: RobotPlantProfile) -> None: @@ -309,6 +399,12 @@ def _fit_hw( pre_roll_s: float, warmup_s: float, max_dist: float, + gate_input: Callable[[str], str] = input, + gate_keys_label: str = "ENTER=run s=skip q=quit", + savgol_window: int = 11, + savgol_order: int = 2, + hampel_window: int = 11, + hampel_n_sigma: float = 3.0, ) -> tuple[TwistBasePlantParams, dict, list[dict]]: _prereq_banner(profile) hw_dt = 1.0 / profile.tick_rate_hz @@ -350,7 +446,7 @@ def safe_stop() -> None: print(f"[hw] waiting up to {warmup_s:.0f}s for {_JOINT_STATE_TOPIC} ...") deadline = time.perf_counter() + warmup_s while time.perf_counter() < deadline: - p, _, _ = stream.latest() + p, _ = stream.latest() if p is not None: break time.sleep(0.05) @@ -369,9 +465,9 @@ def safe_stop() -> None: for amp in profile.si_amplitudes[channel]: safe_stop() resp = ( - input( + gate_input( f"\n[{channel}@{amp}] reposition robot into clear space, " - f"ENTER=run s=skip q=quit: " + f"{gate_keys_label}: " ) .strip() .lower() @@ -382,8 +478,10 @@ def safe_stop() -> None: print(" skipped") continue - # pre-roll zeros (settle + prime estimator) - stream.reset_velocity() + # pre-roll zeros (settle). Buffer during pre-roll too so + # the pre-step samples give us a noise_std estimate + # (robot at rest, zero commanded — pure odom noise). + stream.start_buffering() t_end = time.perf_counter() + pre_roll_s while time.perf_counter() < t_end: publish(0.0, 0.0, 0.0) @@ -394,11 +492,12 @@ def safe_stop() -> None: # time cap would run the robot out of the test area), or # t_rel > step_s (time safety cap; also the terminator for # wz, which spins in place and never accumulates distance). + # Body-frame velocity is reconstructed from buffered pose + # AFTER the step — smooth-then-differentiate gives a much + # cleaner trace on a legged base than per-tick differentiation. cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} cmd[channel] = amp - ts: list[float] = [] - ys: list[float] = [] - sp, _, _ = stream.latest() + sp, _ = stream.latest() if sp is None: print(" [abort] lost odom before step") continue @@ -411,7 +510,7 @@ def safe_stop() -> None: if t_rel > step_s: break publish(cmd["vx"], cmd["vy"], cmd["wz"]) - p, pt, v = stream.latest() + p, pt = stream.latest() if p is None or now - pt > profile.odom_stale_s: print(f" [abort] stale odom ({now - pt:.2f}s)") end_reason = "stale" @@ -420,21 +519,50 @@ def safe_stop() -> None: if dist >= max_dist: end_reason = "dist" break - ts.append(t_rel) - ys.append(v[_CHANNELS.index(channel)]) time.sleep(hw_dt) + ts_abs, x_buf, y_buf, yaw_buf = stream.stop_and_pop() safe_stop() - if len(ys) < 5: + if len(ts_abs) < max(5, savgol_window): print(f" [warn] {channel}@{amp}: too few samples, skip") continue - fp = fit_fopdt(np.asarray(ts), np.asarray(ys), u_step=amp) + ts_rel = ts_abs - t0 + vx_all, vy_all, dyaw_all = reconstruct_body_velocities( + ts_rel, x_buf, y_buf, yaw_buf, window=savgol_window, order=savgol_order + ) + ys_all = {"vx": vx_all, "vy": vy_all, "wz": dyaw_all}[channel] + # Split pre/post step. Pre is zero-commanded baseline - + # std of that velocity = our noise estimate. + pre_mask = ts_rel < 0.0 + post_mask = ~pre_mask + if post_mask.sum() < max(5, savgol_window): + print(f" [warn] {channel}@{amp}: too few post-step samples, skip") + continue + noise_std: float | None = None + if pre_mask.sum() >= 3: + noise_std = float(np.std(ys_all[pre_mask])) + ts_fit = ts_rel[post_mask] + ys_raw = ys_all[post_mask] + ys_filt, n_replaced = _hampel(ys_raw, hampel_window, hampel_n_sigma) + if n_replaced: + print(f" hampel: replaced {n_replaced}/{len(ys_raw)} outliers") + # Floor L at the actual data sample period — fit cannot + # physically resolve deadtime finer than odom rate. + dt_med = float(np.median(np.diff(ts_fit))) if len(ts_fit) > 1 else 0.0 + fp = fit_fopdt( + ts_fit, + ys_filt, + u_step=amp, + min_deadtime=dt_med, + noise_std=noise_std, + two_stage=noise_std is not None, + ) if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") continue print( f" {channel}@{amp}: K={fp.K:.3f} tau={fp.tau:.3f} " - f"L={fp.L:.3f} ({len(ys)} samples, ended on {end_reason})" + f"L={fp.L:.3f} ({len(ys_raw)} samples, ended on {end_reason})" ) fits.append(fp) per_amplitude[channel].append( @@ -444,8 +572,10 @@ def safe_stop() -> None: { "channel": channel, "amp": amp, - "t": np.asarray(ts, dtype=float), - "y": np.asarray(ys, dtype=float), + "t": np.asarray(ts_fit, dtype=float), + "y_raw": ys_raw, + "n_replaced": n_replaced, + "y": ys_filt, "K": fp.K, "tau": fp.tau, "L": fp.L, @@ -482,11 +612,185 @@ def safe_stop() -> None: ) +class CharacterizerConfig(ModuleConfig): + """Config for :class:`Characterizer`. Each field mirrors a CLI flag on + the existing ``characterization`` entrypoint; ``None`` means "fall + back to the selected ``RobotPlantProfile``". + + ``gate_source`` selects how each SI step is gated. ``"stdin"`` + (default, CLI mode) reads ENTER/s/q from the terminal; ``"stream"`` + (blueprint mode) consumes events from the ``gate`` In port wired to + a co-running :class:`~dimos.robot.unitree.keyboard_teleop.KeyboardTeleop` + that publishes ENTER/K/Backspace as ``"advance"``/``"skip"``/``"quit"``. + """ + + robot: str = "go2" + mode: Literal["hw", "self-test"] = "hw" + out: str | None = None + robot_id: str | None = None + surface: str = "concrete" + gait_mode: str = "default" + step_s: float | None = None + pre_roll_s: float | None = None + odom_warmup: float | None = None + max_dist: float | None = None + gate_source: Literal["stdin", "stream"] = "stdin" + # Savitzky-Golay smoothing applied to the POSITION trace before + # central-differencing to body-frame velocity. This is the main + # noise-rejection knob for legged-robot data: gait-frequency body + # bob lives in the raw pose; smooth-then-differentiate keeps it out + # of the velocity signal. Window must be odd and > order. Set + # window=0 or window <= order to disable. + # Default 11 = ~2 gait cycles at 10 Hz tick rate / 2 Hz Go2 trot; + # synthetic-noise sanity check (3cm gait bob + 5mm odom noise) shows + # window=5 leaves vx-std ~= 0.18 m/s, window=11 drops it to ~= 0.05. + savgol_window: int = 11 + savgol_order: int = 2 + # Hampel outlier filter on the post-reconstruction velocity trace + # before FOPDT fitting. Catches residual single-sample spikes that + # survive savgol smoothing (e.g. odom estimator hiccups). Set + # window=0 to disable. + hampel_window: int = 11 + hampel_n_sigma: float = 3.0 + # Two-stage FOPDT fit: estimate L directly from data (first time the + # response crosses ~5*noise_std above the pre-step noise floor), + # then fit K and tau with L pinned. Decouples L from tau - joint + # LM-fit can trade off a tiny L for a slightly inflated tau when the + # data is noisy. Auto-disabled if noise_std can't be estimated + # (need >=3 pre-step samples in the pose buffer). + two_stage_fit: bool = True + + +class Characterizer(Module): + """Module wrapper around the FOPDT characterization sequence. + + Driven via the CLI shim in :func:`main` (``gate_source="stdin"``) or + composed into the all-in-one blueprint + ``unitree-go2-characterization`` (``gate_source="stream"``, gating + events arrive on the ``gate`` In port from a co-running + :class:`~dimos.robot.unitree.keyboard_teleop.KeyboardTeleop`). + Stdin-driven gating cannot work inside the blueprint runner because + deployed modules run in forkserver-spawned worker subprocesses that + don't share the parent CLI's TTY — hence the stream-based path. + + ``start()`` blocks for the full operator-gated SI loop, then returns. + """ + + config: CharacterizerConfig + + gate: In[Int8] + + _gate_queue: queue.Queue[str] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._gate_queue = queue.Queue() + + @rpc + def start(self) -> None: + super().start() + if self.config.gate_source == "stream": + self.register_disposable(Disposable(self.gate.subscribe(self._on_gate_event))) + self._run() + + def _on_gate_event(self, msg: Int8) -> None: + # Translate the pygame-side gate codes to the legacy CLI vocab so + # _fit_hw's response check (``""``=run, ``"s"``=skip, ``"q"``=quit) + # is unchanged. + code = int(msg.data) + translated = {GATE_ADVANCE: "", GATE_SKIP: "s", GATE_QUIT: "q"}.get(code, "") + self._gate_queue.put(translated) + + def _wait_gate_stream(self, prompt: str) -> str: + print(prompt, end="", flush=True) + return self._gate_queue.get() + + def _run(self) -> None: + cfg = self.config + profile = _resolve_profile(cfg.robot) + step_s = cfg.step_s if cfg.step_s is not None else profile.step_s + pre_roll_s = cfg.pre_roll_s if cfg.pre_roll_s is not None else profile.pre_roll_s + warmup_s = cfg.odom_warmup if cfg.odom_warmup is not None else profile.odom_warmup_s + max_dist = cfg.max_dist if cfg.max_dist is not None else profile.max_dist_m + robot_id = cfg.robot_id if cfg.robot_id is not None else profile.robot_id + out_root = Path(cfg.out).expanduser() if cfg.out else DEFAULT_OUT_DIR + + if cfg.mode == "hw": + if cfg.gate_source == "stream": + gate_input: Callable[[str], str] = self._wait_gate_stream + gate_keys_label = "focus pygame window: ENTER=run K=skip Backspace=quit" + else: + gate_input = input + gate_keys_label = "ENTER=run s=skip q=quit" + fitted, per_amplitude, traces = _fit_hw( + profile, + step_s, + pre_roll_s, + warmup_s, + max_dist, + gate_input=gate_input, + gate_keys_label=gate_keys_label, + savgol_window=cfg.savgol_window, + savgol_order=cfg.savgol_order, + hampel_window=cfg.hampel_window, + hampel_n_sigma=cfg.hampel_n_sigma, + ) + else: + fitted, per_amplitude, traces = _fit_selftest(profile) + + provenance = Provenance( + robot_id=robot_id, + surface=cfg.surface, + mode=cfg.gait_mode, + date=date.today().isoformat(), + git_sha=git_sha(), + sim_or_hw="hw" if cfg.mode == "hw" else "self-test", + characterization_session_dir=( + f"(real {profile.name}, LCM SI)" if cfg.mode == "hw" else "(in-process self-test)" + ), + ) + artifact = derive_config( + fitted, + provenance, + per_amplitude=per_amplitude, + vx_max=profile.vx_max, + wz_max=profile.wz_max, + ) + if cfg.mode == "hw" and "vy" not in profile.excited_channels: + artifact.caveats.append( + f"vy was NOT characterized on hardware ({profile.name} does not " + "strafe in this gait); plant.vy / feedforward.K_vy are a " + "placeholder copy of vx. The benchmark paths are vx+wz only, so " + "this does not affect tuning; re-characterize vy if a " + "lateral-capable gait is used." + ) + + out_dir = out_root / robot_id + out_dir.mkdir(parents=True, exist_ok=True) + out_path = ( + out_dir + / f"{robot_id}_config_{cfg.mode}_{cfg.surface}_{provenance.date}_{provenance.git_sha}.json" + ) + artifact.to_json(out_path) + plot_path = out_path.with_suffix(".png") + _plot_fits(traces, provenance, profile, plot_path) + + tag = "ROBOT-VALID" if artifact.valid_for_tuning else "NOT robot-valid (plumbing check)" + print("\nFOPDT fit graph (the deliverable — model vs real data):") + print(f" {plot_path.resolve()}") + print(f"Config artifact [{tag}] (machine handoff for the benchmark):") + print(f" {out_path.resolve()}") + + def main() -> None: ap = argparse.ArgumentParser(description="Twist-base characterization -> tuning artifact") ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PLANT_PROFILES)}") ap.add_argument("--mode", choices=["hw", "self-test"], default="hw") - ap.add_argument("--out", default=str(REPORTS_DIR)) + ap.add_argument( + "--out", + default=None, + help=f"output dir (default: {DEFAULT_OUT_DIR}//)", + ) ap.add_argument("--robot-id", default=None, help="provenance id (default: profile.robot_id)") ap.add_argument("--surface", default="concrete") ap.add_argument("--gait-mode", default="default") @@ -510,58 +814,19 @@ def main() -> None: ) args = ap.parse_args() - profile = _resolve_profile(args.robot) - step_s = args.step_s if args.step_s is not None else profile.step_s - pre_roll_s = args.pre_roll_s if args.pre_roll_s is not None else profile.pre_roll_s - warmup_s = args.odom_warmup if args.odom_warmup is not None else profile.odom_warmup_s - max_dist = args.max_dist if args.max_dist is not None else profile.max_dist_m - robot_id = args.robot_id if args.robot_id is not None else profile.robot_id - - if args.mode == "hw": - fitted, per_amplitude, traces = _fit_hw(profile, step_s, pre_roll_s, warmup_s, max_dist) - else: - fitted, per_amplitude, traces = _fit_selftest(profile) - - provenance = Provenance( - robot_id=robot_id, + instance = Characterizer( + robot=args.robot, + mode=args.mode, + out=args.out, + robot_id=args.robot_id, surface=args.surface, - mode=args.gait_mode, - date=date.today().isoformat(), - git_sha=git_sha(), - sim_or_hw="hw" if args.mode == "hw" else "self-test", - characterization_session_dir=( - f"(real {profile.name}, LCM SI)" if args.mode == "hw" else "(in-process self-test)" - ), - ) - cfg = derive_config( - fitted, - provenance, - per_amplitude=per_amplitude, - vx_max=profile.vx_max, - wz_max=profile.wz_max, - ) - if args.mode == "hw" and "vy" not in profile.excited_channels: - cfg.caveats.append( - f"vy was NOT characterized on hardware ({profile.name} does not " - "strafe in this gait); plant.vy / feedforward.K_vy are a " - "placeholder copy of vx. The benchmark paths are vx+wz only, so " - "this does not affect tuning; re-characterize vy if a " - "lateral-capable gait is used." - ) - - out_path = ( - Path(args.out).expanduser() - / f"{robot_id}_config_{args.mode}_{args.surface}_{provenance.date}_{provenance.git_sha}.json" + gait_mode=args.gait_mode, + step_s=args.step_s, + pre_roll_s=args.pre_roll_s, + odom_warmup=args.odom_warmup, + max_dist=args.max_dist, ) - cfg.to_json(out_path) - plot_path = out_path.with_suffix(".png") - _plot_fits(traces, provenance, profile, plot_path) - - tag = "ROBOT-VALID" if cfg.valid_for_tuning else "NOT robot-valid (plumbing check)" - print("\nFOPDT fit graph (the deliverable — model vs real data):") - print(f" {plot_path.resolve()}") - print(f"Config artifact [{tag}] (machine handoff for the benchmark):") - print(f" {out_path.resolve()}") + instance.start() if __name__ == "__main__": diff --git a/dimos/utils/characterization/modeling/fopdt.py b/dimos/utils/characterization/modeling/fopdt.py index deac609dd0..2e489eaa26 100644 --- a/dimos/utils/characterization/modeling/fopdt.py +++ b/dimos/utils/characterization/modeling/fopdt.py @@ -158,6 +158,9 @@ def fit_fopdt( *, noise_std: float | None = None, fit_window_s: tuple[float, float] | None = None, + min_deadtime: float | None = None, + two_stage: bool = False, + l_detect_sigma: float = 5.0, ) -> FopdtParams: """Fit FOPDT to a step-response trace. @@ -167,6 +170,18 @@ def fit_fopdt( ``noise_std`` (optional) is per-sample sigma for weighted least squares. ``fit_window_s`` is recorded into the result for traceability. + + ``min_deadtime`` (optional) floors the L lower bound. Use to prevent + the optimizer from claiming sub-sample-period precision on L: pass + the data's median sample interval, since you can't physically + resolve deadtime finer than your odom sample rate. + + ``two_stage`` (default ``False``): when ``True``, estimate L + directly from the data (first time ``|y| > l_detect_sigma * + noise_std``) and pin it, then fit only ``K`` and ``tau``. Removes + the joint-fit correlation between ``L`` and ``tau`` that lets the + optimizer trade off a tiny ``L`` for a slightly inflated ``tau``. + Requires ``noise_std`` to be set; falls back to joint fit if not. """ from scipy.optimize import curve_fit @@ -214,13 +229,95 @@ def fit_fopdt( ) K0, tau0, L0 = _initial_guess(t, y, u_step, noise_std) - p0 = (K0, tau0, L0) lo, hi = _bounds_for(u_step) + if min_deadtime is not None and min_deadtime > _L_MIN: + # Re-floor the L bound (and the initial guess) so the optimizer + # can't claim a deadtime smaller than the data's sample period. + l_floor = min(float(min_deadtime), hi[2]) + lo = (lo[0], lo[1], l_floor) + L0 = max(L0, l_floor) + p0 = (K0, tau0, L0) sigma = None if noise_std is not None and noise_std > 0: sigma = np.full_like(y, float(noise_std)) + # --- Two-stage path: detect L from the data, then fit (K, tau) --- + # with L pinned. Decouples L vs tau in the joint optimizer. + if two_stage and noise_std is not None and noise_std > 0: + band = float(l_detect_sigma) * float(noise_std) + above = np.flatnonzero(np.abs(y) > band) + L_detected = float(t[above[0]]) if above.size else L0 + L_detected = float(np.clip(L_detected, lo[2], hi[2])) + + def _model_pinned(t_, K, tau): + return fopdt_step_response(t_, K, tau, L_detected, u_step) + + try: + popt2, pcov2 = curve_fit( + _model_pinned, + t, + y, + p0=(K0, tau0), + bounds=((lo[0], lo[1]), (hi[0], hi[1])), + sigma=sigma, + absolute_sigma=False, + maxfev=5000, + ) + except Exception as e: + return FopdtParams( + K=float("nan"), + tau=float("nan"), + L=float("nan"), + K_ci=(float("nan"), float("nan")), + tau_ci=(float("nan"), float("nan")), + L_ci=(float("nan"), float("nan")), + rmse=float("nan"), + r_squared=float("nan"), + n_samples=int(t.size), + fit_window_s=fit_window, + degenerate=True, + converged=False, + reason=f"two-stage curve_fit failed: {type(e).__name__}: {e}", + initial_guess={"K": K0, "tau": tau0, "L": L_detected}, + ) + K = float(popt2[0]) + tau = float(popt2[1]) + L = L_detected + y_hat = _model_pinned(t, K, tau) + resid = y - y_hat + rmse = float(np.sqrt(np.mean(resid**2))) if resid.size else float("nan") + ss_res = float(np.sum(resid**2)) + y_mean = float(np.mean(y)) + ss_tot = float(np.sum((y - y_mean) ** 2)) + r2 = 1.0 - ss_res / ss_tot if ss_tot > 0 else float("nan") + diag2 = np.diag(pcov2) + degenerate = bool(np.any(~np.isfinite(diag2)) or np.any(diag2 <= 0)) + if degenerate: + K_ci = (float("nan"), float("nan")) + tau_ci = (float("nan"), float("nan")) + else: + s2 = np.sqrt(diag2) + K_ci = (K - 1.96 * float(s2[0]), K + 1.96 * float(s2[0])) + tau_ci = (tau - 1.96 * float(s2[1]), tau + 1.96 * float(s2[1])) + # L CI is degenerate by construction (pinned, not estimated). + return FopdtParams( + K=K, + tau=tau, + L=L, + K_ci=K_ci, + tau_ci=tau_ci, + L_ci=(L, L), + rmse=rmse, + r_squared=r2, + n_samples=int(t.size), + fit_window_s=fit_window, + degenerate=degenerate, + converged=True, + reason=None, + initial_guess={"K": K0, "tau": tau0, "L": L_detected}, + ) + def _model(t_, K, tau, L): return fopdt_step_response(t_, K, tau, L, u_step) From 0b0297b3cdd9967594abe2822e3043c41444c336 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 17:45:43 -0700 Subject: [PATCH 25/51] mem2 characterization recorder module added --- .../benchmarking/characterization_recorder.py | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 dimos/utils/benchmarking/characterization_recorder.py diff --git a/dimos/utils/benchmarking/characterization_recorder.py b/dimos/utils/benchmarking/characterization_recorder.py new file mode 100644 index 0000000000..4228b30017 --- /dev/null +++ b/dimos/utils/benchmarking/characterization_recorder.py @@ -0,0 +1,98 @@ +# 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. + +"""Telemetry recorder for the Go2 characterization blueprint. + +Captures the live streams a characterization session produces — commanded +twist, coord-aggregated joint_state, raw odom, and operator gate events +— into a per-session SQLite DB so post-process tools can re-fit, dissect +spikes, or compare runs without re-running on hardware. + +The DB lands next to the JSON+PNG artifact at +``/data/characterization//_recording__.db`` +by default. Read it back with:: + + from dimos.memory2.store.sqlite import SqliteStore + store = SqliteStore(path="") + store.start() + for obs in store.stream("joint_state", JointState).iterate_ts(): + ts, msg = obs.ts, obs.data + # re-fit, plot, etc. +""" + +from __future__ import annotations + +from datetime import date +from pathlib import Path + +from dimos.core.core import rpc +from dimos.core.stream import In +from dimos.memory2.module import Recorder, RecorderConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.utils.benchmarking.tuning import git_sha +from dimos.utils.path_utils import get_project_root + +DEFAULT_OUT_DIR = get_project_root() / "data" / "characterization" + + +class CharacterizationRecorderConfig(RecorderConfig): + """Same as :class:`RecorderConfig` but with per-session db_path + resolution from ``out_dir`` + ``robot_id``. Set ``db_path`` explicitly + to bypass the default naming convention.""" + + out_dir: str | None = None # None -> /data/characterization/ + robot_id: str = "go2" + # Timestamped filenames make rerun-safe defaults; never silently + # clobber prior recordings. + overwrite: bool = False + + +class CharacterizationRecorder(Recorder): + """Records the streams a characterization session emits. + + Composed alongside :class:`~dimos.utils.benchmarking.characterization.Characterizer` + in the ``unitree-go2-characterization`` blueprint; ports are wired + to the same LCM topics the rest of the stack already uses (LCM is + multicast — additional subscribers are free). + """ + + config: CharacterizationRecorderConfig + + cmd_vel: In[Twist] # commanded /cmd_vel during each SI step + joint_state: In[JointState] # /coordinator/joint_state (x, y, yaw) + odom: In[PoseStamped] # raw /go2/odom from GO2Connection + gate: In[Int8] # operator gate events (advance/skip/quit) + + @rpc + def start(self) -> None: + # Resolve a per-session db_path before super().start() opens the + # SqliteStore. Mirrors the JSON+PNG artifact naming so a + # session's three files (config.json, fits.png, recording.db) + # land in the same dir with the same date+sha suffix. + out_dir = ( + Path(self.config.out_dir).expanduser() + if self.config.out_dir + else DEFAULT_OUT_DIR / self.config.robot_id + ) + out_dir.mkdir(parents=True, exist_ok=True) + self.config.db_path = ( + out_dir / f"{self.config.robot_id}_recording_{date.today().isoformat()}_{git_sha()}.db" + ) + super().start() + + +__all__ = ["CharacterizationRecorder", "CharacterizationRecorderConfig"] From 8cb2952d82844ea2417da9fb41678c2be09831be Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 17:45:57 -0700 Subject: [PATCH 26/51] added characterization blueprint --- dimos/robot/all_blueprints.py | 4 + .../basic/unitree_go2_characterization.py | 88 +++++++++++++++++ .../benchmarking/reports/tuning_README.md | 97 +++++++++++++++++++ 3 files changed, 189 insertions(+) create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index dd43cda2fd..6999adcc62 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -98,6 +98,7 @@ "unitree-go2-agentic-huggingface": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_huggingface:unitree_go2_agentic_huggingface", "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", + "unitree-go2-characterization": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_characterization:unitree_go2_characterization", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet", @@ -129,6 +130,8 @@ "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", + "characterization-recorder": "dimos.utils.benchmarking.characterization_recorder.CharacterizationRecorder", + "characterizer": "dimos.utils.benchmarking.characterization.Characterizer", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", "demo-calculator-skill": "dimos.agents.skills.demo_calculator_skill.DemoCalculatorSkill", @@ -195,6 +198,7 @@ "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", "receiver-module": "dimos.utils.demo_image_encoding.ReceiverModule", "recorder": "dimos.memory2.module.Recorder", + "reference-governor": "dimos.navigation.reference_governor.reference_governor.ReferenceGovernor", "reid-module": "dimos.perception.detection.reid.module.ReidModule", "replanning-a-star-planner": "dimos.navigation.replanning_a_star.module.ReplanningAStarPlanner", "rerun-bridge-module": "dimos.visualization.rerun.bridge.RerunBridgeModule", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py new file mode 100644 index 0000000000..da9ec9e242 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py @@ -0,0 +1,88 @@ +#!/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 FOPDT characterization — one-terminal HW flow. + +Bundles GO2Connection + ControlCoordinator + (publish-only-when-active) +pygame keyboard teleop with the :class:`Characterizer` module so the +operator runs a single command: + + dimos run unitree-go2-characterization + +instead of the two-terminal flow (``dimos run +unitree-go2-webrtc-keyboard-teleop`` + ``python -m +dimos.utils.benchmarking.characterization``). All operator input goes +through the pygame window: + + * **WASD/QE** — reposition the robot between steps (existing teleop). + * **ENTER** — advance to the next SI step. + * **K** — skip the current amplitude. + * **Backspace** — quit (no artifact written). + +Why the gate stream: ``dimos run`` deploys modules into forkserver +worker subprocesses that don't share the parent CLI's TTY, so +``input()`` inside the Characterizer would EOF immediately. Routing the +operator's ENTER/K/Backspace through KeyboardTeleop's pygame event loop +(which already owns its own X11 window for movement keys) and an +``Out[str]`` -> ``In[str]`` stream avoids stdin entirely. + +For ``--mode self-test`` (pure in-process math) keep using the CLI +entrypoint. For end-to-end sim characterization, bring up +``coordinator-sim-fopdt`` in one terminal and run the CLI with +``--mode hw`` in another. +""" + +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.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( + unitree_go2_coordinator, +) +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.utils.benchmarking.characterization import Characterizer +from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder + +unitree_go2_characterization = autoconnect( + unitree_go2_coordinator, + KeyboardTeleop.blueprint(publish_only_when_active=True), + Characterizer.blueprint(robot="go2", mode="hw", gate_source="stream"), + CharacterizationRecorder.blueprint(robot_id="go2"), +).transports( + { + # Operator gate events from the pygame window -> Characterizer. + # autoconnect pairs KeyboardTeleop.gate (Out) with + # Characterizer.gate (In) by name+type; the LCM transport gives + # them a wire since the modules live in different worker + # subprocesses. Int8 carries the gate code (0=advance, 1=skip, + # 2=quit) — see GATE_* constants in keyboard_teleop. + ("gate", Int8): LCMTransport("/characterizer/gate", Int8), + # CharacterizationRecorder taps the LCM topics the rest of the + # stack already publishes on. LCM is multicast so additional + # subscribers are free. The recorder's `odom: In[PoseStamped]` + # port is named plain `odom` — GO2Connection's outbound odom is + # remapped to `go2_odom` on the bus, so we explicitly route the + # recorder's port to /go2/odom here. + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } +) + +__all__ = ["unitree_go2_characterization"] diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index a7810581cd..8b59867065 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -66,6 +66,100 @@ result; a different robot's headroom is TBD until characterized.) ## Tool 1 — `characterization` +For Go2 HW the preferred entrypoint is the all-in-one blueprint +(one terminal, autoconnects GO2Connection + ControlCoordinator + pygame +keyboard teleop + the Characterizer module + a per-session telemetry +recorder): + +``` +dimos run unitree-go2-characterization +``` + +All operator input goes through the pygame teleop window. Movement keys +(WASD/QE) are unchanged; the SI loop adds three gate keys that the +operator presses while the pygame window has focus: + +- **Enter** — advance to the next step +- **K** — skip the current amplitude +- **Backspace** — quit (no artifact written) + +Mechanism: `KeyboardTeleop.gate: Out[Int8]` publishes those events on +`/characterizer/gate`, `Characterizer.gate: In[Int8]` consumes them. +Other blueprints that bundle `KeyboardTeleop` are unaffected (the new +port is just unwired). Configuration fields map 1:1 to CLI flags, so +``dimos run unitree-go2-characterization --module.characterizer.surface +grass`` is equivalent to ``--surface grass`` on the CLI. + +### Telemetry recording (blueprint only) + +The blueprint composes a `CharacterizationRecorder` that captures four +streams into a per-session SQLite DB so post-process tools can re-fit +or dissect spikes without re-running on hardware: + +- `cmd_vel` (commanded `/cmd_vel`) +- `joint_state` (`/coordinator/joint_state` — x/y/yaw) +- `odom` (raw `/go2/odom` from GO2Connection) +- `gate` (operator advance/skip/quit events) + +The DB lands next to the JSON+PNG artifact: +``/data/characterization//_recording__.db``. +Read back with: + +```python +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.sensor_msgs.JointState import JointState + +store = SqliteStore(path="") +store.start() +for obs in store.stream("joint_state", JointState).iterate_ts(): + ts, msg = obs.ts, obs.data + # re-fit, plot, etc. +``` + +### Noise rejection + fit procedure (legged-base data) + +Raw Go2 odom (~15-19 Hz on default tick) carries a ~2 Hz body bob from +the trot gait plus single-sample spikes from leg impacts. The per-step +pipeline before `fit_fopdt`: + +1. **Buffer raw pose** during pre-roll + step (``_JointStatePoseStream``). + The pre-roll samples are zero-commanded → robot at rest → their + std becomes our `noise_std` estimate for the fit (see step 4). +2. **Savitzky-Golay on position, then central difference**. + ``reconstruct_body_velocities`` applies ``scipy.signal.savgol_filter`` + to (x, y, yaw) (default window=11 ≈ 2 trot cycles, order=2), then + ``np.gradient`` to recover (vx, vy, dyaw). Smooth-then-differentiate + keeps gait-frequency noise out of the velocity signal — + differentiate-then-smooth (the obvious wrong order) can't recover + what the differentiation step destroyed. Knobs: ``savgol_window`` + / ``savgol_order``; set window=0 to disable. +3. **Hampel filter** on reconstructed velocity. Catches residual + single-sample spikes that savgol smooths over but doesn't replace. + Window=11, n_sigma=3.0; set window=0 to disable. +4. **`fit_fopdt` with three guards** to keep the deadtime estimate + honest at our sample rate: + - ``min_deadtime``: floor L at the data's median sample interval + (you can't physically resolve deadtime finer than odom rate). + - ``noise_std``: from step 1; used for weighted least squares and + for the data-driven L detection. + - ``two_stage=True``: detect L as the first time the response + crosses ``5σ`` above ``noise_std``, pin it, then fit only (K, τ). + Removes the joint-fit's L/τ correlation that lets the optimizer + trade off a tiny L for a slightly inflated τ. Auto-disabled if + ``noise_std`` can't be estimated. Toggle via + ``CharacterizerConfig.two_stage_fit``. + +The PNG plot dots the raw post-reconstruction velocity under the +filtered+fit lines when Hampel replaced any points, and annotates how +many. Note that in two-stage mode the reported L corresponds to "when +the response crossed 5σ above the rest-noise floor" — not a free +optimization parameter — so cross-channel L comparisons are +interpretable. + +The two-terminal CLI flow still works and is required for +``--mode self-test`` (no robot needed) and for the end-to-end sim +variant via ``coordinator-sim-fopdt``: + ``` uv run python -m dimos.utils.benchmarking.characterization \ --robot go2 --mode hw --surface concrete --gait-mode default @@ -84,6 +178,9 @@ Drift is bounded to one step (operator gate before each). Safety: clamp to the profile envelope, stale-odom abort, distance + time caps, zero-Twist on exit / Ctrl-C / `q`. +Artifacts land at `/data/characterization//_config_<…>.{json,png}` +by default (overridable via `--out` / `CharacterizerConfig.out`). + **Primary output is a graph** — `_config_<…>.png`, one column per channel overlaying every step's *measured* velocity (solid) with its *fitted FOPDT* step response (dashed), annotated K/τ/L/r² per amplitude From 2dfea76eeaa8f9478532016722a8a02991248c0d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 20:39:27 -0700 Subject: [PATCH 27/51] modified recorder to support benchmarking blueprint --- .../benchmarking/characterization_recorder.py | 29 ++++++++++++------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/dimos/utils/benchmarking/characterization_recorder.py b/dimos/utils/benchmarking/characterization_recorder.py index 4228b30017..adcef87c60 100644 --- a/dimos/utils/benchmarking/characterization_recorder.py +++ b/dimos/utils/benchmarking/characterization_recorder.py @@ -12,16 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Telemetry recorder for the Go2 characterization blueprint. +"""Telemetry recorder for the Go2 characterization / benchmark blueprints. -Captures the live streams a characterization session produces — commanded -twist, coord-aggregated joint_state, raw odom, and operator gate events -— into a per-session SQLite DB so post-process tools can re-fit, dissect +Captures the live streams a session produces — commanded twist, +coord-aggregated joint_state, raw odom, and operator gate events — +into a per-session SQLite DB so post-process tools can re-fit, dissect spikes, or compare runs without re-running on hardware. -The DB lands next to the JSON+PNG artifact at -``/data/characterization//_recording__.db`` -by default. Read it back with:: +Per-session DB filenames have a ``tag`` to differentiate use cases +(characterization runs vs benchmark runs land in separate DBs even on +the same date+sha): ``___.db``. By default +files land in ``/data/characterization//`` — both the +``out_dir`` and ``tag`` are config-settable so the benchmark blueprint +can route to ``/data/benchmark//`` with ``tag="benchmark"``. + +Read back with:: from dimos.memory2.store.sqlite import SqliteStore store = SqliteStore(path="") @@ -51,11 +56,12 @@ class CharacterizationRecorderConfig(RecorderConfig): """Same as :class:`RecorderConfig` but with per-session db_path - resolution from ``out_dir`` + ``robot_id``. Set ``db_path`` explicitly - to bypass the default naming convention.""" + resolution from ``out_dir`` + ``robot_id`` + ``tag``. Set ``db_path`` + explicitly to bypass the default naming convention.""" - out_dir: str | None = None # None -> /data/characterization/ + out_dir: str | None = None # None -> DEFAULT_OUT_DIR / robot_id: str = "go2" + tag: str = "recording" # filename discriminator: "recording" (char), "benchmark" (bench) # Timestamped filenames make rerun-safe defaults; never silently # clobber prior recordings. overwrite: bool = False @@ -90,7 +96,8 @@ def start(self) -> None: ) out_dir.mkdir(parents=True, exist_ok=True) self.config.db_path = ( - out_dir / f"{self.config.robot_id}_recording_{date.today().isoformat()}_{git_sha()}.db" + out_dir + / f"{self.config.robot_id}_{self.config.tag}_{date.today().isoformat()}_{git_sha()}.db" ) super().start() From b1a751dcf3dbfee051a6be03828bcd5359b10224 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 20:39:51 -0700 Subject: [PATCH 28/51] added benchmark module --- .../tasks/baseline_path_follower_task.py | 50 +- dimos/utils/benchmarking/benchmark.py | 455 +++++++++++++----- 2 files changed, 383 insertions(+), 122 deletions(-) diff --git a/dimos/control/tasks/baseline_path_follower_task.py b/dimos/control/tasks/baseline_path_follower_task.py index 0c037c9929..62efc9f472 100644 --- a/dimos/control/tasks/baseline_path_follower_task.py +++ b/dimos/control/tasks/baseline_path_follower_task.py @@ -141,6 +141,12 @@ def __init__( # 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 @@ -201,8 +207,26 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: # Static gain compensation: cmd_to_robot = controller_cmd / K_plant vx, vy, wz = self._ff.compute(vx, vy, wz) - # Curvature velocity-profile cap (preserves commanded turn radius). - if self._profile_cap is not None: + # Speed cap: prefer a precomputed per-waypoint profile (e.g. from + # the Benchmarker's --rg arm, shipped as a list[float] across RPC) + # over the auto-built curvature-based PathSpeedCap. Both preserve + # the commanded turn radius by scaling (vx, vy, wz) uniformly. + # + # Lookahead window mirrors PathSpeedCap.speed_limit_at — take the + # min over the next ~8 waypoints so braking starts BEFORE a corner + # rather than at it. Keeps arm-to-arm comparison fair across the + # static-profile arm and the RG arm. + 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 ) @@ -346,7 +370,12 @@ def configure( ) return True - def start_path(self, path: Path, current_odom: PoseStamped) -> bool: + def start_path( + self, + path: Path, + current_odom: PoseStamped, + velocity_profile: list[float] | None = None, + ) -> bool: if path is None or len(path.poses) < 2: logger.warning(f"BaselinePathFollowerTask '{self._name}': invalid path") return False @@ -361,6 +390,21 @@ def start_path(self, path: Path, current_odom: PoseStamped) -> bool: self._ff.reset() if self._profile_cap is not None: self._profile_cap.for_path(path) + # Install the optional precomputed per-waypoint speed profile. + # Length must match path.poses; falls back to clearing if mismatched. + if velocity_profile is not None and len(velocity_profile) == len(path.poses): + self._velocity_profile = np.asarray(velocity_profile, dtype=float) + self._velocity_profile_pts = np.array( + [[p.position.x, p.position.y] for p in path.poses], dtype=float + ) + else: + if velocity_profile is not None: + logger.warning( + f"BaselinePathFollowerTask '{self._name}': velocity_profile length " + f"{len(velocity_profile)} != path.poses {len(path.poses)}; ignoring" + ) + self._velocity_profile = None + self._velocity_profile_pts = None first_yaw = path.poses[0].orientation.euler[2] robot_yaw = current_odom.orientation.euler[2] diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py index 139b8ae5e3..22e20af19c 100644 --- a/dimos/utils/benchmarking/benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -36,24 +36,30 @@ from __future__ import annotations import argparse +from collections.abc import Callable from dataclasses import asdict import json import math import os from pathlib import Path -import sys +import queue import threading import time +from typing import Any, Literal import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt +from reactivex.disposable import Disposable from dimos.control.components import make_twist_base_joints from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.feedforward_gain_compensator import FeedforwardGainConfig +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig from dimos.core.rpc_client import RPCClient +from dimos.core.stream import In from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -61,6 +67,8 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Path import Path as NavPath from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.robot.unitree.keyboard_teleop import GATE_ADVANCE, GATE_QUIT, GATE_SKIP from dimos.utils.benchmarking.paths import circle, single_corner, square, straight_line from dimos.utils.benchmarking.plant import ROBOT_PLANT_PROFILES, RobotPlantProfile from dimos.utils.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run @@ -71,6 +79,7 @@ invert_tolerance, ) from dimos.utils.benchmarking.velocity_profile import VelocityProfileConfig +from dimos.utils.path_utils import get_project_root # Well-known topic the operator coord publishes its JointState Out on. # Positions carry [x,y,yaw] (ConnectedTwistBase populates them from @@ -85,6 +94,9 @@ _FAILED_STATES = frozenset({"aborted"}) REPORTS_DIR = Path(__file__).parent / "reports" +# New default landing dir for benchmark plots + standalone-arm JSONs. +# REPORTS_DIR is retained for legacy callers only; new artifacts go here. +DEFAULT_OUT_DIR = get_project_root() / "data" / "benchmark" def _resolve_profile(name: str) -> RobotPlantProfile: @@ -279,6 +291,7 @@ def _run_baseline( profile_config: VelocityProfileConfig | None, timeout_s: float, label: str, + velocity_profile: list[float] | None = None, ) -> tuple[ExecutedTrajectory, NavPath]: """Send a path to the operator coord's ``baseline_follower`` task and wait for it to terminate. The task is pre-added by the operator's @@ -308,7 +321,13 @@ def _run_baseline( print(f" [{label}] configure rejected — task still active from prior run?") return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w - if not _invoke(coord, "start_path", path=path_w, current_odom=pose0): + start_kwargs: dict[str, Any] = {"path": path_w, "current_odom": pose0} + if velocity_profile is not None: + # Path was rigid-transformed by _shift_path_to_start_at_pose; + # waypoint count + local curvature are preserved, so the + # per-waypoint speeds computed on the original path remain valid. + start_kwargs["velocity_profile"] = velocity_profile + if not _invoke(coord, "start_path", **start_kwargs): print(f" [{label}] start_path rejected") return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w @@ -363,24 +382,71 @@ def _run_ladder( use_profile: bool, coord_rpc: RPCClient, recorder: _JointStateRecorder, + use_rg: bool = False, + e_max: float = 0.05, + rg_min_speed: float | None = None, + gate_input: Callable[[str], str] = input, + gate_keys_label: str = "ENTER=run s=skip q=quit", ) -> tuple[list[OperatingPoint], list[dict]]: # Bare stock baseline by default: this is the physical-limit - # measurement. FF / velocity profile are opt-in comparison arms. + # measurement. FF / velocity profile / RG are opt-in comparison arms. ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) + # Build RG constraint set + plant once if --rg arm is enabled. We + # import the math directly from the reference_governor module + # (solve_profile + the constraint classes are public module-level + # exports); no RG Module composition needed. + rg_constraints = None + rg_plant = None + rg_vp = None + if use_rg: + from dimos.navigation.reference_governor.reference_governor import ( + GeometricMVC, + LateralMVC, + PrecisionMVC, + SaturationMVC, + solve_profile, + ) + + rg_plant = cfg.plant + rg_vp = cfg.velocity_profile + # Closure over e_max so PrecisionMVC stays decoupled from a + # specific value — same callable contract RG itself uses. + rg_constraints = [ + GeometricMVC(v_max=rg_vp.max_linear_speed), + SaturationMVC(omega_max=rg_vp.max_angular_speed), + LateralMVC(a_lat_max=rg_vp.max_centripetal_accel), + PrecisionMVC(e_max_provider=lambda: e_max), + ] + _solve_profile = solve_profile + points: list[OperatingPoint] = [] runs: list[dict] = [] # for the XY trajectory overlay for name, path in _path_set().items(): + # Precompute RG-derived per-waypoint speeds once per path (same + # for every speed cell — RG output is path-shape dependent, not + # commanded-speed dependent). Plain list[float] crosses the + # configure RPC cleanly. + rg_speeds: list[float] | None = None + if use_rg and rg_constraints is not None and rg_plant is not None and rg_vp is not None: + arr = _solve_profile( + path, + rg_plant, + rg_constraints, + accel_max=rg_vp.max_linear_accel, + decel_max=rg_vp.max_linear_decel, + min_speed=(rg_min_speed if rg_min_speed is not None else rg_vp.min_speed), + ) + rg_speeds = [float(v) for v in arr] for speed in speeds: prof_cfg = ( cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None ) if mode == "hw": resp = ( - input( - f"\n[{name} v={speed:.2f}] reposition+aim robot, " - f"ENTER=run s=skip q=quit: " + gate_input( + f"\n[{name} v={speed:.2f}] reposition+aim robot, {gate_keys_label}: " ) .strip() .lower() @@ -401,6 +467,7 @@ def _run_ladder( prof_cfg, timeout_s, f"{name}@{speed:.2f}", + velocity_profile=rg_speeds, ) # Score/plot against the executed-frame reference (anchored path). s = score_run(ref, traj) @@ -548,6 +615,230 @@ def _prereq_banner(profile: RobotPlantProfile, mode: str) -> None: ) +class BenchmarkerConfig(ModuleConfig): + """Config for :class:`Benchmarker`. Each field mirrors a CLI flag on + the existing ``benchmark`` entrypoint. + + ``gate_source`` selects how each (path, speed) cell is gated. + ``"stdin"`` (default, CLI mode) reads ENTER/s/q from the terminal; + ``"stream"`` (blueprint mode) consumes events from the ``gate`` In + port wired to a co-running ``KeyboardTeleop`` that publishes + ENTER/K/Backspace as ``GATE_ADVANCE / GATE_SKIP / GATE_QUIT``. + """ + + robot: str = "go2" + config: str | None = None # path to characterization artifact (required at runtime) + mode: Literal["hw", "sim"] = "hw" + speeds: str = "0.3,0.5,0.7,0.9,1.0" + tolerances: str = "5,10,15" + timeout: float = 60.0 + ff: bool = False + profile: bool = False + # OPT-IN: apply RG-derived per-waypoint speed profile (uses the + # artifact's plant + cfg.e_max for the precision constraint). + rg: bool = False + e_max: float = 0.05 + # RG OUTPUT FLOOR (m/s). Overrides artifact.velocity_profile.min_speed + # for the RG arm. Default 0.2 matches the Go2's practical min motion + # threshold — characterization typically writes ~0.05 into the + # artifact, which is below where the platform actually moves, so the + # RG output at SaturationMVC-binding corners falls below threshold + # and the robot stalls. Set explicitly (or None to defer to the + # artifact) for other platforms. + min_speed: float | None = 0.2 + # OPT-IN: output directory for plots + standalone-arm JSONs (the + # input config artifact augmentation always lands at args.config). + out_dir: str | None = None + gate_source: Literal["stdin", "stream"] = "stdin" + + +class Benchmarker(Module): + """Module wrapper around the operating-point benchmark sequence. + + Driven via the CLI shim in :func:`main` (``gate_source="stdin"``) or + composed into the all-in-one blueprint ``unitree-go2-benchmark`` + (``gate_source="stream"``). ``start()`` blocks for the full + operator-gated speed-ladder loop, then returns. + + See :mod:`dimos.utils.benchmarking.characterization`'s ``Characterizer`` + for the same pattern — gate stream is the only way to drive operator + confirmation in the blueprint context because module workers don't + share the parent CLI's TTY. + """ + + config: BenchmarkerConfig + + gate: In[Int8] + + _gate_queue: queue.Queue[str] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._gate_queue = queue.Queue() + + @rpc + def start(self) -> None: + super().start() + if self.config.gate_source == "stream": + self.register_disposable(Disposable(self.gate.subscribe(self._on_gate_event))) + self._run() + + def _on_gate_event(self, msg: Int8) -> None: + # Translate pygame-side gate codes to the legacy CLI vocab so + # _run_ladder's response check (""=run, "s"=skip, "q"=quit) is + # unchanged. Mirrors Characterizer._on_gate_event. + code = int(msg.data) + translated = {GATE_ADVANCE: "", GATE_SKIP: "s", GATE_QUIT: "q"}.get(code, "") + self._gate_queue.put(translated) + + def _wait_gate_stream(self, prompt: str) -> str: + print(prompt, end="", flush=True) + return self._gate_queue.get() + + def _run(self) -> None: + cfg = self.config + if not cfg.config: + raise SystemExit("benchmark: --config (path to characterization artifact) is required") + + profile = _resolve_profile(cfg.robot) + config_path = Path(cfg.config).expanduser() + artifact = TuningConfig.from_json(config_path) # asserts schema_version + speeds = [float(s) for s in cfg.speeds.split(",")] + tolerances = [float(t) for t in cfg.tolerances.split(",")] + arm = ( + "+".join( + x for x, on in (("ff", cfg.ff), ("profile", cfg.profile), ("rg", cfg.rg)) if on + ) + or "bare" + ) + + # Sim-derived ff/profile/rg are only meaningless on the real robot + # if you actually apply them; the bare baseline doesn't use them. + if cfg.mode == "hw" and (cfg.ff or cfg.profile or cfg.rg) and not artifact.valid_for_tuning: + raise SystemExit( + f"Refusing --mode hw with --{arm} and a non-robot-valid config " + f"({config_path.name}, sim_or_hw={artifact.provenance.sim_or_hw!r}): its " + "feedforward/profile/plant were derived from the sim plant. Re-run " + "`characterization --mode hw` first, drop --ff/--profile/--rg for " + "the bare physical-limit run, or use --mode sim." + ) + if cfg.mode == "sim": + print( + "[pre-check] --mode sim: validates wiring against the FOPDT sim " + "plant only; the operating-point map is NOT a real-robot result." + ) + + _prereq_banner(profile, cfg.mode) + + arm_desc = ( + "BARE stock baseline (no FF, no profile, no RG) — the plant's physical tracking limit" + if arm == "bare" + else f"baseline + {arm} (comparison arm, vs the bare physical limit)" + ) + print( + f"{profile.name} {cfg.mode} speed ladder {speeds} over {len(_path_set())} paths\n" + f" controller: {arm_desc}\n" + f" k_angular={artifact.recommended_controller.params.get('k_angular')}" + ) + coord_rpc: RPCClient = RPCClient(None, ControlCoordinator) + joints = make_twist_base_joints(profile.joints_prefix) + recorder = _JointStateRecorder(joint_names=joints) + js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) + js_unsub = js_sub.subscribe(recorder.on_joint_state) + + # Gate input wiring (mirrors Characterizer._run). + if cfg.gate_source == "stream": + gate_input: Callable[[str], str] = self._wait_gate_stream + gate_keys_label = "focus pygame window: ENTER=run K=skip Backspace=quit" + else: + gate_input = input + gate_keys_label = "ENTER=run s=skip q=quit" + + try: + points, runs = _run_ladder( + artifact, + profile, + speeds, + cfg.timeout, + cfg.mode, + use_ff=cfg.ff, + use_profile=cfg.profile, + coord_rpc=coord_rpc, + recorder=recorder, + use_rg=cfg.rg, + e_max=cfg.e_max, + rg_min_speed=cfg.min_speed, + gate_input=gate_input, + gate_keys_label=gate_keys_label, + ) + except KeyboardInterrupt: + points, runs = [], [] + print("\n[hw] aborted by operator — robot stopped, no artifact written.") + os._exit(1) + + # === ARTIFACTS FIRST (before any teardown that might segfault) === + inversion = invert_tolerance(points, tolerances) + opm = OperatingPointMap(speeds=speeds, points=points, tolerance_inversion=inversion) + + sha = artifact.provenance.git_sha + rid = artifact.provenance.robot_id + out_root = Path(cfg.out_dir).expanduser() if cfg.out_dir else DEFAULT_OUT_DIR / rid + out_root.mkdir(parents=True, exist_ok=True) + # Only the BARE run defines section 5 (the canonical physical-limit + # operating-point map). Comparison arms emit standalone artifacts so + # they never clobber the physical-limit map in the config. + if arm == "bare": + artifact.operating_point_map = opm + artifact.to_json(config_path) + artifact_msg = ( + f"Augmented artifact (section 5 = physical limit): {config_path.resolve()}" + ) + else: + artifact_msg = ( + f"Config NOT modified (arm '{arm}' is a comparison, not the " + f"physical-limit map). See standalone outputs below." + ) + bench_path = out_root / f"{rid}_benchmark_{arm}_{sha}.json" + bench_path.write_text(json.dumps(asdict(opm), indent=2)) + plot_path = out_root / f"{rid}_benchmark_cte_vs_speed_{arm}_{sha}.png" + _plot(points, plot_path, profile.name, arm) + xy_path = out_root / f"{rid}_benchmark_xy_{arm}_{sha}.png" + _plot_xy(runs, xy_path, profile.name, arm) + + print(f"\n{artifact_msg}") + print(f"Benchmark json : {bench_path.resolve()}") + print(f"CTE-vs-speed plot : {plot_path.resolve()}") + print(f"XY trajectory plot : {xy_path.resolve()} <-- the diagnostic view") + print("\nOperating-point recommendation:") + for row in inversion: + if row.max_speed is None: + print( + f" tolerance {row.tol_cm:g} cm: NO tested speed keeps every " + f"path within tolerance — relax the tolerance or slow the fleet." + ) + else: + print( + f" For tolerance {row.tol_cm:g} cm, run at speed " + f"{row.max_speed:.2f} m/s with this profile " + f"(binding path: {row.binding_path})." + ) + + # === BEST-EFFORT TEARDOWN (artifacts already on disk) === + for label, cleanup in ( + ("unsubscribe", js_unsub), + ("rpc client", coord_rpc.stop_rpc_client), + ): + try: + cleanup() + except Exception as e: + print(f" [cleanup warning] {label}: {e}") + + # Skip Python interp shutdown to avoid LCM/portal C-library teardown + # segfaults. Artifacts are already saved; nothing useful happens after + # this point. Exit code 0 (success). + os._exit(0) + + def main() -> None: ap = argparse.ArgumentParser(description="Twist-base operating-point benchmark") ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PLANT_PROFILES)}") @@ -568,122 +859,48 @@ def main() -> None: help="OPT-IN arm: apply the artifact's derived curvature velocity " "profile (default OFF — bare stock baseline)", ) - args = ap.parse_args() - - profile = _resolve_profile(args.robot) - config_path = Path(args.config).expanduser() - cfg = TuningConfig.from_json(config_path) # asserts schema_version - speeds = [float(s) for s in args.speeds.split(",")] - tolerances = [float(t) for t in args.tolerances.split(",")] - arm = "+".join(x for x, on in (("ff", args.ff), ("profile", args.profile)) if on) or "bare" - - # The sim-derived ff/profile are only meaningless on the real robot - # if you actually apply them; the bare baseline doesn't use them. - if args.mode == "hw" and (args.ff or args.profile) and not cfg.valid_for_tuning: - sys.exit( - f"Refusing --mode hw with --{arm} and a non-robot-valid config " - f"({config_path.name}, sim_or_hw={cfg.provenance.sim_or_hw!r}): its " - "feedforward/profile were derived from the sim plant. Re-run " - "`characterization --mode hw` first, drop --ff/--profile for " - "the bare physical-limit run, or use --mode sim." - ) - if args.mode == "sim": - print( - "[pre-check] --mode sim: validates wiring against the FOPDT sim " - "plant only; the operating-point map is NOT a real-robot result." - ) - - _prereq_banner(profile, args.mode) - - arm_desc = ( - "BARE stock baseline (no FF, no profile) — the plant's physical tracking limit" - if arm == "bare" - else f"baseline + {arm} (comparison arm, vs the bare physical limit)" + ap.add_argument( + "--rg", + action="store_true", + help="OPT-IN arm: apply RG-derived per-waypoint speed profile " + "(uses the same artifact + --e-max corridor; default OFF)", ) - print( - f"{profile.name} {args.mode} speed ladder {speeds} over {len(_path_set())} paths\n" - f" controller: {arm_desc}\n" - f" k_angular={cfg.recommended_controller.params.get('k_angular')}" + ap.add_argument( + "--e-max", + type=float, + default=0.05, + help="RG corridor half-width in m (only used when --rg is set)", ) - coord_rpc: RPCClient = RPCClient(None, ControlCoordinator) - joints = make_twist_base_joints(profile.joints_prefix) - recorder = _JointStateRecorder(joint_names=joints) - js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) - js_unsub = js_sub.subscribe(recorder.on_joint_state) - - try: - points, runs = _run_ladder( - cfg, - profile, - speeds, - args.timeout, - args.mode, - use_ff=args.ff, - use_profile=args.profile, - coord_rpc=coord_rpc, - recorder=recorder, - ) - except KeyboardInterrupt: - # Try not to lose accumulated data even on operator quit. - points, runs = [], [] - print("\n[hw] aborted by operator — robot stopped, no artifact written.") - os._exit(1) - - # === ARTIFACTS FIRST (before any teardown that might segfault) === - inversion = invert_tolerance(points, tolerances) - opm = OperatingPointMap(speeds=speeds, points=points, tolerance_inversion=inversion) - - sha = cfg.provenance.git_sha - rid = cfg.provenance.robot_id - # Only the BARE run defines section 5 (the canonical physical-limit - # operating-point map). Comparison arms emit standalone artifacts so - # they never clobber the physical-limit map in the config. - if arm == "bare": - cfg.operating_point_map = opm - cfg.to_json(config_path) - artifact_msg = f"Augmented artifact (section 5 = physical limit): {config_path.resolve()}" - else: - artifact_msg = ( - f"Config NOT modified (arm '{arm}' is a comparison, not the " - f"physical-limit map). See standalone outputs below." - ) - bench_path = REPORTS_DIR / f"{rid}_benchmark_{arm}_{sha}.json" - bench_path.parent.mkdir(parents=True, exist_ok=True) - bench_path.write_text(json.dumps(asdict(opm), indent=2)) - plot_path = REPORTS_DIR / f"{rid}_benchmark_cte_vs_speed_{arm}_{sha}.png" - _plot(points, plot_path, profile.name, arm) - xy_path = REPORTS_DIR / f"{rid}_benchmark_xy_{arm}_{sha}.png" - _plot_xy(runs, xy_path, profile.name, arm) - - print(f"\n{artifact_msg}") - print(f"Benchmark json : {bench_path.resolve()}") - print(f"CTE-vs-speed plot : {plot_path.resolve()}") - print(f"XY trajectory plot : {xy_path.resolve()} <-- the diagnostic view") - print("\nOperating-point recommendation:") - for row in inversion: - if row.max_speed is None: - print( - f" tolerance {row.tol_cm:g} cm: NO tested speed keeps every " - f"path within tolerance — relax the tolerance or slow the fleet." - ) - else: - print( - f" For tolerance {row.tol_cm:g} cm, run at speed " - f"{row.max_speed:.2f} m/s with this profile " - f"(binding path: {row.binding_path})." - ) + ap.add_argument( + "--min-speed", + type=float, + default=None, + help="override the RG arm's min_speed floor (default: artifact's " + "velocity_profile.min_speed). Useful when the robot's actual min " + "motion threshold is higher than what was characterized.", + ) + ap.add_argument( + "--out", + default=None, + help=f"output dir for plots + standalone-arm JSON (default: {DEFAULT_OUT_DIR}//)", + ) + args = ap.parse_args() - # === BEST-EFFORT TEARDOWN (artifacts already on disk) === - for label, cleanup in (("unsubscribe", js_unsub), ("rpc client", coord_rpc.stop_rpc_client)): - try: - cleanup() - except Exception as e: - print(f" [cleanup warning] {label}: {e}") - - # Skip Python interp shutdown to avoid LCM/portal C-library teardown - # segfaults. Artifacts are already saved; nothing useful happens after - # this point. Exit code 0 (success). - os._exit(0) + instance = Benchmarker( + robot=args.robot, + config=args.config, + mode=args.mode, + speeds=args.speeds, + tolerances=args.tolerances, + timeout=args.timeout, + ff=args.ff, + profile=args.profile, + rg=args.rg, + e_max=args.e_max, + min_speed=args.min_speed, + out_dir=args.out, + ) + instance.start() if __name__ == "__main__": From ecde3b1e86b91c6872eaf2f83e2fcc297009693d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 20:40:11 -0700 Subject: [PATCH 29/51] benchmark blueprint added --- dimos/robot/all_blueprints.py | 2 + .../blueprints/basic/unitree_go2_benchmark.py | 93 +++++++++++++++++++ .../benchmarking/reports/tuning_README.md | 42 ++++++++- 3 files changed, 133 insertions(+), 4 deletions(-) create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 6999adcc62..6e8a4c1fbf 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -98,6 +98,7 @@ "unitree-go2-agentic-huggingface": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_huggingface:unitree_go2_agentic_huggingface", "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", + "unitree-go2-benchmark": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark:unitree_go2_benchmark", "unitree-go2-characterization": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_characterization:unitree_go2_characterization", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", @@ -128,6 +129,7 @@ "arm-teleop-module": "dimos.teleop.quest.quest_extensions.ArmTeleopModule", "b-box-navigation-module": "dimos.navigation.bbox_navigation.BBoxNavigationModule", "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", + "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", "characterization-recorder": "dimos.utils.benchmarking.characterization_recorder.CharacterizationRecorder", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py new file mode 100644 index 0000000000..24487e1c5f --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py @@ -0,0 +1,93 @@ +#!/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 operating-point benchmark — one-terminal HW flow. + +Bundles GO2Connection + ControlCoordinator + pygame keyboard teleop + +the :class:`Benchmarker` module + a per-session telemetry recorder so +the operator runs a single command: + + dimos run unitree-go2-benchmark --module.benchmarker.config + +instead of the two-terminal CLI flow. Operator UX matches B1 +(``unitree-go2-characterization``): WASD/QE in the pygame window to +reposition/aim the robot between runs; ENTER to advance, K to skip, +Backspace to quit. + +Comparison arms are config flags (all default OFF — bare baseline): + + --module.benchmarker.ff=true # apply derived feedforward + --module.benchmarker.profile=true # apply derived static velocity profile + --module.benchmarker.rg=true \\ + --module.benchmarker.e_max=0.05 # apply RG-derived per-waypoint profile + +The RG arm uses ``solve_profile()`` imported directly from +``reference_governor.py`` — there is no RG ``Module`` in this blueprint +because the per-cell math is one-shot (no live ``e_max`` stream to react +to), and a Module wrapper would force a cross-process per-tick RPC on +the controller's hot path. The Benchmarker computes the per-waypoint +speeds once per path and ships them to the follower as a plain +``list[float]`` via the new ``BaselinePathFollowerTask.start_path( +velocity_profile=...)`` kwarg. + +Recordings land at +``/data/benchmark//_benchmark__.db`` +(tag="benchmark" so they don't collide with characterization recordings). + +For ``--mode sim`` (FOPDT plant pre-check, no robot), keep using the +CLI entrypoint against ``coordinator-sim-fopdt``. +""" + +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.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( + unitree_go2_coordinator, +) +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.utils.benchmarking.benchmark import Benchmarker +from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder +from dimos.utils.path_utils import get_project_root + +unitree_go2_benchmark = autoconnect( + unitree_go2_coordinator, + KeyboardTeleop.blueprint(publish_only_when_active=True), + Benchmarker.blueprint(robot="go2", mode="hw", gate_source="stream"), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag="benchmark", + out_dir=str(get_project_root() / "data" / "benchmark" / "go2"), + ), +).transports( + { + # Operator gate events from the pygame window -> Benchmarker. + # Distinct topic from B1 (`/characterizer/gate`) so the two + # blueprints don't cross-talk if both happen to run on the same + # LCM bus. + ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + # Recorder taps the same LCM topics the rest of the stack + # already uses; no new wires, just additional subscribers. + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } +) + +__all__ = ["unitree_go2_benchmark"] diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index 8b59867065..58f55fb784 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -203,6 +203,25 @@ differs. ## Tool 2 — `benchmark` +For Go2 HW the preferred entrypoint is the all-in-one blueprint +(one terminal, autoconnects GO2Connection + ControlCoordinator + pygame +keyboard teleop + Benchmarker + a per-session telemetry recorder): + +``` +dimos run unitree-go2-benchmark --module.benchmarker.config +``` + +Operator UX matches the characterization blueprint: WASD/QE in the +pygame window to reposition/aim the robot between runs; **Enter** to +advance, **K** to skip, **Backspace** to quit. Comparison arms map 1:1 +to CLI flags via `--module.benchmarker.` overrides; recordings +land at `/data/benchmark//_benchmark__.db` +(tag="benchmark" so they don't collide with characterization recordings +in the sibling `data/characterization/` dir). + +The two-terminal CLI flow still works and is required for `--mode sim` +(end-to-end pre-check against `coordinator-sim-fopdt`): + ``` uv run python -m dimos.utils.benchmarking.benchmark \ --robot go2 --config reports/go2_config_hw_concrete__.json \ @@ -228,10 +247,25 @@ clobber section 5: - `--ff` — apply the artifact's derived feedforward. - `--profile` — apply the artifact's derived curvature velocity profile. - `--ff --profile` — both (the fully-derived config). - -`--mode hw` only **refuses a non-robot-valid config when `--ff`/`--profile` -is set** (sim-derived gains are meaningless on the real robot). The bare -physical-limit run accepts any config. +- `--rg --e-max ` — apply the **reference governor's** per-waypoint + speed profile (geometric + saturation + lateral + precision + constraints, then forward/backward accel passes). `--e-max` (default + 0.05 m) is the corridor half-width fed into the precision constraint + `v ≤ e_max / max(τ_vx+L_vx, τ_wz+L_wz)`. The follower applies the + resulting list[float] cap via `start_path(velocity_profile=…)` — + the same 8-waypoint lookahead the static profile uses so braking + starts before each corner. + +The RG arm imports `solve_profile()` directly from the reference +governor module — there is no RG Module instance in this blueprint. The +math runs in-process inside the Benchmarker, and the resolved profile +crosses to the follower as a plain list of floats. RG-as-Module (live +`e_max` stream → reactive recompute) is a B3 concern. + +`--mode hw` **refuses a non-robot-valid config when any comparison arm +(`--ff` / `--profile` / `--rg`) is set** (sim-derived gains/plant are +meaningless on the real robot). The bare physical-limit run accepts +any config. `--mode sim`: optional fast pre-check. Same baseline + coordinator + `transport_lcm` path as hw, but the LCM peer is the From 35e5dbb18c618417cf7c917faac68b8206062d9e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 27 May 2026 20:44:34 -0700 Subject: [PATCH 30/51] updated readme --- .../benchmarking/reports/tuning_README.md | 517 ++---------------- 1 file changed, 54 insertions(+), 463 deletions(-) diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index 58f55fb784..ce86e9e5f1 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -1,503 +1,94 @@ -# Twist-base controller tuning — measure → derive → validate (HARDWARE) +# Twist-base controller tuning — operator guide -Two CLI tools that turn one real measurement of a velocity-commanded -mobile base into a single versioned config artifact with every parameter -needed to tune its path controller, then validate it on the real robot. -**Robot-agnostic**: everything robot-specific lives in a -`RobotPlantProfile` (`--robot`, default `go2`). Adding a robot = one -profile entry (see *Adding a robot* below); the two commands are -otherwise identical. +Two blueprints, run in order: -``` -characterization --robot R --mode hw ──▶ R_config_hw_*.json (robot-valid) -benchmark --robot R --mode hw --config … ──▶ same file + section 5 - "for tolerance X cm, run Y m/s" -``` - -Both tools run the baseline path follower **inside a real -`ControlCoordinator`** in this process, driving the existing -`transport_lcm` twist-base adapter. The operator brings up whichever -connection module owns the robot side of the LCM topics in another -terminal — `unitree-go2-webrtc-keyboard-teleop` for hw, the new -`coordinator-sim-fopdt` (an in-process FOPDT plant exposed on the same -`/{robot_id}/cmd_vel|odom`) for sim. **The two modes are architecturally -identical**: same coordinator, same adapter, same task; only the robot -on the other side differs. - -**This is a hardware deliverable.** Sim exists only as a plumbing -self-test / pre-check and is explicitly stamped not-robot-valid — never -tune from it. - -## Why these numbers (settled findings, not re-derived) +1. **`unitree-go2-characterization`** — measures the robot's FOPDT plant, writes a config JSON. +2. **`unitree-go2-benchmark`** — runs the baseline controller across a speed ladder, scores CTE, writes the operating-point map. -A velocity-commanded base is FOPDT per axis. At a given speed the -tracking error is the plant floor `(τ+L)·v`; no reactive control law -beats it. So the recommended controller is hardcoded to the production -baseline P-controller, and the only real levers — feedforward gain -(`1/K`) and a curvature velocity profile — are *derived from the measured -plant*, not hand-tuned. (The embedded evidence string cites the Go2 -result; a different robot's headroom is TBD until characterized.) +Both bundle GO2Connection + ControlCoordinator + pygame teleop + the relevant module + a per-session SQLite recorder. One terminal each. -## Prerequisites (real robot) +## Prerequisites -1. The host that reaches the robot (for the Go2 profile: - **`dimensional-gpu-0`**). -2. Terminal 1: `dimos run ` — for `--robot go2` that - is `unitree-go2-webrtc-keyboard-teleop`, which brings up the Go2 - connection (publishes the odom topic, consumes the cmd topic) **and** - a keyboard teleop for repositioning, run **publish-only-when-active** - (silent while idle, so it does not flood the cmd topic / fight the - tool). A different robot needs an equivalent bring-up blueprint that - speaks Twist on the profile's cmd topic + `PoseStamped` odom. -3. Terminal 2: strip nix from the linker path or `.venv` numpy breaks - (`GLIBC_2.38`): +1. From an X11 desktop terminal: ``` - export LD_LIBRARY_PATH="$(echo "$LD_LIBRARY_PATH" | tr ':' '\n' \ - | grep -v /nix/store | paste -sd:)" + cd ~/dimos && source .venv/bin/activate ``` -4. Repositioning: the robot is **stopped** at every prompt. Reposition - (Go2: keyboard teleop WASD/QE, then **release all keys** so it goes - silent), then press ENTER. The tool then owns the cmd topic for that - run. Do not hold teleop keys while a run is going. -5. Operator-tunable timings (defaults come from the profile): - `--step-s` (time safety cap), `--max-dist` (real-space bound — each - step ends at whichever of distance/time comes first; `wz` spins in - place so it ends on time), `--pre-roll-s`, `--odom-warmup`. - -## Tool 1 — `characterization` - -For Go2 HW the preferred entrypoint is the all-in-one blueprint -(one terminal, autoconnects GO2Connection + ControlCoordinator + pygame -keyboard teleop + the Characterizer module + a per-session telemetry -recorder): - -``` -dimos run unitree-go2-characterization -``` - -All operator input goes through the pygame teleop window. Movement keys -(WASD/QE) are unchanged; the SI loop adds three gate keys that the -operator presses while the pygame window has focus: - -- **Enter** — advance to the next step -- **K** — skip the current amplitude -- **Backspace** — quit (no artifact written) - -Mechanism: `KeyboardTeleop.gate: Out[Int8]` publishes those events on -`/characterizer/gate`, `Characterizer.gate: In[Int8]` consumes them. -Other blueprints that bundle `KeyboardTeleop` are unaffected (the new -port is just unwired). Configuration fields map 1:1 to CLI flags, so -``dimos run unitree-go2-characterization --module.characterizer.surface -grass`` is equivalent to ``--surface grass`` on the CLI. - -### Telemetry recording (blueprint only) +2. Robot powered on, network reachable. Clear ~3m × 3m of floor space. -The blueprint composes a `CharacterizationRecorder` that captures four -streams into a per-session SQLite DB so post-process tools can re-fit -or dissect spikes without re-running on hardware: +## Pygame controls (both blueprints) -- `cmd_vel` (commanded `/cmd_vel`) -- `joint_state` (`/coordinator/joint_state` — x/y/yaw) -- `odom` (raw `/go2/odom` from GO2Connection) -- `gate` (operator advance/skip/quit events) +Window must have focus. **WASD/QE** = reposition. **Enter** = advance. **K** = skip. **Backspace** = quit. -The DB lands next to the JSON+PNG artifact: -``/data/characterization//_recording__.db``. -Read back with: +## Step 1 — characterization -```python -from dimos.memory2.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.JointState import JointState - -store = SqliteStore(path="") -store.start() -for obs in store.stream("joint_state", JointState).iterate_ts(): - ts, msg = obs.ts, obs.data - # re-fit, plot, etc. -``` - -### Noise rejection + fit procedure (legged-base data) - -Raw Go2 odom (~15-19 Hz on default tick) carries a ~2 Hz body bob from -the trot gait plus single-sample spikes from leg impacts. The per-step -pipeline before `fit_fopdt`: - -1. **Buffer raw pose** during pre-roll + step (``_JointStatePoseStream``). - The pre-roll samples are zero-commanded → robot at rest → their - std becomes our `noise_std` estimate for the fit (see step 4). -2. **Savitzky-Golay on position, then central difference**. - ``reconstruct_body_velocities`` applies ``scipy.signal.savgol_filter`` - to (x, y, yaw) (default window=11 ≈ 2 trot cycles, order=2), then - ``np.gradient`` to recover (vx, vy, dyaw). Smooth-then-differentiate - keeps gait-frequency noise out of the velocity signal — - differentiate-then-smooth (the obvious wrong order) can't recover - what the differentiation step destroyed. Knobs: ``savgol_window`` - / ``savgol_order``; set window=0 to disable. -3. **Hampel filter** on reconstructed velocity. Catches residual - single-sample spikes that savgol smooths over but doesn't replace. - Window=11, n_sigma=3.0; set window=0 to disable. -4. **`fit_fopdt` with three guards** to keep the deadtime estimate - honest at our sample rate: - - ``min_deadtime``: floor L at the data's median sample interval - (you can't physically resolve deadtime finer than odom rate). - - ``noise_std``: from step 1; used for weighted least squares and - for the data-driven L detection. - - ``two_stage=True``: detect L as the first time the response - crosses ``5σ`` above ``noise_std``, pin it, then fit only (K, τ). - Removes the joint-fit's L/τ correlation that lets the optimizer - trade off a tiny L for a slightly inflated τ. Auto-disabled if - ``noise_std`` can't be estimated. Toggle via - ``CharacterizerConfig.two_stage_fit``. - -The PNG plot dots the raw post-reconstruction velocity under the -filtered+fit lines when Hampel replaced any points, and annotates how -many. Note that in two-stage mode the reported L corresponds to "when -the response crossed 5σ above the rest-noise floor" — not a free -optimization parameter — so cross-channel L comparisons are -interpretable. - -The two-terminal CLI flow still works and is required for -``--mode self-test`` (no robot needed) and for the end-to-end sim -variant via ``coordinator-sim-fopdt``: - -``` -uv run python -m dimos.utils.benchmarking.characterization \ - --robot go2 --mode hw --surface concrete --gait-mode default -``` - -Per excited channel (`profile.excited_channels`; Go2 = vx, wz — it does -not strafe in the default gait) × a few amplitudes: - -1. Robot **stopped**; prompt `ENTER=run s=skip q=quit`. Reposition, ENTER. -2. Pre-roll zeros (settle), then a velocity step (`--step-s`) at the - profile tick rate, recording commanded vs body-frame velocity - differentiated from the odom topic. Ends at `--max-dist` or `--step-s`. -3. `safe_stop`, fit FOPDT. - -Drift is bounded to one step (operator gate before each). Safety: clamp -to the profile envelope, stale-odom abort, distance + time caps, -zero-Twist on exit / Ctrl-C / `q`. - -Artifacts land at `/data/characterization//_config_<…>.{json,png}` -by default (overridable via `--out` / `CharacterizerConfig.out`). - -**Primary output is a graph** — `_config_<…>.png`, one column -per channel overlaying every step's *measured* velocity (solid) with its -*fitted FOPDT* step response (dashed), annotated K/τ/L/r² per amplitude -— this is what you read to judge whether the model matches the real -robot. The `.json` alongside is the machine handoff the benchmark -consumes (sections 1–4 + 6; section 5 pending; `valid_for_tuning=true`). -Channels not excited (e.g. vy on a non-strafing robot) are placeholdered -= vx and flagged in the caveats. - -`--mode self-test` (no robot, no blueprint, no coord): steps the -profile's in-process FOPDT sim plant and recovers it. Proves the -measure→fit→derive code runs; artifact stamped `valid_for_tuning=false`. -The pytest/CI path — **not a tuning artifact**. - -For a `--mode hw` *style* run against the sim plant (full coordinator + -transport_lcm path, no robot): bring up `coordinator-sim-fopdt` in -terminal 1, then run the benchmark/characterization with `--mode sim` -in terminal 2. Same architectural shape as hw — only the LCM peer -differs. - -## Tool 2 — `benchmark` - -For Go2 HW the preferred entrypoint is the all-in-one blueprint -(one terminal, autoconnects GO2Connection + ControlCoordinator + pygame -keyboard teleop + Benchmarker + a per-session telemetry recorder): - -``` -dimos run unitree-go2-benchmark --module.benchmarker.config ``` - -Operator UX matches the characterization blueprint: WASD/QE in the -pygame window to reposition/aim the robot between runs; **Enter** to -advance, **K** to skip, **Backspace** to quit. Comparison arms map 1:1 -to CLI flags via `--module.benchmarker.` overrides; recordings -land at `/data/benchmark//_benchmark__.db` -(tag="benchmark" so they don't collide with characterization recordings -in the sibling `data/characterization/` dir). - -The two-terminal CLI flow still works and is required for `--mode sim` -(end-to-end pre-check against `coordinator-sim-fopdt`): - -``` -uv run python -m dimos.utils.benchmarking.benchmark \ - --robot go2 --config reports/go2_config_hw_concrete__.json \ - --mode hw --speeds 0.3,0.5,0.7,0.9,1.0 --tolerances 5,10,15 +dimos run unitree-go2-characterization ``` -**By default it runs the BARE stock baseline P-controller — no -feedforward, no velocity profile.** That is the point: this measures the -**plant's physical tracking limit** with the existing production -controller, the number you compare everything against and check against -the `(τ+L)·v` floor. Path set is fixed (`straight_line`, `single_corner` -2 m/90°, `square` 2 m, `circle` R1.0). For each (path, speed): operator -gate, the path is **anchored to the robot's current pose**, then tracked -closed-loop at the profile tick rate off real odom; CTE scored from the -real trajectory. The **bare** run writes section 5 (operating-point map -+ tolerance→max-safe-speed inversion) back into the artifact — the -canonical physical-limit map. Same safety as Tool 1. - -Optional comparison arms (off by default), each measured *against* the -bare physical limit, written to standalone `__` files that never -clobber section 5: - -- `--ff` — apply the artifact's derived feedforward. -- `--profile` — apply the artifact's derived curvature velocity profile. -- `--ff --profile` — both (the fully-derived config). -- `--rg --e-max ` — apply the **reference governor's** per-waypoint - speed profile (geometric + saturation + lateral + precision - constraints, then forward/backward accel passes). `--e-max` (default - 0.05 m) is the corridor half-width fed into the precision constraint - `v ≤ e_max / max(τ_vx+L_vx, τ_wz+L_wz)`. The follower applies the - resulting list[float] cap via `start_path(velocity_profile=…)` — - the same 8-waypoint lookahead the static profile uses so braking - starts before each corner. - -The RG arm imports `solve_profile()` directly from the reference -governor module — there is no RG Module instance in this blueprint. The -math runs in-process inside the Benchmarker, and the resolved profile -crosses to the follower as a plain list of floats. RG-as-Module (live -`e_max` stream → reactive recompute) is a B3 concern. - -`--mode hw` **refuses a non-robot-valid config when any comparison arm -(`--ff` / `--profile` / `--rg`) is set** (sim-derived gains/plant are -meaningless on the real robot). The bare physical-limit run accepts -any config. - -`--mode sim`: optional fast pre-check. Same baseline + coordinator + -`transport_lcm` path as hw, but the LCM peer is the -`coordinator-sim-fopdt` blueprint (FOPDT plant + odom integrator) -instead of the real Go2 bring-up. Loudly labelled a pre-check; the map -is not a real-robot result. +Per axis (vx, wz on Go2 default gait) × a few amplitudes, the SI loop prompts you to reposition, then issues a velocity step and records the response. Settling pre-roll (~1.5 s) happens *after* you press Enter — wait for it before assuming nothing's happening. -## Reading the artifact +**Duration**: ~5–8 minutes for the default amplitude set. -| Section | Field | Meaning | -|---|---|---| -| 1 | `provenance` | robot/surface/mode/date/sha, `sim_or_hw` | -| 1 | `valid_for_tuning` | **false ⇒ do not tune from this** (self-test) | -| 1 | `plant` | fitted FOPDT `{K,τ,L}` per axis | -| 2 | `feedforward` | `1/K` per axis + clamps | -| 3 | `velocity_profile` | curvature speed profile | -| 4 | `recommended_controller` | baseline + plant-floor evidence | -| 5 | `operating_point_map` | per (path,speed) CTE + tolerance→speed (null until Tool 2 bare run) | -| 6 | `caveats` | validity scope; self-test artifacts lead with a loud DO-NOT-TUNE banner | - -## Adding a robot - -Append one `RobotPlantProfile` to `ROBOT_PLANT_PROFILES` in -`dimos/utils/benchmarking/plant.py`: its `robot_id` (= LCM topic prefix -= `transport_lcm` adapter `hardware_id`), the hw `blueprint` and the -sim `sim_blueprint` the operator runs in the other terminal, saturation -envelope (`vx_max`, `wz_max`), `tick_rate_hz`, `excited_channels` -(omit `vy` if it doesn't strafe), `si_amplitudes`, and a `sim_plant` -(`TwistBasePlantParams`) used as the self-test ground truth and by -`FopdtPlantConnection` when the sim blueprint is composed. Then the -identical two commands with `--robot `. For a brand-new sim plant -shape (different topic prefix), add a tiny blueprint mirroring -`coordinator_sim_fopdt` in `dimos/control/blueprints/mobile.py`. - -## When to re-run - -Re-run Tool 1 (then Tool 2) on any plant change: different surface -(friction → K/τ), gait mode, firmware/locomotion change. The `caveats` -state exactly what the artifact is valid for. - -## Tests +**Output** (lands together with the same timestamp suffix): ``` -uv run pytest dimos/utils/benchmarking/test_tuning.py -q +data/characterization/go2/ +├── go2_config_hw_concrete__.json ← the tuning artifact +├── go2_config_hw_concrete__.png ← fit-quality plot +└── go2_recording__.db ← raw streams (cmd_vel, joint_state, odom, gate) ``` -Pure DERIVE (1/K per axis, wz-ceiling margin + envelope clamp, accel -formulas, hardcoded baseline + evidence), `valid_for_tuning` (true only -for hw; self-test false + leading DO-NOT-TUNE caveat; survives -round-trip), artifact round-trip + schema rejection, tolerance→max-speed -inversion. HW loops require a robot — covered by the manual prerequisites -above, not pytest. +The PNG is what you READ to judge the fit. K/τ/L per channel + r² are annotated; raw (dotted) overlays the savgol-filtered fit (solid) when Hampel replaced points. -## Not here (by design) +Override defaults with `-o characterizer.=`, e.g.: +- `-o characterizer.surface=grass` +- `-o characterizer.step_s=10` +- `-o characterizer.savgol_window=15` (more aggressive gait smoothing) -The MPC/RPP/Lyapunov bake-off, command smoothers, sweeps, and plotting -R&D were the evidence for "baseline + FF + curvature profile"; they are -the appendix, archived off-repo, not the product. - -## Reference governor (precision → per-waypoint v) - -The artifact is the *static* tuning. The reference governor is the -*runtime* layer that consumes a corridor half-width `e_max` (metres) -and produces a per-waypoint velocity profile. It is intentionally -**open-loop, model-based** — no measured-CTE feedback, no new -controller, no toppra; just one new closed-form constraint composed -with the existing curvature MVC + accel passes. - -Data flow: +## Step 2 — benchmark ``` - ┌──────────────────────────────────────────────┐ -nav path ──▶│ ReferenceGovernor (Module) │ - │ • Loads TuningConfig artifact (plant + caps)│ -e_max ─────▶│ • Composes 4 constraints (one is NEW): │ - │ v ≤ v_max (geometric) │ - │ v ≤ ω_max / κ (saturation) │ - │ v ≤ √(a_lat / κ) (lateral) │ - │ v ≤ e_max / max(τ+L per channel) │ - │ ── precision; NEW │ - │ • Forward/backward accel passes (reused) │ - │ • Atomic-snapshot read API │ - └──────────────────────────────────────────────┘ - │ - ▼ installed as external_profile_cap - ┌──────────────────────────────────────────────┐ - │ BaselinePathFollowerTask │ - │ (control law unchanged; only cap source │ - │ swaps from VelocityProfileConfig→governor) │ - └──────────────────────────────────────────────┘ +dimos run unitree-go2-benchmark \ + -o benchmarker.config=/path/to/go2_config_hw_*.json ``` -### The precision constraint - -The straight-line CTE floor of a velocity-commanded FOPDT base is -`(τ_vx + L_vx) · v` (issue #921 characterization, see -`project_go2_plant_and_diagnostic`). On *curved* segments the dominant -lag is the wz channel rather than vx — the heading-tracking lag of -`τ_wz + L_wz` produces a comparable CTE term. Empirically (sim runs -on `smooth_corner`, `slalom`, `figure_eight`), using only the vx -channel under-predicts CTE on curved paths by ~2×. The shipped -constraint takes the worse channel: +Per (path, speed) × fixed path set (`straight_line`, `single_corner`, `square`, `circle`), the loop prompts you to aim the robot, then drives the baseline follower over the anchored path. CTE is scored from the real trajectory. - v ≤ e_max / max(τ_vx + L_vx, τ_wz + L_wz) +**Comparison arms** (each measures *against* the bare physical limit): -For the vendored Go2 plant this gives `max(0.46, 0.65) = 0.65 s`, so -e_max=5 cm caps v at 0.077 m/s (was 0.109 m/s under the vx-only -formula). This is the *only* new math. The other three caps already -lived in `velocity_profiler.py` (geometric/lateral) or in the artifact -(saturation envelope). The constraint set composes with `min()` per -waypoint; the existing accel passes run on top. +- `-o benchmarker.ff=true` — apply derived feedforward. +- `-o benchmarker.profile=true` — apply derived curvature velocity profile. +- `-o benchmarker.rg=true` `-o benchmarker.e_max=0.20` — apply reference governor's per-waypoint cap (precision = `e_max / max(τ+L)`). -The remaining residual on continuously-curving paths (~30% above the -predicted floor) reflects closed-loop heading-chase dynamics that no -single-segment FOPDT model captures. Closing the loop on measured CTE -is the next layer — see the demo's `cte_max` print for the residual on -each path. +For the RG arm, **the Go2 stalls below ~0.2 m/s commanded velocity** even when the math says go slower. `benchmarker.min_speed` (default `0.2`) is the floor; set to `None` to defer to the artifact's value. Tight corners against tight `e_max` will track badly because saturation-binding cells get floored above ω_max — that's a physical limit, not a bug. -### Where it lives +**Duration**: ~15–25 min depending on speeds × arms. -| Path | What | -|---|---| -| `dimos/navigation/reference_governor/reference_governor.py` | Module + 4 constraint classes + `solve_profile` | -| `dimos/control/tasks/baseline_path_follower_task.py` | Added `external_profile_cap` injection seam (control law unchanged) | -| `examples/go2_reference_governor_demo.py` | End-to-end demo (in-process FOPDT sim) | - -### Demo +**Output**: ``` -# Static e_max (5 cm corridor on the canonical 90° corner) -uv run python examples/go2_reference_governor_demo.py --path single_corner --e-max 0.05 - -# Time-varying e_max (loose → tight every 2s) — exercises the -# atomic-snapshot recompute path on a hot stream -uv run python examples/go2_reference_governor_demo.py --path circle --mode square-wave \ - --e-max-high 0.10 --e-max-low 0.02 --period 4.0 +data/benchmark/go2/ +├── go2_benchmark__.png ← XY trajectory overlay + CTE plot +├── …___.json ← per-arm operating-point map (bare run also appends section 5 to the input artifact) +└── go2_benchmark__.db ← raw streams ``` -A `*.png` plot is written to `/tmp/reference_governor_demo.png` by -default: reference path + executed trajectory, plus an e_max(t) vs -commanded `|vx|(t)` overlay in square-wave mode. - -### Open-loop assumption — what the calibration plot would tell you - -The model predicts `cte ≈ max(τ_vx+L_vx, τ_wz+L_wz) · v_binding`. The -demo prints actual `cte_max` from `score_run` so you can compare -against the predicted floor for each `e_max`. On the vendored Go2 -plant (max τ+L = 0.65 s), `--e-max 0.05` predicts ~5 cm CTE; sim runs -on `single_corner` measure ~8 cm and on `smooth_corner` measure ~7 cm. -The ~30–50% residual is the heading-chase term the open-loop model -doesn't see. - -For a systematic precision-vs-speed sweep, lean on the existing -benchmark harness: +## Reading recordings +```python +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.sensor_msgs.JointState import JointState +store = SqliteStore(path="<.db file>"); store.start() +for obs in store.stream("joint_state", JointState): + ts, msg = obs.ts, obs.data # re-fit, plot, etc. ``` -uv run python -m dimos.utils.benchmarking.benchmark \ - --robot go2 --config --mode sim --profile -``` - -(the `--profile` arm exercises the static curvature profiler — directly -analogous to the governor in the `e_max → ∞` limit). For the -governor's precision axis specifically, drive the demo script across a -grid of `--e-max` values and read off the per-run `cte_max` it prints. - -### Scope honesty - -The constraint set is validated against the same path battery used by -the static tuning (`straight_line`, `single_corner`, `square`, `circle`, -`smooth_corner` etc.). For geometries with materially different local -curvature distributions (cusps, near-zero-radius corners no arc-radius -blend smooths), the characterization → derive → governor chain must be -re-checked. The governor is open-loop by design: it does *not* see the -live CTE, so a model gap manifests as silently wider-than-predicted -tracking error. The demo's `cte_max` print is your sanity check. - -### Closed-loop α-feedback variant — NEGATIVE result (shipped, opt-in, default OFF) - -A closed-loop variant is wired and shipped behind the `--closed-loop` -flag on the demo (and the `closed_loop=True` config field on -`ReferenceGovernor`). It observes per-tick measured CTE via -`scoring.nearest_segment`, runs a PI law on `(cte - e_max)` with -anti-windup + EMA filter, and applies a multiplicative α ∈ [α_min, -1.0] scaling to the open-loop profile output. The architecture and -unit tests pass cleanly. **The empirical result on the canonical path -battery, however, does not show convergence:** - -| Path | OL (option A) | CL default | Final α | Bare follower | -|---|---|---|---|---| -| `single_corner` | 8.8 cm | **8.8 cm** | 1.000 | 10.3 cm | -| `smooth_corner` | 9.9 cm | **10.0 cm** | 1.000 | 7.7 cm | -| `slalom` | 9.8 cm | **10.4 cm** | 0.722 | 5.0 cm | -| `figure_eight` | 11.9 cm | **12.1 cm** | 0.852 | 5.2 cm | - -Two failure modes: - -1. **Spike-CTE paths** (`single_corner`, `smooth_corner`): α stays at - 1.0 because the high-CTE excursion at the corner is too brief — the - EMA filter dampens it before the PI integrator can wind up. Could - be addressed with more aggressive gains (`kp_alpha ≈ 20`, - `cte_ema_alpha ≈ 0.7`), but the corner is anyway a transient - feature. -2. **Sustained-CTE paths** (`slalom`, `figure_eight`): α DOES drop - (0.72–0.85, the loop is actuating) — but `cte_max` gets slightly - *worse*, not better. Slowing further does not help because the - bottleneck on these paths is **wz authority**, and the cap's - geometry-preserving scaling reduces `wz` proportionally with `vx`. - The controller loses turning authority at exactly the moment it - needs more. The closed-loop is doing the algebraically correct - thing for the wrong physics model. - -The fix is structural, not a gain tune. Three options, all larger -than the V1 closed-loop scope: - -- **Decouple α** into `α_vx` and `α_wz`. The cap stops preserving - turn radius when wz is the bottleneck. Simplest concept; adds a - second feedback channel. -- **Per-waypoint α gating** — apply α only at waypoints where - precision binds (not saturation/lateral). Requires the solver to - track binding constraints. -- **Adapt e_max** — when wz is the binding bottleneck, widen the - corridor instead of slowing the robot. Concedes the precision - promise but stays inside the open-loop framework. -The V1 closed-loop is left in place (opt-in, default off) so the -machinery exists for the structural follow-up. The demo prints a -"converged?" line and an α(t) overlay to make this state visible. +Streams: `cmd_vel` (Twist), `joint_state` (JointState, x/y/yaw), `odom` (PoseStamped, raw), `gate` (Int8, operator events). -### Non-goals +## Troubleshooting -- No toppra dependency. The constraint-generator architecture is - future-ready for a toppra swap but does not pull it in. -- No new control law in the follower. -- The shipped closed-loop variant is wired but does not converge on - the canonical battery — see "Closed-loop α-feedback variant" above. +- **Pygame window doesn't open**: X11 not reachable. `xeyes` test, then `export DISPLAY=:1; export XAUTHORITY=/run/user/$(id -u)/.Xauthority`. +- **Enter does nothing**: pygame window isn't focused. Click it before pressing. +- **Terminal flooded with TF warnings**: pipe through `grep --line-buffered -E 'Benchmarker|Characterizer|reject|aborted|arrived|timeout|configure|start_path|required|reposition'`. +- **Robot won't move on RG arm**: see `min_speed` note above. Try `-o benchmarker.e_max=0.2` first; if still stuck, raise `-o benchmarker.min_speed=0.25`. +- **Self-test (no robot)**: keep using the CLI: `uv run python -m dimos.utils.benchmarking.characterization --mode self-test`. From 727ff69b0f8dcfd69b706bb6a4076ebba94979b4 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 11:47:15 -0700 Subject: [PATCH 31/51] refactor after merge --- dimos/control/blueprints/mobile.py | 32 +++++----- .../tasks/path_follower_task/__registry__.py | 17 +++++ .../path_follower_task.py} | 64 +++++++++++++------ .../blueprints/basic/unitree_go2_benchmark.py | 7 +- .../basic/unitree_go2_benchmark_rg.py | 62 ++++++++++++++++++ .../basic/unitree_go2_coordinator.py | 6 +- dimos/utils/benchmarking/benchmark.py | 12 ++-- 7 files changed, 154 insertions(+), 46 deletions(-) create mode 100644 dimos/control/tasks/path_follower_task/__registry__.py rename dimos/control/tasks/{baseline_path_follower_task.py => path_follower_task/path_follower_task.py} (91%) create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 7be3503f88..cd1230e380 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -90,13 +90,13 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=10, - velocity_zero_on_timeout=False, + params={"zero_on_timeout": False}, ), # Closed-loop path follower used by the benchmark tool. # Inactive until the tool RPCs configure(...) + start_path(...). TaskConfig( - name="baseline_follower", - type="baseline_path_follower", + name="path_follower", + type="path_follower", joint_names=_base_joints, priority=20, ), @@ -117,13 +117,13 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=10, - velocity_zero_on_timeout=False, + params={"zero_on_timeout": False}, ), # Closed-loop path follower used by the benchmark tool. # Inactive until the tool RPCs configure(...) + start_path(...). TaskConfig( - name="baseline_follower", - type="baseline_path_follower", + name="path_follower", + type="path_follower", joint_names=_base_joints, priority=20, ), @@ -146,13 +146,13 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=20, - velocity_zero_on_timeout=False, + params={"zero_on_timeout": False}, ), # Closed-loop path follower used by the benchmark tool. Inactive # until the tool RPCs configure(...) + start_path(...). TaskConfig( - name="baseline_follower", - type="baseline_path_follower", + name="path_follower", + type="path_follower", joint_names=_base_joints, priority=10, ), @@ -260,13 +260,13 @@ def _flowbase_twist_base( type="velocity", joint_names=_base_joints, priority=10, - velocity_zero_on_timeout=False, + params={"zero_on_timeout": False}, ), # Closed-loop path follower used by the benchmark tool. # Inactive until the tool RPCs configure(...) + start_path(...). TaskConfig( - name="baseline_follower", - type="baseline_path_follower", + name="path_follower", + type="path_follower", joint_names=_base_joints, priority=20, ), @@ -285,7 +285,7 @@ def _flowbase_twist_base( # and /sim/odom (Out); the coord drives /sim/cmd_vel via its # transport_lcm adapter (hardware_id="sim"), reads pose back via the # same adapter's /sim/odom subscription, and publishes JointState + -# hosts the baseline_follower task. Drop-in stand-in for a real robot. +# hosts the path_follower task. Drop-in stand-in for a real robot. _sim_joints = make_twist_base_joints("sim") coordinator_sim_fopdt = ( @@ -306,11 +306,11 @@ def _flowbase_twist_base( type="velocity", joint_names=_sim_joints, priority=10, - velocity_zero_on_timeout=False, + params={"zero_on_timeout": False}, ), TaskConfig( - name="baseline_follower", - type="baseline_path_follower", + name="path_follower", + type="path_follower", joint_names=_sim_joints, priority=20, ), 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/baseline_path_follower_task.py b/dimos/control/tasks/path_follower_task/path_follower_task.py similarity index 91% rename from dimos/control/tasks/baseline_path_follower_task.py rename to dimos/control/tasks/path_follower_task/path_follower_task.py index 62efc9f472..30c30fc4df 100644 --- a/dimos/control/tasks/baseline_path_follower_task.py +++ b/dimos/control/tasks/path_follower_task/path_follower_task.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Baseline path-follower ControlTask: production LocalPlanner algorithm, +"""Path-follower ControlTask: production LocalPlanner algorithm, unwrapped from its daemon thread and rebuilt as a passive ControlTask. Algorithm is a faithful port of @@ -27,7 +27,7 @@ from __future__ import annotations from dataclasses import dataclass, field -from typing import TYPE_CHECKING, Literal +from typing import TYPE_CHECKING, Any, Literal import numpy as np @@ -51,6 +51,7 @@ 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, VelocityProfileConfig from dimos.utils.logging_config import setup_logger from dimos.utils.trigonometry import angle_diff @@ -62,7 +63,7 @@ logger = setup_logger() -BaselineState = Literal[ +PathFollowerState = Literal[ "idle", "initial_rotation", "path_following", "final_rotation", "arrived", "aborted" ] @@ -72,7 +73,7 @@ @dataclass -class BaselinePathFollowerTaskConfig: +class PathFollowerTaskConfig: joint_names: list[str] = field(default_factory=lambda: ["base/vx", "base/vy", "base/wz"]) priority: int = 20 speed: float = 0.55 @@ -92,19 +93,19 @@ class BaselinePathFollowerTaskConfig: velocity_profile_config: VelocityProfileConfig | None = None -class BaselinePathFollowerTask(BaseControlTask): +class PathFollowerTask(BaseControlTask): """Production LocalPlanner algorithm as a passive ControlTask.""" def __init__( self, name: str, - config: BaselinePathFollowerTaskConfig, + config: PathFollowerTaskConfig, global_config: GlobalConfig, external_profile_cap: PathSpeedCapProtocol | None = None, ) -> None: if len(config.joint_names) != 3: raise ValueError( - f"BaselinePathFollowerTask '{name}' needs 3 joints (vx, vy, wz), " + f"PathFollowerTask '{name}' needs 3 joints (vx, vy, wz), " f"got {len(config.joint_names)}" ) @@ -134,7 +135,7 @@ def __init__( else None ) - self._state: BaselineState = "idle" + self._state: PathFollowerState = "idle" self._path: Path | None = None self._distancer: PathDistancer | None = None self._current_odom: PoseStamped | None = None @@ -239,7 +240,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: if joints & self._joint_names and self.is_active(): - logger.warning(f"BaselinePathFollowerTask '{self._name}' preempted by {by_task}") + logger.warning(f"PathFollowerTask '{self._name}' preempted by {by_task}") self._state = "aborted" # ------------------------------------------------------------------ @@ -312,7 +313,7 @@ def _step_final_rotation(self) -> tuple[float, float, float]: if abs(yaw_err) < self._config.orientation_tolerance: self._state = "arrived" - logger.info(f"BaselinePathFollowerTask '{self._name}' arrived") + logger.info(f"PathFollowerTask '{self._name}' arrived") return 0.0, 0.0, 0.0 twist = self._controller.rotate(yaw_err) @@ -342,9 +343,7 @@ def configure( as the per-tick cap source. """ if self.is_active(): - logger.warning( - f"BaselinePathFollowerTask '{self._name}': cannot configure while active" - ) + logger.warning(f"PathFollowerTask '{self._name}': cannot configure while active") return False if speed is not None: self._config.speed = speed @@ -377,7 +376,7 @@ def start_path( velocity_profile: list[float] | None = None, ) -> bool: if path is None or len(path.poses) < 2: - logger.warning(f"BaselinePathFollowerTask '{self._name}': invalid path") + logger.warning(f"PathFollowerTask '{self._name}': invalid path") return False self._path = path self._distancer = PathDistancer(path) @@ -400,7 +399,7 @@ def start_path( else: if velocity_profile is not None: logger.warning( - f"BaselinePathFollowerTask '{self._name}': velocity_profile length " + f"PathFollowerTask '{self._name}': velocity_profile length " f"{len(velocity_profile)} != path.poses {len(path.poses)}; ignoring" ) self._velocity_profile = None @@ -421,7 +420,7 @@ def start_path( self._state = "initial_rotation" logger.info( - f"BaselinePathFollowerTask '{self._name}' started " + f"PathFollowerTask '{self._name}' started " f"({len(path.poses)} poses, initial state={self._state})" ) return True @@ -448,11 +447,38 @@ def reset(self) -> bool: self._current_odom = None return True - def get_state(self) -> BaselineState: + def get_state(self) -> PathFollowerState: return self._state __all__ = [ - "BaselinePathFollowerTask", - "BaselinePathFollowerTaskConfig", + "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 + + +def create_task(cfg: Any, hardware: Any) -> PathFollowerTask: + from dimos.core.global_config import global_config as _gc + + 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, + ), + global_config=_gc, + ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py index 24487e1c5f..d4a7dacd5f 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py @@ -21,7 +21,10 @@ dimos run unitree-go2-benchmark --module.benchmarker.config -instead of the two-terminal CLI flow. Operator UX matches B1 +instead of the two-terminal CLI flow. Defaults are the bare baseline +arm (ff/profile/rg all OFF) — use a sibling blueprint +(``unitree-go2-benchmark-rg``) to bake the RG arm in, rather than +overriding ``--module.benchmarker.rg`` at every invocation. Operator UX matches B1 (``unitree-go2-characterization``): WASD/QE in the pygame window to reposition/aim the robot between runs; ENTER to advance, K to skip, Backspace to quit. @@ -39,7 +42,7 @@ to), and a Module wrapper would force a cross-process per-tick RPC on the controller's hot path. The Benchmarker computes the per-waypoint speeds once per path and ships them to the follower as a plain -``list[float]`` via the new ``BaselinePathFollowerTask.start_path( +``list[float]`` via the new ``PathFollowerTask.start_path( velocity_profile=...)`` kwarg. Recordings land at diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py new file mode 100644 index 0000000000..caca7d14b7 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py @@ -0,0 +1,62 @@ +#!/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 benchmark — RG arm variant. + +Same composition as ``unitree-go2-benchmark`` but with ``rg=True`` baked +into the Benchmarker config so the RG-derived per-waypoint speed +profile is applied without any ``--module.benchmarker.rg`` override at +launch. ``e_max`` is left at the BenchmarkerConfig default +(0.05) — pass ``--module.benchmarker.e_max `` to sweep corridor +widths. + + dimos run unitree-go2-benchmark-rg --module.benchmarker.config +""" + +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.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( + unitree_go2_coordinator, +) +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.utils.benchmarking.benchmark import Benchmarker +from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder +from dimos.utils.path_utils import get_project_root + +unitree_go2_benchmark_rg = autoconnect( + unitree_go2_coordinator, + KeyboardTeleop.blueprint(publish_only_when_active=True), + Benchmarker.blueprint(robot="go2", mode="hw", gate_source="stream", rg=True), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag="benchmark", + out_dir=str(get_project_root() / "data" / "benchmark" / "go2"), + ), +).transports( + { + ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } +) + +__all__ = ["unitree_go2_benchmark_rg"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index a12c62441f..8b10bb0bdd 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -51,13 +51,13 @@ type="velocity", joint_names=_go2_joints, priority=20, - velocity_zero_on_timeout=False, + params={"zero_on_timeout": False}, ), # Closed-loop path follower used by the benchmark tool. # Inactive until the tool RPCs configure(...) + start_path(...). TaskConfig( - name="baseline_follower", - type="baseline_path_follower", + name="path_follower", + type="path_follower", joint_names=_go2_joints, priority=10, ), diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py index 22e20af19c..6973903ecf 100644 --- a/dimos/utils/benchmarking/benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -85,10 +85,10 @@ # Positions carry [x,y,yaw] (ConnectedTwistBase populates them from # adapter.read_odometry). The tool subscribes to this for trajectory # recording; the baseline task — which actually drives the robot — lives -# inside the operator coord (see TaskConfig type="baseline_path_follower" +# inside the operator coord (see TaskConfig type="path_follower" # wired into each robot's coord blueprint). _JOINT_STATE_TOPIC = "/coordinator/joint_state" -_BASELINE_TASK_NAME = "baseline_follower" # task name in operator coord blueprint +_PATH_FOLLOWER_TASK_NAME = "path_follower" # task name in operator coord blueprint _ARRIVED_STATES = frozenset({"arrived", "completed"}) _FAILED_STATES = frozenset({"aborted"}) @@ -270,11 +270,11 @@ def reset_trajectory(self) -> None: def _invoke(coord: RPCClient, method: str, **kwargs: object) -> object: - """RPC `task_invoke(_BASELINE_TASK_NAME, method, kwargs)` on the operator + """RPC `task_invoke(_PATH_FOLLOWER_TASK_NAME, method, kwargs)` on the operator coord. Centralises the .task_invoke wrapping so the run loop reads as plain method calls on a remote object.""" return coord.task_invoke( - task_name=_BASELINE_TASK_NAME, + task_name=_PATH_FOLLOWER_TASK_NAME, method=method, kwargs=dict(kwargs), ) @@ -293,7 +293,7 @@ def _run_baseline( label: str, velocity_profile: list[float] | None = None, ) -> tuple[ExecutedTrajectory, NavPath]: - """Send a path to the operator coord's ``baseline_follower`` task and + """Send a path to the operator coord's ``path_follower`` task and wait for it to terminate. The task is pre-added by the operator's blueprint (priority 20, claims base/{vx,vy,wz}) so it preempts the operator's teleop velocity task while a run is active. We only RPC @@ -607,7 +607,7 @@ def _prereq_banner(profile: RobotPlantProfile, mode: str) -> None: f"Prereqs:\n" f" 1. Another terminal: `dimos run {bp}`\n" f" (its ControlCoordinator publishes {_JOINT_STATE_TOPIC} and\n" - f" hosts the `{_BASELINE_TASK_NAME}` task at priority 20).\n" + f" hosts the `{_PATH_FOLLOWER_TASK_NAME}` task at priority 20).\n" f" 2. This process: strip /nix/store from LD_LIBRARY_PATH (README).\n" f"Each (path,speed): reposition+aim, then ENTER. We RPC the operator\n" f"coord to configure + start_path; the task drives the robot from\n" From bd81c07401b04d0585e312caa7cc7978b693a921 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 12:21:14 -0700 Subject: [PATCH 32/51] dimos float32 message added --- dimos/msgs/std_msgs/Float32.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 dimos/msgs/std_msgs/Float32.py 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 From eaa076e63d1f3edbe0762b9f4139be589484e41e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 13:52:46 -0700 Subject: [PATCH 33/51] added precision based path follower --- dimos/control/coordinator.py | 35 ++- .../path_follower_task/path_follower_task.py | 24 +- .../__registry__.py | 19 ++ .../precision_path_follower_task.py | 214 ++++++++++++++++++ 4 files changed, 272 insertions(+), 20 deletions(-) create mode 100644 dimos/control/tasks/precision_path_follower_task/__registry__.py create mode 100644 dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 94d4837062..8c212a7b2f 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -45,6 +45,7 @@ ConnectedWholeBody, ) from dimos.control.task import ControlTask +from dimos.control.tasks.registry import control_task_registry from dimos.control.tick_loop import TickLoop from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig @@ -57,6 +58,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist 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,9 @@ class ControlCoordinator(Module): # Input: Teleop buttons for engage/disengage signaling buttons: In[Buttons] + # Input: Live RG corridor half-width (m) for tasks with set_e_max(). + e_max: In[Float32] + # Arming and dry-run are one-shot RPCs, not streams. def __init__(self, *args: Any, **kwargs: Any) -> None: @@ -181,6 +186,7 @@ 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._e_max_unsub: Callable[[], None] | None = None logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz") @@ -271,8 +277,6 @@ def _create_whole_body_adapter(self, component: HardwareComponent) -> WholeBodyA def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: """Create a control task from config via the task registry.""" - from dimos.control.tasks.registry import control_task_registry - return control_task_registry.create(cfg.type, cfg, hardware=self._hardware) @rpc @@ -521,6 +525,15 @@ def _on_buttons(self, msg: Buttons) -> None: for task in self._tasks.values(): task.on_buttons(msg) + def _on_e_max(self, msg: Float32) -> None: + """Forward e_max (RG corridor half-width) to any task with set_e_max().""" + value = float(msg.data) + with self._task_lock: + for task in self._tasks.values(): + set_e_max = getattr(task, "set_e_max", None) + if set_e_max is not None: + set_e_max(value) + @rpc def set_activated(self, engaged: bool) -> None: """Arm/disarm every task exposing ``arm()`` / ``disarm()``.""" @@ -680,6 +693,21 @@ def start(self) -> None: self._buttons_unsub = self.buttons.subscribe(self._on_buttons) logger.info("Subscribed to buttons for engage/disengage") + # Subscribe to e_max if any task implements set_e_max. + with self._task_lock: + has_e_max_task = any( + callable(getattr(task, "set_e_max", None)) for task in self._tasks.values() + ) + if has_e_max_task: + try: + self._e_max_unsub = self.e_max.subscribe(self._on_e_max) + logger.info("Subscribed to e_max for precision-capable tasks") + except Exception: + logger.warning( + "Precision-capable task configured but could not subscribe to e_max. " + "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") @@ -702,6 +730,9 @@ def stop(self) -> None: if self._buttons_unsub: self._buttons_unsub() self._buttons_unsub = None + if self._e_max_unsub: + self._e_max_unsub() + self._e_max_unsub = None if self._tick_loop: self._tick_loop.stop() diff --git a/dimos/control/tasks/path_follower_task/path_follower_task.py b/dimos/control/tasks/path_follower_task/path_follower_task.py index 30c30fc4df..3093e28e3b 100644 --- a/dimos/control/tasks/path_follower_task/path_follower_task.py +++ b/dimos/control/tasks/path_follower_task/path_follower_task.py @@ -46,6 +46,7 @@ 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 @@ -373,7 +374,6 @@ def start_path( self, path: Path, current_odom: PoseStamped, - velocity_profile: list[float] | None = None, ) -> bool: if path is None or len(path.poses) < 2: logger.warning(f"PathFollowerTask '{self._name}': invalid path") @@ -389,21 +389,11 @@ def start_path( self._ff.reset() if self._profile_cap is not None: self._profile_cap.for_path(path) - # Install the optional precomputed per-waypoint speed profile. - # Length must match path.poses; falls back to clearing if mismatched. - if velocity_profile is not None and len(velocity_profile) == len(path.poses): - self._velocity_profile = np.asarray(velocity_profile, dtype=float) - self._velocity_profile_pts = np.array( - [[p.position.x, p.position.y] for p in path.poses], dtype=float - ) - else: - if velocity_profile is not None: - logger.warning( - f"PathFollowerTask '{self._name}': velocity_profile length " - f"{len(velocity_profile)} != path.poses {len(path.poses)}; ignoring" - ) - self._velocity_profile = None - self._velocity_profile_pts = None + # 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] @@ -466,8 +456,6 @@ class PathFollowerTaskParams(BaseConfig): def create_task(cfg: Any, hardware: Any) -> PathFollowerTask: - from dimos.core.global_config import global_config as _gc - params = PathFollowerTaskParams.model_validate(cfg.params) return PathFollowerTask( cfg.name, diff --git a/dimos/control/tasks/precision_path_follower_task/__registry__.py b/dimos/control/tasks/precision_path_follower_task/__registry__.py new file mode 100644 index 0000000000..6fc55de7f8 --- /dev/null +++ b/dimos/control/tasks/precision_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 = { + "precision_path_follower": ( + "dimos.control.tasks.precision_path_follower_task.precision_path_follower_task:create_task" + ), +} diff --git a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py new file mode 100644 index 0000000000..7da6ff00de --- /dev/null +++ b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py @@ -0,0 +1,214 @@ +# 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. + +"""Precision path-follower: PathFollowerTask + live e_max corridor. + +Subclass of :class:`PathFollowerTask` that adds a reactive precision +input: ``set_e_max(value)`` updates the RG corridor half-width and +recomputes the per-waypoint speed profile in place. The parent's tick +loop / ``compute()`` consumes the swapped ``_velocity_profile`` array +unchanged — no override of the control law needed. + +The plant model and velocity-profile constants come from a tuning +artifact loaded once on the first ``start_path()`` call. +""" + +from __future__ import annotations + +from pathlib import Path as _Path +from typing import TYPE_CHECKING, Any + +import numpy as np +from numpy.typing import NDArray + +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.navigation.reference_governor.reference_governor import ( + GeometricMVC, + LateralMVC, + PrecisionMVC, + SaturationMVC, + solve_profile, +) +from dimos.protocol.service.spec import BaseConfig +from dimos.utils.benchmarking.tuning import TuningConfig +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.core.global_config import GlobalConfig + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + from dimos.msgs.nav_msgs.Path import Path + +logger = setup_logger() + + +class PrecisionPathFollowerTask(PathFollowerTask): + """Path follower whose speed profile reacts to a live corridor + half-width (e_max). The ControlCoordinator broadcasts e_max updates + via :meth:`set_e_max`; each update triggers ``solve_profile()`` and + atomically swaps the parent's ``_velocity_profile`` so the per-tick + indexing inherits the new values immediately.""" + + def __init__( + self, + name: str, + config: PathFollowerTaskConfig, + global_config: GlobalConfig, + artifact_path: str, + e_max_default: float = 0.05, + ) -> None: + super().__init__(name, config, global_config=global_config) + self._artifact_path = artifact_path + self._e_max: float = float(e_max_default) + # Plant + vp_spec lazy-load on first start_path(). + self._plant: Any = None + self._vp_spec: Any = None + self._constraints: list[Any] | None = None + # Cached path geometry — solve_profile() accepts these as kwargs + # so e_max updates skip the re-computation. + self._cached_pts: NDArray[np.float64] | None = None + self._cached_curvatures: NDArray[np.float64] | None = None + + # ------------------------------------------------------------------ + # ControlCoordinator stream hook + # ------------------------------------------------------------------ + + def set_e_max(self, value: float) -> None: + """Coordinator broadcast hook. Update e_max; recompute profile if + a path is loaded.""" + self._e_max = float(value) + if self._path is not None: + self._recompute_profile() + + # ------------------------------------------------------------------ + # Path lifecycle + # ------------------------------------------------------------------ + + def start_path(self, path: Path, current_odom: PoseStamped) -> bool: + ok = super().start_path(path, current_odom) + if not ok: + return False + + # Lazy-load the artifact + build constraints the first time we run. + if self._plant is None or self._vp_spec is None: + self._load_artifact() + + # Per-path geometry cache (skip on every e_max update). + self._cached_pts = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) + self._cached_curvatures = None # solve_profile recomputes once on first call + + self._recompute_profile() + return True + + def _load_artifact(self) -> None: + if not self._artifact_path: + raise RuntimeError( + f"PrecisionPathFollowerTask '{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"PrecisionPathFollowerTask '{self._name}': artifact not found at " + f"{self._artifact_path}" + ) + + art = TuningConfig.from_json(self._artifact_path) + self._plant = art.plant + self._vp_spec = art.velocity_profile + vp = self._vp_spec + # PrecisionMVC reads e_max via a closure so live updates don't + # require rebuilding the constraint list. + self._constraints = [ + GeometricMVC(v_max=vp.max_linear_speed), + SaturationMVC(omega_max=vp.max_angular_speed), + LateralMVC(a_lat_max=vp.max_centripetal_accel), + PrecisionMVC(e_max_provider=lambda: self._e_max), + ] + logger.info( + f"PrecisionPathFollowerTask '{self._name}': loaded artifact " + f"{self._artifact_path} (plant + vp_spec ready, e_max={self._e_max:.3f})" + ) + + def _recompute_profile(self) -> None: + """Run solve_profile() against current e_max + cached geometry. + Atomic swap of self._velocity_profile / _velocity_profile_pts so + the parent's compute() picks up the new array on the next tick.""" + if ( + self._path is None + or self._plant is None + or self._vp_spec is None + or self._constraints is None + or self._cached_pts is None + ): + return + + vp = self._vp_spec + arr = solve_profile( + self._path, + self._plant, + self._constraints, + accel_max=vp.max_linear_accel, + decel_max=vp.max_linear_decel, + min_speed=vp.min_speed, + pts=self._cached_pts, + curvatures=self._cached_curvatures, + ) + # Single-attribute assignment is atomic under the GIL — safe race + # vs. the tick thread reading the prior array. + self._velocity_profile = arr + self._velocity_profile_pts = self._cached_pts + logger.info( + f"PrecisionPathFollowerTask '{self._name}': recomputed profile " + f"(e_max={self._e_max:.3f}, n={len(arr)}, " + f"v_min={float(np.min(arr)):.3f}, v_max={float(np.max(arr)):.3f})" + ) + + +class PrecisionPathFollowerTaskParams(BaseConfig): + artifact_path: str + speed: float = 0.55 + control_frequency: float = 10.0 + goal_tolerance: float = 0.05 + orientation_tolerance: float = 0.1 + k_angular: float = 0.5 + e_max_default: float = 0.2 + + +def create_task(cfg: Any, hardware: Any) -> PrecisionPathFollowerTask: + params = PrecisionPathFollowerTaskParams.model_validate(cfg.params) + return PrecisionPathFollowerTask( + 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, + ), + global_config=_gc, + artifact_path=params.artifact_path, + e_max_default=params.e_max_default, + ) + + +__all__ = [ + "PrecisionPathFollowerTask", + "PrecisionPathFollowerTaskParams", + "create_task", +] From c8afeefb740fdf297e9fd4f1dd283671921c6faa Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 14:00:22 -0700 Subject: [PATCH 34/51] demoted reference governor from module to just utils file --- .../path_follower_task/path_follower_task.py | 4 +- .../precision_path_follower_task.py | 4 +- .../reference_governor.py | 236 +++++++ .../reference_governor/reference_governor.py | 584 ----------------- .../test_reference_governor.py | 561 ---------------- examples/go2_reference_governor_demo.py | 600 ------------------ 6 files changed, 241 insertions(+), 1748 deletions(-) create mode 100644 dimos/control/tasks/precision_path_follower_task/reference_governor.py delete mode 100644 dimos/navigation/reference_governor/reference_governor.py delete mode 100644 dimos/navigation/reference_governor/test_reference_governor.py delete mode 100644 examples/go2_reference_governor_demo.py diff --git a/dimos/control/tasks/path_follower_task/path_follower_task.py b/dimos/control/tasks/path_follower_task/path_follower_task.py index 3093e28e3b..a63e7e555f 100644 --- a/dimos/control/tasks/path_follower_task/path_follower_task.py +++ b/dimos/control/tasks/path_follower_task/path_follower_task.py @@ -58,9 +58,11 @@ from dimos.utils.trigonometry import angle_diff if TYPE_CHECKING: + from dimos.control.tasks.precision_path_follower_task.reference_governor import ( + PathSpeedCapProtocol, + ) from dimos.core.global_config import GlobalConfig from dimos.msgs.nav_msgs.Path import Path - from dimos.navigation.reference_governor import PathSpeedCapProtocol logger = setup_logger() diff --git a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py index 7da6ff00de..b3faa675ff 100644 --- a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py +++ b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py @@ -36,14 +36,14 @@ PathFollowerTask, PathFollowerTaskConfig, ) -from dimos.core.global_config import global_config as _gc -from dimos.navigation.reference_governor.reference_governor import ( +from dimos.control.tasks.precision_path_follower_task.reference_governor import ( GeometricMVC, LateralMVC, PrecisionMVC, SaturationMVC, solve_profile, ) +from dimos.core.global_config import global_config as _gc from dimos.protocol.service.spec import BaseConfig from dimos.utils.benchmarking.tuning import TuningConfig from dimos.utils.logging_config import setup_logger diff --git a/dimos/control/tasks/precision_path_follower_task/reference_governor.py b/dimos/control/tasks/precision_path_follower_task/reference_governor.py new file mode 100644 index 0000000000..8298619220 --- /dev/null +++ b/dimos/control/tasks/precision_path_follower_task/reference_governor.py @@ -0,0 +1,236 @@ +# Copyright 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. + +"""Reference governor math: per-waypoint Maximum Velocity Constraints + +forward/backward accel-limited solver. + +The reference-governor algorithm here gives a per-waypoint velocity +profile that keeps tracking error inside an ``±e_max`` corridor under a +given FOPDT plant. ``solve_profile()`` composes the constraint +upper-bounds (``min`` across a list of :class:`VelocityConstraint` +implementations) and runs the existing forward/backward accel passes +from :class:`VelocityProfiler`. + +The four built-in MVCs each express one physical/actuator/error limit: + + GeometricMVC v <= v_max (absolute platform cap) + SaturationMVC v <= omega_max / |kappa| (turn-rate saturation) + LateralMVC v <= sqrt(a_lat_max / |kappa|) (centripetal accel cap) + PrecisionMVC v <= e_max / max(tau+L per chan) (FOPDT tracking budget) + +Add a new constraint by writing one more class implementing +:class:`VelocityConstraint` and including it in the solver's constraint +list — no solver change needed. + +Live consumer: :class:`PrecisionPathFollowerTask`, which calls +:func:`solve_profile` once per path and again on each ``e_max`` update, +atomically swapping its cached profile. +""" + +from __future__ import annotations + +from collections.abc import Callable, Sequence +from dataclasses import dataclass +from typing import Protocol, runtime_checkable + +import numpy as np +from numpy.typing import NDArray + +from dimos.control.tasks.velocity_profiler import VelocityProfiler +from dimos.msgs.nav_msgs.Path import Path +from dimos.utils.benchmarking.tuning import PlantModelDC + +# --------------------------------------------------------------------------- +# PathSpeedCap method contract — the consumption seam in PathFollowerTask. +# --------------------------------------------------------------------------- + + +@runtime_checkable +class PathSpeedCapProtocol(Protocol): + """Duck-type contract for objects that can be installed as the + follower's ``_profile_cap``. Mirrors the shape of + :class:`dimos.utils.benchmarking.velocity_profile.PathSpeedCap`. + """ + + 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]: ... + + +# --------------------------------------------------------------------------- +# Velocity constraint generators — per-waypoint pure-function upper bounds. +# --------------------------------------------------------------------------- + + +@dataclass +class ConstraintContext: + """Path-derived context passed to each constraint's upper_bound. + + Bundling curvatures (computed once per path) avoids each curvature- + dependent constraint recomputing the same numbers. + """ + + path: Path + curvatures: NDArray[np.float64] # length len(path.poses); abs curvature in 1/m + plant: PlantModelDC + + +class VelocityConstraint(Protocol): + name: str + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: ... + + +_INF = float("inf") +_KAPPA_EPS = 1e-6 + + +@dataclass +class GeometricMVC: + """Static linear-speed cap (artifact's ``velocity_profile.max_linear_speed``).""" + + v_max: float + name: str = "geometric" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + return float(self.v_max) + + +@dataclass +class SaturationMVC: + """Turn-rate saturation: v <= omega_max / |kappa|. HARD cap — above + this the controller can't track the geometry at all.""" + + omega_max: float + name: str = "saturation" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + kappa = float(ctx.curvatures[s_idx]) + if kappa < _KAPPA_EPS: + return _INF + return self.omega_max / kappa + + +@dataclass +class LateralMVC: + """Lateral-comfort cap: v <= sqrt(a_lat_max / |kappa|).""" + + a_lat_max: float + name: str = "lateral" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + kappa = float(ctx.curvatures[s_idx]) + if kappa < _KAPPA_EPS: + return _INF + return float(np.sqrt(self.a_lat_max / kappa)) + + +@dataclass +class PrecisionMVC: + """Precision cap derived from the FOPDT plant CTE floor: + + v <= e_max / max(tau_vx + L_vx, tau_wz + L_wz) + + The straight-line characterization fits only `(tau_vx + L_vx) * v` as + the CTE floor; that holds on straight segments. On curved segments + the heading-tracking lag (a function of `tau_wz + L_wz`) dominates + instead. Empirically (slalom/smooth_corner/figure_eight sim runs) + using only the vx channel under-predicts CTE on curved paths by ~2x; + taking max(vx_chan, wz_chan) halves the residual at the cost of a + proportionally lower v. + + e_max read via a callable so callers can hot-update the runtime + corridor half-width without rebuilding the constraint. The bound is + constant across waypoints (κ-independent); the min() in the solver + handles composition with the κ-dependent caps. + """ + + e_max_provider: Callable[[], float] + min_e_max: float = 0.005 # 5 mm floor: keeps v from collapsing to 0 + name: str = "precision" + + def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: + e_max = max(float(self.e_max_provider()), self.min_e_max) + tau_plus_L = max( + float(ctx.plant.vx.tau + ctx.plant.vx.L), + float(ctx.plant.wz.tau + ctx.plant.wz.L), + ) + if tau_plus_L < 1e-9: + return _INF + return e_max / tau_plus_L + + +# --------------------------------------------------------------------------- +# Solver: compose constraints, then reuse existing accel passes. +# --------------------------------------------------------------------------- + + +def _path_pts(path: Path) -> NDArray[np.float64]: + return np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) + + +def solve_profile( + path: Path, + plant: PlantModelDC, + constraints: Sequence[VelocityConstraint], + *, + accel_max: float, + decel_max: float, + min_speed: float, + curvatures: NDArray[np.float64] | None = None, + pts: NDArray[np.float64] | None = None, +) -> NDArray[np.float64]: + """Per-waypoint MVC = min(constraints), then forward/backward accel + passes via the existing :class:`VelocityProfiler` helpers. ``pts`` + and ``curvatures`` can be passed in to avoid recomputing on hot + paths (e_max updates with a fixed path).""" + n = len(path.poses) + if n < 2: + return np.array([min_speed], dtype=float) + + if pts is None: + pts = _path_pts(path) + if curvatures is None: + # _compute_curvatures depends only on pts (it's stateless w.r.t. profiler config). + curvatures = VelocityProfiler()._compute_curvatures(pts) + + ctx = ConstraintContext(path=path, curvatures=curvatures, plant=plant) + mvc = np.empty(n, dtype=float) + for i in range(n): + mvc[i] = min(c.upper_bound(ctx, i) for c in constraints) + + # Reuse the forward/backward accel passes verbatim. + profiler = VelocityProfiler( + max_linear_accel=accel_max, + max_linear_decel=decel_max, + ) + v = profiler._acceleration_pass(pts, mvc, forward=True) + v = profiler._acceleration_pass(pts, v, forward=False) + return np.maximum(v, min_speed) + + +__all__ = [ + "ConstraintContext", + "GeometricMVC", + "LateralMVC", + "PathSpeedCapProtocol", + "PrecisionMVC", + "SaturationMVC", + "VelocityConstraint", + "solve_profile", +] diff --git a/dimos/navigation/reference_governor/reference_governor.py b/dimos/navigation/reference_governor/reference_governor.py deleted file mode 100644 index bff3bf58bc..0000000000 --- a/dimos/navigation/reference_governor/reference_governor.py +++ /dev/null @@ -1,584 +0,0 @@ -# Copyright 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. - -"""Reference governor: model-based per-waypoint velocity profile. - -Wraps the existing curvature MVC + forward/backward accel passes from -:class:`dimos.control.tasks.velocity_profiler.VelocityProfiler` with one -NEW constraint — a precision cap derived from the FOPDT plant floor: - - v <= e_max / max(tau_vx + L_vx, tau_wz + L_wz) - -(the empirical straight-line CTE floor is ~(tau+L)*v below ω-saturation; -on curved paths the wz-channel lag dominates instead, so the constraint -takes the worse channel — see PrecisionMVC docstring). The artifact's -v_max / ω_max / a_lat caps are unchanged and remain in the constraint -set; the governor composes ``min(all_constraints)`` per waypoint and -runs the existing accel passes. - -An optional closed-loop alpha-feedback variant (config flag ``closed_loop``) -observes per-tick CTE and multiplicatively scales the open-loop output -by alpha ∈ [alpha_min, 1.0]. Shipped opt-in; empirically does NOT converge on -continuously-curving paths (see ``tuning_README.md`` "Closed-loop -alpha-feedback variant — NEGATIVE result" for the data and the structural -follow-up options). - -The governor is a dimos Module (not a ControlTask) — the coordinator's -tick loop has no upstream-non-actuating slot. It exposes the -:class:`PathSpeedCapProtocol` method shape (``for_path``, -``speed_limit_at``, ``cap``) so it can be injected as the baseline -follower's ``_profile_cap`` (the existing per-tick consumption seam). -""" - -from __future__ import annotations - -from collections.abc import Callable, Sequence -from dataclasses import dataclass -import threading -from typing import Any, Protocol, runtime_checkable - -import numpy as np -from numpy.typing import NDArray -from reactivex.disposable import Disposable - -from dimos.control.tasks.velocity_profiler import VelocityProfiler -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Path import Path -from dimos.utils.benchmarking.scoring import nearest_segment -from dimos.utils.benchmarking.tuning import PlantModelDC, TuningConfig -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -# --------------------------------------------------------------------------- -# PathSpeedCap method contract — the consumption seam in BaselinePathFollowerTask. -# --------------------------------------------------------------------------- - - -@runtime_checkable -class PathSpeedCapProtocol(Protocol): - """Duck-type contract for objects that can be installed as the - follower's ``_profile_cap``. Mirrors the shape of - :class:`dimos.utils.benchmarking.velocity_profile.PathSpeedCap`. - """ - - 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]: ... - - -# --------------------------------------------------------------------------- -# Velocity constraint generators — per-waypoint pure-function upper bounds. -# -# Architecturally the solver applies min() over the constraint set then runs -# the existing accel passes. To add a new constraint: drop in one more class, -# include it in the governor's constraint list. No solver change needed. -# --------------------------------------------------------------------------- - - -@dataclass -class ConstraintContext: - """Path-derived context passed to each constraint's upper_bound. - - Bundling curvatures (computed once per path) avoids each curvature- - dependent constraint recomputing the same numbers. - """ - - path: Path - curvatures: NDArray[np.float64] # length len(path.poses); abs curvature in 1/m - plant: PlantModelDC - - -class VelocityConstraint(Protocol): - name: str - - def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: ... - - -_INF = float("inf") -_KAPPA_EPS = 1e-6 - - -@dataclass -class GeometricMVC: - """Static linear-speed cap (artifact's ``velocity_profile.max_linear_speed``).""" - - v_max: float - name: str = "geometric" - - def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: - return float(self.v_max) - - -@dataclass -class SaturationMVC: - """Turn-rate saturation: v <= omega_max / |kappa|. HARD cap — above - this the controller can't track the geometry at all.""" - - omega_max: float - name: str = "saturation" - - def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: - kappa = float(ctx.curvatures[s_idx]) - if kappa < _KAPPA_EPS: - return _INF - return self.omega_max / kappa - - -@dataclass -class LateralMVC: - """Lateral-comfort cap: v <= sqrt(a_lat_max / |kappa|).""" - - a_lat_max: float - name: str = "lateral" - - def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: - kappa = float(ctx.curvatures[s_idx]) - if kappa < _KAPPA_EPS: - return _INF - return float(np.sqrt(self.a_lat_max / kappa)) - - -@dataclass -class PrecisionMVC: - """Precision cap derived from the FOPDT plant CTE floor: - - v <= e_max / max(tau_vx + L_vx, tau_wz + L_wz) - - The straight-line characterization fits only `(tau_vx + L_vx) * v` as - the CTE floor; that holds on straight segments. On curved segments - the heading-tracking lag (a function of `tau_wz + L_wz`) dominates - instead. Empirically (slalom/smooth_corner/figure_eight sim runs) - using only the vx channel under-predicts CTE on curved paths by ~2x; - taking max(vx_chan, wz_chan) halves the residual at the cost of a - proportionally lower v. - - e_max read via a callable so the governor can hot-update the runtime - corridor half-width without rebuilding the constraint. The bound is - constant across waypoints (κ-independent); the min() in the solver - handles composition with the κ-dependent caps. - """ - - e_max_provider: Callable[[], float] - min_e_max: float = 0.005 # 5 mm floor: keeps v from collapsing to 0 - name: str = "precision" - - def upper_bound(self, ctx: ConstraintContext, s_idx: int) -> float: - e_max = max(float(self.e_max_provider()), self.min_e_max) - tau_plus_L = max( - float(ctx.plant.vx.tau + ctx.plant.vx.L), - float(ctx.plant.wz.tau + ctx.plant.wz.L), - ) - if tau_plus_L < 1e-9: - return _INF - return e_max / tau_plus_L - - -# --------------------------------------------------------------------------- -# Solver: compose constraints, then reuse existing accel passes. -# --------------------------------------------------------------------------- - - -def _path_pts(path: Path) -> NDArray[np.float64]: - return np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) - - -def solve_profile( - path: Path, - plant: PlantModelDC, - constraints: Sequence[VelocityConstraint], - *, - accel_max: float, - decel_max: float, - min_speed: float, - curvatures: NDArray[np.float64] | None = None, - pts: NDArray[np.float64] | None = None, -) -> NDArray[np.float64]: - """Per-waypoint MVC = min(constraints), then forward/backward accel - passes via the existing :class:`VelocityProfiler` helpers. ``pts`` - and ``curvatures`` can be passed in to avoid recomputing on hot - paths (e_max updates with a fixed path).""" - n = len(path.poses) - if n < 2: - return np.array([min_speed], dtype=float) - - if pts is None: - pts = _path_pts(path) - if curvatures is None: - # _compute_curvatures depends only on pts (it's stateless w.r.t. profiler config). - curvatures = VelocityProfiler()._compute_curvatures(pts) - - ctx = ConstraintContext(path=path, curvatures=curvatures, plant=plant) - mvc = np.empty(n, dtype=float) - for i in range(n): - mvc[i] = min(c.upper_bound(ctx, i) for c in constraints) - - # Reuse the forward/backward accel passes verbatim. - profiler = VelocityProfiler( - max_linear_accel=accel_max, - max_linear_decel=decel_max, - ) - v = profiler._acceleration_pass(pts, mvc, forward=True) - v = profiler._acceleration_pass(pts, v, forward=False) - return np.maximum(v, min_speed) - - -# --------------------------------------------------------------------------- -# Module: lifecycle, In streams, atomic-snapshot state. -# --------------------------------------------------------------------------- - - -class ReferenceGovernorConfig(ModuleConfig): - plant_artifact_path: str - e_max_default: float = 0.1 - min_e_max: float = 0.005 - lookahead_pts: int = 8 - - # --- Closed-loop alpha-feedback (OPT-IN; default off = open-loop) ------- - # Background: the open-loop precision bound v ≤ e_max / max(τ+L per - # channel) under-promises CTE on continuously-curving paths by ~2× - # because the FOPDT lag model doesn't see the controller's - # heading-chase dynamics. Closing the loop on measured CTE corrects - # this via a multiplicative scaling factor alpha ∈ [alpha_min, 1.0] applied - # to the open-loop profile output. - closed_loop: bool = False - kp_alpha: float = 4.0 # P gain on (cte - e_max) → -Δalpha - ki_alpha: float = 0.5 # I gain (1/s); produces zero steady-state error - alpha_min: float = 0.2 # floor: never starve v below 20% of open-loop - max_integral: float = 0.5 # anti-windup clamp on the integral state - cte_ema_alpha: float = 0.3 # EMA factor on raw CTE; 0.3 ≈ 3-tick window @ 10Hz - tick_dt_s: float = 0.05 # nominal tick period for integrating error - - -class ReferenceGovernor(Module): - """Per-waypoint velocity-profile producer for the baseline path - follower. Consumes (path, e_max). Outputs are read via the - :class:`PathSpeedCapProtocol` methods on each follower tick. - """ - - config: ReferenceGovernorConfig - - path: In[Path] - corridor_half_width: In[float] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - cfg = self.config - - artifact = TuningConfig.from_json(cfg.plant_artifact_path) - self._tuning_config = artifact - self._plant = artifact.plant - vp = artifact.velocity_profile - self._accel_max = vp.max_linear_accel - self._decel_max = vp.max_linear_decel - self._min_speed = vp.min_speed - self._lookahead_pts = cfg.lookahead_pts - - # State (all guarded by _state_lock). - self._state_lock = threading.Lock() - self._path: Path | None = None - self._pts: NDArray[np.float64] | None = None - self._curvatures: NDArray[np.float64] | None = None - self._profile: NDArray[np.float64] | None = None - if cfg.e_max_default <= 0: - raise ValueError(f"e_max_default must be > 0, got {cfg.e_max_default}") - self._e_max: float = max(cfg.e_max_default, cfg.min_e_max) - - # Constraints — PrecisionMVC reads _e_max via _current_e_max - # under the lock so the constraint always sees a consistent value - # even while recompute is running on another thread. - self._constraints: list[VelocityConstraint] = [ - GeometricMVC(v_max=vp.max_linear_speed), - SaturationMVC(omega_max=vp.max_angular_speed), - LateralMVC(a_lat_max=vp.max_centripetal_accel), - PrecisionMVC(e_max_provider=self._current_e_max, min_e_max=cfg.min_e_max), - ] - - # Closed-loop alpha-feedback state (also guarded by _state_lock). - # When closed_loop=False these stay at their initial values and - # speed_limit_at() short-circuits the feedback path entirely. - self._alpha: float = 1.0 - self._alpha_integral: float = 0.0 - self._cte_filtered: float = 0.0 - - # ----- lifecycle ------------------------------------------------------ - - @rpc - def start(self) -> None: - super().start() - self.register_disposable(Disposable(self.path.subscribe(self._on_path))) - self.register_disposable(Disposable(self.corridor_half_width.subscribe(self._on_e_max))) - - @rpc - def stop(self) -> None: - super().stop() - - # ----- imperative API (also RPC-callable) ----------------------------- - - @rpc - def set_path(self, path: Path) -> None: - """Install a new path and compute its profile.""" - self._on_path(path) - - @rpc - def update_e_max(self, value: float) -> None: - """Update the corridor half-width and recompute (if a path is set).""" - self._on_e_max(value) - - # ----- PathSpeedCap interface (duck-typed; called per-tick from the - # follower's compute() on the coordinator tick thread) ---------------- - - def for_path(self, path: Path) -> None: - """Alias of set_path. Lets the governor drop into the follower's - existing PathSpeedCap injection seam (the follower calls - ``cap.for_path(path)`` inside ``start_path``).""" - self._on_path(path) - - def speed_limit_at(self, x: float, y: float) -> float: - # Atomic snapshot under the lock; lock-free work afterwards. - with self._state_lock: - profile = self._profile - pts = self._pts - min_speed = self._min_speed - alpha = self._alpha - if profile is None or pts is None or len(profile) == 0: - # No path installed yet — fall back to the artifact's v_max - # so the follower's cap is a no-op until set_path is called. - return float(self._tuning_config.velocity_profile.max_linear_speed) - - # Closed-loop alpha-feedback: measure CTE from current pose, update alpha - # via PI law, write back atomically. Open-loop short-circuit keeps - # alpha at its initial 1.0 so this becomes a no-op multiplier. - if self.config.closed_loop: - cte_raw = self._measure_cte(x, y, pts) - alpha, integral, filtered = self._update_alpha(cte_raw) - with self._state_lock: - self._alpha = alpha - self._alpha_integral = integral - self._cte_filtered = filtered - - i = int(np.argmin(np.sum((pts - np.array([x, y])) ** 2, axis=1))) - j = min(len(profile), i + max(1, self._lookahead_pts)) - return float(max(alpha * float(np.min(profile[i:j])), min_speed)) - - def cap( - self, x: float, y: float, vx: float, vy: float, wz: float - ) -> tuple[float, float, float]: - 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 - - def speed_at(self, s_idx: int, fallback: float | None = None) -> float: - """Direct index-based lookup (debug/test seam).""" - with self._state_lock: - profile = self._profile - if profile is None or len(profile) == 0: - if fallback is None: - raise RuntimeError("ReferenceGovernor.speed_at: no path installed") - return float(fallback) - idx = max(0, min(s_idx, len(profile) - 1)) - return float(profile[idx]) - - # ----- internals ------------------------------------------------------ - - def _current_e_max(self) -> float: - # No lock here — the constraint is called from within a recompute - # that already holds the snapshot of e_max it was triggered with. - # (Holding the lock here would self-deadlock.) - return self._e_max - - # ----- closed-loop alpha-feedback helpers --------------------------------- - - def _measure_cte(self, x: float, y: float, pts: NDArray[np.float64]) -> float: - """Perpendicular distance from (x, y) to the nearest path segment. - Thin wrapper over scoring.nearest_segment so the governor's - closed-loop math is decoupled from the scoring module's call shape.""" - _, perp_dist, _ = nearest_segment(np.array([x, y], dtype=float), pts) - return float(perp_dist) - - def _update_alpha(self, cte_raw: float) -> tuple[float, float, float]: - """PI feedback on (cte - e_max) with anti-windup + EMA pre-filter. - - Pure function of inputs + the current alpha/integral/filtered_cte - snapshot — caller writes back under _state_lock. Returns - (new_alpha, new_integral, new_filtered_cte). - - Convention: positive error (cte > e_max) shrinks alpha toward - alpha_min; negative error (cte < e_max) grows alpha back toward 1.0. - The 1-sided clamp at alpha=1.0 means the closed-loop never asks for v - ABOVE the open-loop bound — only at-or-below. - """ - cfg = self.config - - # Snapshot current state (atomic; cheap). - with self._state_lock: - integral = self._alpha_integral - filtered_prev = self._cte_filtered - - # 1. EMA-smooth the raw CTE to reject single-tick noise spikes. - ema = float(cfg.cte_ema_alpha) - cte = ema * float(cte_raw) + (1.0 - ema) * filtered_prev - - # 2. Error in metres; positive means we're outside the corridor. - error = cte - self._e_max - dt = float(cfg.tick_dt_s) - - # 3. Tentative integral update + clamp (anti-windup). - integral_candidate = integral + error * dt - max_int = float(cfg.max_integral) - integral_candidate = max(-max_int, min(max_int, integral_candidate)) - - # 4. PI output; clamp alpha into [alpha_min, 1.0]. - alpha_min = float(cfg.alpha_min) - kp = float(cfg.kp_alpha) - ki = float(cfg.ki_alpha) - alpha_raw = 1.0 - kp * error - ki * integral_candidate - alpha_new = max(alpha_min, min(1.0, alpha_raw)) - - # 5. Anti-windup: if alpha saturated against a clamp this tick AND - # the integral move is in the saturating direction, freeze the - # integral. Prevents the integral from winding up while alpha can't - # respond. - if alpha_new == alpha_min and alpha_raw < alpha_min: - # alpha pegged low; integral would push alpha even lower → freeze. - integral_new = integral - elif alpha_new == 1.0 and alpha_raw > 1.0: - integral_new = integral - else: - integral_new = integral_candidate - - return alpha_new, integral_new, cte - - def _on_path(self, path: Path) -> None: - if path is None or len(path.poses) < 2: - logger.warning( - "ReferenceGovernor: ignored invalid path " - f"(poses={0 if path is None else len(path.poses)})" - ) - return - # Compute pts + curvatures outside the lock (pure numpy). - pts = _path_pts(path) - curvatures = VelocityProfiler()._compute_curvatures(pts) - # Lock briefly to snapshot e_max for the recompute. - with self._state_lock: - e_max_snapshot = self._e_max - profile = self._compute_profile(path, pts, curvatures, e_max_snapshot) - with self._state_lock: - self._path = path - self._pts = pts - self._curvatures = curvatures - self._profile = profile - # Reset closed-loop state on every new path so feedback from - # the prior path doesn't carry over (different geometry, fresh - # convergence). - self._alpha = 1.0 - self._alpha_integral = 0.0 - self._cte_filtered = 0.0 - self._log_provenance("path", profile, e_max_snapshot) - - def _on_e_max(self, value: float) -> None: - if not np.isfinite(value) or value <= 0: - logger.warning(f"ReferenceGovernor: ignored non-positive e_max={value}") - return - clamped = max(float(value), self.config.min_e_max) - # Need pts/curvatures/path under the lock; recompute outside. - with self._state_lock: - path = self._path - pts = self._pts - curvatures = self._curvatures - if path is None or pts is None or curvatures is None: - # No path yet — just stash the value; recompute will pick it up - # when set_path is called. - with self._state_lock: - self._e_max = clamped - return - # Set e_max BEFORE solving so PrecisionMVC reads the new value. - with self._state_lock: - self._e_max = clamped - profile = self._compute_profile(path, pts, curvatures, clamped) - with self._state_lock: - self._profile = profile - self._log_provenance("e_max", profile, clamped) - - def _compute_profile( - self, - path: Path, - pts: NDArray[np.float64], - curvatures: NDArray[np.float64], - e_max: float, # unused directly; PrecisionMVC reads via provider - ) -> NDArray[np.float64]: - del e_max # documented above; kept for log signature parity - return solve_profile( - path, - self._plant, - self._constraints, - accel_max=self._accel_max, - decel_max=self._decel_max, - min_speed=self._min_speed, - curvatures=curvatures, - pts=pts, - ) - - def _log_provenance( - self, - trigger: str, - profile: NDArray[np.float64], - e_max: float, - ) -> None: - # Which constraint binds at each waypoint? (Debug surface — count by name.) - with self._state_lock: - curvatures = self._curvatures - path = self._path - if curvatures is None or path is None: - return - ctx = ConstraintContext(path=path, curvatures=curvatures, plant=self._plant) - n = len(profile) - binding_counts: dict[str, int] = {c.name: 0 for c in self._constraints} - for i in range(n): - bounds = [(c.name, c.upper_bound(ctx, i)) for c in self._constraints] - binding = min(bounds, key=lambda t: t[1])[0] - binding_counts[binding] = binding_counts.get(binding, 0) + 1 - logger.info( - "ReferenceGovernor recomputed", - trigger=trigger, - e_max=round(e_max, 4), - v_min=round(float(np.min(profile)), 3), - v_max=round(float(np.max(profile)), 3), - n_waypoints=n, - binding=binding_counts, - ) - - -__all__ = [ - "ConstraintContext", - "GeometricMVC", - "LateralMVC", - "PathSpeedCapProtocol", - "PrecisionMVC", - "ReferenceGovernor", - "ReferenceGovernorConfig", - "SaturationMVC", - "VelocityConstraint", - "solve_profile", -] diff --git a/dimos/navigation/reference_governor/test_reference_governor.py b/dimos/navigation/reference_governor/test_reference_governor.py deleted file mode 100644 index c8145066ba..0000000000 --- a/dimos/navigation/reference_governor/test_reference_governor.py +++ /dev/null @@ -1,561 +0,0 @@ -# Copyright 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. - -from __future__ import annotations - -from collections.abc import Generator -import math -from pathlib import Path as FsPath -import threading -import time - -import numpy as np -import pytest - -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.navigation.reference_governor.reference_governor import ( - ConstraintContext, - GeometricMVC, - LateralMVC, - PrecisionMVC, - ReferenceGovernor, - SaturationMVC, - solve_profile, -) -from dimos.utils.benchmarking.paths import circle, single_corner, straight_line -from dimos.utils.benchmarking.tuning import ( - FeedforwardDC, - FopdtChannelDC, - PlantModelDC, - Provenance, - RecommendedControllerDC, - TuningConfig, - VelocityProfileDC, -) - -# --------------------------------------------------------------------------- -# Fixtures -# --------------------------------------------------------------------------- - - -def _plant() -> PlantModelDC: - """Go2-class FOPDT plant. - vx: tau+L = 0.46s; wz: tau+L = 0.65s. - PrecisionMVC takes max() across channels, so 0.65 is the divisor.""" - return PlantModelDC( - vx=FopdtChannelDC(K=0.922, tau=0.4, L=0.06), - vy=FopdtChannelDC(K=1.0, tau=0.4, L=0.06), - wz=FopdtChannelDC(K=2.45, tau=0.6, L=0.05), - ) - - -def _ctx(path: Path) -> ConstraintContext: - from dimos.control.tasks.velocity_profiler import VelocityProfiler - - pts = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) - curvatures = VelocityProfiler()._compute_curvatures(pts) - return ConstraintContext(path=path, curvatures=curvatures, plant=_plant()) - - -@pytest.fixture() -def artifact_path(tmp_path: FsPath) -> FsPath: - """Write a minimal valid TuningConfig artifact to tmp_path.""" - cfg = TuningConfig( - provenance=Provenance( - robot_id="go2-test", - surface="test", - mode="default", - date="2026-05-25", - git_sha="testtest", - sim_or_hw="hw", - ), - plant=_plant(), - feedforward=FeedforwardDC(K_vx=1.085, K_vy=1.0, K_wz=0.408), - velocity_profile=VelocityProfileDC( - max_linear_speed=1.0, - max_angular_speed=1.275, - max_centripetal_accel=1.0, - max_linear_accel=2.5, - max_linear_decel=5.0, - ), - recommended_controller=RecommendedControllerDC(), - caveats=[], - ) - p = tmp_path / "test_config.json" - cfg.to_json(p) - return p - - -@pytest.fixture() -def governor(artifact_path: FsPath) -> Generator[ReferenceGovernor, None, None]: - gov = ReferenceGovernor( - plant_artifact_path=str(artifact_path), - e_max_default=0.05, - ) - try: - yield gov - finally: - gov._close_module() - - -# --------------------------------------------------------------------------- -# Constraint generators — pure-function tests -# --------------------------------------------------------------------------- - - -class TestGeometricMVC: - def test_returns_v_max_everywhere(self) -> None: - ctx = _ctx(straight_line(length=2.0)) - c = GeometricMVC(v_max=0.7) - assert c.upper_bound(ctx, 0) == pytest.approx(0.7) - assert c.upper_bound(ctx, len(ctx.curvatures) // 2) == pytest.approx(0.7) - assert c.upper_bound(ctx, len(ctx.curvatures) - 1) == pytest.approx(0.7) - - -class TestSaturationMVC: - def test_infinite_at_zero_curvature(self) -> None: - ctx = _ctx(straight_line(length=2.0)) - c = SaturationMVC(omega_max=1.5) - # All curvatures should be ~0 on a straight line. - for i in range(len(ctx.curvatures)): - assert c.upper_bound(ctx, i) == float("inf") - - def test_omega_over_kappa_on_circle(self) -> None: - # Circle of radius R: the existing _compute_curvatures uses a - # (d1+d2) arc-length denominator (2-step), so reported kappa is - # ~1/(2R) = 0.5 for R=1 — half the textbook 1/R. The whole - # tuning pipeline is calibrated against this convention; our - # constraints follow it consistently. - ctx = _ctx(circle(radius=1.0, n_points=200)) - c = SaturationMVC(omega_max=1.275) - v = c.upper_bound(ctx, len(ctx.curvatures) // 2) - # omega/kappa = 1.275 / 0.5 = 2.55 - assert v == pytest.approx(2.55, rel=0.05) - - -class TestLateralMVC: - def test_infinite_at_zero_curvature(self) -> None: - ctx = _ctx(straight_line(length=2.0)) - c = LateralMVC(a_lat_max=1.0) - for i in range(len(ctx.curvatures)): - assert c.upper_bound(ctx, i) == float("inf") - - def test_sqrt_a_over_kappa_on_circle(self) -> None: - # Same kappa convention as SaturationMVC: kappa ~ 1/(2R) = 0.5 for R=1. - ctx = _ctx(circle(radius=1.0, n_points=200)) - c = LateralMVC(a_lat_max=1.0) - v = c.upper_bound(ctx, len(ctx.curvatures) // 2) - # sqrt(a_lat / kappa) = sqrt(1.0 / 0.5) = sqrt(2) ≈ 1.414 - assert v == pytest.approx(math.sqrt(2.0), rel=0.05) - - -class TestPrecisionMVC: - def test_constant_across_waypoints(self) -> None: - ctx = _ctx(single_corner(leg_length=2.0, angle_deg=90.0)) - c = PrecisionMVC(e_max_provider=lambda: 0.05) - # max(vx tau+L, wz tau+L) = max(0.46, 0.65) = 0.65 - # → 0.05/0.65 ≈ 0.0769 - expected = 0.05 / 0.65 - for i in (0, len(ctx.curvatures) // 2, len(ctx.curvatures) - 1): - assert c.upper_bound(ctx, i) == pytest.approx(expected, rel=1e-6) - - def test_min_floor_when_e_max_is_tiny(self) -> None: - ctx = _ctx(straight_line(length=1.0)) - c = PrecisionMVC(e_max_provider=lambda: 0.0001, min_e_max=0.005) - # 0.005/0.65 ≈ 0.0077 — the e_max floor wins. - assert c.upper_bound(ctx, 0) == pytest.approx(0.005 / 0.65, rel=1e-6) - - def test_reads_provider_at_call_time(self) -> None: - ctx = _ctx(straight_line(length=1.0)) - state = {"e": 0.05} - c = PrecisionMVC(e_max_provider=lambda: state["e"]) - v0 = c.upper_bound(ctx, 0) - state["e"] = 0.10 - v1 = c.upper_bound(ctx, 0) - assert v1 == pytest.approx(v0 * 2.0, rel=1e-6) - - -# --------------------------------------------------------------------------- -# Solver -# --------------------------------------------------------------------------- - - -class TestSolveProfile: - def test_straight_line_clamped_by_precision(self) -> None: - path = straight_line(length=2.0) - plant = _plant() - constraints = [ - GeometricMVC(v_max=1.0), - SaturationMVC(omega_max=1.275), - LateralMVC(a_lat_max=1.0), - PrecisionMVC(e_max_provider=lambda: 0.05), - ] - v = solve_profile( - path, - plant, - constraints, - accel_max=2.5, - decel_max=5.0, - min_speed=0.05, - ) - # On a straight line, only GeometricMVC and PrecisionMVC are - # finite; precision is tighter (0.109 < 1.0). - assert np.all(v == pytest.approx(0.05 / 0.65, rel=1e-4)) - - def test_short_path_returns_min_speed(self) -> None: - # Path with a single pose — degenerate, solver returns [min_speed]. - path = Path( - poses=[ - PoseStamped( - position=Vector3(0.0, 0.0, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), - ) - ] - ) - plant = _plant() - constraints: list = [GeometricMVC(v_max=1.0)] - v = solve_profile( - path, - plant, - constraints, - accel_max=2.5, - decel_max=5.0, - min_speed=0.05, - ) - assert len(v) == 1 - assert v[0] == pytest.approx(0.05) - - def test_corner_slows_at_curvature(self) -> None: - path = single_corner(leg_length=2.0, angle_deg=90.0) - plant = _plant() - constraints = [ - GeometricMVC(v_max=1.0), - SaturationMVC(omega_max=1.275), - LateralMVC(a_lat_max=1.0), - PrecisionMVC(e_max_provider=lambda: 0.5), # loose enough not to bind on legs - ] - v = solve_profile( - path, - plant, - constraints, - accel_max=2.5, - decel_max=5.0, - min_speed=0.05, - ) - # Far from corner v should ~ geometric cap, at corner v should drop. - assert v.min() < v.max(), "expected corner to slow profile" - - def test_min_speed_floor(self) -> None: - # Sharp corner + impossibly tight e_max should still floor at min_speed. - path = single_corner(leg_length=1.0, angle_deg=170.0) - constraints = [ - GeometricMVC(v_max=1.0), - SaturationMVC(omega_max=1.275), - LateralMVC(a_lat_max=1.0), - PrecisionMVC(e_max_provider=lambda: 0.001, min_e_max=0.001), - ] - v = solve_profile( - path, - _plant(), - constraints, - accel_max=2.5, - decel_max=5.0, - min_speed=0.05, - ) - assert np.all(v >= 0.05 - 1e-9) - - -# --------------------------------------------------------------------------- -# ReferenceGovernor — Module integration -# --------------------------------------------------------------------------- - - -class TestReferenceGovernor: - def test_no_path_returns_v_max_fallback(self, governor: ReferenceGovernor) -> None: - # Before any path is set, speed_limit_at falls back to artifact v_max. - assert governor.speed_limit_at(0.0, 0.0) == pytest.approx(1.0) - - def test_set_path_then_speed_limit_drops_to_precision( - self, governor: ReferenceGovernor - ) -> None: - governor.set_path(straight_line(length=2.0)) - # e_max_default=0.05 → 0.05/0.46 ≈ 0.10870 - v = governor.speed_limit_at(1.0, 0.0) - assert v == pytest.approx(0.05 / 0.65, rel=1e-3) - - def test_update_e_max_recomputes(self, governor: ReferenceGovernor) -> None: - governor.set_path(straight_line(length=2.0)) - v0 = governor.speed_limit_at(1.0, 0.0) - governor.update_e_max(0.20) - v1 = governor.speed_limit_at(1.0, 0.0) - # e_max quadrupled → v_cap should quadruple (still below geometric cap of 1.0) - assert v1 == pytest.approx(min(4 * v0, 1.0), rel=1e-3) - - def test_update_e_max_clamps_below_floor(self, governor: ReferenceGovernor) -> None: - # Two floors stack: min_e_max (governor, 5 mm) AND the artifact's - # velocity_profile.min_speed (solver, 0.05 m/s here). For a Go2 - # plant with max(vx,wz)tau+L=0.65s, 0.005/0.65 ≈ 0.008 m/s — below the - # solver's min_speed, so the output floors at min_speed. - governor.set_path(straight_line(length=2.0)) - governor.update_e_max(0.0001) # below min_e_max=0.005 - v = governor.speed_limit_at(1.0, 0.0) - assert v == pytest.approx(0.05, rel=1e-6) # solver min_speed floor - # Sanity: not zero, not NaN. - assert v > 0.0 - - def test_update_e_max_rejects_non_positive(self, governor: ReferenceGovernor) -> None: - governor.set_path(straight_line(length=2.0)) - before = governor.speed_limit_at(1.0, 0.0) - governor.update_e_max(-1.0) - governor.update_e_max(0.0) - governor.update_e_max(float("nan")) - after = governor.speed_limit_at(1.0, 0.0) - assert before == pytest.approx(after, rel=1e-6) - - def test_cap_preserves_direction_when_under_limit(self, governor: ReferenceGovernor) -> None: - governor.set_path(straight_line(length=2.0)) - # Commanded vx already below vlim → pass-through. - vlim = governor.speed_limit_at(1.0, 0.0) - vx, vy, wz = governor.cap(1.0, 0.0, vlim * 0.5, 0.0, 0.0) - assert vx == pytest.approx(vlim * 0.5) - assert vy == pytest.approx(0.0) - assert wz == pytest.approx(0.0) - - def test_cap_scales_overcommanded_geometry_preserved(self, governor: ReferenceGovernor) -> None: - governor.set_path(straight_line(length=2.0)) - vlim = governor.speed_limit_at(1.0, 0.0) - # Command 2x vlim with some wz → output should be vlim with wz scaled - # by the same factor so turn radius is preserved. - out_vx, out_vy, out_wz = governor.cap(1.0, 0.0, 2 * vlim, 0.0, 1.0) - assert out_vx == pytest.approx(vlim, rel=1e-3) - assert out_wz == pytest.approx(0.5, rel=1e-3) # scaled by vlim / (2*vlim) = 0.5 - - def test_speed_at_index_lookup(self, governor: ReferenceGovernor) -> None: - path = straight_line(length=2.0) - governor.set_path(path) - v_mid = governor.speed_at(len(path.poses) // 2) - assert v_mid == pytest.approx(0.05 / 0.65, rel=1e-3) - - def test_speed_at_fallback_when_no_path(self, governor: ReferenceGovernor) -> None: - assert governor.speed_at(5, fallback=0.42) == pytest.approx(0.42) - with pytest.raises(RuntimeError): - governor.speed_at(0) - - def test_invalid_path_ignored(self, governor: ReferenceGovernor) -> None: - # Path with 0 or 1 poses is rejected; pre-state preserved. - before = governor.speed_limit_at(1.0, 0.0) - governor.set_path(Path(poses=[])) - governor.set_path(Path(poses=[straight_line(length=0.1).poses[0]])) - after = governor.speed_limit_at(1.0, 0.0) - assert before == pytest.approx(after, rel=1e-6) - - -class TestClosedLoopAlpha: - """alpha-feedback unit tests. The PI law is exercised by directly poking - ``_update_alpha`` (pure function) and by driving ``speed_limit_at`` - with synthetic poses that land at known perpendicular distances from - a straight reference path.""" - - @pytest.fixture() - def closed_loop_governor( - self, artifact_path: FsPath - ) -> Generator[ReferenceGovernor, None, None]: - gov = ReferenceGovernor( - plant_artifact_path=str(artifact_path), - e_max_default=0.05, - closed_loop=True, - # Tighter gains than defaults so unit tests converge in - # 10s-100s of synthetic ticks rather than seconds. - kp_alpha=4.0, - ki_alpha=0.5, - alpha_min=0.2, - max_integral=0.5, - cte_ema_alpha=1.0, # disable filter to make tests deterministic - tick_dt_s=0.05, - ) - try: - yield gov - finally: - gov._close_module() - - def test_closed_loop_off_alpha_stays_one(self, governor: ReferenceGovernor) -> None: - # Sanity: with closed_loop=False, speed_limit_at never touches alpha. - governor.set_path(straight_line(length=2.0)) - # Query at a series of poses — including off-path ones that would - # produce nonzero CTE if measured. - v_a = governor.speed_limit_at(1.0, 0.0) - v_b = governor.speed_limit_at(1.0, 0.2) # 20cm off path - v_c = governor.speed_limit_at(1.0, -0.5) - # All identical (open-loop is pose-independent past nearest-idx). - assert v_a == pytest.approx(v_b, rel=1e-6) - assert v_b == pytest.approx(v_c, rel=1e-6) - # And alpha never moved. - assert governor._alpha == pytest.approx(1.0) - - def test_alpha_decreases_on_excess_cte(self, closed_loop_governor: ReferenceGovernor) -> None: - gov = closed_loop_governor - gov.set_path(straight_line(length=2.0)) - # Feed cte = 2 * e_max = 0.10m for 5 ticks. - prev_alpha = 1.0 - for _ in range(5): - alpha, integral, filtered = gov._update_alpha(cte_raw=0.10) - with gov._state_lock: - gov._alpha = alpha - gov._alpha_integral = integral - gov._cte_filtered = filtered - assert alpha <= prev_alpha + 1e-9 # monotonic decrease - prev_alpha = alpha - assert gov._alpha < 1.0 - assert gov._alpha >= gov.config.alpha_min - - def test_alpha_recovers_when_cte_low(self, closed_loop_governor: ReferenceGovernor) -> None: - gov = closed_loop_governor - gov.set_path(straight_line(length=2.0)) - # Drive alpha down with high CTE. - for _ in range(10): - a, i, f = gov._update_alpha(cte_raw=0.15) - with gov._state_lock: - gov._alpha = a - gov._alpha_integral = i - gov._cte_filtered = f - depressed = gov._alpha - assert depressed < 0.9, f"setup failed: alpha={depressed}" - # Now feed cte well below e_max → alpha should rise back. - for _ in range(20): - a, i, f = gov._update_alpha(cte_raw=0.01) - with gov._state_lock: - gov._alpha = a - gov._alpha_integral = i - gov._cte_filtered = f - assert gov._alpha > depressed - - def test_anti_windup_at_saturation(self, closed_loop_governor: ReferenceGovernor) -> None: - gov = closed_loop_governor - gov.set_path(straight_line(length=2.0)) - # Feed a catastrophic CTE (10x e_max) for many ticks. alpha will peg - # at alpha_min; the integral must NOT keep accumulating past the - # max_integral clamp. - for _ in range(500): - a, i, f = gov._update_alpha(cte_raw=0.50) - with gov._state_lock: - gov._alpha = a - gov._alpha_integral = i - gov._cte_filtered = f - assert gov._alpha == pytest.approx(gov.config.alpha_min) - assert abs(gov._alpha_integral) <= gov.config.max_integral + 1e-9 - - def test_alpha_resets_on_set_path(self, closed_loop_governor: ReferenceGovernor) -> None: - gov = closed_loop_governor - gov.set_path(straight_line(length=2.0)) - # Drive alpha down. - for _ in range(20): - a, i, f = gov._update_alpha(cte_raw=0.20) - with gov._state_lock: - gov._alpha = a - gov._alpha_integral = i - gov._cte_filtered = f - assert gov._alpha < 1.0 - # New path → fresh alpha, integral, filter. - gov.set_path(circle(radius=1.0, n_points=120)) - assert gov._alpha == pytest.approx(1.0) - assert gov._alpha_integral == pytest.approx(0.0) - assert gov._cte_filtered == pytest.approx(0.0) - - def test_speed_limit_at_uses_alpha(self, closed_loop_governor: ReferenceGovernor) -> None: - gov = closed_loop_governor - gov.set_path(straight_line(length=2.0)) - # On-path query (alpha==1.0 at start; alpha update pushes back to 1.0). - v_on_path = gov.speed_limit_at(1.0, 0.0) - # Set both alpha=0.5 AND a consistent integral state so the alpha update - # at error=0 keeps alpha at 0.5 (the integral cancels the +Δalpha the P - # term would otherwise inject toward 1.0). With kp=4, ki=0.5, - # error=0 ⟹ alpha_raw = 1.0 - 0 - 0.5·I = 0.5 ⇒ I = 1.0. Then - # max_integral=0.5 clamps to 0.5, giving alpha_raw = 0.75 → alpha=0.75. - # Just assert that v scales with alpha after one tick (not exactly - # 0.5x, but strictly less than the on-path alpha=1.0 case). - with gov._state_lock: - gov._alpha = 0.5 - gov._alpha_integral = 1.0 # gets clamped to max_integral=0.5 - v_with_alpha_low = gov.speed_limit_at(1.0, 0.0) - assert v_with_alpha_low < v_on_path - - def test_ema_filter_dampens_spikes(self, artifact_path: FsPath) -> None: - # Use the public ctor with EMA enabled (cte_ema_alpha < 1). - gov = ReferenceGovernor( - plant_artifact_path=str(artifact_path), - e_max_default=0.05, - closed_loop=True, - cte_ema_alpha=0.2, # heavy smoothing - ) - try: - gov.set_path(straight_line(length=2.0)) - # Single CTE spike at 0.3m followed by zeros. - inputs = [0.30] + [0.0] * 10 - filtered_history = [] - for cte_raw in inputs: - a, i, f = gov._update_alpha(cte_raw=cte_raw) - with gov._state_lock: - gov._alpha = a - gov._alpha_integral = i - gov._cte_filtered = f - filtered_history.append(f) - # First filtered value < raw spike (low-pass). - assert filtered_history[0] < 0.30 - # Filter decays back toward zero (subsequent zeros). - assert filtered_history[-1] < filtered_history[0] - finally: - gov._close_module() - - -class TestConcurrentUpdates: - """Atomic-snapshot test: hammer update_e_max from one thread while - cap() reads from another. Should never crash or return torn state. - """ - - def test_concurrent_update_and_read(self, governor: ReferenceGovernor) -> None: - governor.set_path(circle(radius=1.0, n_points=200)) - stop = threading.Event() - errors: list[BaseException] = [] - - def writer() -> None: - try: - e_values = [0.02, 0.05, 0.10, 0.20, 0.50] - i = 0 - while not stop.is_set(): - governor.update_e_max(e_values[i % len(e_values)]) - i += 1 - except BaseException as e: - errors.append(e) - - def reader() -> None: - try: - while not stop.is_set(): - out = governor.cap(0.5, 0.5, 1.0, 0.0, 0.5) - # All outputs must be finite and bounded. - assert all(math.isfinite(v) for v in out) - assert abs(out[0]) <= 1.0 + 1e-6 - except BaseException as e: - errors.append(e) - - threads = [threading.Thread(target=writer), threading.Thread(target=reader)] - for t in threads: - t.start() - time.sleep(0.5) - stop.set() - for t in threads: - t.join(timeout=2.0) - assert not errors, f"Concurrent execution raised: {errors}" diff --git a/examples/go2_reference_governor_demo.py b/examples/go2_reference_governor_demo.py deleted file mode 100644 index cb979a00ad..0000000000 --- a/examples/go2_reference_governor_demo.py +++ /dev/null @@ -1,600 +0,0 @@ -#!/usr/bin/env python -# Copyright 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. - -"""End-to-end ReferenceGovernor demo (in-process sim). - -Wires the governor as the BaselinePathFollowerTask's `external_profile_cap`, -drives a manual tick loop against a TwistBasePlantSim, and prints the CTE -summary using the existing benchmark scoring. - -Two modes: - --mode static fixed e_max throughout the run - --mode square-wave e_max alternates high/low in a background thread, - exercising the atomic-snapshot recompute path - -The same coordinator + follower + plant code path runs against real -hardware when the operator coord brings up a DDS adapter instead of the -in-process FOPDT plant — this script is the simplest reproducible variant. - -Usage examples (from repo root): - uv run python examples/go2_reference_governor_demo.py --path single_corner --e-max 0.05 - uv run python examples/go2_reference_governor_demo.py --path circle --mode square-wave \\ - --e-max-high 0.10 --e-max-low 0.02 --period 4.0 -""" - -from __future__ import annotations - -import argparse -from dataclasses import dataclass -from pathlib import Path as FsPath -import sys -import tempfile -import threading -import time - -import matplotlib - -matplotlib.use("Agg") -import matplotlib.pyplot as plt - -from dimos.control.task import ( - CoordinatorState, - JointStateSnapshot, -) -from dimos.control.tasks.baseline_path_follower_task import ( - BaselinePathFollowerTask, - BaselinePathFollowerTaskConfig, -) -from dimos.core.global_config import global_config -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.navigation.reference_governor import ReferenceGovernor -from dimos.utils.benchmarking import paths as path_battery -from dimos.utils.benchmarking.plant import ( - GO2_PLANT_FITTED, - TwistBasePlantSim, -) -from dimos.utils.benchmarking.scoring import ( - ExecutedTrajectory, - TrajectoryTick, - score_run, -) -from dimos.utils.benchmarking.tuning import ( - FopdtChannelDC, - PlantModelDC, - Provenance, - derive_config, - git_sha, -) - -# --- Joints used by the BaselinePathFollowerTask (twist-base trio) --------- -_JX, _JY, _JYAW = "base/vx", "base/vy", "base/wz" -_JOINTS = [_JX, _JY, _JYAW] - - -def _write_self_test_artifact(tmpdir: FsPath) -> FsPath: - """Emit a minimal TuningConfig artifact at tmpdir/config.json built - from the vendored Go2 plant. Avoids requiring a real characterization - artifact on disk just to run the demo.""" - plant = GO2_PLANT_FITTED - prov = Provenance( - robot_id="go2-demo", - surface="demo", - mode="default", - date="demo", - git_sha=git_sha(), - sim_or_hw="hw", # hw so valid_for_tuning=True (we want the gates open) - characterization_session_dir="self-test (demo script)", - ) - cfg = derive_config(plant, prov) - # Substitute the artifact's plant DC for explicit clarity in JSON. - cfg.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), - ) - p = tmpdir / "go2_demo_config.json" - cfg.to_json(p) - return p - - -def _resolve_path(name: str) -> NavPath: - table = { - "straight_line": lambda: path_battery.straight_line(length=2.0), - "single_corner": lambda: path_battery.single_corner(leg_length=2.0, angle_deg=90.0), - "circle": lambda: path_battery.circle(radius=1.0, n_points=120), - "square": lambda: path_battery.square(side=2.0), - "figure_eight": lambda: path_battery.figure_eight(loop_radius=1.0, n_points=200), - "slalom": lambda: path_battery.slalom(), - "smooth_corner": lambda: path_battery.smooth_corner( - leg_length=2.0, angle_deg=90.0, arc_radius=0.5 - ), - } - if name not in table: - raise SystemExit(f"unknown --path {name!r}; pick one of {sorted(table)}") - return table[name]() - - -def _state(plant: TwistBasePlantSim, t_now: float, dt: float) -> CoordinatorState: - return CoordinatorState( - joints=JointStateSnapshot( - joint_positions={_JX: plant.x, _JY: plant.y, _JYAW: plant.yaw}, - joint_velocities={_JX: plant.vx, _JY: plant.vy, _JYAW: plant.wz}, - joint_efforts={_JX: 0.0, _JY: 0.0, _JYAW: 0.0}, - timestamp=t_now, - ), - t_now=t_now, - dt=dt, - ) - - -def _start_pose(plant: TwistBasePlantSim, t_now: float) -> PoseStamped: - return PoseStamped( - ts=t_now, - position=Vector3(plant.x, plant.y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, plant.yaw)), - ) - - -def _square_wave_e_max( - governor: ReferenceGovernor, - e_high: float, - e_low: float, - period: float, - stop: threading.Event, - log: list[tuple[float, float]], -) -> None: - t0 = time.perf_counter() - half = period / 2.0 - while not stop.is_set(): - for value in (e_high, e_low): - if stop.is_set(): - break - governor.update_e_max(value) - log.append((time.perf_counter() - t0, value)) - stop.wait(half) - - -@dataclass -class RunResult: - traj: ExecutedTrajectory - e_log: list[tuple[float, float]] - alpha_log: list[tuple[float, float]] # (t, alpha) — empty unless closed_loop - cte_log: list[tuple[float, float]] # (t, instantaneous CTE m) — empty unless closed_loop - - -def _run_once( - path: NavPath, - args: argparse.Namespace, - artifact_path: FsPath, - use_governor: bool, - label: str, -) -> RunResult: - """Single pass: build follower (+ optional governor), drive plant, return - trajectory + telemetry. Used once normally, twice in --compare mode.""" - governor: ReferenceGovernor | None = None - if use_governor: - governor = ReferenceGovernor( - plant_artifact_path=str(artifact_path), - e_max_default=args.e_max, - closed_loop=bool(args.closed_loop), - ) - governor.set_path(path) - - follower_cfg = BaselinePathFollowerTaskConfig( - joint_names=_JOINTS, - priority=20, - speed=args.speed, - control_frequency=float(args.tick_hz), - goal_tolerance=0.2, - orientation_tolerance=0.35, - k_angular=0.5, - ) - follower = BaselinePathFollowerTask( - name="baseline_follower", - config=follower_cfg, - global_config=global_config, - external_profile_cap=governor, - ) - - dt = 1.0 / args.tick_hz - plant = TwistBasePlantSim(GO2_PLANT_FITTED) - plant.reset(0.0, 0.0, 0.0, dt) - - start_pose = _start_pose(plant, time.perf_counter()) - if not follower.start_path(path, start_pose): - print(f"[{label}] start_path rejected", file=sys.stderr) - if governor is not None: - governor._close_module() - return RunResult( - traj=ExecutedTrajectory(ticks=[], arrived=False), - e_log=[], - alpha_log=[], - cte_log=[], - ) - - e_log: list[tuple[float, float]] = [] - alpha_log: list[tuple[float, float]] = [] - cte_log: list[tuple[float, float]] = [] - stop_ev = threading.Event() - sq_thread: threading.Thread | None = None - if use_governor and args.mode == "square-wave" and governor is not None: - sq_thread = threading.Thread( - target=_square_wave_e_max, - args=( - governor, - args.e_max_high, - args.e_max_low, - args.period, - stop_ev, - e_log, - ), - daemon=True, - ) - sq_thread.start() - - ticks: list[TrajectoryTick] = [] - t0 = time.perf_counter() - deadline = t0 + args.timeout - prev_t = t0 - arrived = False - - while time.perf_counter() < deadline: - now = time.perf_counter() - dt_tick = max(dt, now - prev_t) - state = _state(plant, now, dt_tick) - output = follower.compute(state) - if output is None or output.velocities is None: - if follower.get_state() == "arrived": - arrived = True - break - vx, vy, wz = output.velocities - plant.step(vx, vy, wz, dt_tick) - prev_t = now - ticks.append( - TrajectoryTick( - t=now - t0, - pose=_start_pose(plant, now), - cmd_twist=Twist( - linear=Vector3(vx, vy, 0.0), - angular=Vector3(0.0, 0.0, wz), - ), - actual_twist=Twist( - linear=Vector3(plant.vx, plant.vy, 0.0), - angular=Vector3(0.0, 0.0, plant.wz), - ), - ) - ) - # Closed-loop telemetry: lock-safe snapshot of alpha + filtered CTE - # (these are updated inside speed_limit_at on every follower tick). - if governor is not None and args.closed_loop: - with governor._state_lock: - alpha = governor._alpha - cte_filt = governor._cte_filtered - alpha_log.append((now - t0, alpha)) - cte_log.append((now - t0, cte_filt)) - - sleep_for = (prev_t + dt) - time.perf_counter() - if sleep_for > 0: - time.sleep(sleep_for) - - if sq_thread is not None: - stop_ev.set() - sq_thread.join(timeout=1.0) - if governor is not None: - governor._close_module() - - return RunResult( - traj=ExecutedTrajectory(ticks=ticks, arrived=arrived), - e_log=e_log, - alpha_log=alpha_log, - cte_log=cte_log, - ) - - -def _report( - label: str, - path: NavPath, - result: RunResult, - e_max: float, -) -> None: - score = score_run(path, result.traj) - duration = result.traj.ticks[-1].t if result.traj.ticks else 0.0 - lines = [ - f"\n=== {label} ===", - f" arrived : {result.traj.arrived}", - f" cte_max : {score.cte_max * 100:.1f} cm", - f" cte_rms : {score.cte_rms * 100:.1f} cm", - f" duration : {duration:.1f} s ({len(result.traj.ticks)} ticks)", - ] - if result.alpha_log: - final_alpha = result.alpha_log[-1][1] - within = "YES" if score.cte_max <= e_max else "NO" - lines.extend( - [ - f" final alpha : {final_alpha:.3f}", - f" converged? : {within} (cte_max {score.cte_max * 100:.1f}cm vs target {e_max * 100:.1f}cm)", - ] - ) - print("\n".join(lines)) - - -def run(args: argparse.Namespace) -> int: - with tempfile.TemporaryDirectory() as raw_tmp: - tmpdir = FsPath(raw_tmp) - artifact_path = FsPath(args.config) if args.config else _write_self_test_artifact(tmpdir) - path = _resolve_path(args.path) - out = FsPath(args.out) - out.parent.mkdir(parents=True, exist_ok=True) - - if args.compare: - r_g = _run_once( - path, - args, - artifact_path, - use_governor=True, - label=f"{args.path} WITH governor", - ) - r_n = _run_once( - path, - args, - artifact_path, - use_governor=False, - label=f"{args.path} WITHOUT governor", - ) - _report(f"{args.path} (mode={args.mode}) WITH governor", path, r_g, args.e_max) - _report(f"{args.path} (mode={args.mode}) WITHOUT governor", path, r_n, args.e_max) - _plot_compare(path, r_g, r_n, args, out) - else: - r = _run_once( - path, - args, - artifact_path, - use_governor=not args.no_governor, - label=args.path, - ) - if args.no_governor: - tag = "WITHOUT governor" - elif args.closed_loop: - tag = f"CLOSED-LOOP governor e_max={args.e_max:g}m" - else: - tag = f"open-loop governor e_max={args.e_max:g}m" - _report(f"{args.path} (mode={args.mode}) {tag}", path, r, args.e_max) - _plot(path, r, args, out) - - print(f"plot : {out.resolve()}") - # Single-run mode returns 0 only if arrived; compare always returns 0 - # (it's diagnostic, not a pass/fail gate). - return 0 - - -def _plot_compare( - path: NavPath, - r_g: RunResult, - r_n: RunResult, - args: argparse.Namespace, - out: FsPath, -) -> None: - """Side-by-side: XY overlay (left) + |cmd vx|(t) overlay (right), with - an optional third subplot showing alpha(t) + measured CTE(t) when the - closed-loop variant ran. The closed-loop subplot is the diagnostic - surface for "did alpha converge to a steady value that meets the - corridor".""" - have_cl = bool(r_g.alpha_log) - ncols = 3 if have_cl else 2 - fig, axes = plt.subplots(1, ncols, figsize=(6.5 * ncols, 5.0)) - ax_xy = axes[0] - ax_v = axes[1] - - ref_x = [p.position.x for p in path.poses] - ref_y = [p.position.y for p in path.poses] - ax_xy.plot(ref_x, ref_y, "k--", lw=2, label="reference") - - def _draw(ax, ticks, color, label): - xs = [tk.pose.position.x for tk in ticks] - ys = [tk.pose.position.y for tk in ticks] - if not xs: - return - ax.plot(xs, ys, color=color, lw=1.4, label=label) - ax.plot(xs[0], ys[0], "o", color=color, ms=5) - ax.plot(xs[-1], ys[-1], "s", color=color, ms=5) - - sg = score_run(path, r_g.traj) - sn = score_run(path, r_n.traj) - _draw(ax_xy, r_g.traj.ticks, "tab:blue", f"WITH governor (cte_max={sg.cte_max * 100:.1f}cm)") - _draw( - ax_xy, r_n.traj.ticks, "tab:orange", f"WITHOUT governor (cte_max={sn.cte_max * 100:.1f}cm)" - ) - ax_xy.set_aspect("equal", adjustable="datalim") - ax_xy.set_xlabel("x (m)") - ax_xy.set_ylabel("y (m)") - ax_xy.grid(True, alpha=0.3) - ax_xy.legend(fontsize=8) - ax_xy.set_title(f"{args.path} — XY tracking") - - tg = [tk.t for tk in r_g.traj.ticks] - vg = [abs(tk.cmd_twist.linear.x) for tk in r_g.traj.ticks] - tn = [tk.t for tk in r_n.traj.ticks] - vn = [abs(tk.cmd_twist.linear.x) for tk in r_n.traj.ticks] - ax_v.plot(tg, vg, "tab:blue", lw=1.0, label="WITH governor") - ax_v.plot(tn, vn, "tab:orange", lw=1.0, label="WITHOUT governor") - if r_g.e_log: - ax2 = ax_v.twinx() - ts = [t for t, _ in r_g.e_log] - es = [e for _, e in r_g.e_log] - ax2.step(ts, es, "r-", where="post", lw=0.8, alpha=0.6, label="e_max") - ax2.set_ylabel("e_max (m)", color="r") - ax_v.set_xlabel("t (s)") - ax_v.set_ylabel("|cmd vx| (m/s)") - ax_v.set_title("Commanded forward speed") - ax_v.grid(True, alpha=0.3) - ax_v.legend(fontsize=8, loc="upper right") - - if have_cl: - _plot_closed_loop_panel(axes[2], r_g, args.e_max) - - fig.tight_layout() - fig.savefig(out, dpi=120) - plt.close(fig) - - -def _plot( - path: NavPath, - r: RunResult, - args: argparse.Namespace, - out: FsPath, -) -> None: - have_cl = bool(r.alpha_log) - have_e = bool(r.e_log) - ncols = 1 + int(have_e) + int(have_cl) - fig, axes = plt.subplots(1, ncols, figsize=(6.0 * ncols, 5.0), squeeze=False) - panel = list(axes[0]) - ax_xy = panel.pop(0) - - ref_x = [p.position.x for p in path.poses] - ref_y = [p.position.y for p in path.poses] - exec_x = [tk.pose.position.x for tk in r.traj.ticks] - exec_y = [tk.pose.position.y for tk in r.traj.ticks] - ax_xy.plot(ref_x, ref_y, "k--", lw=2, label="reference") - ax_xy.plot(exec_x, exec_y, "b-", lw=1.3, label="executed") - if exec_x: - ax_xy.plot(exec_x[0], exec_y[0], "go", ms=6, label="start") - ax_xy.plot(exec_x[-1], exec_y[-1], "rs", ms=6, label="end") - ax_xy.set_aspect("equal", adjustable="datalim") - ax_xy.set_xlabel("x (m)") - ax_xy.set_ylabel("y (m)") - ax_xy.grid(True, alpha=0.3) - ax_xy.legend(fontsize=8) - mode_tag = "CL" if args.closed_loop else "OL" - ax_xy.set_title(f"{args.path} — {mode_tag} governor e_max={args.e_max:g}m") - - if have_e: - ax_e = panel.pop(0) - ts = [t for t, _ in r.e_log] - es = [e for _, e in r.e_log] - cmd_t = [tk.t for tk in r.traj.ticks] - cmd_v = [abs(tk.cmd_twist.linear.x) for tk in r.traj.ticks] - ax_e.step(ts, es, "r-", where="post", label="e_max (m)") - ax2 = ax_e.twinx() - ax2.plot(cmd_t, cmd_v, "b-", lw=0.8, alpha=0.7, label="|cmd vx|") - ax_e.set_xlabel("t (s)") - ax_e.set_ylabel("e_max (m)", color="r") - ax2.set_ylabel("|cmd vx| (m/s)", color="b") - ax_e.set_title("e_max vs commanded speed") - ax_e.grid(True, alpha=0.3) - - if have_cl: - _plot_closed_loop_panel(panel.pop(0), r, args.e_max) - - fig.tight_layout() - fig.savefig(out, dpi=120) - plt.close(fig) - - -def _plot_closed_loop_panel(ax, r: RunResult, e_max: float) -> None: - """alpha(t) on the left axis, instantaneous CTE(t) on the right axis, - with a horizontal line at e_max so convergence is visible at a glance.""" - if not r.alpha_log: - return - t_a = [t for t, _ in r.alpha_log] - a = [v for _, v in r.alpha_log] - t_c = [t for t, _ in r.cte_log] - c = [v * 100 for _, v in r.cte_log] # cm - ax.plot(t_a, a, "tab:blue", lw=1.0, label="alpha(t)") - ax.set_ylabel("alpha", color="tab:blue") - ax.set_ylim(0.0, 1.05) - ax2 = ax.twinx() - ax2.plot(t_c, c, "tab:red", lw=0.9, alpha=0.7, label="|CTE| (cm)") - ax2.axhline( - e_max * 100, color="tab:red", ls=":", lw=0.8, alpha=0.6, label=f"e_max={e_max * 100:.0f}cm" - ) - ax2.set_ylabel("|CTE| (cm)", color="tab:red") - ax.set_xlabel("t (s)") - ax.set_title("Closed-loop alpha vs measured CTE") - ax.grid(True, alpha=0.3) - ax.legend(loc="upper left", fontsize=8) - ax2.legend(loc="upper right", fontsize=8) - - -def main() -> int: - ap = argparse.ArgumentParser(description=__doc__) - ap.add_argument( - "--config", - help="TuningConfig JSON. If omitted, a self-test artifact " - "is generated from the vendored Go2 plant.", - ) - ap.add_argument( - "--path", - default="single_corner", - help="straight_line | single_corner | circle | square | figure_eight | " - "slalom | smooth_corner", - ) - ap.add_argument( - "--speed", - type=float, - default=0.55, - help="follower base speed (m/s). Governor caps it per-waypoint.", - ) - ap.add_argument("--tick-hz", type=float, default=20.0) - ap.add_argument("--timeout", type=float, default=60.0) - ap.add_argument("--mode", choices=["static", "square-wave"], default="static") - # static mode - ap.add_argument("--e-max", type=float, default=0.05, help="static corridor half-width (m)") - # square-wave mode - ap.add_argument("--e-max-high", type=float, default=0.10) - ap.add_argument("--e-max-low", type=float, default=0.02) - ap.add_argument( - "--period", - type=float, - default=4.0, - help="square-wave period (s); half-period at each level", - ) - ap.add_argument("--out", default="/tmp/reference_governor_demo.png") - # --- comparison knobs --- - ap.add_argument( - "--no-governor", - action="store_true", - help="Run a single pass WITHOUT the governor (bare follower at " - "--speed). Use as a baseline reference.", - ) - ap.add_argument( - "--compare", - action="store_true", - help="Run the same path TWICE: once with the governor, once " - "without. Overlays both trajectories on the output plot.", - ) - # --- closed-loop knob (only meaningful when the governor is in use) --- - ap.add_argument( - "--closed-loop", - action="store_true", - help="Enable the closed-loop alpha-feedback variant: the governor " - "observes per-tick CTE and dynamically scales the open-loop " - "bound to drive actual tracking error toward e_max. Default " - "off (open-loop). Implies the governor is in use; ignored " - "when --no-governor is set.", - ) - args = ap.parse_args() - if args.no_governor and args.compare: - ap.error("--no-governor and --compare are mutually exclusive") - if args.no_governor and args.closed_loop: - ap.error("--closed-loop has no effect with --no-governor") - return run(args) - - -if __name__ == "__main__": - sys.exit(main()) From baac57001b2e24626ac7ac2b5e895ffb0e3c12f1 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 14:16:10 -0700 Subject: [PATCH 35/51] added governor blueprint that uses precision based controller --- .../blueprints/basic/unitree_go2_benchmark.py | 21 ++++++++++--------- .../basic/unitree_go2_benchmark_rg.py | 20 ++++++++++++------ .../basic/unitree_go2_coordinator.py | 17 +++++++++++++++ dimos/robot/unitree/keyboard_teleop.py | 8 +++++++ 4 files changed, 50 insertions(+), 16 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py index d4a7dacd5f..86db2e8eb7 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py @@ -33,17 +33,13 @@ --module.benchmarker.ff=true # apply derived feedforward --module.benchmarker.profile=true # apply derived static velocity profile - --module.benchmarker.rg=true \\ - --module.benchmarker.e_max=0.05 # apply RG-derived per-waypoint profile + --module.benchmarker.rg=true # route runs through precision_follower -The RG arm uses ``solve_profile()`` imported directly from -``reference_governor.py`` — there is no RG ``Module`` in this blueprint -because the per-cell math is one-shot (no live ``e_max`` stream to react -to), and a Module wrapper would force a cross-process per-tick RPC on -the controller's hot path. The Benchmarker computes the per-waypoint -speeds once per path and ships them to the follower as a plain -``list[float]`` via the new ``PathFollowerTask.start_path( -velocity_profile=...)`` kwarg. +The RG arm runs against the operator coord's ``precision_follower`` task +(a ``PathFollowerTask`` subclass that owns its own ``solve_profile()`` +recompute and reacts to ``KeyboardTeleop``'s ``e_max`` stream — number +keys 0-9 set the corridor half-width live). No RG math inside Benchmarker; +it just picks the task name based on the ``rg`` flag. Recordings land at ``/data/benchmark//_benchmark__.db`` @@ -60,6 +56,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist 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.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, @@ -85,6 +82,10 @@ # blueprints don't cross-talk if both happen to run on the same # LCM bus. ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + # KeyboardTeleop's number-key (0-9) corridor stream -> any task in + # the coord with set_e_max(). No-ops for the bare arm (path_follower + # ignores it), live for the RG arm (precision_follower recomputes). + ("e_max", Float32): LCMTransport("/e_max", Float32), # Recorder taps the same LCM topics the rest of the stack # already uses; no new wires, just additional subscribers. ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py index caca7d14b7..0b2e566717 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py @@ -16,13 +16,17 @@ """Unitree Go2 benchmark — RG arm variant. Same composition as ``unitree-go2-benchmark`` but with ``rg=True`` baked -into the Benchmarker config so the RG-derived per-waypoint speed -profile is applied without any ``--module.benchmarker.rg`` override at -launch. ``e_max`` is left at the BenchmarkerConfig default -(0.05) — pass ``--module.benchmarker.e_max `` to sweep corridor -widths. +into the Benchmarker config. Runs are routed through the operator coord's +``precision_follower`` task (a ``PathFollowerTask`` subclass that owns its +own ``solve_profile()`` recompute reacting to ``KeyboardTeleop``'s +``e_max`` stream — number keys 0-9 set the corridor half-width live). - dimos run unitree-go2-benchmark-rg --module.benchmarker.config +The precision_follower needs its own copy of the artifact path so it can +load the plant + velocity-profile constants: + + dimos run unitree-go2-benchmark-rg \\ + --module.benchmarker.config \\ + -o coordinator.tasks[1].params.artifact_path= """ from __future__ import annotations @@ -32,6 +36,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist 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.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, @@ -53,6 +58,9 @@ ).transports( { ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + # KeyboardTeleop's number-key (0-9) corridor stream -> coord's + # e_max input -> precision_follower.set_e_max(). Live RG corridor. + ("e_max", Float32): LCMTransport("/e_max", Float32), ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index 8b10bb0bdd..96ad6c2d1d 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -30,6 +30,7 @@ from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.utils.path_utils import get_project_root _go2_joints = make_twist_base_joints("go2") @@ -61,6 +62,22 @@ joint_names=_go2_joints, priority=10, ), + # RG-arm path follower — same control law as path_follower + # but owns its own solve_profile() recompute reacting to + # KeyboardTeleop's e_max stream. artifact_path is the + # tuning JSON the task loads on start_path() for the plant + # model + velocity-profile constants; + TaskConfig( + name="precision_follower", + type="precision_path_follower", + joint_names=_go2_joints, + priority=10, + params={ + "artifact_path": str( + get_project_root() / "data" / "characterization" / "go2" / "" + ), + }, + ), ], ), ) diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index f69eb7d5e0..9d4ee92d70 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -25,6 +25,7 @@ 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 from dimos.utils.logging_config import setup_logger @@ -67,6 +68,9 @@ class KeyboardTeleop(Module): 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 @@ -156,6 +160,9 @@ def _pygame_loop(self) -> None: 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) @@ -257,6 +264,7 @@ def _update_display(self, twist: Twist) -> None: "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) From 2c6814c53cb56fa7838622fec0a2c21347f1b8c1 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 14:17:06 -0700 Subject: [PATCH 36/51] refactor code for better UX --- dimos/robot/all_blueprints.py | 1 + dimos/utils/benchmarking/benchmark.py | 130 +++++------------- .../benchmarking/characterization_recorder.py | 19 ++- .../benchmarking/reports/tuning_README.md | 9 +- 4 files changed, 59 insertions(+), 100 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index d5e66b28ed..02d4461589 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -100,6 +100,7 @@ "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", "unitree-go2-benchmark": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark:unitree_go2_benchmark", + "unitree-go2-benchmark-rg": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark_rg:unitree_go2_benchmark_rg", "unitree-go2-characterization": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_characterization:unitree_go2_characterization", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", diff --git a/dimos/utils/benchmarking/benchmark.py b/dimos/utils/benchmarking/benchmark.py index 6973903ecf..10d1ba6d2a 100644 --- a/dimos/utils/benchmarking/benchmark.py +++ b/dimos/utils/benchmarking/benchmark.py @@ -88,7 +88,8 @@ # inside the operator coord (see TaskConfig type="path_follower" # wired into each robot's coord blueprint). _JOINT_STATE_TOPIC = "/coordinator/joint_state" -_PATH_FOLLOWER_TASK_NAME = "path_follower" # task name in operator coord blueprint +_PATH_FOLLOWER_TASK_NAME = "path_follower" # bare/ff/profile arms +_PRECISION_FOLLOWER_TASK_NAME = "precision_follower" # rg arm _ARRIVED_STATES = frozenset({"arrived", "completed"}) _FAILED_STATES = frozenset({"aborted"}) @@ -269,12 +270,13 @@ def reset_trajectory(self) -> None: self._t0 = None -def _invoke(coord: RPCClient, method: str, **kwargs: object) -> object: - """RPC `task_invoke(_PATH_FOLLOWER_TASK_NAME, method, kwargs)` on the operator +def _invoke(coord: RPCClient, task_name: str, method: str, **kwargs: object) -> object: + """RPC `task_invoke(task_name, method, kwargs)` on the operator coord. Centralises the .task_invoke wrapping so the run loop reads as - plain method calls on a remote object.""" + plain method calls on a remote object. ``task_name`` selects between + the bare path_follower and the precision_follower (RG arm).""" return coord.task_invoke( - task_name=_PATH_FOLLOWER_TASK_NAME, + task_name=task_name, method=method, kwargs=dict(kwargs), ) @@ -291,15 +293,17 @@ def _run_baseline( profile_config: VelocityProfileConfig | None, timeout_s: float, label: str, - velocity_profile: list[float] | None = None, + task_name: str, ) -> tuple[ExecutedTrajectory, NavPath]: - """Send a path to the operator coord's ``path_follower`` task and - wait for it to terminate. The task is pre-added by the operator's - blueprint (priority 20, claims base/{vx,vy,wz}) so it preempts the - operator's teleop velocity task while a run is active. We only RPC - configure/start/cancel; the coord owns the tick-loop compute and the - adapter that drives the robot. ``ff_config``/``profile_config`` are - optional arms (``None`` = bare = the physical-limit measurement). + """Send a path to the operator coord's path-follower task (selected + by ``task_name`` — ``"path_follower"`` for the bare/ff/profile arms, + ``"precision_follower"`` for the RG arm) and wait for it to terminate. + The task is pre-added by the operator's blueprint (priority 20, claims + base/{vx,vy,wz}) so it preempts the operator's teleop velocity task + while a run is active. We only RPC configure/start/cancel; the coord + owns the tick-loop compute and the adapter that drives the robot. + ``ff_config``/``profile_config`` are optional arms (``None`` = bare = + the physical-limit measurement). Path is anchored to the robot's first observed pose so the operator only has to roughly aim the robot. Returns the executed trajectory @@ -312,6 +316,7 @@ def _run_baseline( if not _invoke( coord, + task_name, "configure", speed=speed, k_angular=k_angular, @@ -321,13 +326,7 @@ def _run_baseline( print(f" [{label}] configure rejected — task still active from prior run?") return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w - start_kwargs: dict[str, Any] = {"path": path_w, "current_odom": pose0} - if velocity_profile is not None: - # Path was rigid-transformed by _shift_path_to_start_at_pose; - # waypoint count + local curvature are preserved, so the - # per-waypoint speeds computed on the original path remain valid. - start_kwargs["velocity_profile"] = velocity_profile - if not _invoke(coord, "start_path", **start_kwargs): + if not _invoke(coord, task_name, "start_path", path=path_w, current_odom=pose0): print(f" [{label}] start_path rejected") return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=False), path_w @@ -337,7 +336,7 @@ def _run_baseline( terminated = False try: while time.perf_counter() < deadline: - st = _invoke(coord, "get_state") + st = _invoke(coord, task_name, "get_state") if st in _ARRIVED_STATES: arrived = True terminated = True @@ -353,7 +352,7 @@ def _run_baseline( finally: # Best-effort cancel; safe to ignore if already terminal. try: - _invoke(coord, "cancel") + _invoke(coord, task_name, "cancel") except Exception: pass return ExecutedTrajectory(ticks=recorder.snapshot(), arrived=arrived), path_w @@ -383,8 +382,6 @@ def _run_ladder( coord_rpc: RPCClient, recorder: _JointStateRecorder, use_rg: bool = False, - e_max: float = 0.05, - rg_min_speed: float | None = None, gate_input: Callable[[str], str] = input, gate_keys_label: str = "ENTER=run s=skip q=quit", ) -> tuple[list[OperatingPoint], list[dict]]: @@ -393,52 +390,14 @@ def _run_ladder( ff = cfg.feedforward.to_runtime() if use_ff else None k_angular = float(cfg.recommended_controller.params.get("k_angular", 0.5)) - # Build RG constraint set + plant once if --rg arm is enabled. We - # import the math directly from the reference_governor module - # (solve_profile + the constraint classes are public module-level - # exports); no RG Module composition needed. - rg_constraints = None - rg_plant = None - rg_vp = None - if use_rg: - from dimos.navigation.reference_governor.reference_governor import ( - GeometricMVC, - LateralMVC, - PrecisionMVC, - SaturationMVC, - solve_profile, - ) - - rg_plant = cfg.plant - rg_vp = cfg.velocity_profile - # Closure over e_max so PrecisionMVC stays decoupled from a - # specific value — same callable contract RG itself uses. - rg_constraints = [ - GeometricMVC(v_max=rg_vp.max_linear_speed), - SaturationMVC(omega_max=rg_vp.max_angular_speed), - LateralMVC(a_lat_max=rg_vp.max_centripetal_accel), - PrecisionMVC(e_max_provider=lambda: e_max), - ] - _solve_profile = solve_profile + # The RG arm runs against the precision_follower task — a path-follower + # subclass that owns its own solve_profile() recompute on e_max + # updates. Benchmarker just picks the task name; no RG math here. + task_name = _PRECISION_FOLLOWER_TASK_NAME if use_rg else _PATH_FOLLOWER_TASK_NAME points: list[OperatingPoint] = [] runs: list[dict] = [] # for the XY trajectory overlay for name, path in _path_set().items(): - # Precompute RG-derived per-waypoint speeds once per path (same - # for every speed cell — RG output is path-shape dependent, not - # commanded-speed dependent). Plain list[float] crosses the - # configure RPC cleanly. - rg_speeds: list[float] | None = None - if use_rg and rg_constraints is not None and rg_plant is not None and rg_vp is not None: - arr = _solve_profile( - path, - rg_plant, - rg_constraints, - accel_max=rg_vp.max_linear_accel, - decel_max=rg_vp.max_linear_decel, - min_speed=(rg_min_speed if rg_min_speed is not None else rg_vp.min_speed), - ) - rg_speeds = [float(v) for v in arr] for speed in speeds: prof_cfg = ( cfg.velocity_profile.to_runtime(max_linear_speed=speed) if use_profile else None @@ -467,7 +426,7 @@ def _run_ladder( prof_cfg, timeout_s, f"{name}@{speed:.2f}", - velocity_profile=rg_speeds, + task_name=task_name, ) # Score/plot against the executed-frame reference (anchored path). s = score_run(ref, traj) @@ -634,18 +593,10 @@ class BenchmarkerConfig(ModuleConfig): timeout: float = 60.0 ff: bool = False profile: bool = False - # OPT-IN: apply RG-derived per-waypoint speed profile (uses the - # artifact's plant + cfg.e_max for the precision constraint). + # OPT-IN: route runs through the precision_follower task (RG arm — + # the operator coord's precision_follower owns its own artifact + + # solve_profile() recompute reacting to KeyboardTeleop's e_max stream). rg: bool = False - e_max: float = 0.05 - # RG OUTPUT FLOOR (m/s). Overrides artifact.velocity_profile.min_speed - # for the RG arm. Default 0.2 matches the Go2's practical min motion - # threshold — characterization typically writes ~0.05 into the - # artifact, which is below where the platform actually moves, so the - # RG output at SaturationMVC-binding corners falls below threshold - # and the robot stalls. Set explicitly (or None to defer to the - # artifact) for other platforms. - min_speed: float | None = 0.2 # OPT-IN: output directory for plots + standalone-arm JSONs (the # input config artifact augmentation always lands at args.config). out_dir: str | None = None @@ -766,8 +717,6 @@ def _run(self) -> None: coord_rpc=coord_rpc, recorder=recorder, use_rg=cfg.rg, - e_max=cfg.e_max, - rg_min_speed=cfg.min_speed, gate_input=gate_input, gate_keys_label=gate_keys_label, ) @@ -862,22 +811,9 @@ def main() -> None: ap.add_argument( "--rg", action="store_true", - help="OPT-IN arm: apply RG-derived per-waypoint speed profile " - "(uses the same artifact + --e-max corridor; default OFF)", - ) - ap.add_argument( - "--e-max", - type=float, - default=0.05, - help="RG corridor half-width in m (only used when --rg is set)", - ) - ap.add_argument( - "--min-speed", - type=float, - default=None, - help="override the RG arm's min_speed floor (default: artifact's " - "velocity_profile.min_speed). Useful when the robot's actual min " - "motion threshold is higher than what was characterized.", + help="OPT-IN arm: route runs through the precision_follower task " + "(the coord's precision-path-follower owns the RG profile + live " + "e_max from KeyboardTeleop's 0-9 keys; default OFF)", ) ap.add_argument( "--out", @@ -896,8 +832,6 @@ def main() -> None: ff=args.ff, profile=args.profile, rg=args.rg, - e_max=args.e_max, - min_speed=args.min_speed, out_dir=args.out, ) instance.start() diff --git a/dimos/utils/benchmarking/characterization_recorder.py b/dimos/utils/benchmarking/characterization_recorder.py index adcef87c60..7ad0d3463d 100644 --- a/dimos/utils/benchmarking/characterization_recorder.py +++ b/dimos/utils/benchmarking/characterization_recorder.py @@ -40,9 +40,13 @@ from datetime import date from pathlib import Path +import time +from typing import Any + +from reactivex.disposable import Disposable from dimos.core.core import rpc -from dimos.core.stream import In +from dimos.core.stream import In, Stream from dimos.memory2.module import Recorder, RecorderConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist @@ -101,5 +105,18 @@ def start(self) -> None: ) super().start() + def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: + """Override the parent to skip the per-message TF lookup. These + blueprints don't publish a TF graph, so the parent's "No tf + available" warning fires once per message and floods the terminal. + Pose is stored as ``None``; rerun-time analysis tools either don't + need a pose anchor or pull it from the recorded ``odom`` stream.""" + + def on_msg(msg: Any) -> None: + ts = getattr(msg, "ts", None) or time.time() + stream.append(msg, ts=ts, pose=None) + + self.register_disposable(Disposable(input_topic.subscribe(on_msg))) + __all__ = ["CharacterizationRecorder", "CharacterizationRecorderConfig"] diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index ce86e9e5f1..f01f0b4060 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -49,9 +49,16 @@ Override defaults with `-o characterizer.=`, e.g.: ``` dimos run unitree-go2-benchmark \ - -o benchmarker.config=/path/to/go2_config_hw_*.json + -o benchmarker.config=/path/to/go2_config_hw_*.json # bare baseline + +dimos run unitree-go2-benchmark-rg \ + -o benchmarker.config=/path/to/go2_config_hw_*.json # RG arm baked in (rg=true) ``` +Each comparison arm is its own blueprint variant; the `-o` overrides +are reserved for runtime knobs only (the artifact path, e_max sweeps, +etc.). + Per (path, speed) × fixed path set (`straight_line`, `single_corner`, `square`, `circle`), the loop prompts you to aim the robot, then drives the baseline follower over the anchored path. CTE is scored from the real trajectory. **Comparison arms** (each measures *against* the bare physical limit): From 1a1c6ee168db4c2da9e83c1f6853c7548a856fc7 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 16:08:39 -0700 Subject: [PATCH 37/51] updated coordinator to support nav path --- dimos/control/coordinator.py | 74 ++++++++++++++++++++++++++++++++++++ dimos/control/tick_loop.py | 12 ++++++ 2 files changed, 86 insertions(+) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 8c212a7b2f..ab40a315bb 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -56,7 +56,10 @@ from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.hardware.whole_body.spec import WholeBodyAdapter 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 from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.std_msgs.Float32 import Float32 from dimos.teleop.quest.quest_types import ( @@ -162,6 +165,12 @@ class ControlCoordinator(Module): # Input: Live RG corridor half-width (m) for tasks with set_e_max(). e_max: In[Float32] + # Input: Planned path for tasks with set_path(). Typical source is the + # nav stack (FarPlanner/LocalPlanner). The coord snapshots the latest + # odom from its tick state and passes it alongside the path so the + # task's anchoring isn't racing the first compute() tick. + path: In[Path] + # Arming and dry-run are one-shot RPCs, not streams. def __init__(self, *args: Any, **kwargs: Any) -> None: @@ -187,6 +196,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._twist_command_unsub: Callable[[], None] | None = None self._buttons_unsub: Callable[[], None] | None = None self._e_max_unsub: Callable[[], None] | None = None + self._path_unsub: Callable[[], None] | None = None logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz") @@ -534,6 +544,52 @@ def _on_e_max(self, msg: Float32) -> None: if set_e_max is not None: set_e_max(value) + def _snapshot_odom(self) -> PoseStamped | None: + """Reconstruct a PoseStamped from the tick loop's latest cached + state. Looks at the first twist-base hardware's [x, y, yaw] + joints (same convention PathFollowerTask.compute uses). Returns + None if no tick has fired yet or no twist-base is registered.""" + if self._tick_loop is None: + return None + state = self._tick_loop.latest_state + if state is None: + return None + positions = state.joints.joint_positions + with self._hardware_lock: + for hw in self._hardware.values(): + if hw.component.hardware_type != HardwareType.BASE: + continue + names = hw.joint_names + if len(names) < 3: + continue + x = positions.get(names[0]) + y = positions.get(names[1]) + yaw = positions.get(names[2]) + if x is None or y is None or yaw is None: + return None + return PoseStamped( + ts=state.t_now, + position=Vector3(float(x), float(y), 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + ) + return None + + def _on_path(self, msg: Path) -> None: + """Forward a planned path to any task with set_path(path, odom). + Snapshots latest odom from the tick loop's cached state so the + task's anchoring doesn't race the first compute() tick.""" + odom = self._snapshot_odom() + if odom is None: + logger.warning( + "ControlCoordinator received path but no odom snapshot available yet; dropping." + ) + return + with self._task_lock: + for task in self._tasks.values(): + set_path = getattr(task, "set_path", None) + if set_path is not None: + set_path(msg, odom) + @rpc def set_activated(self, engaged: bool) -> None: """Arm/disarm every task exposing ``arm()`` / ``disarm()``.""" @@ -708,6 +764,21 @@ def start(self) -> None: "Set transport via blueprint." ) + # Subscribe to path if any task implements set_path (nav-stack-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." + ) + # Arming + dry-run are RPC-only; no stream subscription here. logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz") @@ -733,6 +804,9 @@ def stop(self) -> None: if self._e_max_unsub: self._e_max_unsub() self._e_max_unsub = None + if self._path_unsub: + self._path_unsub() + self._path_unsub = None if self._tick_loop: self._tick_loop.stop() diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index d38b7e76d2..4e985c0934 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -113,6 +113,10 @@ def __init__( self._tick_thread: threading.Thread | None = None self._last_tick_time: float = 0.0 self._tick_count: int = 0 + # Cached most-recently-built state. Off-thread consumers (the + # coord's LCM stream handlers, e.g. _on_path) read this to + # snapshot odom without racing on read_state(). + self._latest_state: CoordinatorState | None = None @property def tick_count(self) -> int: @@ -124,6 +128,12 @@ def is_running(self) -> bool: """Whether the tick loop is currently running.""" return not self._stop_event.is_set() + @property + def latest_state(self) -> CoordinatorState | None: + """Most-recently-built CoordinatorState, or None before the first + tick.""" + return self._latest_state + def start(self) -> None: """Start the tick loop in a daemon thread.""" if not self._stop_event.is_set(): @@ -178,6 +188,8 @@ def _tick(self) -> None: imu_states = self._read_all_imu() state = CoordinatorState(joints=joint_states, imu=imu_states, t_now=t_now, dt=dt) + self._latest_state = state + commands = self._compute_all_tasks(state) joint_commands, preemptions = self._arbitrate(commands) From fcb5d78be2c69ff818f5565e1fd61e4b91b2ebb0 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 16:09:48 -0700 Subject: [PATCH 38/51] set_path method added to start new path from currentl location when preempted --- .../precision_path_follower_task.py | 13 ++++ dimos/robot/unitree/keyboard_teleop.py | 66 ++++++++++++------- 2 files changed, 55 insertions(+), 24 deletions(-) diff --git a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py index b3faa675ff..67ae189575 100644 --- a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py +++ b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py @@ -94,6 +94,19 @@ def set_e_max(self, value: float) -> None: if self._path is not None: self._recompute_profile() + def set_path(self, path: Path, current_odom: PoseStamped) -> None: + """Coordinator broadcast hook for nav-stack-emitted paths. The + coord snapshots the latest odom from its tick state and passes + it here so anchoring doesn't race the first compute() tick. + Delegates to the regular start_path lifecycle (which the override + on this class extends with the lazy artifact load + solve_profile + recompute).""" + logger.info( + f"PrecisionPathFollowerTask '{self._name}': received path " + f"from stream (n={len(path.poses)})" + ) + self.start_path(path, current_odom) + # ------------------------------------------------------------------ # Path lifecycle # ------------------------------------------------------------------ diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index 9d4ee92d70..fffb7718e9 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -86,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) @@ -99,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 @@ -172,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 @@ -259,13 +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", - "Enter: Advance | K: Skip | Backspace: Quit (tools)", - "0-9: e_max corridor (0.0-0.9 m, for RG)", - ] + 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)) From 61f7fc40f42f13a69cfbc10580f6c30629fe0b59 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 16:09:55 -0700 Subject: [PATCH 39/51] nav blueprint added --- dimos/robot/all_blueprints.py | 2 +- .../basic/unitree_go2_precision_nav.py | 132 ++++++++++++++++++ .../benchmarking/reports/tuning_README.md | 60 +++++++- 3 files changed, 192 insertions(+), 2 deletions(-) create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 02d4461589..ba66f5d3ad 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -108,6 +108,7 @@ "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers", "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", + "unitree-go2-precision-nav": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_precision_nav:unitree_go2_precision_nav", "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-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", @@ -203,7 +204,6 @@ "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", "receiver-module": "dimos.utils.demo_image_encoding.ReceiverModule", "recorder": "dimos.memory2.module.Recorder", - "reference-governor": "dimos.navigation.reference_governor.reference_governor.ReferenceGovernor", "reid-module": "dimos.perception.detection.reid.module.ReidModule", "relocalization-module": "dimos.mapping.relocalization.module.RelocalizationModule", "replanning-a-star-planner": "dimos.navigation.replanning_a_star.module.ReplanningAStarPlanner", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py new file mode 100644 index 0000000000..24706f471a --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -0,0 +1,132 @@ +#!/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 precision-nav blueprint — stock-hardware planning → +precision controller. + +End-to-end composition: the operator clicks a goal in the rerun viewer, +the planner builds a path, and the coord's ``precision_follower`` task +(``PrecisionPathFollowerTask``) follows it with a live corridor +half-width tunable via the existing ``KeyboardTeleop`` 0-9 keys. + +Stock Go2 hardware (L1 lidar, GO2Connection-published PoseStamped odom) +is sufficient — no Mid360 / FastLio2 required. The composition mirrors +the working smart-tier Go2 nav blueprint +([dimos.robot.unitree.go2.blueprints.smart.unitree_go2.unitree_go2](dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py)) +but swaps the final actuation seam: instead of ``MovementManager`` mixing +``nav_cmd_vel`` into ``cmd_vel`` and driving GO2Connection directly, +``ReplanningAStarPlanner.path`` flows into ``ControlCoordinator.path``, +the coord broadcasts ``set_path(path, odom)`` to ``precision_follower``, +and the precision controller drives the robot via the coord's tick loop. + +Composition: + +- ``unitree_go2_coordinator`` — GO2Connection + ControlCoordinator + (already declares ``path_follower`` and ``precision_follower`` tasks + and the ``path: In[Path]`` + ``e_max: In[Float32]`` stream ports). +- ``vis_module`` — rerun viewer + ``RerunWebSocketServer`` (provides + the click-to-goal source) + websocket vis. +- ``KeyboardTeleop`` with ``disable_movement=True`` — 0-9 keys publish + ``e_max`` only; WASD/QE Twist generation is disabled. +- ``VoxelGridMapper`` — raw ``lidar: In[PointCloud2]`` (from + GO2Connection) → ``global_map: Out[PointCloud2]``. +- ``CostMapper`` — voxel ``global_map`` → ``global_costmap: + Out[OccupancyGrid]``. +- ``ReplanningAStarPlanner`` — directly accepts GO2Connection's + ``odom: In[PoseStamped]`` (no SLAM step needed) and + ``clicked_point: In[PointStamped]`` from the rerun server. Emits + ``path: Out[Path]`` which the coord consumes. +- ``CharacterizationRecorder`` (kept name; ``tag="precision_nav"``) + for the per-session SQLite black box. Lands at + ``/data/precision_nav/go2/go2_precision_nav__.db``. + +Wiring (all by-port-name, no remappings): + + GO2Connection.lidar (PointCloud2) -> VoxelGridMapper.lidar + VoxelGridMapper.global_map (PointCloud2) -> CostMapper.global_map + CostMapper.global_costmap (OccupancyGrid)-> ReplanningAStarPlanner.global_costmap + GO2Connection.odom (PoseStamped) -> ReplanningAStarPlanner.odom + RerunWebSocketServer.clicked_point -> ReplanningAStarPlanner.clicked_point + ReplanningAStarPlanner.path (Path) -> ControlCoordinator.path + KeyboardTeleop.e_max (Float32) -> ControlCoordinator.e_max + +``ReplanningAStarPlanner.nav_cmd_vel`` is intentionally left unwired — +the precision controller, not the planner, drives the robot. The planner +is only used as a path source. + +Operator flow:: + + dimos run unitree-go2-precision-nav + +Open rerun → click a point on the map → robot drives the planned path. +Press 0-9 in the pygame window to dial the corridor half-width +(0.0-0.9 m) live; ``PrecisionPathFollowerTask`` re-solves the velocity +profile on each keypress and atomically swaps the per-waypoint cap. +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.core.transport import LCMTransport +from dimos.mapping.costmapper import CostMapper +from dimos.mapping.voxels import VoxelGridMapper +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.navigation.replanning_a_star.module import ReplanningAStarPlanner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( + unitree_go2_coordinator, +) +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder +from dimos.utils.path_utils import get_project_root +from dimos.visualization.vis_module import vis_module + +unitree_go2_precision_nav = autoconnect( + unitree_go2_coordinator, + vis_module(viewer_backend=global_config.viewer), + KeyboardTeleop.blueprint( + publish_only_when_active=True, + disable_movement=True, # 0-9 e_max slider only; no WASD Twist + ), + VoxelGridMapper.blueprint(emit_every=5), + CostMapper.blueprint(), + ReplanningAStarPlanner.blueprint(), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag="precision_nav", + out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), + ), +).transports( + { + # KeyboardTeleop 0-9 -> coord.e_max -> precision_follower.set_e_max. + ("e_max", Float32): LCMTransport("/e_max", Float32), + # ReplanningAStarPlanner.path -> coord.path -> precision_follower.set_path. + ("path", Path): LCMTransport("/precision_nav/path", Path), + # Recorder taps — same conventions as B1/B2. + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + # KeyboardTeleop's gate stream stays available for any tool that + # wants ENTER/K/Backspace; no consumer in B3 today. + ("gate", Int8): LCMTransport("/precision_nav/gate", Int8), + } +) + +__all__ = ["unitree_go2_precision_nav"] diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index f01f0b4060..0a1c73ff01 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -80,6 +80,64 @@ data/benchmark/go2/ └── go2_benchmark__.db ← raw streams ``` +## Step 3 — precision-controlled nav (end-to-end) + +``` +dimos run unitree-go2-precision-nav +``` + +Bundles the Go2 coord + rerun viewer + click-to-goal planning chain +(VoxelGridMapper + CostMapper + ReplanningAStarPlanner) + KeyboardTeleop +(0-9 e_max slider, WASD/QE disabled) + per-session recorder. Stock Go2 +hardware (L1 lidar, GO2Connection odom) is sufficient — **no Mid360 / +FastLio2 required**. One terminal. + +Composition mirrors the working smart-tier Go2 nav blueprint and swaps +only the actuation seam: instead of `MovementManager` mixing +`nav_cmd_vel` into `cmd_vel` and driving GO2Connection directly, +`ReplanningAStarPlanner.path` flows into `ControlCoordinator.path`, the +coord broadcasts `set_path(path, odom)` to the `precision_follower` +task, and the precision controller drives the robot via the coord's +tick loop. + +Operator flow: + +1. Open rerun (auto-launches with the blueprint). +2. Click a point on the map. `RerunWebSocketServer` emits a + `clicked_point: PointStamped`. +3. `ReplanningAStarPlanner` consumes the click + the live costmap (from + `VoxelGridMapper → CostMapper`) + GO2Connection's `odom: PoseStamped` + and emits `path: Path`. +4. `ControlCoordinator.path` receives the planned path; `_on_path` + snapshots the latest odom and broadcasts `set_path(path, odom)` to + any task with the method. +5. `PrecisionPathFollowerTask.set_path` delegates to its existing + `start_path` (which lazy-loads the artifact, solves + `solve_profile()`, and starts the state machine). +6. The coord's tick loop drives the precision controller's velocity + commands to GO2Connection. + +Live-tune precision: keys **0-9** in the pygame window set corridor +half-width 0.0-0.9 m. `PrecisionPathFollowerTask` re-solves the profile +on each keypress and atomically swaps the per-waypoint cap mid-path. + +**Output**: + +``` +data/precision_nav/go2/ +└── go2_precision_nav__.db ← cmd_vel, joint_state, odom, gate +``` + +**Hardware notes**: + +- L1 lidar has narrow FOV — the costmap is sparser than what Mid360 + would provide. Quality of click-to-plan paths is limited accordingly. + Mid360 + FastLio2 is the upgrade path if you want the proper + nav-stack pipeline (see `unitree-g1-nav-onboard` for that variant). +- `ReplanningAStarPlanner.nav_cmd_vel: Out[Twist]` is intentionally + unwired — the precision controller, not the planner, drives the + robot. The planner is only used as a path source. + ## Reading recordings ```python @@ -90,7 +148,7 @@ for obs in store.stream("joint_state", JointState): ts, msg = obs.ts, obs.data # re-fit, plot, etc. ``` -Streams: `cmd_vel` (Twist), `joint_state` (JointState, x/y/yaw), `odom` (PoseStamped, raw), `gate` (Int8, operator events). +Streams: `cmd_vel` (Twist), `joint_state` (JointState, x/y/yaw), `odom` (PoseStamped, raw), `gate` (Int8, operator events). The Step 3 (precision-nav) recording adds nothing new — same schema, different `tag`. ## Troubleshooting From 6eeb3d306388a27b09e80bb6a60a32b1f5daed65 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 16:16:13 -0700 Subject: [PATCH 40/51] drop odom snapshot method, task reads it own odom from tickloop --- dimos/control/coordinator.py | 52 +++---------------- .../precision_path_follower_task.py | 22 +++++--- dimos/control/tick_loop.py | 12 ----- 3 files changed, 20 insertions(+), 66 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index ab40a315bb..87ea674968 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -56,9 +56,7 @@ from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.hardware.whole_body.spec import WholeBodyAdapter 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 from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.std_msgs.Float32 import Float32 @@ -165,10 +163,10 @@ class ControlCoordinator(Module): # Input: Live RG corridor half-width (m) for tasks with set_e_max(). e_max: In[Float32] - # Input: Planned path for tasks with set_path(). Typical source is the - # nav stack (FarPlanner/LocalPlanner). The coord snapshots the latest - # odom from its tick state and passes it alongside the path so the - # task's anchoring isn't racing the first compute() tick. + # Input: Planned path for tasks with set_path(). Typical source is a + # nav-stack planner (e.g. ReplanningAStarPlanner). The coord just + # forwards the path; tasks read their own latest odom from internal + # state populated by compute() each tick. path: In[Path] # Arming and dry-run are one-shot RPCs, not streams. @@ -544,51 +542,13 @@ def _on_e_max(self, msg: Float32) -> None: if set_e_max is not None: set_e_max(value) - def _snapshot_odom(self) -> PoseStamped | None: - """Reconstruct a PoseStamped from the tick loop's latest cached - state. Looks at the first twist-base hardware's [x, y, yaw] - joints (same convention PathFollowerTask.compute uses). Returns - None if no tick has fired yet or no twist-base is registered.""" - if self._tick_loop is None: - return None - state = self._tick_loop.latest_state - if state is None: - return None - positions = state.joints.joint_positions - with self._hardware_lock: - for hw in self._hardware.values(): - if hw.component.hardware_type != HardwareType.BASE: - continue - names = hw.joint_names - if len(names) < 3: - continue - x = positions.get(names[0]) - y = positions.get(names[1]) - yaw = positions.get(names[2]) - if x is None or y is None or yaw is None: - return None - return PoseStamped( - ts=state.t_now, - position=Vector3(float(x), float(y), 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), - ) - return None - def _on_path(self, msg: Path) -> None: - """Forward a planned path to any task with set_path(path, odom). - Snapshots latest odom from the tick loop's cached state so the - task's anchoring doesn't race the first compute() tick.""" - odom = self._snapshot_odom() - if odom is None: - logger.warning( - "ControlCoordinator received path but no odom snapshot available yet; dropping." - ) - return + """Forward a planned path to any task with set_path(path).""" with self._task_lock: for task in self._tasks.values(): set_path = getattr(task, "set_path", None) if set_path is not None: - set_path(msg, odom) + set_path(msg) @rpc def set_activated(self, engaged: bool) -> None: diff --git a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py index 67ae189575..fe790a66fe 100644 --- a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py +++ b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py @@ -94,18 +94,24 @@ def set_e_max(self, value: float) -> None: if self._path is not None: self._recompute_profile() - def set_path(self, path: Path, current_odom: PoseStamped) -> None: - """Coordinator broadcast hook for nav-stack-emitted paths. The - coord snapshots the latest odom from its tick state and passes - it here so anchoring doesn't race the first compute() tick. - Delegates to the regular start_path lifecycle (which the override - on this class extends with the lazy artifact load + solve_profile - recompute).""" + def set_path(self, path: Path) -> None: + """Coordinator broadcast hook for nav-stack-emitted paths. + Pulls the latest odom from ``self._current_odom`` (populated by + the parent task's compute() every tick from CoordinatorState). + If no compute tick has fired yet, ``_current_odom`` is None and + we drop the path with a warning — the race window is one tick + wide at startup and shouldn't fire in practice.""" + if self._current_odom is None: + logger.warning( + f"PrecisionPathFollowerTask '{self._name}': received path " + f"but no odom yet; dropping (race at startup)." + ) + return logger.info( f"PrecisionPathFollowerTask '{self._name}': received path " f"from stream (n={len(path.poses)})" ) - self.start_path(path, current_odom) + self.start_path(path, self._current_odom) # ------------------------------------------------------------------ # Path lifecycle diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 4e985c0934..d38b7e76d2 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -113,10 +113,6 @@ def __init__( self._tick_thread: threading.Thread | None = None self._last_tick_time: float = 0.0 self._tick_count: int = 0 - # Cached most-recently-built state. Off-thread consumers (the - # coord's LCM stream handlers, e.g. _on_path) read this to - # snapshot odom without racing on read_state(). - self._latest_state: CoordinatorState | None = None @property def tick_count(self) -> int: @@ -128,12 +124,6 @@ def is_running(self) -> bool: """Whether the tick loop is currently running.""" return not self._stop_event.is_set() - @property - def latest_state(self) -> CoordinatorState | None: - """Most-recently-built CoordinatorState, or None before the first - tick.""" - return self._latest_state - def start(self) -> None: """Start the tick loop in a daemon thread.""" if not self._stop_event.is_set(): @@ -188,8 +178,6 @@ def _tick(self) -> None: imu_states = self._read_all_imu() state = CoordinatorState(joints=joint_states, imu=imu_states, t_now=t_now, dt=dt) - self._latest_state = state - commands = self._compute_all_tasks(state) joint_commands, preemptions = self._arbitrate(commands) From 011e9e498620ad76233ede6afceaf56192b7c918 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 16:58:45 -0700 Subject: [PATCH 41/51] updated flobal config to increase number of worker for blueprint --- .../basic/unitree_go2_precision_nav.py | 62 ++++++++++--------- 1 file changed, 33 insertions(+), 29 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py index 24706f471a..ad448d7d7b 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -98,35 +98,39 @@ from dimos.utils.path_utils import get_project_root from dimos.visualization.vis_module import vis_module -unitree_go2_precision_nav = autoconnect( - unitree_go2_coordinator, - vis_module(viewer_backend=global_config.viewer), - KeyboardTeleop.blueprint( - publish_only_when_active=True, - disable_movement=True, # 0-9 e_max slider only; no WASD Twist - ), - VoxelGridMapper.blueprint(emit_every=5), - CostMapper.blueprint(), - ReplanningAStarPlanner.blueprint(), - CharacterizationRecorder.blueprint( - robot_id="go2", - tag="precision_nav", - out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), - ), -).transports( - { - # KeyboardTeleop 0-9 -> coord.e_max -> precision_follower.set_e_max. - ("e_max", Float32): LCMTransport("/e_max", Float32), - # ReplanningAStarPlanner.path -> coord.path -> precision_follower.set_path. - ("path", Path): LCMTransport("/precision_nav/path", Path), - # Recorder taps — same conventions as B1/B2. - ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - # KeyboardTeleop's gate stream stays available for any tool that - # wants ENTER/K/Backspace; no consumer in B3 today. - ("gate", Int8): LCMTransport("/precision_nav/gate", Int8), - } +unitree_go2_precision_nav = ( + autoconnect( + unitree_go2_coordinator, + vis_module(viewer_backend=global_config.viewer), + KeyboardTeleop.blueprint( + publish_only_when_active=True, + disable_movement=True, # 0-9 e_max slider only; no WASD Twist + ), + VoxelGridMapper.blueprint(emit_every=5), + CostMapper.blueprint(), + ReplanningAStarPlanner.blueprint(), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag="precision_nav", + out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), + ), + ) + .transports( + { + # KeyboardTeleop 0-9 -> coord.e_max -> precision_follower.set_e_max. + ("e_max", Float32): LCMTransport("/e_max", Float32), + # ReplanningAStarPlanner.path -> coord.path -> precision_follower.set_path. + ("path", Path): LCMTransport("/precision_nav/path", Path), + # Recorder taps — same conventions as B1/B2. + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + # KeyboardTeleop's gate stream stays available for any tool that + # wants ENTER/K/Backspace; no consumer in B3 today. + ("gate", Int8): LCMTransport("/precision_nav/gate", Int8), + } + ) + .global_config(n_workers=10, robot_model="unitree_go2") ) __all__ = ["unitree_go2_precision_nav"] From 6d01680b127c0765faab94274f6f0d7dc83850d2 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 22:24:55 -0700 Subject: [PATCH 42/51] final modification for rage mode testing --- .../precision_path_follower_task.py | 14 +- dimos/utils/benchmarking/characterization.py | 1154 +++++++++++------ .../benchmarking/characterization_recorder.py | 19 +- dimos/utils/benchmarking/plant.py | 101 +- .../benchmarking/reports/tuning_README.md | 97 +- dimos/utils/benchmarking/tuning.py | 380 +++++- 6 files changed, 1370 insertions(+), 395 deletions(-) diff --git a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py index fe790a66fe..a49476556e 100644 --- a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py +++ b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py @@ -70,10 +70,15 @@ def __init__( global_config: GlobalConfig, artifact_path: str, e_max_default: float = 0.05, + v_max_override: float | None = None, ) -> None: super().__init__(name, config, global_config=global_config) self._artifact_path = artifact_path self._e_max: float = float(e_max_default) + + self._v_max_override: float | None = ( + float(v_max_override) if v_max_override is not None else None + ) # Plant + vp_spec lazy-load on first start_path(). self._plant: Any = None self._vp_spec: Any = None @@ -149,17 +154,20 @@ def _load_artifact(self) -> None: self._plant = art.plant self._vp_spec = art.velocity_profile vp = self._vp_spec + v_max = self._v_max_override if self._v_max_override is not None else vp.max_linear_speed # PrecisionMVC reads e_max via a closure so live updates don't # require rebuilding the constraint list. self._constraints = [ - GeometricMVC(v_max=vp.max_linear_speed), + GeometricMVC(v_max=v_max), SaturationMVC(omega_max=vp.max_angular_speed), LateralMVC(a_lat_max=vp.max_centripetal_accel), PrecisionMVC(e_max_provider=lambda: self._e_max), ] + override_tag = " (v_max OVERRIDE)" if self._v_max_override is not None else "" logger.info( f"PrecisionPathFollowerTask '{self._name}': loaded artifact " - f"{self._artifact_path} (plant + vp_spec ready, e_max={self._e_max:.3f})" + f"{self._artifact_path} (plant + vp_spec ready, " + f"v_max={v_max:.3f}{override_tag}, e_max={self._e_max:.3f})" ) def _recompute_profile(self) -> None: @@ -205,6 +213,7 @@ class PrecisionPathFollowerTaskParams(BaseConfig): orientation_tolerance: float = 0.1 k_angular: float = 0.5 e_max_default: float = 0.2 + v_max_override: float | None = None def create_task(cfg: Any, hardware: Any) -> PrecisionPathFollowerTask: @@ -223,6 +232,7 @@ def create_task(cfg: Any, hardware: Any) -> PrecisionPathFollowerTask: global_config=_gc, artifact_path=params.artifact_path, e_max_default=params.e_max_default, + v_max_override=params.v_max_override, ) diff --git a/dimos/utils/benchmarking/characterization.py b/dimos/utils/benchmarking/characterization.py index 152220918d..32daa7fd17 100644 --- a/dimos/utils/benchmarking/characterization.py +++ b/dimos/utils/benchmarking/characterization.py @@ -12,39 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Tool 1 of the twist-base tuning deliverable: characterization. - -**This is a hardware tool.** It measures a real velocity-commanded -base's per-axis response (vx, vy, wz), fits FOPDT per channel, runs the -DERIVE step, and emits the versioned config artifact. Robot-agnostic: -everything robot-specific comes from the selected ``RobotPlantProfile`` -(``--robot``, default ``go2``). - - # terminal 1 (the robot's bring-up blueprint, see the profile): - dimos run - # terminal 2 (strip /nix/store from LD_LIBRARY_PATH — see README): - uv run python -m dimos.utils.benchmarking.characterization \\ - --robot go2 --mode hw --surface concrete - -`--mode hw` (default) drives the real robot via the same path the -benchmark does: an in-process ``ControlCoordinator`` with the -``transport_lcm`` twist-base adapter spins up to give us a ``joint_state`` -Out stream sourced from the adapter's odometry. Signal-injection itself -stays a standalone Twist publisher (SI is open-loop by nature). Each -step is **operator-gated**: before every step the robot is stopped and -we wait for ENTER. Safe (velocity clamp, zero-Twist on exit/SIGINT, -stale-odom abort, distance + time caps). - -`--mode self-test` is a **plumbing check, NOT a tuning artifact**: it -steps the profile's in-process FOPDT sim plant and recovers it. It only -proves the measure->fit->derive code runs; the artifact is stamped -`valid_for_tuning=false`. Used by pytest/CI without a robot. -""" - from __future__ import annotations import argparse from collections.abc import Callable +from dataclasses import asdict from datetime import date import math from pathlib import Path @@ -74,32 +46,39 @@ from dimos.robot.unitree.keyboard_teleop import GATE_ADVANCE, GATE_QUIT, GATE_SKIP from dimos.utils.path_utils import get_project_root -# Well-known LCM topics — every ControlCoordinator blueprint honors this -# contract (twist_command In = /cmd_vel, joint_state Out = -# /coordinator/joint_state). Adding a robot to the tools means adding a -# RobotPlantProfile, not a new topic / module / adapter. _CMD_VEL_TOPIC = "/cmd_vel" _JOINT_STATE_TOPIC = "/coordinator/joint_state" from dimos.utils.benchmarking.plant import ( ROBOT_PLANT_PROFILES, + ChannelEnvelope, FopdtChannelParams, RobotPlantProfile, TwistBasePlantParams, TwistBasePlantSim, + VelocityEnvelope, +) +from dimos.utils.benchmarking.tuning import ( + AmplitudeFitDC, + ChannelEnvelopeDC, + DynamicsByAmplitude, + FloorProbeResultDC, + FloorProbeResults, + Provenance, + TuningConfig, + VelocityEnvelopeDC, + _floor_from_probe, + _output_ceiling, + _saturating_at_amp, + derive_config, + git_sha, + re_derive_config, ) -from dimos.utils.benchmarking.tuning import Provenance, derive_config, git_sha from dimos.utils.characterization.modeling.fopdt import fit_fopdt, fopdt_step_response -# Fixed twist-base velocity-tuple order (estimator output / channel -# index). NOT robot-specific — the per-robot excited subset is -# profile.excited_channels. _CHANNELS = ("vx", "vy", "wz") _SIM_DT = 0.02 # in-process self-test integration step (not robot-specific) REPORTS_DIR = Path(__file__).parent / "reports" -# New default landing dir for tuning artifacts: /data/characterization//. -# REPORTS_DIR is retained as the package-local location for the README / docs, -# not as the artifact output default. DEFAULT_OUT_DIR = get_project_root() / "data" / "characterization" @@ -110,23 +89,23 @@ def reconstruct_body_velocities( yaw: np.ndarray, window: int = 5, order: int = 2, -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Recover body-frame (vx, vy, dyaw) from a buffered pose trace. - - The pipeline is intentionally smooth-position-then-differentiate - (not differentiate-then-smooth): odom measures position directly, - and numerical differentiation amplifies high-frequency noise that no - amount of post-hoc smoothing fully recovers from. On a legged base - the dominant "noise" is body bob at gait frequency — buffering raw - pose, applying Savitzky-Golay to the position trace, then - central-differencing the smoothed series gives a much cleaner - velocity signal for the FOPDT fit. Matches the historical - ``analyze.py`` pipeline. - - ``window`` must be odd and > ``order``; falls back to no smoothing - if the buffer is too short.""" +) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: from scipy.signal import savgol_filter + ts = np.asarray(ts, dtype=float) + if len(ts) >= 2: + keep = np.ones(len(ts), dtype=bool) + last = ts[0] + for i in range(1, len(ts)): + if ts[i] - last < 0.005: + keep[i] = False + else: + last = ts[i] + ts = ts[keep] + x = np.asarray(x, dtype=float)[keep] + y = np.asarray(y, dtype=float)[keep] + yaw = np.asarray(yaw, dtype=float)[keep] + yaw_u = np.unwrap(yaw) if len(ts) >= window and window % 2 == 1 and order < window: xf = savgol_filter(x, window, order) @@ -140,18 +119,10 @@ def reconstruct_body_velocities( c, s = np.cos(yawf), np.sin(yawf) vx = c * dx + s * dy vy = -s * dx + c * dy - return vx, vy, dyaw + return ts, vx, vy, dyaw def _hampel(ys: np.ndarray, window: int = 11, n_sigma: float = 3.0) -> tuple[np.ndarray, int]: - """Hampel outlier filter — replace points >n_sigma·MAD from the local - median with the local median. Window-centered; preserves trace length. - - Returns (filtered_array, n_replaced). MAD is scaled by 1.4826 so - n_sigma is in Gaussian-sigma units. Spikes from leg impacts / odom - estimator glitches get surgically removed; the rest of the trace is - untouched (the goal is to keep FOPDT step shape readable, not - smooth-out the dynamics).""" if window <= 0 or len(ys) < window: return ys.copy(), 0 half = window // 2 @@ -172,10 +143,95 @@ def _hampel(ys: np.ndarray, window: int = 11, n_sigma: float = 3.0) -> tuple[np. return out, replaced +def _physical_clip(ys: np.ndarray, max_abs: float) -> tuple[np.ndarray, int]: + out = ys.copy() + bad = np.abs(out) > max_abs + n = int(bad.sum()) + if n == 0: + return out, 0 + good_idx = np.where(~bad)[0] + if len(good_idx) == 0: + out[:] = 0.0 + return out, n + for i in np.where(bad)[0]: + lo = max(0, i - 10) + hi = min(len(out), i + 11) + local_good = out[lo:hi][~bad[lo:hi]] + if len(local_good) > 0: + out[i] = float(np.median(local_good)) + else: + out[i] = float(np.median(out[good_idx])) + return out, n + + def _clamp(v: float, lo: float, hi: float) -> float: return max(lo, min(hi, v)) +def _d3_sustained_pass( + ys: np.ndarray, + amp: float, + motion_threshold: float, + fractional_threshold: float, + sustained: int, +) -> tuple[bool, int]: + if len(ys) == 0 or amp == 0.0: + return False, 0 + abs_ys = np.abs(ys) + pass_mask = (abs_ys > motion_threshold) & (abs_ys > fractional_threshold * abs(amp)) + longest = 0 + cur = 0 + passed = False + for v in pass_mask: + if v: + cur += 1 + if cur > longest: + longest = cur + if cur >= sustained: + passed = True + else: + cur = 0 + return passed, longest + + +def _pick_linear_regime_fit(fits: list[dict], r2_floor: float = 0.9) -> dict | None: + if not fits: + return None + candidates = [f for f in fits if f.get("r2", 0.0) >= r2_floor] + if candidates: + return min(candidates, key=lambda f: abs(f["amp"])) + return max(fits, key=lambda f: f.get("r2", 0.0)) + + +def _channel_cap(profile: RobotPlantProfile, channel: str) -> float: + return profile.wz_max if channel == "wz" else profile.vx_max + + +def _envelope_from_channel_results( + floor_probe: list[dict], + sweep_fits: list[dict], + probe_fits: list[dict], + profile: RobotPlantProfile, + channel: str, +) -> ChannelEnvelope: + cap = _channel_cap(profile, channel) + floor, floor_nf = _floor_from_probe( + floor_probe, profile.floor_probe_amplitudes.get(channel, []) + ) + all_fits = sweep_fits + probe_fits + ceiling, ceiling_nf = _output_ceiling(all_fits, cap) + linear = _pick_linear_regime_fit(sweep_fits or all_fits) + K_linear = linear["K"] if linear is not None else 0.0 + sat = _saturating_at_amp(all_fits, K_linear, profile.ceiling_k_sag_threshold) + return ChannelEnvelope( + floor=float(floor), + ceiling=float(ceiling), + floor_not_found=floor_nf, + ceiling_not_found=ceiling_nf, + saturating_at_amp=sat, + ) + + def _resolve_profile(name: str) -> RobotPlantProfile: try: return ROBOT_PLANT_PROFILES[name] @@ -185,139 +241,353 @@ def _resolve_profile(name: str) -> RobotPlantProfile: ) from None -# --- self-test (in-process FOPDT plant; NOT robot-valid) ----------------- - - -def _fit_selftest(profile: RobotPlantProfile) -> tuple[TwistBasePlantParams, dict, list[dict]]: - """Step the profile's FOPDT sim plant and recover it. Plumbing check - only — proves the measure->fit->derive code path runs.""" +def _selftest_step( + plant: TwistBasePlantSim, channel: str, amp: float, pre_roll_s: float, step_s: float +) -> tuple[np.ndarray, np.ndarray]: + plant.reset(0.0, 0.0, 0.0, _SIM_DT) + n_pre = int(pre_roll_s / _SIM_DT) + n_step = int(step_s / _SIM_DT) + cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + for _ in range(n_pre): + plant.step(cmd["vx"], cmd["vy"], cmd["wz"], _SIM_DT) + cmd[channel] = amp + ys = [] + for _ in range(n_step): + plant.step(cmd["vx"], cmd["vy"], cmd["wz"], _SIM_DT) + ys.append(getattr(plant, channel)) + t = np.arange(len(ys), dtype=float) * _SIM_DT + return t, np.asarray(ys, dtype=float) + + +def _fit_selftest( + profile: RobotPlantProfile, +) -> tuple[ + TwistBasePlantParams, + dict[str, list[dict]], + list[dict], + VelocityEnvelope, + dict[str, list[dict]], +]: truth = profile.sim_plant plant = TwistBasePlantSim(truth) - n_pre = int(profile.pre_roll_s / _SIM_DT) - n_step = int(profile.step_s / _SIM_DT) - pooled: dict[str, FopdtChannelParams] = {} + canonical: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} + floor_results: dict[str, list[dict]] = {} + env_channels: dict[str, ChannelEnvelope] = {} traces: list[dict] = [] + def fit_and_record(channel: str, amp: float, source: str) -> None: + t, ys = _selftest_step(plant, channel, amp, profile.pre_roll_s, profile.step_s) + fp = fit_fopdt(t, ys, u_step=amp) + if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): + print(f" [warn] {channel}@{amp} ({source}): fit failed ({fp.reason})") + return + row = { + "amp": amp, + "amplitude": amp, + "direction": "forward", + "K": fp.K, + "tau": fp.tau, + "L": fp.L, + "r2": fp.r_squared, + "source": source, + } + per_amplitude[channel].append(row) + traces.append({"channel": channel, "t": t, "y": ys, **row}) + for channel in _CHANNELS: - fits = [] - per_amplitude[channel] = [] - for amp in profile.si_amplitudes[channel]: - plant.reset(0.0, 0.0, 0.0, _SIM_DT) - cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} - for _ in range(n_pre): - plant.step(cmd["vx"], cmd["vy"], cmd["wz"], _SIM_DT) - cmd[channel] = amp - ys = [] - for _ in range(n_step): - plant.step(cmd["vx"], cmd["vy"], cmd["wz"], _SIM_DT) - ys.append(getattr(plant, channel)) - t = np.arange(len(ys), dtype=float) * _SIM_DT - fp = fit_fopdt(t, np.asarray(ys, dtype=float), u_step=amp) - if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): - print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") - continue - fits.append(fp) - per_amplitude[channel].append( - {"amplitude": amp, "direction": "forward", "K": fp.K, "tau": fp.tau, "L": fp.L} + floor_results[channel] = [] + for amp in profile.floor_probe_amplitudes.get(channel, []): + _, ys = _selftest_step(plant, channel, amp, profile.pre_roll_s, profile.step_s) + passed, longest = _d3_sustained_pass( + ys, + amp, + profile.floor_motion_threshold, + profile.floor_fractional_threshold, + profile.floor_sustained_samples, ) - traces.append( - { - "channel": channel, - "amp": amp, - "t": np.asarray(t, dtype=float), - "y": np.asarray(ys, dtype=float), - "K": fp.K, - "tau": fp.tau, - "L": fp.L, - "r2": fp.r_squared, - } + floor_results[channel].append( + {"amp": amp, "motion_detected": passed, "sustained_samples": longest} ) - if not fits: + + per_amplitude[channel] = [] + for amp in profile.si_amplitudes[channel]: + fit_and_record(channel, amp, "sweep") + for amp in profile.ceiling_probe_amplitudes.get(channel, []): + fit_and_record(channel, amp, "ceiling_probe") + + if not per_amplitude[channel]: raise RuntimeError(f"self-test: no converged fits for {channel!r}") - pooled[channel] = FopdtChannelParams( - K=float(np.mean([f.K for f in fits])), - tau=float(np.mean([f.tau for f in fits])), - L=float(np.mean([f.L for f in fits])), + sweep_fits = [f for f in per_amplitude[channel] if f["source"] == "sweep"] + probe_fits = [f for f in per_amplitude[channel] if f["source"] == "ceiling_probe"] + env_channels[channel] = _envelope_from_channel_results( + floor_results[channel], sweep_fits, probe_fits, profile, channel ) - fitted = TwistBasePlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]) - print("\nself-test (recovered vs injected FOPDT ground truth):") + linear = _pick_linear_regime_fit(sweep_fits) or sweep_fits[0] + canonical[channel] = FopdtChannelParams( + K=float(linear["K"]), tau=float(linear["tau"]), L=float(linear["L"]) + ) + + fitted = TwistBasePlantParams(vx=canonical["vx"], vy=canonical["vy"], wz=canonical["wz"]) + envelope = VelocityEnvelope(vx=env_channels["vx"], vy=env_channels["vy"], wz=env_channels["wz"]) + print("\nself-test (canonical recovered vs injected FOPDT ground truth):") print(f" {'chan':4} {'K fit/true':>20} {'tau fit/true':>20} {'L fit/true':>20}") for ch in _CHANNELS: f, g = getattr(fitted, ch), getattr(truth, ch) print( f" {ch:4} {f.K:8.3f}/{g.K:<8.3f} {f.tau:8.3f}/{g.tau:<8.3f} {f.L:8.3f}/{g.L:<8.3f}" ) - return fitted, per_amplitude, traces - - -# --- fit-quality graph (the human-facing deliverable) ------------------- + print("self-test envelope (linear plant => ceiling clamps to platform cap):") + for ch in _CHANNELS: + e = getattr(envelope, ch) + print( + f" {ch:4} floor={e.floor:.3f} (nf={e.floor_not_found}) " + f"ceiling={e.ceiling:.3f} (nf={e.ceiling_not_found})" + ) + return fitted, per_amplitude, traces, envelope, floor_results def _plot_fits( - traces: list[dict], provenance: Provenance, profile: RobotPlantProfile, out: Path + traces: list[dict], + provenance: Provenance, + profile: RobotPlantProfile, + out: Path, + envelope: VelocityEnvelope | VelocityEnvelopeDC | None = None, + dynamics: DynamicsByAmplitude | None = None, + *, + kind: Literal["envelope", "steps", "both"] = "envelope", ) -> None: - """One column per channel; each step's measured velocity overlaid - with its fitted FOPDT step response. This is the artifact a human - reads to judge whether the model matches the real robot.""" - if not traces: + if kind == "steps": + if traces: + _plot_step_responses(traces, provenance, profile, out) return - channels = list(dict.fromkeys(t["channel"] for t in traces)) - fig, axes = plt.subplots(1, len(channels), figsize=(6.0 * len(channels), 4.6), squeeze=False) - for ax, ch in zip(axes[0], channels, strict=True): - for tr in [t for t in traces if t["channel"] == ch]: - t_arr = tr["t"] - (line,) = ax.plot(t_arr, tr["y"], lw=1.4, alpha=0.85, label=f"meas @{tr['amp']:g}") - y_raw = tr.get("y_raw") - if y_raw is not None and tr.get("n_replaced", 0) > 0: - ax.plot(t_arr, y_raw, ":", lw=0.9, color=line.get_color(), alpha=0.5) - yhat = fopdt_step_response(t_arr, tr["K"], tr["tau"], tr["L"], tr["amp"]) - ax.plot(t_arr, yhat, "--", lw=1.4, color=line.get_color(), alpha=0.9) - row = list(t2["amp"] for t2 in traces if t2["channel"] == ch).index(tr["amp"]) - ann = ( - f"@{tr['amp']:g}: K={tr['K']:.3f} τ={tr['tau']:.3f} " - f"L={tr['L']:.3f} r²={tr['r2']:.2f}" - ) - if tr.get("n_replaced", 0) > 0: - ann += f" (hampel: {tr['n_replaced']})" - ax.annotate( - ann, - xy=(0.02, 0.97 - 0.06 * row), - xycoords="axes fraction", - ha="left", - va="top", - fontsize=7, - color=line.get_color(), + fits_by_ch: dict[str, list[dict]] = {} + if dynamics is not None: + for ch in ("vx", "vy", "wz"): + rows = getattr(dynamics, ch, []) or [] + fits_by_ch[ch] = [ + { + "amp": r.amp, + "K": r.K, + "tau": r.tau, + "L": r.L, + "r2": getattr(r, "r2", 0.0), + "source": getattr(r, "source", "sweep"), + } + for r in rows + ] + else: + for tr in traces: + fits_by_ch.setdefault(tr["channel"], []).append( + { + "amp": tr["amp"], + "K": tr["K"], + "tau": tr["tau"], + "L": tr["L"], + "r2": tr.get("r2", 0.0), + "source": tr.get("source", "sweep"), + } ) + + channels = list(dict.fromkeys(c for c in ("vx", "vy", "wz") if fits_by_ch.get(c))) + if not channels: + return + + n_rows = 3 + (1 if kind == "both" and traces else 0) + n_cols = len(channels) + fig, axes = plt.subplots(n_rows, n_cols, figsize=(5.5 * n_cols, 3.5 * n_rows), squeeze=False) + row = 0 + + if kind == "both" and traces: + for ax, ch in zip(axes[row], channels, strict=True): + _plot_step_subplot(ax, ch, [t for t in traces if t["channel"] == ch]) + row += 1 + + for ax, ch in zip(axes[row], channels, strict=True): + _plot_amp_metric( + ax, + ch, + fits_by_ch[ch], + envelope, + ylabel="K = output / commanded", + value_fn=lambda r: r["K"], + title_suffix="K(amp)", + ) + row += 1 + + for ax, ch in zip(axes[row], channels, strict=True): unit = "rad/s" if ch == "wz" else "m/s" - ax.set_title(f"{ch} (solid = filtered, dotted = raw, dashed = FOPDT fit)") - ax.set_xlabel("time since step edge (s)") - ax.set_ylabel(f"{ch} ({unit})") - ax.grid(True, alpha=0.3) - ax.legend(loc="lower right", fontsize=7) + _plot_amp_metric( + ax, + ch, + fits_by_ch[ch], + envelope, + ylabel=f"output = K·amp ({unit})", + value_fn=lambda r: r["K"] * r["amp"], + title_suffix="output (steady-state)", + ) + row += 1 + + for ax, ch in zip(axes[row], channels, strict=True): + _plot_amp_metric( + ax, + ch, + fits_by_ch[ch], + envelope, + ylabel="τ (s)", + value_fn=lambda r: r["tau"], + title_suffix="τ(amp)", + mark_envelope=False, + ) + p = provenance fig.suptitle( f"{profile.name} FOPDT characterization — {p.robot_id} / {p.surface} / " - f"{p.mode} / {p.sim_or_hw} — {p.date} ({p.git_sha})" + f"{p.mode} / {p.sim_or_hw} — {p.date} ({p.git_sha}) — " + f"methodology v{getattr(p, 'methodology_version', 1)}" ) fig.tight_layout() fig.savefig(out, dpi=120) plt.close(fig) -# --- hardware SI (real robot over LCM, operator-gated, safe) ------------- +def _plot_step_subplot(ax, channel: str, ch_traces: list[dict]) -> None: + for tr in ch_traces: + t_arr = tr["t"] + (line,) = ax.plot(t_arr, tr["y"], lw=1.4, alpha=0.85, label=f"meas @{tr['amp']:g}") + y_raw = tr.get("y_raw") + if y_raw is not None and tr.get("n_replaced", 0) > 0: + ax.plot(t_arr, y_raw, ":", lw=0.9, color=line.get_color(), alpha=0.5) + yhat = fopdt_step_response(t_arr, tr["K"], tr["tau"], tr["L"], tr["amp"]) + ax.plot(t_arr, yhat, "--", lw=1.4, color=line.get_color(), alpha=0.9) + unit = "rad/s" if channel == "wz" else "m/s" + ax.set_title(f"{channel} step response (solid=meas, dashed=fit)") + ax.set_xlabel("time since step edge (s)") + ax.set_ylabel(f"{channel} ({unit})") + ax.grid(True, alpha=0.3) + ax.legend(loc="lower right", fontsize=7) + + +def _plot_step_responses( + traces: list[dict], + provenance: Provenance, + profile: RobotPlantProfile, + out: Path, +) -> None: + channels = list(dict.fromkeys(t["channel"] for t in traces)) + if not channels: + return + amps = sorted({float(t["amp"]) for t in traces}) + by_key = {(t["channel"], float(t["amp"])): t for t in traces} + + fig, axes = plt.subplots( + len(amps), + len(channels), + figsize=(4.8 * len(channels), 2.6 * len(amps)), + squeeze=False, + ) + for r, amp in enumerate(amps): + for c, ch in enumerate(channels): + ax = axes[r][c] + tr = by_key.get((ch, amp)) + if tr is None: + ax.set_axis_off() + ax.set_title(f"{ch} @ {amp:g} (no data)", fontsize=9) + continue + _plot_single_step(ax, ch, tr) + p = provenance + fig.suptitle( + f"{profile.name} step responses (per amp) — {p.robot_id} / {p.surface} / " + f"{p.mode} / {p.sim_or_hw} — {p.date} ({p.git_sha})", + y=1.00, + ) + fig.tight_layout() + fig.savefig(out, dpi=120) + plt.close(fig) -class _JointStatePoseStream: - """Pose stream sourced from a coordinator's ``joint_state`` Out. - Positions are [x, y, yaw] (twist-base adapter convention). Buffers - raw pose samples between :meth:`start_buffering` and - :meth:`stop_and_pop` so callers can reconstruct body-frame - velocity at step end (via :func:`reconstruct_body_velocities`) - rather than per-sample. Smooth-then-differentiate is much cleaner - than differentiate-then-smooth on legged-robot data — see that - function's docstring.""" +def _plot_single_step(ax, channel: str, tr: dict) -> None: + t_arr = tr["t"] + ax.plot(t_arr, tr["y"], "-", color="tab:blue", lw=1.4, label="measured") + y_raw = tr.get("y_raw") + if y_raw is not None and tr.get("n_replaced", 0) > 0: + ax.plot(t_arr, y_raw, ":", color="tab:gray", lw=0.9, alpha=0.7, label="raw") + yhat = fopdt_step_response(t_arr, tr["K"], tr["tau"], tr["L"], tr["amp"]) + ax.plot(t_arr, yhat, "--", color="tab:red", lw=1.3, alpha=0.9, label="FOPDT fit") + + src = tr.get("source", "sweep") + src_tag = "ceiling" if src == "ceiling_probe" else "sweep" + ax.set_title(f"{channel} @ {tr['amp']:g} ({src_tag})", fontsize=9) + unit = "rad/s" if channel == "wz" else "m/s" + ax.set_xlabel("t (s)", fontsize=8) + ax.set_ylabel(f"{channel} ({unit})", fontsize=8) + ax.tick_params(labelsize=7) + ax.grid(True, alpha=0.3) + ax.annotate( + f"K={tr['K']:.3f} τ={tr['tau']:.3f} L={tr['L']:.3f} r²={tr.get('r2', 0.0):.2f}" + + (f" hampel:{tr['n_replaced']}" if tr.get("n_replaced", 0) > 0 else ""), + xy=(0.02, 0.97), + xycoords="axes fraction", + ha="left", + va="top", + fontsize=7, + ) + ax.legend(loc="lower right", fontsize=6) + + +def _plot_amp_metric( + ax, + channel: str, + rows: list[dict], + envelope: VelocityEnvelope | VelocityEnvelopeDC | None, + *, + ylabel: str, + value_fn, + title_suffix: str, + mark_envelope: bool = True, +) -> None: + if not rows: + ax.set_title(f"{channel}: {title_suffix} (no data)") + return + rows = sorted(rows, key=lambda r: r["amp"]) + sweep = [r for r in rows if r.get("source", "sweep") == "sweep"] + probe = [r for r in rows if r.get("source") == "ceiling_probe"] + if sweep: + xs = [r["amp"] for r in sweep] + ys = [value_fn(r) for r in sweep] + ax.plot(xs, ys, "o-", color="tab:blue", lw=1.4, label="sweep") + if probe: + xs = [r["amp"] for r in probe] + ys = [value_fn(r) for r in probe] + ax.plot(xs, ys, "s--", color="tab:orange", lw=1.4, label="ceiling probe") + + unit = "rad/s" if channel == "wz" else "m/s" + if mark_envelope and envelope is not None: + ce = getattr(envelope, channel, None) + if ce is not None: + ax.axvline( + ce.floor, + color="green", + lw=1.0, + ls=":", + label=f"floor = {ce.floor:.3g} {unit}" + (" *" if ce.floor_not_found else ""), + ) + ax.axvline( + ce.ceiling, + color="red", + lw=1.0, + ls=":", + label=f"ceiling = {ce.ceiling:.3g} {unit}" + (" *" if ce.ceiling_not_found else ""), + ) + + ax.set_xlabel(f"commanded amplitude ({unit})") + ax.set_ylabel(ylabel) + ax.set_title(f"{channel}: {title_suffix}") + ax.grid(True, alpha=0.3) + ax.legend(loc="best", fontsize=7) + +class _JointStatePoseStream: def __init__(self, joint_names: list[str]) -> None: self._jx, self._jy, self._jyaw = joint_names self._lock = threading.Lock() @@ -336,10 +606,6 @@ def on_joint_state(self, msg: JointState) -> None: yaw = float(msg.position[idx[self._jyaw]]) except (KeyError, IndexError): return - # The caller waits a grace period after coord.start before - # sampling, so the (0,0,0) placeholder from ConnectedTwistBase - # (emitted before the adapter receives its first /odom) does - # not get latched as the start pose. now = time.perf_counter() pose = PoseStamped( ts=now, @@ -349,14 +615,14 @@ def on_joint_state(self, msg: JointState) -> None: with self._lock: self._pose, self._pose_t = pose, now if self._buffering: - self._buffer.append((now, x, y, yaw)) + if not self._buffer or now - self._buffer[-1][0] >= 0.005: + self._buffer.append((now, x, y, yaw)) def latest(self) -> tuple[PoseStamped | None, float]: with self._lock: return self._pose, self._pose_t def start_buffering(self) -> None: - """Begin recording raw pose samples — called at step start.""" with self._lock: self._buffer = [] self._buffering = True @@ -364,7 +630,6 @@ def start_buffering(self) -> None: def stop_and_pop( self, ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Stop buffering and return (ts, x, y, yaw) arrays for the step.""" with self._lock: self._buffering = False buf = self._buffer @@ -405,14 +670,16 @@ def _fit_hw( savgol_order: int = 2, hampel_window: int = 11, hampel_n_sigma: float = 3.0, -) -> tuple[TwistBasePlantParams, dict, list[dict]]: +) -> tuple[ + TwistBasePlantParams, + dict[str, list[dict]], + list[dict], + VelocityEnvelope, + dict[str, list[dict]], +]: _prereq_banner(profile) hw_dt = 1.0 / profile.tick_rate_hz - # Signal-injection is open-loop — publish Twist directly on the coord's - # twist_command topic (/cmd_vel). The operator coord's velocity task - # picks it up and routes through whichever adapter (transport_lcm for - # Go2, flowbase Portal RPC for FlowBase, ...) the operator brought up. cmd_pub = LCMTransport(_CMD_VEL_TOPIC, Twist) def publish(vx: float, vy: float, wz: float) -> None: @@ -433,11 +700,6 @@ def safe_stop() -> None: publish(0.0, 0.0, 0.0) time.sleep(0.05) - # Observation: subscribe to the operator coord's /coordinator/joint_state - # Out directly over LCM. JointState positions carry [x, y, yaw] because - # ConnectedTwistBase populates them from adapter.read_odometry(). Body - # velocity recovered by pose-differentiation in the stream (commanded - # velocities in the JointState are not measured). joints = make_twist_base_joints(profile.joints_prefix) stream = _JointStatePoseStream(joint_names=joints) js_sub = LCMTransport(_JOINT_STATE_TOPIC, JointState) @@ -455,142 +717,217 @@ def safe_stop() -> None: unsub() raise SystemExit(f"No {_JOINT_STATE_TOPIC} — is `dimos run {profile.blueprint}` up?") - pooled: dict[str, FopdtChannelParams] = {} + def run_one_step(channel: str, amp: float, tag: str) -> dict: + safe_stop() + resp = ( + gate_input( + f"\n[{tag} {channel}@{amp}] reposition robot into clear space, {gate_keys_label}: " + ) + .strip() + .lower() + ) + if resp == "q": + raise KeyboardInterrupt("operator quit") + if resp == "s": + print(" skipped") + return {"action": "skip"} + + stream.start_buffering() + t_end = time.perf_counter() + pre_roll_s + while time.perf_counter() < t_end: + publish(0.0, 0.0, 0.0) + time.sleep(hw_dt) + + cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} + cmd[channel] = amp + sp, _ = stream.latest() + if sp is None: + print(" [abort] lost odom before step") + stream.stop_and_pop() + return {"action": "abort"} + x0, y0 = sp.position.x, sp.position.y + t0 = time.perf_counter() + end_reason = "time" + while True: + now = time.perf_counter() + t_rel = now - t0 + if t_rel > step_s: + break + publish(cmd["vx"], cmd["vy"], cmd["wz"]) + p, pt = stream.latest() + if p is None or now - pt > profile.odom_stale_s: + print(f" [abort] stale odom ({now - pt:.2f}s)") + end_reason = "stale" + break + dist = math.hypot(p.position.x - x0, p.position.y - y0) + if dist >= max_dist: + end_reason = "dist" + break + time.sleep(hw_dt) + ts_abs, x_buf, y_buf, yaw_buf = stream.stop_and_pop() + safe_stop() + + if len(ts_abs) < max(5, savgol_window): + print(f" [warn] {channel}@{amp}: too few samples, skip") + return {"action": "abort"} + ts_in = ts_abs - t0 + ts_rel, vx_all, vy_all, dyaw_all = reconstruct_body_velocities( + ts_in, x_buf, y_buf, yaw_buf, window=savgol_window, order=savgol_order + ) + n_dropped = len(ts_in) - len(ts_rel) + if n_dropped: + print(f" dedupe: dropped {n_dropped} near-zero-dt samples") + ys_all = {"vx": vx_all, "vy": vy_all, "wz": dyaw_all}[channel] + pre_mask = ts_rel < 0.0 + post_mask = ~pre_mask + if post_mask.sum() < max(5, savgol_window): + print(f" [warn] {channel}@{amp}: too few post-step samples, skip") + return {"action": "abort"} + noise_std: float | None = None + if pre_mask.sum() >= 3: + noise_std = float(np.std(ys_all[pre_mask])) + ts_fit = ts_rel[post_mask] + ys_raw = ys_all[post_mask] + clip_max = max(5.0 * abs(amp), 0.5) if amp != 0.0 else 5.0 + ys_clipped, n_clipped = _physical_clip(ys_raw, clip_max) + if n_clipped: + print(f" physical-clip: replaced {n_clipped}/{len(ys_raw)} samples > {clip_max:g}") + ys_filt, n_replaced = _hampel(ys_clipped, hampel_window, hampel_n_sigma) + if n_replaced: + print(f" hampel: replaced {n_replaced}/{len(ys_raw)} outliers") + return { + "action": "ok", + "ts_fit": ts_fit, + "ys_filt": ys_filt, + "ys_raw": ys_raw, + "n_replaced": n_replaced, + "noise_std": noise_std, + "end_reason": end_reason, + } + + def fit_step_result(r: dict, channel: str, amp: float) -> Any | None: + ts_fit, ys_filt = r["ts_fit"], r["ys_filt"] + dt_med = float(np.median(np.diff(ts_fit))) if len(ts_fit) > 1 else 0.0 + fp = fit_fopdt( + ts_fit, + ys_filt, + u_step=amp, + min_deadtime=dt_med, + noise_std=r["noise_std"], + two_stage=r["noise_std"] is not None, + ) + if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): + print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") + return None + print( + f" {channel}@{amp}: K={fp.K:.3f} tau={fp.tau:.3f} " + f"L={fp.L:.3f} ({len(r['ys_raw'])} samples, ended on {r['end_reason']})" + ) + return fp + + def record_fit(channel: str, amp: float, r: dict, fp: Any, source: str) -> None: + per_amplitude[channel].append( + { + "amp": amp, + "amplitude": amp, + "direction": "forward", + "K": fp.K, + "tau": fp.tau, + "L": fp.L, + "r2": fp.r_squared, + "source": source, + } + ) + traces.append( + { + "channel": channel, + "amp": amp, + "t": np.asarray(r["ts_fit"], dtype=float), + "y_raw": r["ys_raw"], + "n_replaced": r["n_replaced"], + "y": r["ys_filt"], + "K": fp.K, + "tau": fp.tau, + "L": fp.L, + "r2": fp.r_squared, + "source": source, + } + ) + + canonical: dict[str, FopdtChannelParams] = {} per_amplitude: dict[str, list[dict]] = {} + floor_results: dict[str, list[dict]] = {} + env_channels: dict[str, ChannelEnvelope] = {} traces: list[dict] = [] try: for channel in profile.excited_channels: - fits = [] per_amplitude[channel] = [] - for amp in profile.si_amplitudes[channel]: - safe_stop() - resp = ( - gate_input( - f"\n[{channel}@{amp}] reposition robot into clear space, " - f"{gate_keys_label}: " + floor_results[channel] = [] + + print(f"\n=== [{channel}] floor probe ===") + for amp in profile.floor_probe_amplitudes.get(channel, []): + r = run_one_step(channel, amp, "FLOOR") + if r["action"] != "ok": + floor_results[channel].append( + {"amp": amp, "motion_detected": False, "sustained_samples": 0} ) - .strip() - .lower() - ) - if resp == "q": - raise KeyboardInterrupt("operator quit") - if resp == "s": - print(" skipped") continue + passed, longest = _d3_sustained_pass( + r["ys_filt"], + amp, + profile.floor_motion_threshold, + profile.floor_fractional_threshold, + profile.floor_sustained_samples, + ) + floor_results[channel].append( + {"amp": amp, "motion_detected": bool(passed), "sustained_samples": int(longest)} + ) + print( + f" {channel}@{amp}: motion={'YES' if passed else 'no'} longest_run={longest}" + ) - # pre-roll zeros (settle). Buffer during pre-roll too so - # the pre-step samples give us a noise_std estimate - # (robot at rest, zero commanded — pure odom noise). - stream.start_buffering() - t_end = time.perf_counter() + pre_roll_s - while time.perf_counter() < t_end: - publish(0.0, 0.0, 0.0) - time.sleep(hw_dt) - - # step. Ends on whichever comes first: travelled distance - # >= max_dist (the real-space bound — at high speed the - # time cap would run the robot out of the test area), or - # t_rel > step_s (time safety cap; also the terminator for - # wz, which spins in place and never accumulates distance). - # Body-frame velocity is reconstructed from buffered pose - # AFTER the step — smooth-then-differentiate gives a much - # cleaner trace on a legged base than per-tick differentiation. - cmd = {"vx": 0.0, "vy": 0.0, "wz": 0.0} - cmd[channel] = amp - sp, _ = stream.latest() - if sp is None: - print(" [abort] lost odom before step") + print(f"\n=== [{channel}] sweep ===") + for amp in profile.si_amplitudes[channel]: + r = run_one_step(channel, amp, "SWEEP") + if r["action"] != "ok": continue - x0, y0 = sp.position.x, sp.position.y - t0 = time.perf_counter() - end_reason = "time" - while True: - now = time.perf_counter() - t_rel = now - t0 - if t_rel > step_s: - break - publish(cmd["vx"], cmd["vy"], cmd["wz"]) - p, pt = stream.latest() - if p is None or now - pt > profile.odom_stale_s: - print(f" [abort] stale odom ({now - pt:.2f}s)") - end_reason = "stale" - break - dist = math.hypot(p.position.x - x0, p.position.y - y0) - if dist >= max_dist: - end_reason = "dist" - break - time.sleep(hw_dt) - ts_abs, x_buf, y_buf, yaw_buf = stream.stop_and_pop() - safe_stop() - - if len(ts_abs) < max(5, savgol_window): - print(f" [warn] {channel}@{amp}: too few samples, skip") + fp = fit_step_result(r, channel, amp) + if fp is None: continue - ts_rel = ts_abs - t0 - vx_all, vy_all, dyaw_all = reconstruct_body_velocities( - ts_rel, x_buf, y_buf, yaw_buf, window=savgol_window, order=savgol_order - ) - ys_all = {"vx": vx_all, "vy": vy_all, "wz": dyaw_all}[channel] - # Split pre/post step. Pre is zero-commanded baseline - - # std of that velocity = our noise estimate. - pre_mask = ts_rel < 0.0 - post_mask = ~pre_mask - if post_mask.sum() < max(5, savgol_window): - print(f" [warn] {channel}@{amp}: too few post-step samples, skip") + record_fit(channel, amp, r, fp, source="sweep") + + print(f"\n=== [{channel}] ceiling probe ===") + for amp in profile.ceiling_probe_amplitudes.get(channel, []): + r = run_one_step(channel, amp, "CEILING") + if r["action"] != "ok": continue - noise_std: float | None = None - if pre_mask.sum() >= 3: - noise_std = float(np.std(ys_all[pre_mask])) - ts_fit = ts_rel[post_mask] - ys_raw = ys_all[post_mask] - ys_filt, n_replaced = _hampel(ys_raw, hampel_window, hampel_n_sigma) - if n_replaced: - print(f" hampel: replaced {n_replaced}/{len(ys_raw)} outliers") - # Floor L at the actual data sample period — fit cannot - # physically resolve deadtime finer than odom rate. - dt_med = float(np.median(np.diff(ts_fit))) if len(ts_fit) > 1 else 0.0 - fp = fit_fopdt( - ts_fit, - ys_filt, - u_step=amp, - min_deadtime=dt_med, - noise_std=noise_std, - two_stage=noise_std is not None, - ) - if not fp.converged or not np.isfinite([fp.K, fp.tau, fp.L]).all(): - print(f" [warn] {channel}@{amp}: fit failed ({fp.reason})") + fp = fit_step_result(r, channel, amp) + if fp is None: continue - print( - f" {channel}@{amp}: K={fp.K:.3f} tau={fp.tau:.3f} " - f"L={fp.L:.3f} ({len(ys_raw)} samples, ended on {end_reason})" - ) - fits.append(fp) - per_amplitude[channel].append( - {"amplitude": amp, "direction": "forward", "K": fp.K, "tau": fp.tau, "L": fp.L} - ) - traces.append( - { - "channel": channel, - "amp": amp, - "t": np.asarray(ts_fit, dtype=float), - "y_raw": ys_raw, - "n_replaced": n_replaced, - "y": ys_filt, - "K": fp.K, - "tau": fp.tau, - "L": fp.L, - "r2": fp.r_squared, - } - ) - if not fits: - raise RuntimeError(f"hw SI: no converged fits for {channel!r}") - pooled[channel] = FopdtChannelParams( - K=float(np.mean([f.K for f in fits])), - tau=float(np.mean([f.tau for f in fits])), - L=float(np.mean([f.L for f in fits])), + record_fit(channel, amp, r, fp, source="ceiling_probe") + + sweep_fits = [f for f in per_amplitude[channel] if f["source"] == "sweep"] + probe_fits = [f for f in per_amplitude[channel] if f["source"] == "ceiling_probe"] + if not sweep_fits: + raise RuntimeError(f"hw SI: no converged sweep fits for {channel!r}") + env_channels[channel] = _envelope_from_channel_results( + floor_results[channel], sweep_fits, probe_fits, profile, channel + ) + linear = _pick_linear_regime_fit(sweep_fits) or sweep_fits[0] + canonical[channel] = FopdtChannelParams( + K=float(linear["K"]), tau=float(linear["tau"]), L=float(linear["L"]) + ) + print( + f"\n[{channel}] canonical (linear-regime, amp={linear['amp']:g}): " + f"K={linear['K']:.3f} tau={linear['tau']:.3f} L={linear['L']:.3f} " + f"r2={linear['r2']:.2f}" + ) + ce = env_channels[channel] + print( + f"[{channel}] envelope: floor={ce.floor:.3f} (nf={ce.floor_not_found}) " + f"ceiling={ce.ceiling:.3f} (nf={ce.ceiling_not_found})" ) except KeyboardInterrupt: - # finally below does safe_stop + unsub — don't duplicate raise SystemExit( "\n[hw] aborted by operator — robot stopped, no artifact written." ) from None @@ -598,34 +935,30 @@ def safe_stop() -> None: safe_stop() unsub() - # Channels not excited (e.g. vy on a non-strafing robot) are - # placeholdered = vx so FF / profile stay sane; flagged in caveats. + fallback_env = ChannelEnvelope( + floor=0.0, ceiling=0.0, floor_not_found=True, ceiling_not_found=True + ) for ch in _CHANNELS: - if ch not in pooled: - pooled[ch] = pooled["vx"] + if ch not in canonical: + canonical[ch] = canonical["vx"] per_amplitude[ch] = [] + floor_results.setdefault(ch, []) + env_channels[ch] = fallback_env print(f" [note] {ch} not excited on hw — placeholder {ch} = vx") + envelope = VelocityEnvelope(vx=env_channels["vx"], vy=env_channels["vy"], wz=env_channels["wz"]) return ( - TwistBasePlantParams(vx=pooled["vx"], vy=pooled["vy"], wz=pooled["wz"]), + TwistBasePlantParams(vx=canonical["vx"], vy=canonical["vy"], wz=canonical["wz"]), per_amplitude, traces, + envelope, + floor_results, ) class CharacterizerConfig(ModuleConfig): - """Config for :class:`Characterizer`. Each field mirrors a CLI flag on - the existing ``characterization`` entrypoint; ``None`` means "fall - back to the selected ``RobotPlantProfile``". - - ``gate_source`` selects how each SI step is gated. ``"stdin"`` - (default, CLI mode) reads ENTER/s/q from the terminal; ``"stream"`` - (blueprint mode) consumes events from the ``gate`` In port wired to - a co-running :class:`~dimos.robot.unitree.keyboard_teleop.KeyboardTeleop` - that publishes ENTER/K/Backspace as ``"advance"``/``"skip"``/``"quit"``. - """ - robot: str = "go2" - mode: Literal["hw", "self-test"] = "hw" + mode: Literal["hw", "self-test", "re-derive"] = "hw" + artifact: str | None = None out: str | None = None robot_id: str | None = None surface: str = "concrete" @@ -635,47 +968,14 @@ class CharacterizerConfig(ModuleConfig): odom_warmup: float | None = None max_dist: float | None = None gate_source: Literal["stdin", "stream"] = "stdin" - # Savitzky-Golay smoothing applied to the POSITION trace before - # central-differencing to body-frame velocity. This is the main - # noise-rejection knob for legged-robot data: gait-frequency body - # bob lives in the raw pose; smooth-then-differentiate keeps it out - # of the velocity signal. Window must be odd and > order. Set - # window=0 or window <= order to disable. - # Default 11 = ~2 gait cycles at 10 Hz tick rate / 2 Hz Go2 trot; - # synthetic-noise sanity check (3cm gait bob + 5mm odom noise) shows - # window=5 leaves vx-std ~= 0.18 m/s, window=11 drops it to ~= 0.05. savgol_window: int = 11 savgol_order: int = 2 - # Hampel outlier filter on the post-reconstruction velocity trace - # before FOPDT fitting. Catches residual single-sample spikes that - # survive savgol smoothing (e.g. odom estimator hiccups). Set - # window=0 to disable. hampel_window: int = 11 hampel_n_sigma: float = 3.0 - # Two-stage FOPDT fit: estimate L directly from data (first time the - # response crosses ~5*noise_std above the pre-step noise floor), - # then fit K and tau with L pinned. Decouples L from tau - joint - # LM-fit can trade off a tiny L for a slightly inflated tau when the - # data is noisy. Auto-disabled if noise_std can't be estimated - # (need >=3 pre-step samples in the pose buffer). two_stage_fit: bool = True class Characterizer(Module): - """Module wrapper around the FOPDT characterization sequence. - - Driven via the CLI shim in :func:`main` (``gate_source="stdin"``) or - composed into the all-in-one blueprint - ``unitree-go2-characterization`` (``gate_source="stream"``, gating - events arrive on the ``gate`` In port from a co-running - :class:`~dimos.robot.unitree.keyboard_teleop.KeyboardTeleop`). - Stdin-driven gating cannot work inside the blueprint runner because - deployed modules run in forkserver-spawned worker subprocesses that - don't share the parent CLI's TTY — hence the stream-based path. - - ``start()`` blocks for the full operator-gated SI loop, then returns. - """ - config: CharacterizerConfig gate: In[Int8] @@ -694,9 +994,6 @@ def start(self) -> None: self._run() def _on_gate_event(self, msg: Int8) -> None: - # Translate the pygame-side gate codes to the legacy CLI vocab so - # _fit_hw's response check (``""``=run, ``"s"``=skip, ``"q"``=quit) - # is unchanged. code = int(msg.data) translated = {GATE_ADVANCE: "", GATE_SKIP: "s", GATE_QUIT: "q"}.get(code, "") self._gate_queue.put(translated) @@ -715,6 +1012,10 @@ def _run(self) -> None: robot_id = cfg.robot_id if cfg.robot_id is not None else profile.robot_id out_root = Path(cfg.out).expanduser() if cfg.out else DEFAULT_OUT_DIR + if cfg.mode == "re-derive": + self._run_re_derive(cfg, profile, robot_id, out_root) + return + if cfg.mode == "hw": if cfg.gate_source == "stream": gate_input: Callable[[str], str] = self._wait_gate_stream @@ -722,7 +1023,7 @@ def _run(self) -> None: else: gate_input = input gate_keys_label = "ENTER=run s=skip q=quit" - fitted, per_amplitude, traces = _fit_hw( + fitted, per_amplitude, traces, envelope, floor_results = _fit_hw( profile, step_s, pre_roll_s, @@ -736,7 +1037,7 @@ def _run(self) -> None: hampel_n_sigma=cfg.hampel_n_sigma, ) else: - fitted, per_amplitude, traces = _fit_selftest(profile) + fitted, per_amplitude, traces, envelope, floor_results = _fit_selftest(profile) provenance = Provenance( robot_id=robot_id, @@ -749,12 +1050,63 @@ def _run(self) -> None: f"(real {profile.name}, LCM SI)" if cfg.mode == "hw" else "(in-process self-test)" ), ) + + env_dc = VelocityEnvelopeDC( + vx=ChannelEnvelopeDC(**asdict(envelope.vx)), + vy=ChannelEnvelopeDC(**asdict(envelope.vy)), + wz=ChannelEnvelopeDC(**asdict(envelope.wz)), + ) + dyn_by_amp = DynamicsByAmplitude( + vx=[ + AmplitudeFitDC( + amp=e["amp"], + K=e["K"], + tau=e["tau"], + L=e["L"], + r2=e.get("r2", 0.0), + source=e.get("source", "sweep"), + ) + for e in per_amplitude.get("vx", []) + ], + vy=[ + AmplitudeFitDC( + amp=e["amp"], + K=e["K"], + tau=e["tau"], + L=e["L"], + r2=e.get("r2", 0.0), + source=e.get("source", "sweep"), + ) + for e in per_amplitude.get("vy", []) + ], + wz=[ + AmplitudeFitDC( + amp=e["amp"], + K=e["K"], + tau=e["tau"], + L=e["L"], + r2=e.get("r2", 0.0), + source=e.get("source", "sweep"), + ) + for e in per_amplitude.get("wz", []) + ], + ) + floor_dc = FloorProbeResults( + vx=[FloorProbeResultDC(**r) for r in floor_results.get("vx", [])], + vy=[FloorProbeResultDC(**r) for r in floor_results.get("vy", [])], + wz=[FloorProbeResultDC(**r) for r in floor_results.get("wz", [])], + ) + artifact = derive_config( fitted, provenance, per_amplitude=per_amplitude, vx_max=profile.vx_max, wz_max=profile.wz_max, + velocity_envelope=env_dc, + dynamics_by_amplitude=dyn_by_amp, + floor_probe_results=floor_dc, + min_speed_floor=profile.min_speed_floor, ) if cfg.mode == "hw" and "vy" not in profile.excited_channels: artifact.caveats.append( @@ -772,20 +1124,99 @@ def _run(self) -> None: / f"{robot_id}_config_{cfg.mode}_{cfg.surface}_{provenance.date}_{provenance.git_sha}.json" ) artifact.to_json(out_path) - plot_path = out_path.with_suffix(".png") - _plot_fits(traces, provenance, profile, plot_path) + env_png = out_path.with_suffix(".png") + steps_png = out_path.with_name(out_path.stem + "_steps.png") + _plot_fits( + traces, + provenance, + profile, + env_png, + envelope=envelope, + kind="envelope", + ) + _plot_fits(traces, provenance, profile, steps_png, kind="steps") tag = "ROBOT-VALID" if artifact.valid_for_tuning else "NOT robot-valid (plumbing check)" - print("\nFOPDT fit graph (the deliverable — model vs real data):") - print(f" {plot_path.resolve()}") + print("\nEnvelope plot (the primary deliverable — K, output, τ vs amp):") + print(f" {env_png.resolve()}") + print("Per-amp step responses (eyeball the FOPDT fits):") + print(f" {steps_png.resolve()}") print(f"Config artifact [{tag}] (machine handoff for the benchmark):") print(f" {out_path.resolve()}") + def _run_re_derive( + self, + cfg: CharacterizerConfig, + profile: RobotPlantProfile, + robot_id: str, + out_root: Path, + ) -> None: + if not cfg.artifact: + raise SystemExit("--mode re-derive requires --artifact ") + src_path = Path(cfg.artifact).expanduser() + print(f"[re-derive] reading {src_path}") + src = TuningConfig.from_json(src_path) + if src.dynamics_by_amplitude is None: + raise SystemExit( + f"{src_path}: artifact has no dynamics_by_amplitude — " + "this artifact is methodology v1 (sparse sweep), nothing to " + "re-derive. Re-run characterization in v2 mode." + ) + + new = re_derive_config( + src, + vx_max=profile.vx_max, + wz_max=profile.wz_max, + floor_probe_amplitudes=dict(profile.floor_probe_amplitudes), + min_speed_floor=profile.min_speed_floor, + sag_threshold=profile.ceiling_k_sag_threshold, + ) + new.caveats.insert( + 0, + f"Re-derived on {date.today().isoformat()} from {src_path.name} " + f"using the operational-ceiling logic (max(K·amp) clamped to " + f"envelope). Plant + FF passed through unchanged.", + ) + + out_dir = out_root / robot_id + out_dir.mkdir(parents=True, exist_ok=True) + suffix = "rederived" + out_path = out_dir / f"{src_path.stem}__{suffix}.json" + new.to_json(out_path) + + env = new.velocity_envelope + png_path = out_path.with_suffix(".png") + _plot_fits( + [], # no raw step traces in the artifact — row 1 omitted + new.provenance, + profile, + png_path, + envelope=env, + dynamics=new.dynamics_by_amplitude, + ) + + print("\nRe-derived velocity_envelope:") + for ch in ("vx", "vy", "wz"): + e = getattr(env, ch) + sat = f" sat@={e.saturating_at_amp:g}" if e.saturating_at_amp is not None else "" + print( + f" {ch}: floor={e.floor:.3f} (nf={e.floor_not_found}) " + f"ceiling={e.ceiling:.3f} (nf={e.ceiling_not_found}){sat}" + ) + print() + print(f"New artifact: {out_path.resolve()}") + print(f"New PNG: {png_path.resolve()}") + def main() -> None: ap = argparse.ArgumentParser(description="Twist-base characterization -> tuning artifact") ap.add_argument("--robot", default="go2", help=f"one of {sorted(ROBOT_PLANT_PROFILES)}") - ap.add_argument("--mode", choices=["hw", "self-test"], default="hw") + ap.add_argument("--mode", choices=["hw", "self-test", "re-derive"], default="hw") + ap.add_argument( + "--artifact", + default=None, + help="re-derive mode: existing artifact JSON to recompute envelope from", + ) ap.add_argument( "--out", default=None, @@ -817,6 +1248,7 @@ def main() -> None: instance = Characterizer( robot=args.robot, mode=args.mode, + artifact=args.artifact, out=args.out, robot_id=args.robot_id, surface=args.surface, diff --git a/dimos/utils/benchmarking/characterization_recorder.py b/dimos/utils/benchmarking/characterization_recorder.py index 7ad0d3463d..21074ed36d 100644 --- a/dimos/utils/benchmarking/characterization_recorder.py +++ b/dimos/utils/benchmarking/characterization_recorder.py @@ -111,10 +111,25 @@ def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) available" warning fires once per message and floods the terminal. Pose is stored as ``None``; rerun-time analysis tools either don't need a pose anchor or pull it from the recorded ``odom`` stream.""" + import sqlite3 + from threading import Lock + + last_ts = [0.0] # mutable single-cell for the closure + lock = Lock() + _EPS = 1e-6 # 1 microsecond def on_msg(msg: Any) -> None: - ts = getattr(msg, "ts", None) or time.time() - stream.append(msg, ts=ts, pose=None) + raw_ts = getattr(msg, "ts", None) or time.time() + with lock: + # Strictly monotonic: bump duplicates / regressions to + # last + EPS so the primary key never collides. + ts = raw_ts if raw_ts > last_ts[0] else last_ts[0] + _EPS + last_ts[0] = ts + try: + stream.append(msg, ts=ts, pose=None) + except sqlite3.IntegrityError: + # Defense in depth — never let the LCM thread die. + pass self.register_disposable(Disposable(input_topic.subscribe(on_msg))) diff --git a/dimos/utils/benchmarking/plant.py b/dimos/utils/benchmarking/plant.py index 16837f21ee..25ac81e88a 100644 --- a/dimos/utils/benchmarking/plant.py +++ b/dimos/utils/benchmarking/plant.py @@ -46,6 +46,62 @@ class FopdtChannelParams: 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 + + +@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. @@ -191,9 +247,34 @@ class RobotPlantProfile: 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], + } + ) + 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 + # 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) @@ -215,7 +296,21 @@ def joints_prefix(self) -> str: odom_warmup_s=10.0, odom_stale_s=1.0, excited_channels=("vx", "vy", "wz"), - si_amplitudes={"vx": [0.3, 0.6, 0.9], "vy": [0.2, 0.4], "wz": [0.4, 0.8, 1.2]}, + # 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], + }, + ceiling_probe_amplitudes={"vx": [2.5, 3.0], "vy": [1.5, 2.0], "wz": [2.5, 3.0]}, step_s=8.0, pre_roll_s=1.0, max_dist_m=6.0, @@ -264,9 +359,13 @@ def joints_prefix(self) -> str: "GO2_VX_RISE", "GO2_WZ_RISE", "ROBOT_PLANT_PROFILES", + "AmplitudeFit", + "ChannelEnvelope", "FOPDTChannel", + "FloorProbeResult", "FopdtChannelParams", "RobotPlantProfile", "TwistBasePlantParams", "TwistBasePlantSim", + "VelocityEnvelope", ] diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index 0a1c73ff01..43aef373fa 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -25,20 +25,103 @@ Window must have focus. **WASD/QE** = reposition. **Enter** = advance. **K** = s dimos run unitree-go2-characterization ``` -Per axis (vx, wz on Go2 default gait) × a few amplitudes, the SI loop prompts you to reposition, then issues a velocity step and records the response. Settling pre-roll (~1.5 s) happens *after* you press Enter — wait for it before assuming nothing's happening. - -**Duration**: ~5–8 minutes for the default amplitude set. - -**Output** (lands together with the same timestamp suffix): +Per axis (vx, vy, wz on Go2), the SI loop runs **three phases** with one +ENTER per step: + +1. **Floor probe** (~4 tiny amps, e.g. vx 0.02→0.15 m/s) — the AND-test + (D3) detects the smallest commanded value that actually moves the + robot (`|v_body| > 0.02 m/s` **AND** `> 5% of |amp|`, sustained for + ≥5 samples). +2. **Dense sweep** (5 amps, unified `[0.2, 0.5, 1.0, 1.5, 2.0]` ladder + in m/s for vx/vy and rad/s for wz) — FOPDT fit per amplitude; the + canonical fit used by DERIVE is the lowest-amplitude one with + r² > 0.9 (the **linear regime**, methodology v2 — D1). +3. **Ceiling probe** (~2 supra-sweep amps, e.g. vx 2.5, 3.0) — FOPDT + fit per amplitude. **The operational ceiling DERIVE uses is + `min(max(|K(amp)·amp|), profile.{vx,wz}_max)` across the FULL sweep + + probe table** — i.e. the max steady-state OUTPUT magnitude the + plant can deliver, clamped to the platform cap. This is robust to + noisy FOPDT fits: a misfit K with the right amp still gives a + sensible K·amp because the output magnitude is what matters for RG, + not K alone. The K-sag amplitude (where K drops 15% below the + linear-regime K) is still recorded as `saturating_at_amp` in the + artifact for forensics, but is **not** used as the operational cap + — with low-r² fits the K-sag detector misfires. + +Settling pre-roll (~1.5 s) happens *after* you press Enter — wait for +it. Each phase prints a banner so you know which sub-test is running. + +**Duration**: ~30–45 min hands-on for the full dense flow on Go2 +(~32 ENTERs: 4 floor + 5 sweep + 2 ceiling per channel × 3 channels, +minus one floor amp on vy). Per-step gating is preserved — drift is +bounded to one step. The cost is "do it once, properly." + +**Output** (same timestamp suffix): ``` data/characterization/go2/ ├── go2_config_hw_concrete__.json ← the tuning artifact -├── go2_config_hw_concrete__.png ← fit-quality plot +├── go2_config_hw_concrete__.png ← fit-quality plot + K(amp) panel └── go2_recording__.db ← raw streams (cmd_vel, joint_state, odom, gate) ``` -The PNG is what you READ to judge the fit. K/τ/L per channel + r² are annotated; raw (dotted) overlays the savgol-filtered fit (solid) when Hampel replaced points. +**PNG layout** (four rows × one column per channel; single y-axis per +subplot — easier to read than the old twin-axis version): + +- Step response — measured (solid) + raw (dotted, when Hampel touched + it) + fitted FOPDT (dashed). Skipped on re-derive (no raw traces in + the artifact). +- **K(amp)** — gain per amplitude. Sweep (○-line, blue), ceiling probe + (■-line, orange). Floor/ceiling marked as vertical dotted lines + (green/red). +- **Output K·amp(amp)** — **the steady-state output magnitude the + plant delivers.** This is the most diagnostic panel: it plateaus + visually where the plant saturates. The operational ceiling is the + max of this curve (clamped to the platform cap). +- **τ(amp)** — first-order time constant per amplitude. + +**Artifact sections** (additive — older v1 readers ignore the new +ones): + +- 1–4 + 6 (existing): plant, feedforward, velocity_profile, + recommended_controller, valid_for_tuning. +- 5 **`velocity_envelope`** (new) — per-channel `{floor, ceiling, + floor_not_found, ceiling_not_found}`. +- 7 **`dynamics_by_amplitude`** (new) — full K/τ/L table per channel + across sweep + ceiling-probe amps (forensics + future lookup-based + RG without re-collecting data). +- `floor_probe_results` (new sibling) — per-amp D3 pass/fail. +- `provenance.methodology_version` = 2 (defaults to 1 on legacy + artifacts); `schema_version` is unchanged at 1. + +**DERIVE** consumes the measured envelope: + +- RG `min_speed` = `max(envelope.vx.floor, profile.min_speed_floor)` +- RG `v_max` = `envelope.vx.ceiling` (already clamped to `profile.vx_max`) +- RG `ω_max` = `envelope.wz.ceiling · 0.85` (15% headroom for cross-track) +- FF gain = `1/K` from the linear-regime fit (unchanged). +- `floor_not_found` / `ceiling_not_found` only fire when there is no + data at all (empty probe / no converged fits). Clamping to the + platform cap is silent (it's an envelope decision, not a measurement + failure). + +### Re-derive a stale artifact + +If the methodology changed (e.g. ceiling logic was bug-fixed) and you +don't want to re-run on the robot, re-apply the current logic to an +existing v2 artifact: + +``` +python -m dimos.utils.benchmarking.characterization \ + --mode re-derive --robot go2 \ + --artifact data/characterization/go2/go2_config_hw_concrete__.json +``` + +The tool reads the artifact's stored `dynamics_by_amplitude` + +`floor_probe_results`, recomputes envelope + velocity_profile + +caveats, and writes `__rederived.json` + matching PNG alongside +the source. Plant + feedforward pass through unchanged (those were +already measured). No robot needed. Override defaults with `-o characterizer.=`, e.g.: - `-o characterizer.surface=grass` diff --git a/dimos/utils/benchmarking/tuning.py b/dimos/utils/benchmarking/tuning.py index fb07abe5e4..44d10fb796 100644 --- a/dimos/utils/benchmarking/tuning.py +++ b/dimos/utils/benchmarking/tuning.py @@ -49,6 +49,13 @@ ) 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) ------- @@ -97,6 +104,7 @@ class Provenance: git_sha: str = "unknown" sim_or_hw: str = "sim" characterization_session_dir: str = "" + methodology_version: int = METHODOLOGY_VERSION @dataclass @@ -197,6 +205,70 @@ class OperatingPointMap: 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 + + +@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 @@ -206,6 +278,9 @@ class TuningConfig: 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 @@ -244,8 +319,39 @@ def _chan(d: dict) -> FopdtChannelDC: 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(**data["provenance"]), + provenance=Provenance(**prov_data), plant=PlantModelDC( vx=_chan(data["plant"]["vx"]), vy=_chan(data["plant"]["vy"]), @@ -256,6 +362,9 @@ def _chan(d: dict) -> FopdtChannelDC: 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, ) @@ -287,6 +396,122 @@ def _safe_inv_gain(K: float) -> float: 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 @@ -320,6 +545,10 @@ def derive_config( 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. @@ -327,23 +556,56 @@ def derive_config( - Feedforward gain per axis = ``1 / K`` (the compensator divides the controller command by the plant gain so commanded == achieved). - - ``max_angular_speed`` = measured wz ceiling minus the cross-track - headroom margin. + ``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: - a step of size v settles in ~tau, so the achievable mean accel is - ~v/tau); decel = ``DECEL_ACCEL_RATIO x`` that. + - ``max_linear_accel`` ~= ``vx_ceiling / tau_vx`` (first-order rise); + decel = ``DECEL_ACCEL_RATIO x`` that. - recommended controller = baseline, hardcoded, with cited evidence. - - ``per_amplitude`` (optional) is the fitter's per-amplitude table - ``{channel: [{amplitude, K, ...}, ...]}``; when absent the robot's - saturation envelope (``vx_max``/``wz_max``) is used for the ceilings. """ - # Clamp the measured ceiling to the robot's saturation envelope: an - # un-saturated FOPDT fit extrapolates linearly past what the platform - # can physically deliver, so the envelope is a hard upper bound. - vx_ceiling = min(_channel_ceiling(per_amplitude, "vx", vx_max), vx_max) - wz_ceiling = min(_channel_ceiling(per_amplitude, "wz", wz_max), wz_max) + 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), @@ -358,15 +620,18 @@ def derive_config( 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 = [ - 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}).", - ] + 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( @@ -391,10 +656,74 @@ def derive_config( 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) ------------------------ @@ -440,8 +769,14 @@ def invert_tolerance( __all__ = [ + "METHODOLOGY_VERSION", "SCHEMA_VERSION", + "AmplitudeFitDC", + "ChannelEnvelopeDC", + "DynamicsByAmplitude", "FeedforwardDC", + "FloorProbeResultDC", + "FloorProbeResults", "FopdtChannelDC", "OperatingPoint", "OperatingPointMap", @@ -450,6 +785,7 @@ def invert_tolerance( "RecommendedControllerDC", "ToleranceRow", "TuningConfig", + "VelocityEnvelopeDC", "VelocityProfileDC", "derive_config", "git_sha", From 4fb2d0eb9207726c08ae7ff4169e1944f0383597 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 28 May 2026 22:27:52 -0700 Subject: [PATCH 43/51] final modification for rage mode testing --- dimos/robot/all_blueprints.py | 5 + .../blueprints/basic/unitree_go2_benchmark.py | 68 +++---- .../basic/unitree_go2_benchmark_rg.py | 54 +++--- .../basic/unitree_go2_characterization.py | 84 ++++++--- .../basic/unitree_go2_coordinator.py | 171 +++++++++++------- .../basic/unitree_go2_precision_nav.py | 81 +++++---- 6 files changed, 283 insertions(+), 180 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ba66f5d3ad..f145aeb03a 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -100,15 +100,20 @@ "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", "unitree-go2-benchmark": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark:unitree_go2_benchmark", + "unitree-go2-benchmark-rage": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark:unitree_go2_benchmark_rage", "unitree-go2-benchmark-rg": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark_rg:unitree_go2_benchmark_rg", + "unitree-go2-benchmark-rg-rage": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_benchmark_rg:unitree_go2_benchmark_rg_rage", "unitree-go2-characterization": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_characterization:unitree_go2_characterization", + "unitree-go2-characterization-rage": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_characterization:unitree_go2_characterization_rage", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", + "unitree-go2-coordinator-rage": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator_rage", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet", "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers", "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", "unitree-go2-precision-nav": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_precision_nav:unitree_go2_precision_nav", + "unitree-go2-precision-nav-rage": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_precision_nav:unitree_go2_precision_nav_rage", "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-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py index 86db2e8eb7..4c00770515 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark.py @@ -19,7 +19,7 @@ the :class:`Benchmarker` module + a per-session telemetry recorder so the operator runs a single command: - dimos run unitree-go2-benchmark --module.benchmarker.config + dimos run unitree-go2-benchmark -o benchmarker.config= instead of the two-terminal CLI flow. Defaults are the bare baseline arm (ff/profile/rg all OFF) — use a sibling blueprint @@ -31,9 +31,9 @@ Comparison arms are config flags (all default OFF — bare baseline): - --module.benchmarker.ff=true # apply derived feedforward - --module.benchmarker.profile=true # apply derived static velocity profile - --module.benchmarker.rg=true # route runs through precision_follower + -o benchmarker.ff=true # apply derived feedforward + -o benchmarker.profile=true # apply derived static velocity profile + -o benchmarker.rg=true # route runs through precision_follower The RG arm runs against the operator coord's ``precision_follower`` task (a ``PathFollowerTask`` subclass that owns its own ``solve_profile()`` @@ -60,38 +60,42 @@ from dimos.msgs.std_msgs.Int8 import Int8 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, + unitree_go2_coordinator_rage, ) from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.utils.benchmarking.benchmark import Benchmarker from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder from dimos.utils.path_utils import get_project_root -unitree_go2_benchmark = autoconnect( - unitree_go2_coordinator, - KeyboardTeleop.blueprint(publish_only_when_active=True), - Benchmarker.blueprint(robot="go2", mode="hw", gate_source="stream"), - CharacterizationRecorder.blueprint( - robot_id="go2", - tag="benchmark", - out_dir=str(get_project_root() / "data" / "benchmark" / "go2"), - ), -).transports( - { - # Operator gate events from the pygame window -> Benchmarker. - # Distinct topic from B1 (`/characterizer/gate`) so the two - # blueprints don't cross-talk if both happen to run on the same - # LCM bus. - ("gate", Int8): LCMTransport("/benchmark/gate", Int8), - # KeyboardTeleop's number-key (0-9) corridor stream -> any task in - # the coord with set_e_max(). No-ops for the bare arm (path_follower - # ignores it), live for the RG arm (precision_follower recomputes). - ("e_max", Float32): LCMTransport("/e_max", Float32), - # Recorder taps the same LCM topics the rest of the stack - # already uses; no new wires, just additional subscribers. - ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - } -) -__all__ = ["unitree_go2_benchmark"] +def _make(coord, gait_tag: str): + return autoconnect( + coord, + KeyboardTeleop.blueprint(publish_only_when_active=True), + Benchmarker.blueprint(robot="go2", mode="hw", gate_source="stream"), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag=f"benchmark_{gait_tag}", + out_dir=str(get_project_root() / "data" / "benchmark" / "go2"), + ), + ).transports( + { + ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + ("e_max", Float32): LCMTransport("/e_max", Float32), + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } + ) + + +unitree_go2_benchmark = _make(unitree_go2_coordinator, gait_tag="default") +# Rage variant — composition is identical apart from the coordinator's +# Go2Connection mode. The Benchmarker module itself does NOT need any +# changes: it reads K/τ/L/envelope from whichever artifact you pass via +# ``--module.benchmarker.config``, and the artifact already encodes the +# gait mode in its provenance. Use this with an artifact produced by +# ``unitree-go2-characterization-rage`` for a consistent measurement. +unitree_go2_benchmark_rage = _make(unitree_go2_coordinator_rage, gait_tag="rage") + +__all__ = ["unitree_go2_benchmark", "unitree_go2_benchmark_rage"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py index 0b2e566717..20710e134f 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_benchmark_rg.py @@ -25,8 +25,8 @@ load the plant + velocity-profile constants: dimos run unitree-go2-benchmark-rg \\ - --module.benchmarker.config \\ - -o coordinator.tasks[1].params.artifact_path= + -o benchmarker.config= \\ + -o coordinator.tasks[2].params.artifact_path= """ from __future__ import annotations @@ -40,31 +40,39 @@ from dimos.msgs.std_msgs.Int8 import Int8 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, + unitree_go2_coordinator_rage, ) from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.utils.benchmarking.benchmark import Benchmarker from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder from dimos.utils.path_utils import get_project_root -unitree_go2_benchmark_rg = autoconnect( - unitree_go2_coordinator, - KeyboardTeleop.blueprint(publish_only_when_active=True), - Benchmarker.blueprint(robot="go2", mode="hw", gate_source="stream", rg=True), - CharacterizationRecorder.blueprint( - robot_id="go2", - tag="benchmark", - out_dir=str(get_project_root() / "data" / "benchmark" / "go2"), - ), -).transports( - { - ("gate", Int8): LCMTransport("/benchmark/gate", Int8), - # KeyboardTeleop's number-key (0-9) corridor stream -> coord's - # e_max input -> precision_follower.set_e_max(). Live RG corridor. - ("e_max", Float32): LCMTransport("/e_max", Float32), - ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - } -) -__all__ = ["unitree_go2_benchmark_rg"] +def _make(coord, gait_tag: str): + return autoconnect( + coord, + KeyboardTeleop.blueprint(publish_only_when_active=True), + Benchmarker.blueprint(robot="go2", mode="hw", gate_source="stream", rg=True), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag=f"benchmark_rg_{gait_tag}", + out_dir=str(get_project_root() / "data" / "benchmark" / "go2"), + ), + ).transports( + { + ("gate", Int8): LCMTransport("/benchmark/gate", Int8), + ("e_max", Float32): LCMTransport("/e_max", Float32), + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } + ) + + +unitree_go2_benchmark_rg = _make(unitree_go2_coordinator, gait_tag="default") +# Rage variant — pair with an artifact from +# ``unitree-go2-characterization-rage`` so the precision_follower's +# envelope/plant matches the gait it's tuned against. +unitree_go2_benchmark_rg_rage = _make(unitree_go2_coordinator_rage, gait_tag="rage") + +__all__ = ["unitree_go2_benchmark_rg", "unitree_go2_benchmark_rg_rage"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py index da9ec9e242..22fe209443 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_characterization.py @@ -54,35 +54,71 @@ from dimos.msgs.std_msgs.Int8 import Int8 from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, + unitree_go2_coordinator_rage, ) from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.utils.benchmarking.characterization import Characterizer from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder -unitree_go2_characterization = autoconnect( - unitree_go2_coordinator, - KeyboardTeleop.blueprint(publish_only_when_active=True), - Characterizer.blueprint(robot="go2", mode="hw", gate_source="stream"), - CharacterizationRecorder.blueprint(robot_id="go2"), -).transports( - { - # Operator gate events from the pygame window -> Characterizer. - # autoconnect pairs KeyboardTeleop.gate (Out) with - # Characterizer.gate (In) by name+type; the LCM transport gives - # them a wire since the modules live in different worker - # subprocesses. Int8 carries the gate code (0=advance, 1=skip, - # 2=quit) — see GATE_* constants in keyboard_teleop. - ("gate", Int8): LCMTransport("/characterizer/gate", Int8), - # CharacterizationRecorder taps the LCM topics the rest of the - # stack already publishes on. LCM is multicast so additional - # subscribers are free. The recorder's `odom: In[PoseStamped]` - # port is named plain `odom` — GO2Connection's outbound odom is - # remapped to `go2_odom` on the bus, so we explicitly route the - # recorder's port to /go2/odom here. - ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + +def _make(coord, gait_mode: str, max_dist: float | None = None, step_s: float | None = None): + """Compose the characterization blueprint. + + ``max_dist`` and ``step_s`` are per-step safety caps (the step ends + on whichever comes first). For the default Go2 gait the profile + defaults (6 m / 8 s) are sane. For rage we cut max_dist hard — the + same commanded amplitude produces roughly 2x the output velocity, + so the robot covers the default 6 m well before the FOPDT step + finishes. 3 m keeps the high-amplitude steps inside a reasonable + test arena while still allowing 1.5 s ~= 3.75*tau at 2 m/s output -- + enough to capture the rise to steady-state. + """ + char_kwargs = { + "robot": "go2", + "mode": "hw", + "gate_source": "stream", + "gait_mode": gait_mode, } + if max_dist is not None: + char_kwargs["max_dist"] = max_dist + if step_s is not None: + char_kwargs["step_s"] = step_s + return autoconnect( + coord, + KeyboardTeleop.blueprint(publish_only_when_active=True), + Characterizer.blueprint(**char_kwargs), + CharacterizationRecorder.blueprint(robot_id="go2", tag=f"recording_{gait_mode}"), + ).transports( + { + ("gate", Int8): LCMTransport("/characterizer/gate", Int8), + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + } + ) + + +unitree_go2_characterization = _make(unitree_go2_coordinator, gait_mode="default") +# Rage variant: same composition but the Go2 firmware is put into +# FsmRageMode on startup (faster / harder gait). Use it when you want +# to characterize the plant in rage mode for tuning the precision +# follower against that envelope. ``gait_mode="rage"`` is stamped into +# the artifact's provenance so the resulting JSON is clearly tagged +# (DERIVE uses it for the caveat string). +# +# Distance caps are tightened (3 m, 4 s) because rage roughly doubles +# the output velocity per commanded amp — the default 6 m / 8 s would +# run the robot off the floor at amp=2.0. Override per run with: +# dimos run unitree-go2-characterization-rage \ +# -o characterizer.max_dist=2.0 -o characterizer.step_s=3.0 +unitree_go2_characterization_rage = _make( + unitree_go2_coordinator_rage, + gait_mode="rage", + max_dist=3.0, + step_s=4.0, ) -__all__ = ["unitree_go2_characterization"] +__all__ = [ + "unitree_go2_characterization", + "unitree_go2_characterization_rage", +] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index 96ad6c2d1d..e68b316ab2 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -15,8 +15,20 @@ """Unitree Go2 ControlCoordinator — GO2Connection + coordinator via LCM transport adapter. +Two variants are exposed: + +* ``unitree_go2_coordinator`` — default gait (``mode="default"``) +* ``unitree_go2_coordinator_rage`` — rage gait (``mode="rage"``); same + composition, just toggles the Go2 firmware's FsmRageMode at startup + so the robot runs faster / harder. + +Both are identical apart from the GO2Connection mode, so downstream +blueprints (characterization, benchmark, precision_nav) compose either +without modification. + Usage: dimos run unitree-go2-coordinator + dimos run unitree-go2-coordinator-rage dimos --simulation run unitree-go2-coordinator """ @@ -34,69 +46,100 @@ _go2_joints = make_twist_base_joints("go2") -unitree_go2_coordinator = ( - autoconnect( - GO2Connection.blueprint(), - ControlCoordinator.blueprint( - hardware=[ - HardwareComponent( - hardware_id="go2", - hardware_type=HardwareType.BASE, - joints=_go2_joints, - adapter_type="transport_lcm", - ), - ], - tasks=[ - TaskConfig( - name="vel_go2", - type="velocity", - joint_names=_go2_joints, - priority=20, - params={"zero_on_timeout": False}, - ), - # Closed-loop path follower used by the benchmark tool. - # Inactive until the tool RPCs configure(...) + start_path(...). - TaskConfig( - name="path_follower", - type="path_follower", - joint_names=_go2_joints, - priority=10, - ), - # RG-arm path follower — same control law as path_follower - # but owns its own solve_profile() recompute reacting to - # KeyboardTeleop's e_max stream. artifact_path is the - # tuning JSON the task loads on start_path() for the plant - # model + velocity-profile constants; - TaskConfig( - name="precision_follower", - type="precision_path_follower", - joint_names=_go2_joints, - priority=10, - params={ - "artifact_path": str( - get_project_root() / "data" / "characterization" / "go2" / "" - ), - }, - ), - ], - ), - ) - .remappings( - [ - (GO2Connection, "cmd_vel", "go2_cmd_vel"), - (GO2Connection, "odom", "go2_odom"), - ] - ) - .transports( - { - ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), - ("go2_cmd_vel", Twist): LCMTransport("/go2/cmd_vel", Twist), - ("go2_odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } + +def _make_coordinator(mode: str = "default"): + """Build a coordinator blueprint with the Go2 firmware in the given + gait mode. ``mode="rage"`` toggles FsmRageMode on at startup; any + other string passes through unmodified. + + The ``precision_follower`` task's ``artifact_path`` points at the + ``data/characterization/go2/`` directory by default. Override the + specific file per run with: + -o coordinator.tasks[2].params.artifact_path= + """ + return ( + autoconnect( + GO2Connection.blueprint(mode=mode), + ControlCoordinator.blueprint( + # publish_joint_state=False: the 100Hz JointState publish + # on the shared LCM multicast bus saturates the kernel + # rmem buffer in ~20s and drops large lidar packets first. + # Matches the r1pro README note about heavy sensor UDP + # starving control traffic — same problem in reverse on + # our coord. Re-enable only if a downstream consumer + # actually needs joint_state on LCM, and consider + # throttling the publish rate or routing it via a + # dedicated LCM URL. + publish_joint_state=False, + hardware=[ + HardwareComponent( + hardware_id="go2", + hardware_type=HardwareType.BASE, + joints=_go2_joints, + adapter_type="transport_lcm", + ), + ], + tasks=[ + TaskConfig( + name="vel_go2", + type="velocity", + joint_names=_go2_joints, + priority=20, + params={"zero_on_timeout": False}, + ), + # Closed-loop path follower used by the benchmark tool. + # Inactive until the tool RPCs configure(...) + start_path(...). + TaskConfig( + name="path_follower", + type="path_follower", + joint_names=_go2_joints, + priority=10, + ), + # RG-arm path follower — same control law as path_follower + # but owns its own solve_profile() recompute reacting to + # KeyboardTeleop's e_max stream. artifact_path is the + # tuning JSON the task loads on start_path() for the plant + # model + velocity-profile constants; + TaskConfig( + name="precision_follower", + type="precision_path_follower", + joint_names=_go2_joints, + priority=10, + params={ + "artifact_path": str( + get_project_root() + / "data" + / "characterization" + / "go2" + / "go2_config_hw_concrete_2026-05-28_normal.json" + ), + "speed": 1.4, + "v_max_override": 1.4, + }, + ), + ], + ), + ) + .remappings( + [ + (GO2Connection, "cmd_vel", "go2_cmd_vel"), + (GO2Connection, "odom", "go2_odom"), + ] + ) + .transports( + { + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + ("go2_cmd_vel", Twist): LCMTransport("/go2/cmd_vel", Twist), + ("go2_odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } + ) + .global_config(obstacle_avoidance=False) ) - .global_config(obstacle_avoidance=False) -) -__all__ = ["unitree_go2_coordinator"] + +unitree_go2_coordinator = _make_coordinator(mode="default") +unitree_go2_coordinator_rage = _make_coordinator(mode="rage") + +__all__ = ["unitree_go2_coordinator", "unitree_go2_coordinator_rage"] diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py index ad448d7d7b..ae4054493c 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -79,7 +79,6 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels import VoxelGridMapper @@ -92,45 +91,53 @@ from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, + unitree_go2_coordinator_rage, ) from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder from dimos.utils.path_utils import get_project_root -from dimos.visualization.vis_module import vis_module - -unitree_go2_precision_nav = ( - autoconnect( - unitree_go2_coordinator, - vis_module(viewer_backend=global_config.viewer), - KeyboardTeleop.blueprint( - publish_only_when_active=True, - disable_movement=True, # 0-9 e_max slider only; no WASD Twist - ), - VoxelGridMapper.blueprint(emit_every=5), - CostMapper.blueprint(), - ReplanningAStarPlanner.blueprint(), - CharacterizationRecorder.blueprint( - robot_id="go2", - tag="precision_nav", - out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), - ), - ) - .transports( - { - # KeyboardTeleop 0-9 -> coord.e_max -> precision_follower.set_e_max. - ("e_max", Float32): LCMTransport("/e_max", Float32), - # ReplanningAStarPlanner.path -> coord.path -> precision_follower.set_path. - ("path", Path): LCMTransport("/precision_nav/path", Path), - # Recorder taps — same conventions as B1/B2. - ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - # KeyboardTeleop's gate stream stays available for any tool that - # wants ENTER/K/Backspace; no consumer in B3 today. - ("gate", Int8): LCMTransport("/precision_nav/gate", Int8), - } + + +def _make(coord, gait_tag: str): + return ( + autoconnect( + coord, + # vis_module DISABLED for the lidar-dropout bisect. RerunBridge + # subscribes_all to LCM and forwards everything to the rerun + # gRPC sink; if the viewer is on software GL (DRI3 error in + # logs) the sink drains slowly, blocking the shared LCM + # callback thread and starving lidar publish. + # vis_module(viewer_backend=global_config.viewer), + KeyboardTeleop.blueprint( + publish_only_when_active=True, + disable_movement=True, # 0-9 e_max slider only; no WASD Twist + ), + VoxelGridMapper.blueprint(emit_every=5), + CostMapper.blueprint(), + ReplanningAStarPlanner.blueprint(), + CharacterizationRecorder.blueprint( + robot_id="go2", + tag=f"precision_nav_{gait_tag}", + out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), + ), + ) + .transports( + { + ("e_max", Float32): LCMTransport("/e_max", Float32), + ("path", Path): LCMTransport("/precision_nav/path", Path), + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), + ("gate", Int8): LCMTransport("/precision_nav/gate", Int8), + } + ) + .global_config(n_workers=10, robot_model="unitree_go2") ) - .global_config(n_workers=10, robot_model="unitree_go2") -) -__all__ = ["unitree_go2_precision_nav"] + +unitree_go2_precision_nav = _make(unitree_go2_coordinator, gait_tag="default") +# Rage variant — pair with a rage-mode artifact so the precision +# follower's plant model + envelope match the gait it's tracking. +unitree_go2_precision_nav_rage = _make(unitree_go2_coordinator_rage, gait_tag="rage") + +__all__ = ["unitree_go2_precision_nav", "unitree_go2_precision_nav_rage"] From 201d19983f931e4ea26c489b1019db34a8c4223e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 29 May 2026 12:07:04 -0700 Subject: [PATCH 44/51] updated readme --- .../benchmarking/reports/tuning_README.md | 187 ++++++------------ 1 file changed, 62 insertions(+), 125 deletions(-) diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index 43aef373fa..a2e81866f0 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -25,36 +25,38 @@ Window must have focus. **WASD/QE** = reposition. **Enter** = advance. **K** = s dimos run unitree-go2-characterization ``` -Per axis (vx, vy, wz on Go2), the SI loop runs **three phases** with one -ENTER per step: - -1. **Floor probe** (~4 tiny amps, e.g. vx 0.02→0.15 m/s) — the AND-test - (D3) detects the smallest commanded value that actually moves the - robot (`|v_body| > 0.02 m/s` **AND** `> 5% of |amp|`, sustained for - ≥5 samples). -2. **Dense sweep** (5 amps, unified `[0.2, 0.5, 1.0, 1.5, 2.0]` ladder - in m/s for vx/vy and rad/s for wz) — FOPDT fit per amplitude; the - canonical fit used by DERIVE is the lowest-amplitude one with - r² > 0.9 (the **linear regime**, methodology v2 — D1). -3. **Ceiling probe** (~2 supra-sweep amps, e.g. vx 2.5, 3.0) — FOPDT - fit per amplitude. **The operational ceiling DERIVE uses is - `min(max(|K(amp)·amp|), profile.{vx,wz}_max)` across the FULL sweep - + probe table** — i.e. the max steady-state OUTPUT magnitude the - plant can deliver, clamped to the platform cap. This is robust to - noisy FOPDT fits: a misfit K with the right amp still gives a - sensible K·amp because the output magnitude is what matters for RG, - not K alone. The K-sag amplitude (where K drops 15% below the - linear-regime K) is still recorded as `saturating_at_amp` in the - artifact for forensics, but is **not** used as the operational cap - — with low-r² fits the K-sag detector misfires. +Per axis (vx, vy, wz on Go2), the SI loop runs three phases. One ENTER +per step. *"amp"* below = step amplitude (m/s for vx/vy, rad/s for wz). + +``` +for ch in (vx, vy, wz): + floor_probe(amps=[0.02, 0.05, 0.10, 0.15]) # ~4 tiny amplitudes + dense_sweep(amps=[0.2, 0.5, 1.0, 1.5, 2.0]) # 5 amplitudes, FOPDT fit each + ceiling_probe(amps=[2.5, 3.0]) # ~2 supra-sweep amplitudes +``` + +**Floor probe** — smallest amplitude that actually moves the robot: +- AND-test (D3): `|v_body| > 0.02 m/s` **AND** `> 5%·|amp|` +- Sustained for ≥5 samples + +**Dense sweep** — fit the linear-regime FOPDT: +- One FOPDT fit per amp. +- Canonical `K` = lowest amp with `r² > 0.9` (methodology v2 / D1). + +**Ceiling probe** — max output the plant can actually deliver: +- Operational ceiling = `min(max(|K(amp)·amp|), profile.{vx,wz}_max)` +- Computed across the FULL sweep + ceiling-probe table. +- Uses output magnitude (`K·amp`), not `K` alone — robust to noisy fits. +- K-sag amp (where K drops 15% below linear K) saved as `saturating_at_amp` for forensics, **not** the cap. Settling pre-roll (~1.5 s) happens *after* you press Enter — wait for it. Each phase prints a banner so you know which sub-test is running. -**Duration**: ~30–45 min hands-on for the full dense flow on Go2 -(~32 ENTERs: 4 floor + 5 sweep + 2 ceiling per channel × 3 channels, -minus one floor amp on vy). Per-step gating is preserved — drift is -bounded to one step. The cost is "do it once, properly." +**Space requirements**: +- Open, spacious area. +- A long corridor or basketball-court-sized space works. +- Each step needs clear run-out — several metres for high-amplitude vx. +- Cramped rooms force `--max-dist` cuts that bias the fit. **Output** (same timestamp suffix): @@ -65,68 +67,22 @@ data/characterization/go2/ └── go2_recording__.db ← raw streams (cmd_vel, joint_state, odom, gate) ``` -**PNG layout** (four rows × one column per channel; single y-axis per -subplot — easier to read than the old twin-axis version): - -- Step response — measured (solid) + raw (dotted, when Hampel touched - it) + fitted FOPDT (dashed). Skipped on re-derive (no raw traces in - the artifact). -- **K(amp)** — gain per amplitude. Sweep (○-line, blue), ceiling probe - (■-line, orange). Floor/ceiling marked as vertical dotted lines - (green/red). -- **Output K·amp(amp)** — **the steady-state output magnitude the - plant delivers.** This is the most diagnostic panel: it plateaus - visually where the plant saturates. The operational ceiling is the - max of this curve (clamped to the platform cap). -- **τ(amp)** — first-order time constant per amplitude. - -**Artifact sections** (additive — older v1 readers ignore the new -ones): - -- 1–4 + 6 (existing): plant, feedforward, velocity_profile, - recommended_controller, valid_for_tuning. -- 5 **`velocity_envelope`** (new) — per-channel `{floor, ceiling, - floor_not_found, ceiling_not_found}`. -- 7 **`dynamics_by_amplitude`** (new) — full K/τ/L table per channel - across sweep + ceiling-probe amps (forensics + future lookup-based - RG without re-collecting data). -- `floor_probe_results` (new sibling) — per-amp D3 pass/fail. -- `provenance.methodology_version` = 2 (defaults to 1 on legacy - artifacts); `schema_version` is unchanged at 1. - -**DERIVE** consumes the measured envelope: - -- RG `min_speed` = `max(envelope.vx.floor, profile.min_speed_floor)` -- RG `v_max` = `envelope.vx.ceiling` (already clamped to `profile.vx_max`) -- RG `ω_max` = `envelope.wz.ceiling · 0.85` (15% headroom for cross-track) -- FF gain = `1/K` from the linear-regime fit (unchanged). -- `floor_not_found` / `ceiling_not_found` only fire when there is no - data at all (empty probe / no converged fits). Clamping to the - platform cap is silent (it's an envelope decision, not a measurement - failure). - -### Re-derive a stale artifact - -If the methodology changed (e.g. ceiling logic was bug-fixed) and you -don't want to re-run on the robot, re-apply the current logic to an -existing v2 artifact: - -``` -python -m dimos.utils.benchmarking.characterization \ - --mode re-derive --robot go2 \ - --artifact data/characterization/go2/go2_config_hw_concrete__.json -``` - -The tool reads the artifact's stored `dynamics_by_amplitude` + -`floor_probe_results`, recomputes envelope + velocity_profile + -caveats, and writes `__rederived.json` + matching PNG alongside -the source. Plant + feedforward pass through unchanged (those were -already measured). No robot needed. - -Override defaults with `-o characterizer.=`, e.g.: -- `-o characterizer.surface=grass` -- `-o characterizer.step_s=10` -- `-o characterizer.savgol_window=15` (more aggressive gait smoothing) +The `.json` is the **tuning artifact** — the single versioned source of +truth handed off to Step 2 (benchmark) and Step 3 (precision-nav). +Inside: + +- `provenance` — robot_id, surface, gait mode, date, git sha, hw vs self-test. +- `plant` — fitted FOPDT `{K, τ, L}` per axis (vx, vy, wz). Canonical K = linear-regime fit (lowest amp with r²>0.9). +- `dynamics_by_amplitude` — the full per-amp `K(amp), τ(amp), L(amp), r²` table (sweep + ceiling probe). What lets future controllers interpolate without re-running. +- `floor_probe_results` — per-amp AND-test pass/fail and sustained-sample count for the floor sweep. +- `velocity_envelope` — measured floor + ceiling per channel (`saturating_at_amp` is forensic-only). +- `feedforward` — `1/K_linear` per axis + output clamps. +- `velocity_profile` — curvature speed profile (max linear/angular speed, accel, decel, lookahead). +- `recommended_controller` — baseline P-controller + plant-floor evidence string. +- `operating_point_map` — `null` until Step 2 fills it in. +- `caveats` — validity scope; self-test artifacts lead with a loud DO-NOT-TUNE banner. +- `valid_for_tuning` — `true` only for hw mode. `false` ⇒ refuse to apply. +- `schema_version` — bumped on breaking artifact changes. ## Step 2 — benchmark @@ -138,21 +94,22 @@ dimos run unitree-go2-benchmark-rg \ -o benchmarker.config=/path/to/go2_config_hw_*.json # RG arm baked in (rg=true) ``` -Each comparison arm is its own blueprint variant; the `-o` overrides -are reserved for runtime knobs only (the artifact path, e_max sweeps, -etc.). +**Blueprint `-o` overrides**: +- `-o` is for runtime knobs only (artifact path, e_max sweeps). -Per (path, speed) × fixed path set (`straight_line`, `single_corner`, `square`, `circle`), the loop prompts you to aim the robot, then drives the baseline follower over the anchored path. CTE is scored from the real trajectory. - -**Comparison arms** (each measures *against* the bare physical limit): +**Per-run loop** — for each `(path, speed)`: +- Fixed path set: `straight_line`, `single_corner`, `square`, `circle`. +- CTE scored from the real trajectory. +**Comparison arms** (each measured *against* the bare physical limit): - `-o benchmarker.ff=true` — apply derived feedforward. - `-o benchmarker.profile=true` — apply derived curvature velocity profile. -- `-o benchmarker.rg=true` `-o benchmarker.e_max=0.20` — apply reference governor's per-waypoint cap (precision = `e_max / max(τ+L)`). - -For the RG arm, **the Go2 stalls below ~0.2 m/s commanded velocity** even when the math says go slower. `benchmarker.min_speed` (default `0.2`) is the floor; set to `None` to defer to the artifact's value. Tight corners against tight `e_max` will track badly because saturation-binding cells get floored above ω_max — that's a physical limit, not a bug. +- `-o benchmarker.rg=true -o benchmarker.e_max=0.20` — apply reference-governor per-waypoint cap (precision = `e_max / max(τ+L)`). -**Duration**: ~15–25 min depending on speeds × arms. +**RG arm caveats**: +- Go2 stalls below ~0.2 m/s commanded velocity, even when math says slower. +- `benchmarker.min_speed` (default `0.2`) is the floor; set `None` to use the artifact's value. +- Tight corners + tight `e_max` track badly — saturation-binding cells get floored above ω_max. Physical limit, not a bug. **Output**: @@ -169,35 +126,19 @@ data/benchmark/go2/ dimos run unitree-go2-precision-nav ``` -Bundles the Go2 coord + rerun viewer + click-to-goal planning chain -(VoxelGridMapper + CostMapper + ReplanningAStarPlanner) + KeyboardTeleop -(0-9 e_max slider, WASD/QE disabled) + per-session recorder. Stock Go2 -hardware (L1 lidar, GO2Connection odom) is sufficient — **no Mid360 / -FastLio2 required**. One terminal. - -Composition mirrors the working smart-tier Go2 nav blueprint and swaps -only the actuation seam: instead of `MovementManager` mixing -`nav_cmd_vel` into `cmd_vel` and driving GO2Connection directly, -`ReplanningAStarPlanner.path` flows into `ControlCoordinator.path`, the -coord broadcasts `set_path(path, odom)` to the `precision_follower` -task, and the precision controller drives the robot via the coord's -tick loop. +Go2 coord + rerun + (VoxelGrid + CostMapper + ReplanningAStarPlanner) + KeyboardTeleop (0-9 e_max slider) + recorder. Operator flow: -1. Open rerun (auto-launches with the blueprint). -2. Click a point on the map. `RerunWebSocketServer` emits a - `clicked_point: PointStamped`. -3. `ReplanningAStarPlanner` consumes the click + the live costmap (from - `VoxelGridMapper → CostMapper`) + GO2Connection's `odom: PoseStamped` - and emits `path: Path`. -4. `ControlCoordinator.path` receives the planned path; `_on_path` +1. Open rerun (auto-launches with the blueprint). Click a point on the map. +2. `ReplanningAStarPlanner` consumes the click and emits `path: Path`. +3. `ControlCoordinator.path` receives the planned path; `_on_path` snapshots the latest odom and broadcasts `set_path(path, odom)` to any task with the method. -5. `PrecisionPathFollowerTask.set_path` delegates to its existing +4. `PrecisionPathFollowerTask.set_path` delegates to its existing `start_path` (which lazy-loads the artifact, solves `solve_profile()`, and starts the state machine). -6. The coord's tick loop drives the precision controller's velocity +5. The coord's tick loop drives the precision controller's velocity commands to GO2Connection. Live-tune precision: keys **0-9** in the pygame window set corridor @@ -213,13 +154,9 @@ data/precision_nav/go2/ **Hardware notes**: -- L1 lidar has narrow FOV — the costmap is sparser than what Mid360 - would provide. Quality of click-to-plan paths is limited accordingly. - Mid360 + FastLio2 is the upgrade path if you want the proper - nav-stack pipeline (see `unitree-g1-nav-onboard` for that variant). - `ReplanningAStarPlanner.nav_cmd_vel: Out[Twist]` is intentionally unwired — the precision controller, not the planner, drives the - robot. The planner is only used as a path source. + robot. **The planner is only used as a path source.** ## Reading recordings From cf90bbd09d7dbbd7f03507f510dc7b3b08a26f67 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 29 May 2026 12:21:45 -0700 Subject: [PATCH 45/51] characterization artifact ands benchmark ata pushed to lfs for testing --- data/.lfs/benchmark.tar.gz | 3 +++ data/.lfs/characterization.tar.gz | 3 +++ 2 files changed, 6 insertions(+) create mode 100644 data/.lfs/benchmark.tar.gz create mode 100644 data/.lfs/characterization.tar.gz diff --git a/data/.lfs/benchmark.tar.gz b/data/.lfs/benchmark.tar.gz new file mode 100644 index 0000000000..53e673739b --- /dev/null +++ b/data/.lfs/benchmark.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e19878220c6f0a01aece13c20b0308e855cbe301531b03c3984ff308cb19d141 +size 10960393 diff --git a/data/.lfs/characterization.tar.gz b/data/.lfs/characterization.tar.gz new file mode 100644 index 0000000000..6956882d10 --- /dev/null +++ b/data/.lfs/characterization.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7be28198fd313c0e809e32a93e8cb91ebda7ba12087064374ec3fc135028f0df +size 9698275 From b94cd527cc26e316f54604abecf29f83d6a8d893 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 2 Jun 2026 21:23:23 -0700 Subject: [PATCH 46/51] added odom to path follower task for updated state --- dimos/control/coordinator.py | 45 +++++++++++++++++-- .../precision_path_follower_task.py | 27 ++++++----- .../basic/unitree_go2_coordinator.py | 11 +---- .../basic/unitree_go2_precision_nav.py | 21 ++++----- 4 files changed, 68 insertions(+), 36 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 87ea674968..9107104a66 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -543,12 +543,51 @@ def _on_e_max(self, msg: Float32) -> None: set_e_max(value) def _on_path(self, msg: Path) -> None: - """Forward a planned path to any task with set_path(path).""" + """Forward a planned path + a fresh odom snapshot to any task with + ``set_path(path, odom)``. + + TODO: upgrade to option C — add an always-called ``update_state(state)`` + hook on ``BaseControlTask`` that ``TickLoop._compute_all_tasks`` + invokes BEFORE the ``is_active()`` filter. Then tasks own their + own odom snapshot every tick regardless of active state, and the + ``set_path`` API can drop the ``odom`` arg. Cleaner architecturally + — current odom-via-set_path is a surgical fix for the immediate + race. See PR/issue [TODO link] for the framework-level change. + """ + odom = 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.PoseStamped import PoseStamped + from dimos.msgs.geometry_msgs.Quaternion import Quaternion + from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + odom = 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]))), + ) + break + with self._task_lock: for task in self._tasks.values(): set_path = getattr(task, "set_path", None) - if set_path is not None: - set_path(msg) + if set_path is None: + continue + try: + set_path(msg, odom) + except TypeError: + set_path(msg) # backwards compat with single-arg set_path @rpc def set_activated(self, engaged: bool) -> None: diff --git a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py index a49476556e..6fae0cb795 100644 --- a/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py +++ b/dimos/control/tasks/precision_path_follower_task/precision_path_follower_task.py @@ -99,24 +99,31 @@ def set_e_max(self, value: float) -> None: if self._path is not None: self._recompute_profile() - def set_path(self, path: Path) -> None: + def set_path(self, path: Path, odom: PoseStamped | None = None) -> None: """Coordinator broadcast hook for nav-stack-emitted paths. - Pulls the latest odom from ``self._current_odom`` (populated by - the parent task's compute() every tick from CoordinatorState). - If no compute tick has fired yet, ``_current_odom`` is None and - we drop the path with a warning — the race window is one tick - wide at startup and shouldn't fire in practice.""" - if self._current_odom is None: + + Prefers the caller-supplied ``odom`` (the coord snapshots a fresh + one from the twist-base adapter every time it calls us — see + ``ControlCoordinator._on_path``). Falls back to + ``self._current_odom`` for backwards compatibility with callers + that still use the single-arg form. + + TODO: drop the ``odom`` arg once option C lands (always-called + ``update_state(state)`` hook on ``BaseControlTask``), at which + point ``self._current_odom`` is reliable on its own and the coord + doesn't need to push it. See ``_on_path`` for context.""" + use_odom = odom if odom is not None else self._current_odom + if use_odom is None: logger.warning( f"PrecisionPathFollowerTask '{self._name}': received path " - f"but no odom yet; dropping (race at startup)." + f"but no odom available; dropping." ) return logger.info( f"PrecisionPathFollowerTask '{self._name}': received path " f"from stream (n={len(path.poses)})" ) - self.start_path(path, self._current_odom) + self.start_path(path, use_odom) # ------------------------------------------------------------------ # Path lifecycle @@ -209,7 +216,7 @@ class PrecisionPathFollowerTaskParams(BaseConfig): artifact_path: str speed: float = 0.55 control_frequency: float = 10.0 - goal_tolerance: float = 0.05 + goal_tolerance: float = 0.2 orientation_tolerance: float = 0.1 k_angular: float = 0.5 e_max_default: float = 0.2 diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index e68b316ab2..70e5d1a98a 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -61,16 +61,7 @@ def _make_coordinator(mode: str = "default"): autoconnect( GO2Connection.blueprint(mode=mode), ControlCoordinator.blueprint( - # publish_joint_state=False: the 100Hz JointState publish - # on the shared LCM multicast bus saturates the kernel - # rmem buffer in ~20s and drops large lidar packets first. - # Matches the r1pro README note about heavy sensor UDP - # starving control traffic — same problem in reverse on - # our coord. Re-enable only if a downstream consumer - # actually needs joint_state on LCM, and consider - # throttling the publish rate or routing it via a - # dedicated LCM URL. - publish_joint_state=False, + publish_joint_state=True, hardware=[ HardwareComponent( hardware_id="go2", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py index ae4054493c..6575bde1d9 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -79,6 +79,7 @@ from __future__ import annotations from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels import VoxelGridMapper @@ -94,20 +95,14 @@ unitree_go2_coordinator_rage, ) from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop -from dimos.utils.benchmarking.characterization_recorder import CharacterizationRecorder -from dimos.utils.path_utils import get_project_root +from dimos.visualization.vis_module import vis_module def _make(coord, gait_tag: str): return ( autoconnect( coord, - # vis_module DISABLED for the lidar-dropout bisect. RerunBridge - # subscribes_all to LCM and forwards everything to the rerun - # gRPC sink; if the viewer is on software GL (DRI3 error in - # logs) the sink drains slowly, blocking the shared LCM - # callback thread and starving lidar publish. - # vis_module(viewer_backend=global_config.viewer), + vis_module(viewer_backend=global_config.viewer), KeyboardTeleop.blueprint( publish_only_when_active=True, disable_movement=True, # 0-9 e_max slider only; no WASD Twist @@ -115,11 +110,11 @@ def _make(coord, gait_tag: str): VoxelGridMapper.blueprint(emit_every=5), CostMapper.blueprint(), ReplanningAStarPlanner.blueprint(), - CharacterizationRecorder.blueprint( - robot_id="go2", - tag=f"precision_nav_{gait_tag}", - out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), - ), + # CharacterizationRecorder.blueprint( + # robot_id="go2", + # tag=f"precision_nav_{gait_tag}", + # out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), + # ), ) .transports( { From 320dfe000741eb5437ab26e17c3e105415f54d73 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 3 Jun 2026 12:08:36 -0700 Subject: [PATCH 47/51] updated readme --- .../benchmarking/reports/tuning_README.md | 170 +++++------------- 1 file changed, 45 insertions(+), 125 deletions(-) diff --git a/dimos/utils/benchmarking/reports/tuning_README.md b/dimos/utils/benchmarking/reports/tuning_README.md index a2e81866f0..d73da8de79 100644 --- a/dimos/utils/benchmarking/reports/tuning_README.md +++ b/dimos/utils/benchmarking/reports/tuning_README.md @@ -1,23 +1,20 @@ -# Twist-base controller tuning — operator guide +# Twist-base controller tuning -Two blueprints, run in order: +Three blueprints. Run in order. -1. **`unitree-go2-characterization`** — measures the robot's FOPDT plant, writes a config JSON. -2. **`unitree-go2-benchmark`** — runs the baseline controller across a speed ladder, scores CTE, writes the operating-point map. +1. `unitree-go2-characterization` — drives the robot, fits a velocity model per axis, writes a JSON. +2. `unitree-go2-benchmark` — replays the JSON against fixed test paths, scores path error, writes a results table. +3. `unitree-go2-precision-nav` — click-to-goal nav using both, with live keys to tighten/loosen path tracking. -Both bundle GO2Connection + ControlCoordinator + pygame teleop + the relevant module + a per-session SQLite recorder. One terminal each. +Each is one terminal. Pygame window needs focus; `WASD/QE` reposition, `Enter` advance, `K` skip, `Backspace` quit. -## Prerequisites +## Prereqs -1. From an X11 desktop terminal: - ``` - cd ~/dimos && source .venv/bin/activate - ``` -2. Robot powered on, network reachable. Clear ~3m × 3m of floor space. - -## Pygame controls (both blueprints) +``` +cd ~/dimos && source .venv/bin/activate +``` -Window must have focus. **WASD/QE** = reposition. **Enter** = advance. **K** = skip. **Backspace** = quit. +Robot on, network reachable. Step 1 wants a corridor or basketball-court-sized space (high-amplitude steps cover several meters before settling). Steps 2-3 are fine in ~3m × 3m. ## Step 1 — characterization @@ -25,155 +22,78 @@ Window must have focus. **WASD/QE** = reposition. **Enter** = advance. **K** = s dimos run unitree-go2-characterization ``` -Per axis (vx, vy, wz on Go2), the SI loop runs three phases. One ENTER -per step. *"amp"* below = step amplitude (m/s for vx/vy, rad/s for wz). +Per axis (`vx`, `vy`, `wz`), three loops with one Enter per step: ``` -for ch in (vx, vy, wz): - floor_probe(amps=[0.02, 0.05, 0.10, 0.15]) # ~4 tiny amplitudes - dense_sweep(amps=[0.2, 0.5, 1.0, 1.5, 2.0]) # 5 amplitudes, FOPDT fit each - ceiling_probe(amps=[2.5, 3.0]) # ~2 supra-sweep amplitudes +floor_probe(amps=[0.02, 0.05, 0.10, 0.15]) # smallest command that actually moves the robot +dense_sweep(amps=[0.2, 0.5, 1.0, 1.5, 2.0]) # main fit: per-amp first-order-plus-deadtime (FOPDT) model +ceiling_probe(amps=[2.5, 3.0]) # over-the-top, measures the real top output ``` -**Floor probe** — smallest amplitude that actually moves the robot: -- AND-test (D3): `|v_body| > 0.02 m/s` **AND** `> 5%·|amp|` -- Sustained for ≥5 samples - -**Dense sweep** — fit the linear-regime FOPDT: -- One FOPDT fit per amp. -- Canonical `K` = lowest amp with `r² > 0.9` (methodology v2 / D1). - -**Ceiling probe** — max output the plant can actually deliver: -- Operational ceiling = `min(max(|K(amp)·amp|), profile.{vx,wz}_max)` -- Computed across the FULL sweep + ceiling-probe table. -- Uses output magnitude (`K·amp`), not `K` alone — robust to noisy fits. -- K-sag amp (where K drops 15% below linear K) saved as `saturating_at_amp` for forensics, **not** the cap. +"Amp" = the velocity command magnitude for one step. m/s for `vx`/`vy`, rad/s for `wz`. -Settling pre-roll (~1.5 s) happens *after* you press Enter — wait for -it. Each phase prints a banner so you know which sub-test is running. +The canonical model used downstream is the lowest-amplitude fit with `r² > 0.9` (linear regime, before the actuator saturates). The operational ceiling is the max steady-state output velocity actually reached, clamped to the platform's hard envelope. -**Space requirements**: -- Open, spacious area. -- A long corridor or basketball-court-sized space works. -- Each step needs clear run-out — several metres for high-amplitude vx. -- Cramped rooms force `--max-dist` cuts that bias the fit. +After Enter the tool waits ~1.5s before commanding the step (settling). Each phase prints a banner. -**Output** (same timestamp suffix): +### Output ``` data/characterization/go2/ -├── go2_config_hw_concrete__.json ← the tuning artifact -├── go2_config_hw_concrete__.png ← fit-quality plot + K(amp) panel -└── go2_recording__.db ← raw streams (cmd_vel, joint_state, odom, gate) +├── go2_config_hw_concrete__.json # the artifact (Step 2 + 3 read this) +├── go2_config_hw_concrete__.png # fit-quality plot +└── go2_recording__.db # raw streams ``` -The `.json` is the **tuning artifact** — the single versioned source of -truth handed off to Step 2 (benchmark) and Step 3 (precision-nav). -Inside: - -- `provenance` — robot_id, surface, gait mode, date, git sha, hw vs self-test. -- `plant` — fitted FOPDT `{K, τ, L}` per axis (vx, vy, wz). Canonical K = linear-regime fit (lowest amp with r²>0.9). -- `dynamics_by_amplitude` — the full per-amp `K(amp), τ(amp), L(amp), r²` table (sweep + ceiling probe). What lets future controllers interpolate without re-running. -- `floor_probe_results` — per-amp AND-test pass/fail and sustained-sample count for the floor sweep. -- `velocity_envelope` — measured floor + ceiling per channel (`saturating_at_amp` is forensic-only). -- `feedforward` — `1/K_linear` per axis + output clamps. -- `velocity_profile` — curvature speed profile (max linear/angular speed, accel, decel, lookahead). -- `recommended_controller` — baseline P-controller + plant-floor evidence string. -- `operating_point_map` — `null` until Step 2 fills it in. -- `caveats` — validity scope; self-test artifacts lead with a loud DO-NOT-TUNE banner. -- `valid_for_tuning` — `true` only for hw mode. `false` ⇒ refuse to apply. -- `schema_version` — bumped on breaking artifact changes. +The `.json` carries `plant` (the fitted FOPDT), `dynamics_by_amplitude` (full per-amp table for interpolation), `velocity_envelope` (floor/ceiling per axis), `feedforward` (`1/K` per axis), `velocity_profile` (curvature-aware speed profile), and `valid_for_tuning` (`false` for self-test → downstream tools refuse it). ## Step 2 — benchmark ``` -dimos run unitree-go2-benchmark \ - -o benchmarker.config=/path/to/go2_config_hw_*.json # bare baseline - -dimos run unitree-go2-benchmark-rg \ - -o benchmarker.config=/path/to/go2_config_hw_*.json # RG arm baked in (rg=true) +dimos run unitree-go2-benchmark -o benchmarker.config=.json +dimos run unitree-go2-benchmark-rg -o benchmarker.config=.json ``` -**Blueprint `-o` overrides**: -- `-o` is for runtime knobs only (artifact path, e_max sweeps). - -**Per-run loop** — for each `(path, speed)`: -- Fixed path set: `straight_line`, `single_corner`, `square`, `circle`. -- CTE scored from the real trajectory. +Replays fixed paths (`straight_line`, `single_corner`, `square`, `circle`) at multiple speeds, scores cross-track error. Bare baseline by default. Enable comparison arms one at a time: -**Comparison arms** (each measured *against* the bare physical limit): -- `-o benchmarker.ff=true` — apply derived feedforward. -- `-o benchmarker.profile=true` — apply derived curvature velocity profile. -- `-o benchmarker.rg=true -o benchmarker.e_max=0.20` — apply reference-governor per-waypoint cap (precision = `e_max / max(τ+L)`). +- `-o benchmarker.ff=true` — adds the derived feedforward. +- `-o benchmarker.profile=true` — adds the curvature speed profile. +- `-o benchmarker.rg=true -o benchmarker.e_max=0.20` — adds the reference-governor per-waypoint cap (path-tracking budget = `e_max` meters off the path). -**RG arm caveats**: -- Go2 stalls below ~0.2 m/s commanded velocity, even when math says slower. -- `benchmarker.min_speed` (default `0.2`) is the floor; set `None` to use the artifact's value. -- Tight corners + tight `e_max` track badly — saturation-binding cells get floored above ω_max. Physical limit, not a bug. +The `-rg` blueprint is the same thing with RG enabled by default. The Go2 stalls below ~0.2 m/s commanded, so `benchmarker.min_speed` defaults to 0.2; set `None` to use the artifact's floor. -**Output**: - -``` -data/benchmark/go2/ -├── go2_benchmark__.png ← XY trajectory overlay + CTE plot -├── …___.json ← per-arm operating-point map (bare run also appends section 5 to the input artifact) -└── go2_benchmark__.db ← raw streams -``` +Outputs land in `data/benchmark/go2/` (XY plot + per-arm JSON + recording). The bare run also appends an `operating_point_map` to the artifact. -## Step 3 — precision-controlled nav (end-to-end) +## Step 3 — precision-nav ``` dimos run unitree-go2-precision-nav ``` -Go2 coord + rerun + (VoxelGrid + CostMapper + ReplanningAStarPlanner) + KeyboardTeleop (0-9 e_max slider) + recorder. - -Operator flow: - -1. Open rerun (auto-launches with the blueprint). Click a point on the map. -2. `ReplanningAStarPlanner` consumes the click and emits `path: Path`. -3. `ControlCoordinator.path` receives the planned path; `_on_path` - snapshots the latest odom and broadcasts `set_path(path, odom)` to - any task with the method. -4. `PrecisionPathFollowerTask.set_path` delegates to its existing - `start_path` (which lazy-loads the artifact, solves - `solve_profile()`, and starts the state machine). -5. The coord's tick loop drives the precision controller's velocity - commands to GO2Connection. +Bundles the Go2 coord + rerun viewer + voxel/cost/A* planner chain + the pygame window (now repurposed as a 0-9 slider for live corridor width, no WASD). Click a goal in rerun; planner emits a path; coord broadcasts it to the precision follower task, which loads the artifact, solves a per-waypoint speed profile honoring the current `e_max`, and drives the robot. -Live-tune precision: keys **0-9** in the pygame window set corridor -half-width 0.0-0.9 m. `PrecisionPathFollowerTask` re-solves the profile -on each keypress and atomically swaps the per-waypoint cap mid-path. +Keys `0-9` set the corridor half-width 0.0-0.9 m. Each keypress re-solves the profile mid-path. -**Output**: - -``` -data/precision_nav/go2/ -└── go2_precision_nav__.db ← cmd_vel, joint_state, odom, gate -``` - -**Hardware notes**: - -- `ReplanningAStarPlanner.nav_cmd_vel: Out[Twist]` is intentionally - unwired — the precision controller, not the planner, drives the - robot. **The planner is only used as a path source.** +Output: `data/precision_nav/go2/*.db`. ## Reading recordings ```python from dimos.memory2.store.sqlite import SqliteStore from dimos.msgs.sensor_msgs.JointState import JointState -store = SqliteStore(path="<.db file>"); store.start() + +store = SqliteStore(path="<.db>") +store.start() for obs in store.stream("joint_state", JointState): - ts, msg = obs.ts, obs.data # re-fit, plot, etc. + ts, msg = obs.ts, obs.data ``` -Streams: `cmd_vel` (Twist), `joint_state` (JointState, x/y/yaw), `odom` (PoseStamped, raw), `gate` (Int8, operator events). The Step 3 (precision-nav) recording adds nothing new — same schema, different `tag`. +Streams: `cmd_vel`, `joint_state` (positions = x/y/yaw, velocities = commanded body vel), `odom` (raw), `gate` (operator events). ## Troubleshooting -- **Pygame window doesn't open**: X11 not reachable. `xeyes` test, then `export DISPLAY=:1; export XAUTHORITY=/run/user/$(id -u)/.Xauthority`. -- **Enter does nothing**: pygame window isn't focused. Click it before pressing. -- **Terminal flooded with TF warnings**: pipe through `grep --line-buffered -E 'Benchmarker|Characterizer|reject|aborted|arrived|timeout|configure|start_path|required|reposition'`. -- **Robot won't move on RG arm**: see `min_speed` note above. Try `-o benchmarker.e_max=0.2` first; if still stuck, raise `-o benchmarker.min_speed=0.25`. -- **Self-test (no robot)**: keep using the CLI: `uv run python -m dimos.utils.benchmarking.characterization --mode self-test`. +- Pygame won't open → X11 not reachable. `xeyes` test, then `export DISPLAY=:1; export XAUTHORITY=/run/user/$(id -u)/.Xauthority`. +- Enter does nothing → pygame window unfocused; click on it. +- Flooded with TF warnings → pipe through `grep --line-buffered -E 'Benchmarker|Characterizer|reject|aborted|arrived|timeout|configure|start_path|reposition'`. +- RG arm robot won't move → see `min_speed` above. +- No robot? `uv run python -m dimos.utils.benchmarking.characterization --mode self-test` (artifact is stamped `valid_for_tuning=false`). From 2dacbe4dca6227ffd29b23b50e81c568d8d9f57a Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 3 Jun 2026 12:24:18 -0700 Subject: [PATCH 48/51] removed recorder module for precision nav --- .../basic/unitree_go2_precision_nav.py | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py index 6575bde1d9..dd5b90144e 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -48,9 +48,7 @@ ``odom: In[PoseStamped]`` (no SLAM step needed) and ``clicked_point: In[PointStamped]`` from the rerun server. Emits ``path: Out[Path]`` which the coord consumes. -- ``CharacterizationRecorder`` (kept name; ``tag="precision_nav"``) - for the per-session SQLite black box. Lands at - ``/data/precision_nav/go2/go2_precision_nav__.db``. + Wiring (all by-port-name, no remappings): @@ -88,7 +86,6 @@ 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.navigation.replanning_a_star.module import ReplanningAStarPlanner from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, @@ -98,7 +95,7 @@ from dimos.visualization.vis_module import vis_module -def _make(coord, gait_tag: str): +def _make(coord): return ( autoconnect( coord, @@ -110,11 +107,6 @@ def _make(coord, gait_tag: str): VoxelGridMapper.blueprint(emit_every=5), CostMapper.blueprint(), ReplanningAStarPlanner.blueprint(), - # CharacterizationRecorder.blueprint( - # robot_id="go2", - # tag=f"precision_nav_{gait_tag}", - # out_dir=str(get_project_root() / "data" / "precision_nav" / "go2"), - # ), ) .transports( { @@ -123,16 +115,15 @@ def _make(coord, gait_tag: str): ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), ("odom", PoseStamped): LCMTransport("/go2/odom", PoseStamped), - ("gate", Int8): LCMTransport("/precision_nav/gate", Int8), } ) .global_config(n_workers=10, robot_model="unitree_go2") ) -unitree_go2_precision_nav = _make(unitree_go2_coordinator, gait_tag="default") +unitree_go2_precision_nav = _make(unitree_go2_coordinator) # Rage variant — pair with a rage-mode artifact so the precision # follower's plant model + envelope match the gait it's tracking. -unitree_go2_precision_nav_rage = _make(unitree_go2_coordinator_rage, gait_tag="rage") +unitree_go2_precision_nav_rage = _make(unitree_go2_coordinator_rage) __all__ = ["unitree_go2_precision_nav", "unitree_go2_precision_nav_rage"] From 3c8421cae73bec450dd46872a3555a4283aaaf09 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 3 Jun 2026 11:41:00 -0700 Subject: [PATCH 49/51] added oppucancy grid based controller --- .../costmap_precision_governor/module.py | 205 ++++++++++++++++++ .../costmap_precision_governor/test_module.py | 201 +++++++++++++++++ .../basic/unitree_go2_precision_nav.py | 3 + 3 files changed, 409 insertions(+) create mode 100644 dimos/navigation/costmap_precision_governor/module.py create mode 100644 dimos/navigation/costmap_precision_governor/test_module.py diff --git a/dimos/navigation/costmap_precision_governor/module.py b/dimos/navigation/costmap_precision_governor/module.py new file mode 100644 index 0000000000..aa448d20c0 --- /dev/null +++ b/dimos/navigation/costmap_precision_governor/module.py @@ -0,0 +1,205 @@ +# 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. + +"""Costmap-driven autonomous e_max governor. + +Consumes ``/global_costmap`` (``OccupancyGrid``) + ``/go2/odom`` +(``PoseStamped``), measures local clearance around the robot (or a +look-ahead point), maps it to an ``e_max`` corridor half-width via a +piecewise-linear curve, and publishes ``Float32`` on ``e_max``. The +``ControlCoordinator``'s existing ``_on_e_max`` forwarder broadcasts +the value to :meth:`PrecisionPathFollowerTask.set_e_max`, which +re-solves the velocity profile in place. + +Open space → high ``e_max`` (robot drives faster, loose tracking). +Cluttered space → low ``e_max`` (slower, tight tracking). + +Coexists with :class:`KeyboardTeleop`: both publish on the same topic +and the coord forwards the last value verbatim. A keystroke +(``0``-``9``) instantly overrides the auto value; the next costmap +tick reapplies the auto value. +""" + +from __future__ import annotations + +import math +from typing import Any + +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.mapping.occupancy.gradient import gradient +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +# --------------------------------------------------------------------------- +# Pure helpers — testable without instantiating the Module. The Module is +# thin glue around these. +# --------------------------------------------------------------------------- + + +def sample_point(pose: PoseStamped, lookahead_m: float) -> tuple[float, float]: + """World-frame XY where clearance is sampled. + + ``lookahead_m == 0`` → robot pose. ``lookahead_m > 0`` → project that + distance along the robot's yaw heading.""" + if lookahead_m == 0.0: + return pose.position.x, pose.position.y + yaw = pose.orientation.euler[2] + return ( + pose.position.x + lookahead_m * math.cos(yaw), + pose.position.y + lookahead_m * math.sin(yaw), + ) + + +def clearance_to_e_max( + clearance: float, + d_near: float, + d_far: float, + e_max_low: float, + e_max_high: float, +) -> float: + """Piecewise-linear: clamp below d_near, clamp above d_far, lerp between.""" + if clearance <= d_near: + return e_max_low + if clearance >= d_far: + return e_max_high + t = (clearance - d_near) / (d_far - d_near) + return e_max_low + t * (e_max_high - e_max_low) + + +def compute_e_max_from_costmap( + costmap: OccupancyGrid, + pose: PoseStamped, + *, + d_near: float, + d_far: float, + e_max_low: float, + e_max_high: float, + lookahead_m: float, + obstacle_threshold: int, +) -> float | None: + """End-to-end: costmap + pose → e_max (m), or None if the sample + point falls outside the grid. Pure function — testable without a + Module.""" + gradient_grid = gradient(costmap, obstacle_threshold=obstacle_threshold, max_distance=d_far) + sx, sy = sample_point(pose, lookahead_m) + idx = gradient_grid.world_to_grid(Vector3(sx, sy, 0.0)) + ix, iy = int(idx.x), int(idx.y) + if not (0 <= ix < gradient_grid.width and 0 <= iy < gradient_grid.height): + return None + cell = int(gradient_grid.grid[iy, ix]) + clearance = d_far * (1.0 - cell / 100.0) + return clearance_to_e_max(clearance, d_near, d_far, e_max_low, e_max_high) + + +class CostmapPrecisionGovernorConfig(ModuleConfig): + """Knobs for :class:`CostmapPrecisionGovernor`. All in SI units.""" + + # Clearance (m) below which we clamp to the tight-corridor floor. + d_near: float = 0.30 + # Clearance (m) above which we clamp to the open-space ceiling. + d_far: float = 1.50 + # e_max (m) emitted at or below d_near. + e_max_low: float = 0.1 + # e_max (m) emitted at or above d_far. + e_max_high: float = 0.90 + # Minimum |Δe_max| since last publish required to emit again. + # Suppresses thrashing the task's solve_profile() on every costmap. + hysteresis_delta: float = 0.02 + # Sample point offset (m) ahead of the robot along its heading. + # 0 = sample at robot pose; >0 = anticipate corridors before entry. + lookahead_m: float = 0.50 + # Costmap cell value threshold treated as an obstacle by gradient(). + obstacle_threshold: int = 50 + # Emit one value on first costmap so the consuming task isn't stuck + # at its compile-time default. + publish_initial: bool = True + + +class CostmapPrecisionGovernor(Module): + """Autonomous ``e_max`` publisher driven by local costmap clearance. + + Stream contract — fed by the precision-nav blueprint via the same + LCM topics the planner already consumes: + + - ``global_costmap: In[OccupancyGrid]`` — reactive; recompute on + every new map. + - ``odom: In[PoseStamped]`` — latest pose stored, used to anchor + the clearance sample. + - ``e_max: Out[Float32]`` — published when the clearance-derived + e_max changes by more than ``hysteresis_delta``. + + The math reuses :func:`dimos.mapping.occupancy.gradient.gradient`: + one SciPy distance-transform per costmap, then a single cell read + at the sample point. + """ + + config: CostmapPrecisionGovernorConfig + + global_costmap: In[OccupancyGrid] + odom: In[PoseStamped] + e_max: Out[Float32] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._latest_odom: PoseStamped | None = None + self._last_published: float | None = None + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) + self.register_disposable(Disposable(self.global_costmap.subscribe(self._on_costmap))) + + def _on_odom(self, msg: PoseStamped) -> None: + # Cheap callback — just stash; clearance read happens on costmap. + self._latest_odom = msg + + def _on_costmap(self, msg: OccupancyGrid) -> None: + if self._latest_odom is None: + # No pose yet — nothing to anchor the clearance read at. + return + cfg = self.config + new_e_max = compute_e_max_from_costmap( + msg, + self._latest_odom, + d_near=cfg.d_near, + d_far=cfg.d_far, + e_max_low=cfg.e_max_low, + e_max_high=cfg.e_max_high, + lookahead_m=cfg.lookahead_m, + obstacle_threshold=cfg.obstacle_threshold, + ) + if new_e_max is None: + return # Sample point fell outside the grid. + if self._should_publish(new_e_max): + self.e_max.publish(Float32(data=new_e_max)) + self._last_published = new_e_max + + def _should_publish(self, new_e_max: float) -> bool: + if self._last_published is None: + return self.config.publish_initial + return abs(new_e_max - self._last_published) > self.config.hysteresis_delta + + +__all__ = ["CostmapPrecisionGovernor", "CostmapPrecisionGovernorConfig"] diff --git a/dimos/navigation/costmap_precision_governor/test_module.py b/dimos/navigation/costmap_precision_governor/test_module.py new file mode 100644 index 0000000000..3dc080f679 --- /dev/null +++ b/dimos/navigation/costmap_precision_governor/test_module.py @@ -0,0 +1,201 @@ +# 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. + +"""Pure unit tests for the costmap-clutter-governor math. No Module +instantiation, no LCM, no fixture data — synthetic costmaps only.""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest + +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.OccupancyGrid import OccupancyGrid +from dimos.navigation.costmap_precision_governor.module import ( + clearance_to_e_max, + compute_e_max_from_costmap, + sample_point, +) + +# Common config used across tests so the hand-picked numbers line up. +D_NEAR = 0.30 +D_FAR = 1.50 +E_LOW = 0.05 +E_HIGH = 0.90 + + +def _pose(x: float = 0.0, y: float = 0.0, yaw: float = 0.0) -> PoseStamped: + return PoseStamped( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + ) + + +def _free_grid(side_m: float = 5.0, resolution: float = 0.05) -> OccupancyGrid: + """All-FREE grid centred on the world origin.""" + n = int(side_m / resolution) + grid = np.zeros((n, n), dtype=np.int8) + from dimos.msgs.geometry_msgs.Pose import Pose + + origin = Pose(position=Vector3(-side_m / 2.0, -side_m / 2.0, 0.0)) + return OccupancyGrid(grid=grid, resolution=resolution, origin=origin) + + +def _grid_with_obstacle_at( + obstacle_xy: tuple[float, float], + side_m: float = 5.0, + resolution: float = 0.05, +) -> OccupancyGrid: + """Empty grid with a single OCCUPIED cell at the given world point.""" + g = _free_grid(side_m, resolution) + idx = g.world_to_grid(Vector3(obstacle_xy[0], obstacle_xy[1], 0.0)) + g.grid[int(idx.y), int(idx.x)] = 100 + return g + + +# --------------------------------------------------------------------------- +# clearance_to_e_max — pure piecewise-linear curve +# --------------------------------------------------------------------------- + + +@pytest.mark.parametrize( + "clearance, expected", + [ + (0.0, E_LOW), # well below d_near → floor + (D_NEAR, E_LOW), # exactly at d_near → still floor + (D_FAR, E_HIGH), # exactly at d_far → ceiling + (10.0, E_HIGH), # well above d_far → ceiling + ((D_NEAR + D_FAR) / 2.0, (E_LOW + E_HIGH) / 2.0), # midpoint → linear midpoint + ], +) +def test_clearance_to_e_max_piecewise(clearance: float, expected: float) -> None: + got = clearance_to_e_max(clearance, D_NEAR, D_FAR, E_LOW, E_HIGH) + assert got == pytest.approx(expected, abs=1e-9) + + +# --------------------------------------------------------------------------- +# sample_point — lookahead projection along yaw +# --------------------------------------------------------------------------- + + +def test_sample_point_no_lookahead_returns_pose_xy() -> None: + p = _pose(1.0, 2.0, yaw=0.5) + assert sample_point(p, 0.0) == (1.0, 2.0) + + +@pytest.mark.parametrize( + "yaw, expected_dx, expected_dy", + [ + (0.0, 0.5, 0.0), # +x + (math.pi / 2, 0.0, 0.5), # +y + (math.pi, -0.5, 0.0), # -x + (-math.pi / 2, 0.0, -0.5), # -y + ], +) +def test_sample_point_projects_along_yaw( + yaw: float, expected_dx: float, expected_dy: float +) -> None: + p = _pose(0.0, 0.0, yaw=yaw) + sx, sy = sample_point(p, lookahead_m=0.5) + assert sx == pytest.approx(expected_dx, abs=1e-9) + assert sy == pytest.approx(expected_dy, abs=1e-9) + + +# --------------------------------------------------------------------------- +# compute_e_max_from_costmap — end-to-end pure pipeline +# --------------------------------------------------------------------------- + + +def _kwargs(lookahead_m: float = 0.0) -> dict: + return dict( + d_near=D_NEAR, + d_far=D_FAR, + e_max_low=E_LOW, + e_max_high=E_HIGH, + lookahead_m=lookahead_m, + obstacle_threshold=50, + ) + + +def test_open_space_yields_e_max_high() -> None: + # All FREE → clearance saturates at d_far → e_max_high. + grid = _free_grid() + e = compute_e_max_from_costmap(grid, _pose(0.0, 0.0), **_kwargs()) + assert e == pytest.approx(E_HIGH, abs=1e-6) + + +def test_obstacle_at_robot_yields_e_max_low() -> None: + # Obstacle in the SAME cell as the robot → clearance ~0 → e_max_low. + grid = _grid_with_obstacle_at((0.0, 0.0)) + e = compute_e_max_from_costmap(grid, _pose(0.0, 0.0), **_kwargs()) + assert e == pytest.approx(E_LOW, abs=1e-6) + + +def test_lookahead_sees_obstacle_ahead_of_robot() -> None: + # Robot at origin facing +x; obstacle at (1.0, 0) — robot itself is in + # open space but the lookahead point (0.5, 0) is much closer to the + # obstacle than the robot is, so e_max should drop below the open- + # space ceiling. + grid = _grid_with_obstacle_at((1.0, 0.0)) + e_no_lookahead = compute_e_max_from_costmap(grid, _pose(0.0, 0.0), **_kwargs(0.0)) + e_with_lookahead = compute_e_max_from_costmap(grid, _pose(0.0, 0.0, yaw=0.0), **_kwargs(0.5)) + assert e_with_lookahead < e_no_lookahead + + +def test_returns_none_when_sample_point_outside_grid() -> None: + # Grid is 5m wide centred at origin → bounds roughly [-2.5, +2.5]m. + # Pose at (10, 10) is well outside. + grid = _free_grid(side_m=5.0) + e = compute_e_max_from_costmap(grid, _pose(10.0, 10.0), **_kwargs()) + assert e is None + + +# --------------------------------------------------------------------------- +# Hysteresis — exercise _should_publish via a tiny stand-in +# --------------------------------------------------------------------------- + + +class _HysteresisStub: + """Mirrors CostmapPrecisionGovernor._should_publish without pulling in + the whole Module machinery.""" + + def __init__(self, delta: float, publish_initial: bool = True) -> None: + self.delta = delta + self.publish_initial = publish_initial + self.last_published: float | None = None + + def should_publish(self, new_e_max: float) -> bool: + if self.last_published is None: + return self.publish_initial + return abs(new_e_max - self.last_published) > self.delta + + +def test_hysteresis_suppresses_small_changes() -> None: + h = _HysteresisStub(delta=0.02) + # Initial publish: always (publish_initial=True). + assert h.should_publish(0.50) is True + h.last_published = 0.50 + # Small change (< delta): suppressed. + assert h.should_publish(0.505) is False + # Large change (> delta): published. + assert h.should_publish(0.60) is True + + +def test_hysteresis_skips_initial_when_publish_initial_false() -> None: + h = _HysteresisStub(delta=0.02, publish_initial=False) + assert h.should_publish(0.50) is False diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py index dd5b90144e..57fcb77ff7 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -86,6 +86,8 @@ 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.navigation.costmap_precision_governor.module import CostmapPrecisionGovernor from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, @@ -107,6 +109,7 @@ def _make(coord): VoxelGridMapper.blueprint(emit_every=5), CostMapper.blueprint(), ReplanningAStarPlanner.blueprint(), + CostmapPrecisionGovernor.blueprint(), ) .transports( { From 42939d3f38023101d25c4617324d412b3df16b06 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 3 Jun 2026 14:28:52 -0700 Subject: [PATCH 50/51] adde simple_inflate to costmapper --- dimos/mapping/costmapper.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dimos/mapping/costmapper.py b/dimos/mapping/costmapper.py index 2c374bdf91..7c9fdd820c 100644 --- a/dimos/mapping/costmapper.py +++ b/dimos/mapping/costmapper.py @@ -21,6 +21,7 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.pointclouds.occupancy import ( OCCUPANCY_ALGOS, HeightCostConfig, @@ -36,6 +37,7 @@ class Config(ModuleConfig): algo: str = "height_cost" config: OccupancyConfig = Field(default_factory=HeightCostConfig) + inflation_radius_m: float = 0.0 class CostMapper(Module): @@ -83,4 +85,7 @@ def stop(self) -> None: # @timed() # TODO: fix thread leak in timed decorator def _calculate_costmap(self, msg: PointCloud2) -> OccupancyGrid: fn = OCCUPANCY_ALGOS[self.config.algo] - return fn(msg, **asdict(self.config.config)) + grid = fn(msg, **asdict(self.config.config)) + if self.config.inflation_radius_m > 0.0: + grid = simple_inflate(grid, self.config.inflation_radius_m) + return grid From de25919cc8b700221018b4aa21bdf73ada3e6023 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 3 Jun 2026 14:29:58 -0700 Subject: [PATCH 51/51] add a look ahread to compute the error max based on lookahead distance --- .../costmap_precision_governor/module.py | 149 ++++++++++++++++-- .../costmap_precision_governor/test_module.py | 121 ++++++++++++++ .../basic/unitree_go2_precision_nav.py | 22 ++- 3 files changed, 273 insertions(+), 19 deletions(-) diff --git a/dimos/navigation/costmap_precision_governor/module.py b/dimos/navigation/costmap_precision_governor/module.py index aa448d20c0..00e4f828c0 100644 --- a/dimos/navigation/costmap_precision_governor/module.py +++ b/dimos/navigation/costmap_precision_governor/module.py @@ -45,6 +45,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.std_msgs.Float32 import Float32 from dimos.utils.logging_config import setup_logger @@ -71,6 +72,77 @@ def sample_point(pose: PoseStamped, lookahead_m: float) -> tuple[float, float]: ) +def sample_path_window( + path: Path, + robot_xy: tuple[float, float], + lookahead_m: float, + step_m: float, +) -> list[tuple[float, float]]: + """Sample XY points along ``path`` starting at the waypoint nearest + the robot, walking forward until total accumulated distance reaches + ``lookahead_m``. Points are spaced ``step_m`` apart via linear + interpolation between waypoints. Empty path or zero-length window → + empty list. The nearest waypoint is always emitted first (distance + 0).""" + if not path.poses or lookahead_m <= 0.0 or step_m <= 0.0: + return [] + + pts = [(p.position.x, p.position.y) for p in path.poses] + rx, ry = robot_xy + + best_i = 0 + best_d2 = float("inf") + for i, (x, y) in enumerate(pts): + d2 = (x - rx) ** 2 + (y - ry) ** 2 + if d2 < best_d2: + best_d2 = d2 + best_i = i + + samples: list[tuple[float, float]] = [pts[best_i]] + accumulated = 0.0 + next_emit = step_m + + for i in range(best_i, len(pts) - 1): + x0, y0 = pts[i] + x1, y1 = pts[i + 1] + seg_len = math.hypot(x1 - x0, y1 - y0) + if seg_len < 1e-9: + continue + seg_start = accumulated + while next_emit <= accumulated + seg_len + 1e-9: + t = (next_emit - seg_start) / seg_len + samples.append((x0 + t * (x1 - x0), y0 + t * (y1 - y0))) + if next_emit >= lookahead_m - 1e-9: + return samples + next_emit += step_m + accumulated += seg_len + if accumulated >= lookahead_m: + break + + return samples + + +def min_clearance_along( + gradient_grid: OccupancyGrid, + points: list[tuple[float, float]], + d_far: float, +) -> float | None: + """Read the distance-transform cell at each XY point; return the + minimum clearance in meters across all in-bounds samples. ``None`` + if every sample fell outside the grid.""" + min_c: float | None = None + for sx, sy in points: + idx = gradient_grid.world_to_grid(Vector3(sx, sy, 0.0)) + ix, iy = int(idx.x), int(idx.y) + if not (0 <= ix < gradient_grid.width and 0 <= iy < gradient_grid.height): + continue + cell = int(gradient_grid.grid[iy, ix]) + clearance = d_far * (1.0 - cell / 100.0) + if min_c is None or clearance < min_c: + min_c = clearance + return min_c + + def clearance_to_e_max( clearance: float, d_near: float, @@ -97,18 +169,40 @@ def compute_e_max_from_costmap( e_max_high: float, lookahead_m: float, obstacle_threshold: int, + path: Path | None = None, + path_lookahead_m: float = 3.0, + path_sample_step_m: float = 0.10, ) -> float | None: - """End-to-end: costmap + pose → e_max (m), or None if the sample - point falls outside the grid. Pure function — testable without a - Module.""" + """End-to-end: costmap + pose → e_max (m). Pure function — testable + without a Module. + + Path-aware mode (``path`` is non-empty): sample clearance at every + ``path_sample_step_m`` along the next ``path_lookahead_m`` of the + path starting from the waypoint nearest the robot, take the + minimum, map that to e_max. Anticipates pinch corners before the + robot reaches them. + + Heading-based fallback (no path, or empty path): single sample at + ``sample_point(pose, lookahead_m)``. Used in standalone-spy runs + and during the brief window before the first goal is published. + + Returns ``None`` if every relevant sample falls outside the grid.""" gradient_grid = gradient(costmap, obstacle_threshold=obstacle_threshold, max_distance=d_far) - sx, sy = sample_point(pose, lookahead_m) - idx = gradient_grid.world_to_grid(Vector3(sx, sy, 0.0)) - ix, iy = int(idx.x), int(idx.y) - if not (0 <= ix < gradient_grid.width and 0 <= iy < gradient_grid.height): + + if path is not None and path.poses: + samples = sample_path_window( + path, + (pose.position.x, pose.position.y), + lookahead_m=path_lookahead_m, + step_m=path_sample_step_m, + ) + clearance = min_clearance_along(gradient_grid, samples, d_far) + else: + sx, sy = sample_point(pose, lookahead_m) + clearance = min_clearance_along(gradient_grid, [(sx, sy)], d_far) + + if clearance is None: return None - cell = int(gradient_grid.grid[iy, ix]) - clearance = d_far * (1.0 - cell / 100.0) return clearance_to_e_max(clearance, d_near, d_far, e_max_low, e_max_high) @@ -118,17 +212,24 @@ class CostmapPrecisionGovernorConfig(ModuleConfig): # Clearance (m) below which we clamp to the tight-corridor floor. d_near: float = 0.30 # Clearance (m) above which we clamp to the open-space ceiling. - d_far: float = 1.50 + d_far: float = 1.0 # e_max (m) emitted at or below d_near. e_max_low: float = 0.1 # e_max (m) emitted at or above d_far. - e_max_high: float = 0.90 + e_max_high: float = 0.4 # Minimum |Δe_max| since last publish required to emit again. # Suppresses thrashing the task's solve_profile() on every costmap. hysteresis_delta: float = 0.02 - # Sample point offset (m) ahead of the robot along its heading. - # 0 = sample at robot pose; >0 = anticipate corridors before entry. + # Heading-based fallback lookahead (m). Used only when no Path has + # been seen yet (standalone-spy, pre-first-goal). Once a Path is + # cached, path-window sampling takes over. lookahead_m: float = 0.50 + # Path-window sampling: scan the next path_lookahead_m of the + # planned path starting at the waypoint nearest the robot, sampling + # every path_sample_step_m, and take the MIN clearance. Anticipates + # pinch corners before the robot reaches them. + path_lookahead_m: float = 3.0 + path_sample_step_m: float = 0.10 # Costmap cell value threshold treated as an obstacle by gradient(). obstacle_threshold: int = 50 # Emit one value on first costmap so the consuming task isn't stuck @@ -158,23 +259,30 @@ class CostmapPrecisionGovernor(Module): global_costmap: In[OccupancyGrid] odom: In[PoseStamped] + path: In[Path] e_max: Out[Float32] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._latest_odom: PoseStamped | None = None + self._latest_path: Path | None = None self._last_published: float | None = None @rpc def start(self) -> None: super().start() self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) + self.register_disposable(Disposable(self.path.subscribe(self._on_path))) self.register_disposable(Disposable(self.global_costmap.subscribe(self._on_costmap))) def _on_odom(self, msg: PoseStamped) -> None: # Cheap callback — just stash; clearance read happens on costmap. self._latest_odom = msg + def _on_path(self, msg: Path) -> None: + # Stash; the next costmap tick switches to path-window sampling. + self._latest_path = msg + def _on_costmap(self, msg: OccupancyGrid) -> None: if self._latest_odom is None: # No pose yet — nothing to anchor the clearance read at. @@ -189,9 +297,12 @@ def _on_costmap(self, msg: OccupancyGrid) -> None: e_max_high=cfg.e_max_high, lookahead_m=cfg.lookahead_m, obstacle_threshold=cfg.obstacle_threshold, + path=self._latest_path, + path_lookahead_m=cfg.path_lookahead_m, + path_sample_step_m=cfg.path_sample_step_m, ) if new_e_max is None: - return # Sample point fell outside the grid. + return # All samples fell outside the grid. if self._should_publish(new_e_max): self.e_max.publish(Float32(data=new_e_max)) self._last_published = new_e_max @@ -202,4 +313,12 @@ def _should_publish(self, new_e_max: float) -> bool: return abs(new_e_max - self._last_published) > self.config.hysteresis_delta -__all__ = ["CostmapPrecisionGovernor", "CostmapPrecisionGovernorConfig"] +__all__ = [ + "CostmapPrecisionGovernor", + "CostmapPrecisionGovernorConfig", + "clearance_to_e_max", + "compute_e_max_from_costmap", + "min_clearance_along", + "sample_path_window", + "sample_point", +] diff --git a/dimos/navigation/costmap_precision_governor/test_module.py b/dimos/navigation/costmap_precision_governor/test_module.py index 3dc080f679..3f123c183d 100644 --- a/dimos/navigation/costmap_precision_governor/test_module.py +++ b/dimos/navigation/costmap_precision_governor/test_module.py @@ -22,16 +22,25 @@ import numpy as np import pytest +from dimos.mapping.occupancy.gradient import gradient 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.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.costmap_precision_governor.module import ( clearance_to_e_max, compute_e_max_from_costmap, + min_clearance_along, + sample_path_window, sample_point, ) + +def _path(points: list[tuple[float, float]]) -> Path: + return Path(poses=[_pose(x, y) for x, y in points]) + + # Common config used across tests so the hand-picked numbers line up. D_NEAR = 0.30 D_FAR = 1.50 @@ -199,3 +208,115 @@ def test_hysteresis_suppresses_small_changes() -> None: def test_hysteresis_skips_initial_when_publish_initial_false() -> None: h = _HysteresisStub(delta=0.02, publish_initial=False) assert h.should_publish(0.50) is False + + +# --------------------------------------------------------------------------- +# sample_path_window — nearest-waypoint anchor + walk-forward +# --------------------------------------------------------------------------- + + +def test_sample_path_window_empty_path_returns_empty() -> None: + assert sample_path_window(_path([]), (0.0, 0.0), lookahead_m=3.0, step_m=0.1) == [] + + +def test_sample_path_window_straight_line_emits_evenly_spaced() -> None: + # Path along +x from (0,0) → (5,0). Robot at origin, 1.0 m window, + # 0.25 m step. Expect samples at 0.0, 0.25, 0.50, 0.75, 1.0. + path = _path([(x, 0.0) for x in np.linspace(0.0, 5.0, 51)]) + samples = sample_path_window(path, (0.0, 0.0), lookahead_m=1.0, step_m=0.25) + xs = [s[0] for s in samples] + ys = [s[1] for s in samples] + assert xs == pytest.approx([0.0, 0.25, 0.50, 0.75, 1.00], abs=1e-6) + assert all(y == pytest.approx(0.0, abs=1e-9) for y in ys) + + +def test_sample_path_window_starts_at_nearest_waypoint() -> None: + # Path along +x. Robot at (2.0, 0). Nearest waypoint is x=2.0 → samples + # start there, not at the path origin. + path = _path([(x, 0.0) for x in np.linspace(0.0, 5.0, 51)]) + samples = sample_path_window(path, (2.0, 0.0), lookahead_m=0.5, step_m=0.25) + assert samples[0][0] == pytest.approx(2.0, abs=1e-6) + # Subsequent samples walk forward, not backward. + assert samples[-1][0] > samples[0][0] + + +def test_sample_path_window_diagonal_path_interpolates_correctly() -> None: + # Two-waypoint path on the 45° diagonal: (0,0) → (10,10). 1 m window + # at 0.5 m step from (0,0) → samples on the diagonal at distances + # 0, 0.5, 1.0 (so XY = (0,0), (0.354,0.354), (0.707,0.707)). + path = _path([(0.0, 0.0), (10.0, 10.0)]) + samples = sample_path_window(path, (0.0, 0.0), lookahead_m=1.0, step_m=0.5) + expected = [ + (0.0, 0.0), + (0.5 / math.sqrt(2), 0.5 / math.sqrt(2)), + (1.0 / math.sqrt(2), 1.0 / math.sqrt(2)), + ] + assert len(samples) == 3 + for got, exp in zip(samples, expected, strict=False): + assert got[0] == pytest.approx(exp[0], abs=1e-6) + assert got[1] == pytest.approx(exp[1], abs=1e-6) + + +# --------------------------------------------------------------------------- +# min_clearance_along — pick the tightest pinch over a sample set +# --------------------------------------------------------------------------- + + +def test_min_clearance_along_picks_tightest_sample() -> None: + # Obstacle at (1.0, 0.0). Sample one point next to it, one in open + # space. min should reflect the close one. + grid = _grid_with_obstacle_at((1.0, 0.0)) + g = gradient(grid, obstacle_threshold=50, max_distance=D_FAR) + far_sample = (0.0, -2.0) # well away from obstacle + near_sample = (0.9, 0.0) # 0.1 m from obstacle + c_far = min_clearance_along(g, [far_sample], D_FAR) + c_min = min_clearance_along(g, [far_sample, near_sample], D_FAR) + assert c_far is not None and c_min is not None + assert c_min < c_far + assert c_min == pytest.approx(0.1, abs=0.05) # ~one-cell precision + + +def test_min_clearance_along_all_outside_grid_returns_none() -> None: + grid = _free_grid(side_m=5.0) + g = gradient(grid, obstacle_threshold=50, max_distance=D_FAR) + assert min_clearance_along(g, [(10.0, 10.0), (-10.0, -10.0)], D_FAR) is None + + +# --------------------------------------------------------------------------- +# compute_e_max_from_costmap with path-window — anticipates corners +# --------------------------------------------------------------------------- + + +def test_path_window_sees_obstacle_robot_doesnt() -> None: + # Robot at origin, in open space relative to its own pose. Obstacle + # 2 m ahead at (2.0, 0). Heading-based fallback (lookahead=0) thinks + # we're in open space → high e_max. Path-window mode walks the path + # forward, hits the obstacle within its 3 m lookahead, → low e_max. + grid = _grid_with_obstacle_at((2.0, 0.0)) + path = _path([(x, 0.0) for x in np.linspace(0.0, 4.0, 41)]) + + e_no_path = compute_e_max_from_costmap(grid, _pose(0.0, 0.0), **_kwargs(0.0)) + e_with_path = compute_e_max_from_costmap( + grid, + _pose(0.0, 0.0), + **_kwargs(0.0), + path=path, + path_lookahead_m=3.0, + path_sample_step_m=0.1, + ) + assert e_no_path == pytest.approx(E_HIGH, abs=1e-3) + assert e_with_path < e_no_path + assert e_with_path == pytest.approx(E_LOW, abs=0.05) + + +def test_empty_path_falls_back_to_heading_lookahead() -> None: + # Empty path → identical result to no-path call (heading-based). + grid = _grid_with_obstacle_at((1.0, 0.0)) + e_no_path = compute_e_max_from_costmap(grid, _pose(0.0, 0.0, yaw=0.0), **_kwargs(0.5)) + e_empty_path = compute_e_max_from_costmap( + grid, + _pose(0.0, 0.0, yaw=0.0), + **_kwargs(0.5), + path=_path([]), + ) + assert e_no_path == pytest.approx(e_empty_path, abs=1e-9) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py index 57fcb77ff7..fedd99cc43 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_precision_nav.py @@ -86,9 +86,11 @@ 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.navigation.costmap_precision_governor.module import CostmapPrecisionGovernor from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import ( + rerun_config as _basic_rerun_config, +) from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator import ( unitree_go2_coordinator, unitree_go2_coordinator_rage, @@ -96,18 +98,30 @@ from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.visualization.vis_module import vis_module +_rerun_config = {**_basic_rerun_config, "memory_limit": "4GB"} + def _make(coord): return ( autoconnect( coord, - vis_module(viewer_backend=global_config.viewer), + vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), KeyboardTeleop.blueprint( publish_only_when_active=True, - disable_movement=True, # 0-9 e_max slider only; no WASD Twist + # WASD/QE acts as a safety override: while held it drives + # the coord's vel_go2 task (priority 20) which preempts + # the path/precision follower (priority 10). Release the + # keys → vel_go2 times out (200 ms) and goes inactive → + # path follower resumes. 0-9 still drives e_max. + disable_movement=False, ), VoxelGridMapper.blueprint(emit_every=5), - CostMapper.blueprint(), + # Inflate by ~half the Go2's long axis (~0.70 m / 2). The + # planner downstream also inflates by half global_config.robot_width + # (default 0.30 m → 0.165 m), which is shaped for a circular + # robot, not a long one. Stacking these gives ~0.50 m total + # envelope so the planner stops carving paths through corners. + CostMapper.blueprint(inflation_radius_m=0.35), ReplanningAStarPlanner.blueprint(), CostmapPrecisionGovernor.blueprint(), )