From cf02719460f7e2055fb45a7e44432d9df289c9de Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Sun, 17 May 2026 20:07:12 -0700 Subject: [PATCH 01/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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()