From 693fc9904acb69e675e341b51f48910a30ae1491 Mon Sep 17 00:00:00 2001 From: danvi Date: Fri, 26 Jun 2026 11:48:19 +0800 Subject: [PATCH 1/9] Add holonomic trajectory controller stack Integrate holonomic path tracking into LocalPlanner with speed profiling, trajectory types and metrics, JSONL telemetry, run profiles, Go2 blueprint wiring, hot replan support, and dannav split-stack experiments. --- dimos/core/global_config.py | 28 +- dimos/navigation/dannav/controllers.py | 433 +++++++++ dimos/navigation/dannav/global_planner.py | 512 +++++++++++ .../navigation/dannav/holonomic_controller.py | 190 ++++ dimos/navigation/dannav/local_planner.py | 818 ++++++++++++++++++ dimos/navigation/dannav/module.py | 190 ++++ dimos/navigation/dannav/path_clearance.py | 125 +++ dimos/navigation/dannav/path_distancer.py | 131 +++ dimos/navigation/dannav/rotation_clearance.py | 64 ++ .../dannav/simple_global_planner.py | 130 +++ .../test_local_planner_path_controller.py | 679 +++++++++++++++ .../dannav/test_local_planner_run_envelope.py | 272 ++++++ .../dannav/test_rotation_clearance.py | 72 ++ .../docs/plot_trajectory_control_ticks.py | 540 ++++++++++++ .../docs/trajectory_control_tick_jsonl.md | 78 ++ .../docs/trajectory_run_profiles.md | 66 ++ .../test_trajectory_command_limits.py | 209 +++++ ...rajectory_holonomic_tracking_controller.py | 210 +++++ .../test_trajectory_path_speed_profile.py | 157 ++++ .../test_trajectory_run_profiles.py | 124 +++ .../trajectory_command_limits.py | 124 +++ .../trajectory_control_tick_export.py | 71 ++ .../trajectory_control_tick_log.py | 204 +++++ ...rajectory_holonomic_tracking_controller.py | 166 ++++ .../trajectory_metrics.py | 274 ++++++ .../trajectory_path_speed_profile.py | 227 +++++ .../trajectory_run_profiles.py | 187 ++++ .../trajectory_types.py | 72 ++ dimos/robot/all_blueprints.py | 2 + .../blueprints/smart/unitree_go2_dannav.py | 52 ++ dimos/robot/unitree/go2/connection.py | 28 +- pyproject.toml | 2 + 32 files changed, 6427 insertions(+), 10 deletions(-) create mode 100644 dimos/navigation/dannav/controllers.py create mode 100644 dimos/navigation/dannav/global_planner.py create mode 100644 dimos/navigation/dannav/holonomic_controller.py create mode 100644 dimos/navigation/dannav/local_planner.py create mode 100644 dimos/navigation/dannav/module.py create mode 100644 dimos/navigation/dannav/path_clearance.py create mode 100644 dimos/navigation/dannav/path_distancer.py create mode 100644 dimos/navigation/dannav/rotation_clearance.py create mode 100644 dimos/navigation/dannav/simple_global_planner.py create mode 100644 dimos/navigation/dannav/test_local_planner_path_controller.py create mode 100644 dimos/navigation/dannav/test_local_planner_run_envelope.py create mode 100644 dimos/navigation/dannav/test_rotation_clearance.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md create mode 100644 dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_types.py create mode 100644 dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index c75ea6fd18..e5e205cb91 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -13,7 +13,9 @@ # limitations under the License. import re +from typing import Literal +from pydantic import Field, FiniteFloat from pydantic_settings import BaseSettings, SettingsConfigDict from dimos.constants import DEFAULT_BUILD_NATIVE @@ -63,7 +65,30 @@ class GlobalConfig(BaseSettings): robot_width: float = 0.3 robot_rotation_diameter: float = 0.6 nerf_speed: float = 1.0 + # Local path follower in ``replanning_a_star.LocalPlanner`` (issue 921 / P3-3). + local_planner_path_controller: Literal["differential", "holonomic"] = "holonomic" + local_planner_holonomic_kp: float = 2.0 + local_planner_holonomic_ky: float = 1.5 + local_planner_holonomic_kv: FiniteFloat = Field(default=0.0, ge=0.0) + local_planner_holonomic_kw: FiniteFloat = Field(default=0.0, ge=0.0) + local_planner_max_tangent_accel_m_s2: float = Field(default=1.0, gt=0.0) + local_planner_max_normal_accel_m_s2: float = Field(default=0.6, gt=0.0) + local_planner_goal_decel_m_s2: float = Field(default=1.0, gt=0.0) + local_planner_max_planar_cmd_accel_m_s2: float = Field(default=5.0, gt=0.0) + local_planner_max_yaw_accel_rad_s2: float = Field(default=5.0, gt=0.0) + local_planner_max_yaw_rate_rad_s: float | None = Field(default=None, gt=0.0) + # Issue 921 P4-1: one knob for LocalPlanner sleep pacing and controller dt (e.g. PD). + local_planner_control_rate_hz: float = Field(default=10.0, ge=0.1, le=60.0) + # Optional issue 921 JSONL telemetry export. Set to a file path for live speed-vs-divergence logs. + local_planner_trajectory_tick_log_path: str | None = None planner_robot_speed: float | None = None + # Session-default movement envelope (named run profile from + # ``dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles.GO2_RUN_PROFILES``: walk, trot, + # run_conservative, run_verified). The Go2 locomotion mode (default vs rage) + # is derived from the profile's required_locomotion_mode at connection + # start-up, so navigation/agentic blueprints activate the run mode from config + # instead of only the rage keyboard-teleop blueprint. Default stays "walk". + go2_run_profile: str = "walk" mcp_port: int = 9990 build_native: bool = DEFAULT_BUILD_NATIVE dtop: bool = False @@ -77,12 +102,13 @@ class GlobalConfig(BaseSettings): env_file=".env", env_file_encoding="utf-8", extra="ignore", + validate_assignment=True, ) def update(self, **kwargs: object) -> None: """Update config fields in place.""" for key, value in kwargs.items(): - if not hasattr(self, key): + if key not in type(self).model_fields: raise AttributeError(f"GlobalConfig has no field '{key}'") setattr(self, key, value) diff --git a/dimos/navigation/dannav/controllers.py b/dimos/navigation/dannav/controllers.py new file mode 100644 index 0000000000..5db33f55c9 --- /dev/null +++ b/dimos/navigation/dannav/controllers.py @@ -0,0 +1,433 @@ +# 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. + +from dataclasses import dataclass +import math +from typing import Protocol + +import numpy as np +from numpy.typing import NDArray + +from dimos.core.global_config import GlobalConfig +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 +from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( + HolonomicCommandLimits, + clamp_holonomic_cmd_vel, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import RunProfile +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.utils.trigonometry import angle_diff + + +def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: + return Pose( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + ) + + +def _pose_from_pose_stamped(odom: PoseStamped) -> Pose: + return Pose(odom.position, odom.orientation) + + +@dataclass(frozen=True) +class CommandEnvelopeOverrides: + """Run-profile command caps that replace the GlobalConfig defaults. + + The planar speed cap keeps tracking ``set_speed`` (the geometry-capped + path speed); these are the remaining saturation limits a movement + envelope owns. + """ + + max_yaw_rate_rad_s: float + max_planar_cmd_accel_m_s2: float + max_yaw_accel_rad_s2: float + + +def command_envelope_overrides_for_profile(profile: RunProfile) -> CommandEnvelopeOverrides: + """Map a :class:`RunProfile` to planner command caps (planar speed excluded).""" + limits = profile.command_limits() + return CommandEnvelopeOverrides( + max_yaw_rate_rad_s=limits.max_yaw_rate_rad_s, + max_planar_cmd_accel_m_s2=limits.max_planar_linear_accel_m_s2, + max_yaw_accel_rad_s2=limits.max_yaw_accel_rad_s2, + ) + + +class Controller(Protocol): + def advance( + self, + lookahead_point: NDArray[np.float64], + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: ... + + def advance_reference( + self, + reference: TrajectoryReferenceSample, + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: ... + + def rotate( + self, + yaw_error: float, + current_odom: PoseStamped | None = None, + measured_body_twist: Twist | None = None, + ) -> Twist: ... + + def set_speed(self, speed_m_s: float) -> None: ... + + def reset_errors(self) -> None: ... + + def reset_yaw_error(self, value: float) -> None: ... + + +class PController: + _global_config: GlobalConfig + _speed: float + _control_frequency: float + + _min_linear_velocity: float = 0.2 + _min_angular_velocity: float = 0.2 + _k_angular: float = 0.5 + _max_angular_accel: float = 2.0 + _rotation_threshold: float = 90 * (math.pi / 180) + + def __init__(self, global_config: GlobalConfig, speed: float, control_frequency: float) -> None: + self._global_config = global_config + self._speed = speed + self._control_frequency = control_frequency + + def set_speed(self, speed_m_s: float) -> None: + self._speed = float(speed_m_s) + + def advance( + self, + lookahead_point: NDArray[np.float64], + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: + reference = TrajectoryReferenceSample( + time_s=float(current_odom.ts), + pose_plan=_pose_from_xy_yaw( + float(lookahead_point[0]), + float(lookahead_point[1]), + 0.0, + ), + twist_body=Twist( + linear=Vector3(self._speed, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ), + ) + return self.advance_reference(reference, current_odom, measured_body_twist) + + def advance_reference( + self, + reference: TrajectoryReferenceSample, + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: + del measured_body_twist + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + lookahead_point = np.array( + [ + float(reference.pose_plan.position.x), + float(reference.pose_plan.position.y), + ], + dtype=np.float64, + ) + direction = lookahead_point - current_pos + distance = np.linalg.norm(direction) + + if distance < 1e-6: + # Robot is coincidentally at the lookahead point; skip this cycle. + return Twist() + + robot_yaw = current_odom.orientation.euler[2] + desired_yaw = np.arctan2(direction[1], direction[0]) + yaw_error = angle_diff(desired_yaw, robot_yaw) + + angular_velocity = self._compute_angular_velocity(yaw_error) + + # Rotate-then-drive: if heading error is large, rotate in place first + if abs(yaw_error) > self._rotation_threshold: + return self._angular_twist(angular_velocity) + + # When aligned, drive forward with proportional angular correction + linear_velocity = self._speed * (1.0 - abs(yaw_error) / self._rotation_threshold) + linear_velocity = self._apply_min_velocity(linear_velocity, self._min_linear_velocity) + + return Twist( + linear=Vector3(linear_velocity, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + def rotate( + self, + yaw_error: float, + current_odom: PoseStamped | None = None, + measured_body_twist: Twist | None = None, + ) -> Twist: + del measured_body_twist + del current_odom + angular_velocity = self._compute_angular_velocity(yaw_error) + return self._angular_twist(angular_velocity) + + def _compute_angular_velocity(self, yaw_error: float) -> float: + angular_velocity = self._k_angular * yaw_error + angular_velocity = np.clip(angular_velocity, -self._speed, self._speed) + angular_velocity = self._apply_min_velocity(angular_velocity, self._min_angular_velocity) + return float(angular_velocity) + + def reset_errors(self) -> None: + pass + + def reset_yaw_error(self, value: float) -> None: + pass + + def _apply_min_velocity(self, velocity: float, min_velocity: float) -> float: + """Apply minimum velocity threshold, preserving sign. Returns 0 if velocity is 0.""" + if velocity == 0.0: + return 0.0 + if abs(velocity) < min_velocity: + return min_velocity if velocity > 0 else -min_velocity + return velocity + + def _angular_twist(self, angular_velocity: float) -> Twist: + # In simulation, we need stroger values + if self._global_config.simulation and abs(angular_velocity) < 0.8: + angular_velocity = 0.8 * np.sign(angular_velocity) + + return Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, angular_velocity), + ) + + +class PdController(PController): + _k_derivative: float = 0.15 + + _prev_yaw_error: float + _prev_angular_velocity: float + + def __init__(self, global_config: GlobalConfig, speed: float, control_frequency: float) -> None: + super().__init__(global_config, speed, control_frequency) + + self._prev_yaw_error = 0.0 + self._prev_angular_velocity = 0.0 + + def reset_errors(self) -> None: + self._prev_yaw_error = 0.0 + self._prev_angular_velocity = 0.0 + + def reset_yaw_error(self, value: float) -> None: + self._prev_yaw_error = value + + def _compute_angular_velocity(self, yaw_error: float) -> float: + dt = 1.0 / self._control_frequency + + # PD control: proportional + derivative damping + yaw_error_derivative = (yaw_error - self._prev_yaw_error) / dt + angular_velocity = self._k_angular * yaw_error - self._k_derivative * yaw_error_derivative + + # Rate limiting: limit angular acceleration to prevent jerky corrections + max_delta = self._max_angular_accel * dt + angular_velocity = np.clip( + angular_velocity, + self._prev_angular_velocity - max_delta, + self._prev_angular_velocity + max_delta, + ) + + angular_velocity = np.clip(angular_velocity, -self._speed, self._speed) + angular_velocity = self._apply_min_velocity(angular_velocity, self._min_angular_velocity) + + self._prev_yaw_error = yaw_error + self._prev_angular_velocity = angular_velocity + + return float(angular_velocity) + + +class HolonomicPathController: + """Follow path segments using the holonomic tracking law (P3-3, issue 921). + + Wraps :class:`HolonomicTrackingController` in the :class:`Controller` seam + (lookahead + odom). Rotations in place use the same law with a fixed + position reference. Not a car-style or Pure Pursuit path law. + """ + + def __init__( + self, + global_config: GlobalConfig, + speed: float, + control_frequency: float, + k_position_per_s: float, + k_yaw_per_s: float, + k_velocity_per_s: float = 0.0, + k_yaw_rate_per_s: float = 0.0, + ) -> None: + self._global_config = global_config + self._speed = float(speed) + self._control_frequency = float(control_frequency) + self._inner = HolonomicTrackingController( + k_position_per_s=k_position_per_s, + k_yaw_per_s=k_yaw_per_s, + k_velocity_per_s=k_velocity_per_s, + k_yaw_rate_per_s=k_yaw_rate_per_s, + ) + self._envelope_overrides: CommandEnvelopeOverrides | None = None + self._limits = self._make_limits() + self._inner.configure(self._limits) + self._previous_cmd = Twist() + + def set_speed(self, speed_m_s: float) -> None: + self._speed = float(speed_m_s) + self._limits = self._make_limits() + self._inner.configure(self._limits) + + def set_command_envelope(self, overrides: CommandEnvelopeOverrides | None) -> None: + """Apply (or clear, with ``None``) a run profile's command caps.""" + self._envelope_overrides = overrides + self._limits = self._make_limits() + self._inner.configure(self._limits) + + def _make_limits(self) -> HolonomicCommandLimits: + overrides = self._envelope_overrides + if overrides is not None: + return HolonomicCommandLimits( + max_planar_speed_m_s=self._speed, + max_yaw_rate_rad_s=overrides.max_yaw_rate_rad_s, + max_planar_linear_accel_m_s2=overrides.max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=overrides.max_yaw_accel_rad_s2, + ) + max_yaw_rate = self._global_config.local_planner_max_yaw_rate_rad_s + return HolonomicCommandLimits( + max_planar_speed_m_s=self._speed, + max_yaw_rate_rad_s=self._speed if max_yaw_rate is None else float(max_yaw_rate), + max_planar_linear_accel_m_s2=self._global_config.local_planner_max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=self._global_config.local_planner_max_yaw_accel_rad_s2, + ) + + def advance( + self, + lookahead_point: NDArray[np.float64], + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: + current_pos = np.array([float(current_odom.position.x), float(current_odom.position.y)]) + direction = np.asarray(lookahead_point, dtype=np.float64) - current_pos + distance = float(np.linalg.norm(direction)) + + if distance < 1e-6 or not np.isfinite(distance): + return Twist() + + ref_yaw = float(np.arctan2(direction[1], direction[0])) + ref_pose = _pose_from_xy_yaw(float(lookahead_point[0]), float(lookahead_point[1]), ref_yaw) + # Feedforward along the reference heading in the body frame of the target pose. + ref_ff = Twist( + linear=Vector3(self._speed, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + ref = TrajectoryReferenceSample(0.0, ref_pose, ref_ff) + return self.advance_reference(ref, current_odom, measured_body_twist) + + def advance_reference( + self, + reference: TrajectoryReferenceSample, + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: + twist = Twist() if measured_body_twist is None else measured_body_twist + meas = TrajectoryMeasuredSample(0.0, _pose_from_pose_stamped(current_odom), twist) + return self._limit_output(self._inner.control(reference, meas)) + + def rotate( + self, + yaw_error: float, + current_odom: PoseStamped | None = None, + measured_body_twist: Twist | None = None, + ) -> Twist: + if current_odom is None: + # ``LocalPlanner`` should always pass odom; keep a safe fallback. + wz = float(0.5 * yaw_error) + wz = float(np.clip(wz, -self._speed, self._speed)) + if wz != 0.0 and abs(wz) < 0.2: + wz = 0.2 * (1.0 if wz > 0 else -1.0) + t = Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, wz), + ) + return self._limit_output(self._apply_sim_angular(t)) + + robot_yaw = float(current_odom.orientation.euler[2]) + target_yaw = float(np.arctan2(np.sin(robot_yaw + yaw_error), np.cos(robot_yaw + yaw_error))) + p = _pose_from_xy_yaw( + float(current_odom.position.x), + float(current_odom.position.y), + target_yaw, + ) + ref = TrajectoryReferenceSample(0.0, p, Twist()) + twist = Twist() if measured_body_twist is None else measured_body_twist + meas = TrajectoryMeasuredSample(0.0, _pose_from_pose_stamped(current_odom), twist) + out = self._inner.control(ref, meas) + return self._limit_output(self._apply_sim_angular(out)) + + def reset_errors(self) -> None: + self._inner.reset() + self._previous_cmd = Twist() + + def reset_yaw_error(self, value: float) -> None: + del value + + def _apply_sim_angular(self, t: Twist) -> Twist: + wz = float(t.angular.z) + if self._global_config.simulation and 1e-9 < abs(wz) < 0.8: + wz = 0.8 * (1.0 if wz > 0 else -1.0) + return Twist( + linear=Vector3(float(t.linear.x), float(t.linear.y), float(t.linear.z)), + angular=Vector3(0.0, 0.0, wz), + ) + + def _limit_output(self, raw: Twist) -> Twist: + out = clamp_holonomic_cmd_vel( + self._previous_cmd, + raw, + self._limits, + 1.0 / self._control_frequency, + ) + self._previous_cmd = Twist(out) + return out + + +def make_local_path_controller( + global_config: GlobalConfig, + speed: float, + control_frequency: float, +) -> Controller: + if global_config.local_planner_path_controller == "holonomic": + return HolonomicPathController( + global_config, + speed, + control_frequency, + k_position_per_s=global_config.local_planner_holonomic_kp, + k_yaw_per_s=global_config.local_planner_holonomic_ky, + k_velocity_per_s=global_config.local_planner_holonomic_kv, + k_yaw_rate_per_s=global_config.local_planner_holonomic_kw, + ) + return PController(global_config, speed, control_frequency) diff --git a/dimos/navigation/dannav/global_planner.py b/dimos/navigation/dannav/global_planner.py new file mode 100644 index 0000000000..26af08326b --- /dev/null +++ b/dimos/navigation/dannav/global_planner.py @@ -0,0 +1,512 @@ +# 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. + +import math +from threading import Event, RLock, Thread, current_thread +import time + +from dimos_lcm.std_msgs import Bool +from reactivex import Subject +from reactivex.disposable import CompositeDisposable + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.global_config import GlobalConfig +from dimos.core.resource import Resource +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +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.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.base import NavigationState +from dimos.navigation.replanning_a_star.goal_validator import find_safe_goal +from dimos.navigation.dannav.local_planner import LocalPlanner, StopMessage +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.replanning_a_star.position_tracker import PositionTracker +from dimos.navigation.replanning_a_star.replan_limiter import ReplanLimiter +from dimos.utils.logging_config import setup_logger +from dimos.utils.trigonometry import angle_diff + +logger = setup_logger() + + +class GlobalPlanner(Resource): + path: Subject[Path] + goal_reached: Subject[Bool] + + _current_odom: PoseStamped | None = None + _current_goal: PoseStamped | None = None + _goal_run_profile: str | None = None + _goal_reached: bool = False + _thread: Thread | None = None + + _global_config: GlobalConfig + _navigation_map: NavigationMap + _navigation_map_near: NavigationMap + _local_planner: LocalPlanner + _position_tracker: PositionTracker + _replan_limiter: ReplanLimiter + _disposables: CompositeDisposable + _stop_planner: Event + _replan_event: Event + _replan_reason: StopMessage | None + _lock: RLock + _safe_goal_clearance: float + + _safe_goal_tolerance: float = 4.0 + _goal_tolerance: float = 0.2 + _rotation_tolerance: float = math.radians(15) + _replan_goal_tolerance: float = 0.5 + _stuck_time_window: float = 8.0 + _stuck_threshold: float = 0.4 + _max_path_deviation: float = 0.9 + _replanning_enabled: bool = True + _replan_on_costmap_update: bool + # Target ~0.14 m between hot replans; interval = spacing / speed (clamped). + _inplace_replan_spacing_m: float = 0.14 + _inplace_replan_min_interval_floor_s: float = 0.05 + _inplace_replan_min_interval_ceil_s: float = 0.25 + _last_inplace_replan_monotonic: float = 0.0 + + def __init__(self, global_config: GlobalConfig, *, replan_on_costmap_update: bool = False) -> None: + self.path = Subject() + self.goal_reached = Subject() + + self._global_config = global_config + self._replan_on_costmap_update = replan_on_costmap_update + self._navigation_map = NavigationMap(self._global_config, "voronoi") + self._navigation_map_near = NavigationMap(self._global_config, "gradient") + self._local_planner = LocalPlanner( + self._global_config, + self._navigation_map, + self._goal_tolerance, + hot_replan=replan_on_costmap_update, + ) + + stuck_threshold = self._stuck_threshold + if global_config.simulation: + stuck_threshold = 1.0 + + self._position_tracker = PositionTracker(self._stuck_time_window, stuck_threshold) + self._replan_limiter = ReplanLimiter() + self._disposables = CompositeDisposable() + self._stop_planner = Event() + self._replan_event = Event() + self._replan_reason = None + self._lock = RLock() + self._reset_safe_goal_clearance() + + def start(self) -> None: + self._local_planner.start() + self._disposables.add( + self._local_planner.stopped_navigating.subscribe(self._on_stopped_navigating) + ) + self._stop_planner.clear() + self._thread = Thread(target=self._thread_entrypoint, daemon=True) + self._thread.start() + + def stop(self) -> None: + self.cancel_goal() + self._local_planner.stop() + self._disposables.dispose() + self._stop_planner.set() + self._replan_event.set() + + if self._thread is not None and self._thread is not current_thread(): + self._thread.join(DEFAULT_THREAD_JOIN_TIMEOUT) + if self._thread.is_alive(): + logger.error("GlobalPlanner thread did not stop in time.") + self._thread = None + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._current_odom = msg + + self._local_planner.handle_odom(msg) + self._position_tracker.add_position(msg) + + def handle_global_costmap(self, msg: OccupancyGrid) -> None: + self._navigation_map.update(msg) + self._navigation_map_near.update(msg) + + if not self._replan_on_costmap_update: + return + + with self._lock: + has_goal = self._current_goal is not None + + if not has_goal: + return + + with self._lock: + self._replan_reason = "map_updated" + self._replan_event.set() + + def handle_goal_request(self, goal: PoseStamped, run_profile: str | None = None) -> None: + """Plan toward ``goal``; ``run_profile`` overrides the session envelope for it. + + The per-goal profile sticks to the goal across replans and is cleared + when the goal is cancelled or reached, so the next goal falls back to + the session default. + """ + logger.info("Got new goal", goal=str(goal), run_profile=run_profile) + with self._lock: + self._current_goal = goal + self._goal_run_profile = run_profile + self._goal_reached = False + self._replan_limiter.reset() + self._plan_path() + + def set_run_profile(self, profile: str) -> None: + """Set the session-default run profile used for subsequent goals. + + Module configs are per-process copies, so the operator surface pushes + the chosen profile here instead of relying on its own GlobalConfig + write being visible to the planner. + """ + self._global_config.go2_run_profile = profile + + def set_safe_goal_clearance(self, clearance: float) -> None: + with self._lock: + self._safe_goal_clearance = clearance + + def reset_safe_goal_clearance(self) -> None: + self._reset_safe_goal_clearance() + + def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False) -> None: + # return silently so we don't flood the logs. + with self._lock: + no_goal = self._current_goal is None + if no_goal and self._local_planner.get_state() == NavigationState.IDLE: + return + + logger.info("Cancelling goal.", but_will_try_again=but_will_try_again, arrived=arrived) + + with self._lock: + self._position_tracker.reset_data() + + if not but_will_try_again: + self._current_goal = None + self._goal_run_profile = None + self._goal_reached = arrived + self._replan_limiter.reset() + + self.path.on_next(Path()) + self._local_planner.stop_planning() + + if not but_will_try_again: + self.goal_reached.on_next(Bool(arrived)) + + def set_replanning_enabled(self, enabled: bool) -> None: + with self._lock: + self._replanning_enabled = enabled + + def get_state(self) -> NavigationState: + return self._local_planner.get_state() + + def is_goal_reached(self) -> bool: + with self._lock: + return self._goal_reached + + @property + def cmd_vel(self) -> Subject[Twist]: + return self._local_planner.cmd_vel + + @property + def navigation_costmap(self) -> Subject[OccupancyGrid]: + return self._local_planner.navigation_costmap + + def _thread_entrypoint(self) -> None: + """Monitor if the robot is stuck, veers off track, or stopped navigating.""" + + last_id = -1 + last_stuck_check = time.perf_counter() + + while not self._stop_planner.is_set(): + # Wait for either timeout or replan signal from local planner. + replanning_wanted = self._replan_event.wait(timeout=0.1) + + if self._stop_planner.is_set(): + break + + # Handle stop message from local planner (priority) + if replanning_wanted: + self._replan_event.clear() + with self._lock: + reason = self._replan_reason + self._replan_reason = None + + if reason is not None: + self._handle_stop_message(reason) + last_stuck_check = time.perf_counter() + continue + + with self._lock: + current_goal = self._current_goal + current_odom = self._current_odom + + if not current_goal or not current_odom: + continue + + if ( + current_goal.position.distance(current_odom.position) < self._goal_tolerance + and abs( + angle_diff(current_goal.orientation.euler[2], current_odom.orientation.euler[2]) + ) + < self._rotation_tolerance + ): + logger.info("Close enough to goal. Accepting as arrived.") + self.cancel_goal(arrived=True) + continue + + # Check if robot has veered too far off the path + deviation = self._local_planner.get_distance_to_path() + if deviation is not None and deviation > self._max_path_deviation: + logger.info( + "Robot veered off track. Replanning.", + deviation=round(deviation, 2), + threshold=self._max_path_deviation, + ) + self._replan_path() + last_stuck_check = time.perf_counter() + continue + + _, new_id = self._local_planner.get_unique_state() + + if new_id != last_id: + last_id = new_id + last_stuck_check = time.perf_counter() + continue + + if ( + time.perf_counter() - last_stuck_check > self._stuck_time_window + and self._position_tracker.is_stuck() + ): + logger.info("Robot is stuck. Replanning.") + self._replan_path() + last_stuck_check = time.perf_counter() + + def _on_stopped_navigating(self, stop_message: StopMessage) -> None: + with self._lock: + self._replan_reason = stop_message + # Signal the monitoring thread to do the replanning. This is so we don't have two + # threads which could be replanning at the same time. + self._replan_event.set() + + def _handle_stop_message(self, stop_message: StopMessage) -> None: + # Note, this runs in the monitoring thread. + + if stop_message == "arrived": + self.path.on_next(Path()) + logger.info("Arrived at goal.") + self.cancel_goal(arrived=True) + return + + if stop_message in ("map_updated", "obstacle_found") and self._replan_on_costmap_update: + if stop_message == "obstacle_found": + logger.info("Hot replanning path due to obstacle found.") + else: + logger.info("Hot replanning path due to costmap update.") + self._replan_path_inplace() + return + + self.path.on_next(Path()) + + if stop_message == "obstacle_found": + logger.info("Replanning path due to obstacle found.") + self._replan_path() + elif stop_message == "error": + logger.info("Failure in navigation.") + self._replan_path() + elif stop_message == "run_envelope_rejected": + # Unknown run profile or other envelope resolution failure; replanning + # would not change the outcome, so cancel the goal. + logger.warning("Run profile could not be applied; cancelling goal.") + self.cancel_goal() + else: + logger.error(f"No code to handle '{stop_message}'.") + self.cancel_goal() + + def _replan_path(self) -> None: + with self._lock: + current_odom = self._current_odom + current_goal = self._current_goal + + logger.info("Replanning.", attempt=self._replan_limiter.get_attempt()) + + assert current_odom is not None + assert current_goal is not None + + if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: + self.cancel_goal(arrived=True) + return + + if not self._replanning_enabled: + self.cancel_goal() + return + + if not self._replan_limiter.can_retry(current_odom.position): + self.cancel_goal() + return + + self._replan_limiter.will_retry() + + self._plan_path() + + def _inplace_replan_min_interval_for_speed(self, speed_m_s: float) -> float: + """Shorter throttle at higher planner speed (~fixed spatial replan spacing).""" + if not math.isfinite(speed_m_s) or speed_m_s <= 0.0: + return self._inplace_replan_min_interval_ceil_s + interval = self._inplace_replan_spacing_m / speed_m_s + return max( + self._inplace_replan_min_interval_floor_s, + min(self._inplace_replan_min_interval_ceil_s, interval), + ) + + def _replan_path_inplace(self) -> None: + now = time.monotonic() + min_interval = self._inplace_replan_min_interval_for_speed( + self._local_planner.planner_speed_m_s() + ) + if now - self._last_inplace_replan_monotonic < min_interval: + return + self._last_inplace_replan_monotonic = now + + with self._lock: + current_odom = self._current_odom + current_goal = self._current_goal + goal_run_profile = self._goal_run_profile + + if current_odom is None or current_goal is None: + return + + if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: + self.cancel_goal(arrived=True) + return + + if not self._replanning_enabled: + return + + safe_goal = self._find_safe_goal(current_goal.position) + if not safe_goal: + logger.warning( + "No safe goal found during hot replan.", + x=round(current_goal.x, 3), + y=round(current_goal.y, 3), + ) + return + + path = self._find_wide_path(safe_goal, current_odom.position) + if not path: + logger.warning( + "No path found during hot replan.", + x=round(safe_goal.x, 3), + y=round(safe_goal.y, 3), + ) + return + + resampled_path = smooth_resample_path(path, current_goal, 0.1) + self.path.on_next(resampled_path) + + if self._local_planner.get_state() == NavigationState.IDLE: + self._local_planner.start_planning(resampled_path, run_profile_name=goal_run_profile) + return + + if not self._local_planner.update_path(resampled_path): + self._local_planner.start_planning(resampled_path, run_profile_name=goal_run_profile) + + def _plan_path(self) -> None: + self.cancel_goal(but_will_try_again=True) + + with self._lock: + current_odom = self._current_odom + current_goal = self._current_goal + goal_run_profile = self._goal_run_profile + + assert current_goal is not None + + if current_odom is None: + logger.warning("Cannot handle goal request: missing odometry.") + return + + safe_goal = self._find_safe_goal(current_goal.position) + + if not safe_goal: + logger.warning( + "No safe goal found.", x=round(current_goal.x, 3), y=round(current_goal.y, 3) + ) + self.cancel_goal() + return + + path = self._find_wide_path(safe_goal, current_odom.position) + + if not path: + logger.warning( + "No path found to the goal.", x=round(safe_goal.x, 3), y=round(safe_goal.y, 3) + ) + self.cancel_goal() + return + + resampled_path = smooth_resample_path(path, current_goal, 0.1) + + self.path.on_next(resampled_path) + + self._local_planner.start_planning(resampled_path, run_profile_name=goal_run_profile) + + def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: + # sizes_to_try: list[float] = [2.2, 1.7, 1.3, 1] + sizes_to_try: list[float] = [1.1] + + for size in sizes_to_try: + distance = robot_pos.distance(goal) + navigation_map = self._navigation_map if distance > 1.5 else self._navigation_map_near + costmap = navigation_map.make_gradient_costmap(size) + path = min_cost_astar(costmap, goal, robot_pos) + if path and path.poses: + logger.info(f"Found path {size}x robot width.") + return path + + return None + + def _find_safe_goal(self, goal: Vector3) -> Vector3 | None: + costmap = self._navigation_map.binary_costmap + + if costmap.cell_value(goal) == CostValues.UNKNOWN: + return goal + + safe_goal = find_safe_goal( + costmap, + goal, + algorithm="bfs_contiguous", + cost_threshold=CostValues.OCCUPIED, + min_clearance=self._safe_goal_clearance, + max_search_distance=self._safe_goal_tolerance, + ) + + if safe_goal is None: + logger.warning("No safe goal found near requested target.") + return None + + goals_distance = safe_goal.distance(goal) + if goals_distance > 0.2: + logger.warning(f"Travelling to goal {goals_distance}m away from requested goal.") + + logger.info("Found safe goal.", x=round(safe_goal.x, 2), y=round(safe_goal.y, 2)) + + return safe_goal + + def _reset_safe_goal_clearance(self) -> None: + with self._lock: + self._safe_goal_clearance = self._global_config.robot_rotation_diameter / 2 diff --git a/dimos/navigation/dannav/holonomic_controller.py b/dimos/navigation/dannav/holonomic_controller.py new file mode 100644 index 0000000000..e1885156f9 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_controller.py @@ -0,0 +1,190 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Continuous holonomic path follower for the dannav loop planner. + +Control ticks run on the odom subscription callback (not a background thread). +``_on_path`` only swaps the ``PathDistancer`` under ``_lock``; each throttled +odom update computes and publishes ``nav_cmd_vel``. When the dannav +``GlobalPlanner`` publishes a new path, the next tick tracks it without +stopping the robot. +""" + +from __future__ import annotations + +import math +from threading import RLock +import time +from typing import Any + +import numpy as np +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Path import Path +from dimos.core.global_config import GlobalConfig +from dimos.navigation.dannav.controllers import ( + HolonomicPathController, + command_envelope_overrides_for_profile, +) +from dimos.navigation.dannav.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import GO2_RUN_PROFILES, RunProfileError +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _session_speed_and_envelope( + global_config: GlobalConfig, +) -> tuple[float, CommandEnvelopeOverrides | None, str]: + """Match ``LocalPlanner`` session envelope: ``go2_run_profile`` sets speed and caps.""" + profile_name = global_config.go2_run_profile + if profile_name == GO2_RUN_PROFILES.default_profile_name: + speed = ( + float(global_config.planner_robot_speed) + if global_config.planner_robot_speed is not None + else GO2_RUN_PROFILES.get("walk").requested_planner_speed_m_s + ) + return speed, None, profile_name + + profile = GO2_RUN_PROFILES.get(profile_name) + return profile.requested_planner_speed_m_s, command_envelope_overrides_for_profile(profile), profile_name + + +class HolonomicControllerConfig(ModuleConfig): + goal_tolerance: float = 0.2 + + +class HolonomicController(Module): + """Track the latest ``Path`` with the holonomic law, never stopping to replan.""" + + config: HolonomicControllerConfig + + path: In[Path] + odom: In[PoseStamped] + + nav_cmd_vel: Out[Twist] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._lock = RLock() + self._path_distancer: PathDistancer | None = None + self._current_odom: PoseStamped | None = None + self._controller: HolonomicPathController | None = None + self._speed: float = 0.0 + self._control_frequency: float = 10.0 + self._last_tick_monotonic = 0.0 + + @rpc + def start(self) -> None: + super().start() + + global_config = self.config.g + self._control_frequency = float(global_config.local_planner_control_rate_hz) + try: + speed, envelope_overrides, _ = _session_speed_and_envelope(global_config) + except RunProfileError as exc: + logger.warning( + "Unknown run profile; falling back to walk speed.", + profile=global_config.go2_run_profile, + reason=str(exc), + ) + profile_name = GO2_RUN_PROFILES.default_profile_name + speed = ( + float(global_config.planner_robot_speed) + if global_config.planner_robot_speed is not None + else GO2_RUN_PROFILES.get(profile_name).requested_planner_speed_m_s + ) + envelope_overrides = None + if not math.isfinite(speed) or speed <= 0.0: + raise ValueError(f"planner speed must be a positive finite float, got {speed!r}") + if global_config.nerf_speed < 1.0: + speed *= global_config.nerf_speed + self._speed = speed + self._controller = HolonomicPathController( + global_config, + speed, + self._control_frequency, + k_position_per_s=global_config.local_planner_holonomic_kp, + k_yaw_per_s=global_config.local_planner_holonomic_ky, + k_velocity_per_s=global_config.local_planner_holonomic_kv, + k_yaw_rate_per_s=global_config.local_planner_holonomic_kw, + ) + self._controller.set_command_envelope(envelope_overrides) + + self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) + self.register_disposable(Disposable(self.path.subscribe(self._on_path))) + + @rpc + def stop(self) -> None: + self.nav_cmd_vel.publish(Twist()) + super().stop() + + def _on_odom(self, msg: PoseStamped) -> None: + pose = self._as_pose_stamped(msg) + with self._lock: + self._current_odom = pose + self._tick() + + def _on_path(self, path: Path) -> None: + if not path.poses: + return + path_distancer = PathDistancer(path) + with self._lock: + self._path_distancer = path_distancer + self._tick() + + def _tick(self) -> None: + now = time.monotonic() + period = 1.0 / self._control_frequency + if now - self._last_tick_monotonic < period: + return + self._last_tick_monotonic = now + + with self._lock: + path_distancer = self._path_distancer + current_odom = self._current_odom + + if path_distancer is None or current_odom is None: + return + + cmd = self._compute_cmd(path_distancer, current_odom) + self.nav_cmd_vel.publish(cmd) + + def _compute_cmd( + self, path_distancer: PathDistancer, current_odom: PoseStamped + ) -> Twist: + assert self._controller is not None + current_pos = np.array( + [float(current_odom.position.x), float(current_odom.position.y)], + dtype=np.float64, + ) + + if path_distancer.distance_to_goal(current_pos) < self.config.goal_tolerance: + return Twist() + + closest_index = path_distancer.find_closest_point_index(current_pos) + lookahead_point = path_distancer.find_lookahead_point(closest_index) + return self._controller.advance(lookahead_point, current_odom) + + @staticmethod + def _as_pose_stamped(msg: PoseStamped) -> PoseStamped: + to_pose = getattr(msg, "to_pose_stamped", None) + if callable(to_pose): + return to_pose() + return msg diff --git a/dimos/navigation/dannav/local_planner.py b/dimos/navigation/dannav/local_planner.py new file mode 100644 index 0000000000..476c25019c --- /dev/null +++ b/dimos/navigation/dannav/local_planner.py @@ -0,0 +1,818 @@ +# 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. + +from dataclasses import dataclass +import math +import os +from threading import Event, RLock, Thread +import time +import traceback +from typing import Literal, TypeAlias + +import numpy as np +from reactivex import Subject + +from dimos.core.global_config import GlobalConfig +from dimos.core.resource import Resource +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 +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.base import NavigationState +from dimos.navigation.dannav.controllers import ( + CommandEnvelopeOverrides, + Controller, + HolonomicPathController, + command_envelope_overrides_for_profile, + make_local_path_controller, +) +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.dannav.path_clearance import PathClearance +from dimos.navigation.dannav.path_distancer import PathDistancer +from dimos.navigation.dannav.rotation_clearance import can_rotate_in_place +from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export import JsonlTrajectoryControlTickSink +from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import ( + TrajectoryControlTickSink, + append_trajectory_control_tick, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( + PathSpeedProfileLimits, + profile_speed_along_polyline, + speed_at_progress_m, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( + GO2_RUN_PROFILES, + RunProfile, + RunProfileError, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) +from dimos.utils.logging_config import setup_logger +from dimos.utils.trigonometry import angle_diff + +PlannerState: TypeAlias = Literal[ + "idle", "initial_rotation", "path_following", "final_rotation", "arrived" +] +StopMessage: TypeAlias = Literal[ + "arrived", + "obstacle_found", + "error", + "run_envelope_rejected", + "map_updated", +] + +logger = setup_logger() + + +@dataclass(frozen=True) +class ActiveRunEnvelope: + """The movement envelope governing the current goal. + + ``profile_name`` is ``None`` for the default walking behavior, where the + speed and limits come from ``planner_robot_speed`` and the + ``local_planner_*`` config fields exactly as before run profiles existed. + For a named run profile, the speed is the profile's requested speed after + any global slowdown scaling, and the limits come from the profile. + """ + + profile_name: str | None + speed_m_s: float + path_limits: PathSpeedProfileLimits + goal_decel_m_s2: float + command_overrides: CommandEnvelopeOverrides | None + + +class LocalPlanner(Resource): + cmd_vel: Subject[Twist] + stopped_navigating: Subject[StopMessage] + navigation_costmap: Subject[OccupancyGrid] + + _thread: Thread | None = None + _path: Path | None = None + _path_clearance: PathClearance | None = None + _path_distancer: PathDistancer | None = None + _current_odom: PoseStamped | None = None + + _pose_index: int + _lock: RLock + _stop_planning_event: Event + _state: PlannerState + _state_unique_id: int + _global_config: GlobalConfig + _navigation_map: NavigationMap + _goal_tolerance: float + _controller: Controller + _trajectory_tick_sink: TrajectoryControlTickSink | None + _previous_odom_for_velocity: PoseStamped | None + + _speed: float = 0.55 + _active_envelope: ActiveRunEnvelope + _path_speed_profile_s: list[float] | None + _path_speed_profile_v: list[float] | None + _path_speed_profile_path_id: int | None + _control_frequency: float + _orientation_tolerance: float = 0.35 + _navigation_costmap_interval: float = 1.0 + _navigation_costmap_last: float = 0.0 + _hot_replan: bool + + def __init__( + self, + global_config: GlobalConfig, + navigation_map: NavigationMap, + goal_tolerance: float, + *, + hot_replan: bool = False, + ) -> None: + self.cmd_vel = Subject() + self.stopped_navigating = Subject() + self.navigation_costmap = Subject() + + self._pose_index = 0 + self._lock = RLock() + self._stop_planning_event = Event() + self._state = "idle" + self._state_unique_id = 0 + self._global_config = global_config + self._navigation_map = navigation_map + self._goal_tolerance = goal_tolerance + self._hot_replan = hot_replan + self._control_frequency = float(global_config.local_planner_control_rate_hz) + self._trajectory_tick_sink = self._make_trajectory_tick_sink(global_config) + self._previous_odom_for_velocity = None + + speed = ( + float(global_config.planner_robot_speed) + if global_config.planner_robot_speed is not None + else self._speed + ) + if not math.isfinite(speed) or speed <= 0.0: + raise ValueError(f"planner speed must be a positive finite float, got {speed!r}") + if global_config.nerf_speed < 1.0: + speed *= global_config.nerf_speed + self._speed = speed + self._active_envelope = self._default_run_envelope() + self._path_speed_profile_s = None + self._path_speed_profile_v = None + self._path_speed_profile_path_id = None + + self._controller = make_local_path_controller( + self._global_config, + speed, + self._control_frequency, + ) + + def start(self) -> None: + pass + + def stop(self) -> None: + self.stop_planning() + self._close_trajectory_tick_sink() + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._current_odom = msg + + def start_planning(self, path: Path, run_profile_name: str | None = None) -> None: + self.stop_planning() + + envelope = self._resolve_run_envelope(run_profile_name) + if envelope is None: + self.stopped_navigating.on_next("run_envelope_rejected") + return + self._apply_run_envelope(envelope) + + self._stop_planning_event = Event() + + with self._lock: + self._path = path + self._path_clearance = PathClearance(self._global_config, self._path) + self._path_distancer = PathDistancer(self._path) + self._pose_index = 0 + self._previous_odom_for_velocity = None + self._rebuild_path_speed_profile(self._path_distancer) + self._thread = Thread(target=self._thread_entrypoint, daemon=True) + self._thread.start() + + def update_path(self, path: Path) -> bool: + """Swap the active path without stopping the local planner thread.""" + if not path.poses: + return False + + with self._lock: + if self._path is None or self._thread is None: + return False + + self._path = path + self._path_clearance = PathClearance(self._global_config, self._path) + self._path_distancer = PathDistancer(self._path) + current_odom = self._current_odom + if current_odom is not None: + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + self._pose_index = self._path_distancer.find_closest_point_index(current_pos) + self._rebuild_path_speed_profile(self._path_distancer) + + return True + + def _default_run_envelope(self) -> ActiveRunEnvelope: + """Walking behavior: speed and limits exactly as configured today.""" + return ActiveRunEnvelope( + profile_name=None, + speed_m_s=self._speed, + path_limits=PathSpeedProfileLimits( + max_speed_m_s=self._speed, + max_tangent_accel_m_s2=self._global_config.local_planner_max_tangent_accel_m_s2, + max_normal_accel_m_s2=self._global_config.local_planner_max_normal_accel_m_s2, + ), + goal_decel_m_s2=self._global_config.local_planner_goal_decel_m_s2, + command_overrides=None, + ) + + def _profile_run_envelope(self, profile: RunProfile) -> ActiveRunEnvelope: + speed = profile.requested_planner_speed_m_s + if self._global_config.nerf_speed < 1.0: + speed *= self._global_config.nerf_speed + return ActiveRunEnvelope( + profile_name=profile.name, + speed_m_s=speed, + path_limits=profile.path_speed_profile_limits_at(speed), + goal_decel_m_s2=profile.goal_decel_m_s2, + command_overrides=command_envelope_overrides_for_profile(profile), + ) + + def _resolve_run_envelope(self, run_profile_name: str | None) -> ActiveRunEnvelope | None: + """Resolve the movement envelope for one goal, before motion. + + A per-goal profile name wins over the session default + (``GlobalConfig.go2_run_profile``). The registry default profile keeps + the unchanged walking behavior. Any other named profile resolves its + speed and limits from the registry. + """ + name = ( + run_profile_name + if run_profile_name is not None + else self._global_config.go2_run_profile + ) + if name == GO2_RUN_PROFILES.default_profile_name: + return self._default_run_envelope() + + try: + profile = GO2_RUN_PROFILES.get(name) + except RunProfileError as exc: + logger.warning( + "run profile rejected", + profile=name, + reason=str(exc), + ) + return None + + envelope = self._profile_run_envelope(profile) + + logger.info( + "run envelope applied", + profile=profile.name, + speed_m_s=round(envelope.speed_m_s, 3), + goal_decel_m_s2=envelope.goal_decel_m_s2, + max_yaw_rate_rad_s=profile.max_yaw_rate_rad_s, + ) + return envelope + + def _apply_run_envelope(self, envelope: ActiveRunEnvelope) -> None: + self._active_envelope = envelope + controller = self._controller + if isinstance(controller, HolonomicPathController): + controller.set_command_envelope(envelope.command_overrides) + controller.set_speed(envelope.speed_m_s) + with self._lock: + path_distancer = self._path_distancer + if path_distancer is not None: + self._rebuild_path_speed_profile(path_distancer) + + def stop_planning(self) -> None: + self.cmd_vel.on_next(Twist()) + self._stop_planning_event.set() + + with self._lock: + self._thread = None + + self._reset_state() + + def _make_trajectory_tick_sink( + self, global_config: GlobalConfig + ) -> TrajectoryControlTickSink | None: + path = global_config.local_planner_trajectory_tick_log_path + if path is None or str(path).strip() == "": + return None + return JsonlTrajectoryControlTickSink(path) + + def _close_trajectory_tick_sink(self) -> None: + sink = self._trajectory_tick_sink + close = getattr(sink, "close", None) + if callable(close): + close() + self._trajectory_tick_sink = None + + def get_state(self) -> NavigationState: + with self._lock: + state = self._state + + match state: + case "idle" | "arrived": + return NavigationState.IDLE + case "initial_rotation" | "path_following" | "final_rotation": + return NavigationState.FOLLOWING_PATH + case _: + raise ValueError(f"Unknown planner state: {state}") + + def planner_speed_m_s(self) -> float: + with self._lock: + return self._active_envelope.speed_m_s + + def get_unique_state(self) -> tuple[PlannerState, int]: + with self._lock: + return (self._state, self._state_unique_id) + + def _thread_entrypoint(self) -> None: + try: + self._loop() + except Exception as e: + traceback.print_exc() + logger.exception("Error in local planning", exc_info=e) + self.stopped_navigating.on_next("error") + finally: + self._reset_state() + self.cmd_vel.on_next(Twist()) + + def _change_state(self, new_state: PlannerState) -> None: + if new_state == self._state: + return + self._state = new_state + self._state_unique_id += 1 + logger.info("changed state", state=new_state) + + def _loop(self) -> None: + stop_event = self._stop_planning_event + + with self._lock: + path = self._path + path_clearance = self._path_clearance + current_odom = self._current_odom + + if path is None or path_clearance is None: + raise RuntimeError("No path set for local planner.") + + # Determine initial state: skip initial_rotation if already aligned. + new_state: PlannerState = "initial_rotation" + if current_odom is not None and len(path.poses) > 0: + first_yaw = path.poses[0].orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + initial_yaw_error = angle_diff(first_yaw, robot_yaw) + self._controller.reset_yaw_error(initial_yaw_error) + angle_in_tolerance = abs(initial_yaw_error) < self._orientation_tolerance + if angle_in_tolerance: + position_in_tolerance = ( + path.poses[0].position.distance(current_odom.position) < 0.01 + ) + if position_in_tolerance: + new_state = "final_rotation" + else: + new_state = "path_following" + elif self._holonomic_yaw_lock_active(current_odom): + # Holonomic can translate without aligning first; in narrow + # corridors an initial spin would sweep the body into walls. + new_state = "path_following" + + with self._lock: + self._change_state(new_state) + + while not stop_event.is_set(): + start_time = time.perf_counter() + + with self._lock: + path_clearance.set_planner_speed(self._active_envelope.speed_m_s) + path_clearance.update_costmap(self._navigation_map.binary_costmap) + path_clearance.update_pose_index(self._pose_index) + + self._send_navigation_costmap(path, path_clearance) + + if path_clearance.is_obstacle_ahead(): + if self._hot_replan: + logger.info("Obstacle detected ahead, requesting hot replan.") + self.stopped_navigating.on_next("obstacle_found") + self.cmd_vel.on_next(Twist()) + else: + logger.info("Obstacle detected ahead, stopping local planner.") + self.stopped_navigating.on_next("obstacle_found") + break + elapsed = time.perf_counter() - start_time + sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) + stop_event.wait(sleep_time) + continue + + with self._lock: + state: PlannerState = self._state + + if state == "initial_rotation": + cmd_vel = self._compute_initial_rotation() + elif state == "path_following": + cmd_vel = self._compute_path_following() + elif state == "final_rotation": + cmd_vel = self._compute_final_rotation() + elif state == "arrived": + self.stopped_navigating.on_next("arrived") + break + elif state == "idle": + cmd_vel = None + + if cmd_vel is not None: + self.cmd_vel.on_next(cmd_vel) + + elapsed = time.perf_counter() - start_time + sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) + stop_event.wait(sleep_time) + + if stop_event.is_set(): + logger.info("Local planner loop exited due to stop event.") + + def _compute_initial_rotation(self) -> Twist: + with self._lock: + path = self._path + current_odom = self._current_odom + + assert path is not None + assert current_odom is not None + + if self._holonomic_yaw_lock_active(current_odom): + with self._lock: + self._change_state("path_following") + return self._compute_path_following() + + first_pose = path.poses[0] + first_yaw = first_pose.orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + yaw_error = angle_diff(first_yaw, robot_yaw) + + if abs(yaw_error) < self._orientation_tolerance: + with self._lock: + self._change_state("path_following") + return self._compute_path_following() + + self._controller.set_speed(self._active_envelope.speed_m_s) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) + ref_pose = _pose_from_xy_yaw( + float(current_odom.position.x), + float(current_odom.position.y), + float(first_yaw), + ) + self._append_trajectory_control_tick( + ref_pose, + Twist(), + current_odom, + measured_body_twist, + cmd, + ) + return cmd + + def get_distance_to_path(self) -> float | None: + with self._lock: + path_distancer = self._path_distancer + current_odom = self._current_odom + + if path_distancer is None or current_odom is None: + return None + + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + + return path_distancer.get_distance_to_path(current_pos) + + def _compute_path_following(self) -> Twist: + with self._lock: + path_distancer = self._path_distancer + current_odom = self._current_odom + + assert path_distancer is not None + assert current_odom is not None + + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + + if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: + logger.info("Reached goal position, starting final rotation") + with self._lock: + self._change_state("final_rotation") + return self._compute_final_rotation() + + closest_index = path_distancer.find_closest_point_index(current_pos) + + with self._lock: + self._pose_index = closest_index + + path_speed = self._path_speed_for_index(path_distancer, closest_index, current_pos) + self._controller.set_speed(path_speed) + reference_sample = self._lookahead_reference_sample( + path_distancer, + current_odom, + current_pos, + path_speed, + yaw_lock_rad=self._holonomic_yaw_lock_yaw_rad(current_odom), + ) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + cmd = self._controller.advance_reference( + reference_sample, + current_odom, + measured_body_twist, + ) + self._append_trajectory_control_sample( + reference_sample, + current_odom, + measured_body_twist, + cmd, + ) + return cmd + + def _lookahead_reference_sample( + self, + path_distancer: PathDistancer, + current_odom: PoseStamped, + current_pos: np.ndarray, + path_speed: float, + *, + yaw_lock_rad: float | None = None, + ) -> TrajectoryReferenceSample: + projection = path_distancer.project(current_pos) + s_start = float(projection.s_along_path_m) + s_end = min( + path_distancer.path_length_m, + s_start + path_distancer.lookahead_distance_m, + ) + now_s = float(current_odom.ts) + if not math.isfinite(now_s): + now_s = 0.0 + travel_s = max(0.0, s_end - s_start) + dt_s = 1.0 / self._control_frequency + duration_s = max(dt_s, travel_s / max(path_speed, 1e-6)) + return self._reference_sample_at_progress( + path_distancer, + s_end, + now_s + duration_s, + path_speed, + yaw_lock_rad=yaw_lock_rad, + ) + + def _reference_sample_at_progress( + self, + path_distancer: PathDistancer, + progress_m: float, + time_s: float, + path_speed: float, + *, + yaw_lock_rad: float | None = None, + ) -> TrajectoryReferenceSample: + point = path_distancer.point_at_progress(progress_m) + path_yaw = path_distancer.yaw_at_progress(progress_m) + ref_yaw = path_yaw if yaw_lock_rad is None else float(yaw_lock_rad) + if yaw_lock_rad is None: + feedforward = Twist( + linear=Vector3(path_speed, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + else: + delta = angle_diff(path_yaw, ref_yaw) + feedforward = Twist( + linear=Vector3( + path_speed * math.cos(delta), + path_speed * math.sin(delta), + 0.0, + ), + angular=Vector3(0.0, 0.0, 0.0), + ) + return TrajectoryReferenceSample( + time_s=time_s, + pose_plan=_pose_from_xy_yaw(float(point[0]), float(point[1]), ref_yaw), + twist_body=feedforward, + ) + + def _path_speed_for_index( + self, + path_distancer: PathDistancer, + closest_index: int, + current_pos: np.ndarray, + ) -> float: + del closest_index + self._ensure_path_speed_profile(path_distancer) + envelope = self._active_envelope + progress_m = float(path_distancer.project(current_pos).s_along_path_m) + profile_speed = self._profiled_path_speed_m_s(progress_m) + distance_cap = math.sqrt( + max( + 0.0, + 2.0 * envelope.goal_decel_m_s2 * path_distancer.distance_to_goal(current_pos), + ) + ) + capped = min(envelope.speed_m_s, profile_speed, distance_cap) + return min(envelope.speed_m_s, max(0.05, capped)) + + def _profiled_path_speed_m_s(self, progress_m: float) -> float: + s_profile = self._path_speed_profile_s + v_profile = self._path_speed_profile_v + if s_profile is None or v_profile is None: + return self._active_envelope.speed_m_s + return speed_at_progress_m(progress_m, s_profile, v_profile) + + def _ensure_path_speed_profile(self, path_distancer: PathDistancer) -> None: + path_id = id(path_distancer._path) + if ( + self._path_speed_profile_s is None + or self._path_speed_profile_path_id != path_id + ): + self._rebuild_path_speed_profile(path_distancer) + self._path_speed_profile_path_id = path_id + + def _rebuild_path_speed_profile(self, path_distancer: PathDistancer) -> None: + envelope = self._active_envelope + s_profile, v_profile = profile_speed_along_polyline( + path_distancer._path, + path_distancer._cumulative_dists, + envelope.path_limits, + envelope.goal_decel_m_s2, + ) + self._path_speed_profile_s = s_profile + self._path_speed_profile_v = v_profile + self._path_speed_profile_path_id = id(path_distancer._path) + + def _compute_final_rotation(self) -> Twist: + with self._lock: + path = self._path + current_odom = self._current_odom + + assert path is not None + assert current_odom is not None + + if self._holonomic_yaw_lock_active(current_odom): + logger.info( + "Final rotation deferred: position goal reached in narrow corridor", + ) + with self._lock: + self._change_state("arrived") + return Twist() + + goal_yaw = path.poses[-1].orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + yaw_error = angle_diff(goal_yaw, robot_yaw) + + if abs(yaw_error) < self._orientation_tolerance: + logger.info("Final rotation complete, goal reached") + with self._lock: + self._change_state("arrived") + return Twist() + + self._controller.set_speed(self._active_envelope.speed_m_s) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) + ref_pose = _pose_from_xy_yaw( + float(current_odom.position.x), + float(current_odom.position.y), + float(goal_yaw), + ) + self._append_trajectory_control_tick( + ref_pose, + Twist(), + current_odom, + measured_body_twist, + cmd, + ) + return cmd + + def _reset_state(self) -> None: + with self._lock: + self._change_state("idle") + self._path = None + self._path_clearance = None + self._path_distancer = None + self._pose_index = 0 + self._previous_odom_for_velocity = None + self._controller.set_speed(self._active_envelope.speed_m_s) + self._controller.reset_errors() + + def _append_trajectory_control_tick( + self, + reference_pose: Pose, + reference_twist: Twist, + current_odom: PoseStamped, + measured_body_twist: Twist, + command: Twist, + ) -> None: + reference = TrajectoryReferenceSample( + time_s=float(current_odom.ts), + pose_plan=reference_pose, + twist_body=reference_twist, + ) + self._append_trajectory_control_sample( + reference, + current_odom, + measured_body_twist, + command, + ) + + def _append_trajectory_control_sample( + self, + reference: TrajectoryReferenceSample, + current_odom: PoseStamped, + measured_body_twist: Twist, + command: Twist, + ) -> None: + sink = self._trajectory_tick_sink + if sink is None: + return + measurement = TrajectoryMeasuredSample( + time_s=float(current_odom.ts), + pose_plan=Pose(current_odom.position, current_odom.orientation), + twist_body=measured_body_twist, + ) + append_trajectory_control_tick( + sink, + reference, + measurement, + command, + 1.0 / self._control_frequency, + wall_time_s=time.time(), + ) + + def _estimate_measured_body_twist(self, current_odom: PoseStamped) -> Twist: + previous = self._previous_odom_for_velocity + self._previous_odom_for_velocity = current_odom + if previous is None: + return Twist() + dt = float(current_odom.ts) - float(previous.ts) + if not math.isfinite(dt) or dt <= 0.0: + return Twist() + vx_w = (float(current_odom.position.x) - float(previous.position.x)) / dt + vy_w = (float(current_odom.position.y) - float(previous.position.y)) / dt + yaw = float(current_odom.orientation.euler[2]) + c = math.cos(yaw) + s = math.sin(yaw) + vx_b = c * vx_w + s * vy_w + vy_b = -s * vx_w + c * vy_w + wz = ( + angle_diff( + float(current_odom.orientation.euler[2]), + float(previous.orientation.euler[2]), + ) + / dt + ) + return Twist( + linear=Vector3(vx_b, vy_b, 0.0), + angular=Vector3(0.0, 0.0, wz), + ) + + def _holonomic_yaw_lock_active(self, current_odom: PoseStamped) -> bool: + if self._global_config.local_planner_path_controller != "holonomic": + return False + return self._holonomic_yaw_lock_yaw_rad(current_odom) is not None + + def _holonomic_yaw_lock_yaw_rad(self, current_odom: PoseStamped) -> float | None: + if self._global_config.local_planner_path_controller != "holonomic": + return None + try: + costmap = self._navigation_map.binary_costmap + except ValueError: + return float(current_odom.orientation.euler[2]) + rotation_radius_m = self._global_config.robot_rotation_diameter / 2.0 + if can_rotate_in_place(costmap, current_odom, rotation_radius_m): + return None + return float(current_odom.orientation.euler[2]) + + def _send_navigation_costmap(self, path: Path, path_clearance: PathClearance) -> None: + if "DEBUG_NAVIGATION" not in os.environ: + return + + now = time.time() + if now - self._navigation_costmap_last < self._navigation_costmap_interval: + return + + self._navigation_costmap_last = now + + self.navigation_costmap.on_next(self._navigation_map.gradient_costmap) + + +def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: + return Pose( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + ) diff --git a/dimos/navigation/dannav/module.py b/dimos/navigation/dannav/module.py new file mode 100644 index 0000000000..28c5fd977d --- /dev/null +++ b/dimos/navigation/dannav/module.py @@ -0,0 +1,190 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from typing import Any, Literal + +from dimos_lcm.std_msgs import Bool, String +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.base import NavigationInterface, NavigationState +from dimos.navigation.dannav.global_planner import GlobalPlanner +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import GO2_RUN_PROFILES, RunProfileError +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class DannavPlannerConfig(ModuleConfig): + robot_width: float | None = None + robot_rotation_diameter: float | None = None + replan_on_costmap_update: bool = False + path_controller: Literal["differential", "holonomic"] | None = None + + +class DannavPlanner(Module, NavigationInterface): + config: DannavPlannerConfig + + odom: In[PoseStamped] # TODO: Use TF. + odometry: In[Odometry] + global_costmap: In[OccupancyGrid] + goal_request: In[PoseStamped] + clicked_point: In[PointStamped] + target: In[PoseStamped] + stop_movement: In[Bool] + + goal_reached: Out[Bool] + navigation_state: Out[String] # TODO: set it + nav_cmd_vel: Out[Twist] + path: Out[Path] + navigation_costmap: Out[OccupancyGrid] + + _planner: GlobalPlanner + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + overrides = { + name: value + for name, value in ( + ("robot_width", self.config.robot_width), + ("robot_rotation_diameter", self.config.robot_rotation_diameter), + ("local_planner_path_controller", self.config.path_controller or "holonomic"), + ) + if value is not None + } + effective_global_config = ( + self.config.g.model_copy(update=overrides) if overrides else self.config.g + ) + self._planner = GlobalPlanner( + effective_global_config, + replan_on_costmap_update=self.config.replan_on_costmap_update, + ) + logger.info( + "DannavPlanner configured.", + path_controller=effective_global_config.local_planner_path_controller, + replan_on_costmap_update=self.config.replan_on_costmap_update, + ) + + @rpc + def start(self) -> None: + super().start() + + self.register_disposable(Disposable(self.odom.subscribe(self._planner.handle_odom))) + self.register_disposable( + Disposable( + self.odometry.subscribe( + lambda msg: self._planner.handle_odom(msg.to_pose_stamped()) + ) + ) + ) + self.register_disposable( + Disposable(self.global_costmap.subscribe(self._planner.handle_global_costmap)) + ) + self.register_disposable( + Disposable(self.goal_request.subscribe(self._planner.handle_goal_request)) + ) + self.register_disposable( + Disposable(self.target.subscribe(self._planner.handle_goal_request)) + ) + + self.register_disposable( + Disposable( + self.clicked_point.subscribe( + lambda pt: self._planner.handle_goal_request(pt.to_pose_stamped()) + ) + ) + ) + + if self.stop_movement.transport is not None: + self.register_disposable( + Disposable(self.stop_movement.subscribe(self._on_stop_movement)) + ) + + self.register_disposable(self._planner.path.subscribe(self.path.publish)) + + self.register_disposable(self._planner.cmd_vel.subscribe(self.nav_cmd_vel.publish)) + + self.register_disposable(self._planner.goal_reached.subscribe(self.goal_reached.publish)) + + if "DEBUG_NAVIGATION" in os.environ: + self.register_disposable( + self._planner.navigation_costmap.subscribe(self.navigation_costmap.publish) + ) + + self._planner.start() + + @rpc + def stop(self) -> None: + self.cancel_goal() + self._planner.stop() + + super().stop() + + def _on_stop_movement(self, msg: Bool) -> None: + if msg.data: + self.cancel_goal() + + @rpc + def set_goal(self, goal: PoseStamped, profile: str | None = None) -> bool: + self._planner.handle_goal_request(goal, run_profile=profile) + return True + + @rpc + def set_run_profile(self, profile: str) -> bool: + """Set the session-default movement envelope for subsequent goals. + + Validated against the run-profile registry so a bad name cannot poison + the planner config. + """ + try: + GO2_RUN_PROFILES.get(profile) + except RunProfileError as exc: + logger.warning("Rejected run profile.", profile=profile, reason=str(exc)) + return False + self._planner.set_run_profile(profile) + return True + + @rpc + def get_state(self) -> NavigationState: + return self._planner.get_state() + + @rpc + def is_goal_reached(self) -> bool: + return self._planner.is_goal_reached() + + @rpc + def cancel_goal(self) -> bool: + self._planner.cancel_goal() + return True + + @rpc + def set_replanning_enabled(self, enabled: bool) -> None: + self._planner.set_replanning_enabled(enabled) + + @rpc + def set_safe_goal_clearance(self, clearance: float) -> None: + self._planner.set_safe_goal_clearance(clearance) + + @rpc + def reset_safe_goal_clearance(self) -> None: + self._planner.reset_safe_goal_clearance() diff --git a/dimos/navigation/dannav/path_clearance.py b/dimos/navigation/dannav/path_clearance.py new file mode 100644 index 0000000000..232c5e905b --- /dev/null +++ b/dimos/navigation/dannav/path_clearance.py @@ -0,0 +1,125 @@ +# 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. + +from threading import RLock +import math + +import numpy as np +from numpy.typing import NDArray + +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.path_mask import make_path_mask +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path + +# Minimum corridor check length (walk-speed baseline). +PATH_CLEARANCE_LOOKUP_DISTANCE_M = 3.0 +# Scale lookahead so faster profiles keep ~constant reaction time. +PATH_CLEARANCE_LOOKUP_TIME_S = 5.0 +# Treat elevated costmap cells as blocking (matches navigation gradient threshold). +PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD = 50 + + +def path_clearance_lookup_distance_m(speed_m_s: float) -> float: + """Return speed-scaled path-corridor check distance in meters.""" + if not math.isfinite(speed_m_s) or speed_m_s <= 0.0: + return PATH_CLEARANCE_LOOKUP_DISTANCE_M + return max(PATH_CLEARANCE_LOOKUP_DISTANCE_M, PATH_CLEARANCE_LOOKUP_TIME_S * speed_m_s) + + +class PathClearance: + _costmap: OccupancyGrid | None = None + _last_costmap: OccupancyGrid | None = None + _path_lookup_distance: float + _obstacle_cost_threshold: int = PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD + _max_distance_cache: float = 1.0 + _last_used_shape: tuple[int, ...] | None = None + _last_mask: NDArray[np.bool_] | None = None + _last_used_pose: int | None = None + _global_config: GlobalConfig + _lock: RLock + _path: Path + _pose_index: int + + def __init__(self, global_config: GlobalConfig, path: Path) -> None: + self._global_config = global_config + self._path = path + self._pose_index = 0 + self._lock = RLock() + self._path_lookup_distance = PATH_CLEARANCE_LOOKUP_DISTANCE_M + + def set_planner_speed(self, speed_m_s: float) -> None: + lookup_distance = path_clearance_lookup_distance_m(speed_m_s) + with self._lock: + if abs(lookup_distance - self._path_lookup_distance) > 0.01: + self._path_lookup_distance = lookup_distance + self._last_mask = None + self._last_used_pose = None + self._last_used_shape = None + + def update_costmap(self, costmap: OccupancyGrid) -> None: + with self._lock: + self._costmap = costmap + + def update_pose_index(self, index: int) -> None: + with self._lock: + self._pose_index = index + + @property + def mask(self) -> NDArray[np.bool_]: + with self._lock: + costmap = self._costmap + pose_index = self._pose_index + + assert costmap is not None + + if ( + self._last_mask is not None + and self._last_used_pose is not None + and costmap.grid.shape == self._last_used_shape + and self._pose_distance(self._last_used_pose, pose_index) < self._max_distance_cache + ): + return self._last_mask + + self._last_mask = make_path_mask( + occupancy_grid=costmap, + path=self._path, + robot_width=self._global_config.robot_width, + pose_index=pose_index, + max_length=self._path_lookup_distance, + ) + + self._last_used_shape = costmap.grid.shape + self._last_used_pose = pose_index + + return self._last_mask + + def is_obstacle_ahead(self) -> bool: + with self._lock: + costmap = self._costmap + + if costmap is None: + return True + + cells = costmap.grid[self.mask] + return bool( + np.any( + (cells >= self._obstacle_cost_threshold) & (cells != CostValues.UNKNOWN) + ) + ) + + def _pose_distance(self, index1: int, index2: int) -> float: + p1 = self._path.poses[index1].position + p2 = self._path.poses[index2].position + return p1.distance(p2) diff --git a/dimos/navigation/dannav/path_distancer.py b/dimos/navigation/dannav/path_distancer.py new file mode 100644 index 0000000000..23f373180a --- /dev/null +++ b/dimos/navigation/dannav/path_distancer.py @@ -0,0 +1,131 @@ +# 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. + +from typing import cast + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.holonomic_trajectory_controller.trajectory_metrics import PolylineProjection, project_to_polyline + + +class PathDistancer: + _lookahead_dist: float = 0.5 + _path: NDArray[np.float64] + _cumulative_dists: NDArray[np.float64] + + def __init__(self, path: Path) -> None: + self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) + self._cumulative_dists = _make_cumulative_distance_array(self._path) + + def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: + """ + Given a path, and a precomputed array of cumulative distances, find the + point which is `lookahead_dist` ahead of the current point. + """ + + if start_idx >= len(self._path) - 1: + return cast("NDArray[np.float64]", self._path[-1]) + + # Distance from path[0] to path[start_idx]. + base_dist = self._cumulative_dists[start_idx - 1] if start_idx > 0 else 0.0 + target_dist = base_dist + self._lookahead_dist + + # Binary search: cumulative_dists[i] = distance from path[0] to path[i+1] + idx = int(np.searchsorted(self._cumulative_dists, target_dist)) + + if idx >= len(self._cumulative_dists): + return cast("NDArray[np.float64]", self._path[-1]) + + # Interpolate within segment from path[idx] to path[idx+1]. + prev_cum_dist = self._cumulative_dists[idx - 1] if idx > 0 else 0.0 + segment_dist = self._cumulative_dists[idx] - prev_cum_dist + remaining_dist = target_dist - prev_cum_dist + + if segment_dist > 0: + t = remaining_dist / segment_dist + return cast( + "NDArray[np.float64]", + self._path[idx] + t * (self._path[idx + 1] - self._path[idx]), + ) + + return cast("NDArray[np.float64]", self._path[idx]) + + @property + def lookahead_distance_m(self) -> float: + return self._lookahead_dist + + @property + def path_length_m(self) -> float: + if len(self._path) < 2: + return 0.0 + return float(self._cumulative_dists[-1]) + + def point_at_progress(self, progress_m: float) -> NDArray[np.float64]: + if len(self._path) == 0: + raise ValueError("path must be non-empty") + if len(self._path) == 1: + return cast("NDArray[np.float64]", self._path[0].copy()) + + s = float(np.clip(progress_m, 0.0, self.path_length_m)) + idx = int(np.searchsorted(self._cumulative_dists, s)) + if idx >= len(self._cumulative_dists): + return cast("NDArray[np.float64]", self._path[-1].copy()) + + prev_s = self._cumulative_dists[idx - 1] if idx > 0 else 0.0 + segment_s = self._cumulative_dists[idx] - prev_s + if segment_s <= 1e-12: + return cast("NDArray[np.float64]", self._path[idx].copy()) + alpha = (s - prev_s) / segment_s + point = self._path[idx] + alpha * (self._path[idx + 1] - self._path[idx]) + return cast("NDArray[np.float64]", point) + + def yaw_at_progress(self, progress_m: float) -> float: + if len(self._path) < 2: + return 0.0 + s = float(np.clip(progress_m, 0.0, self.path_length_m)) + idx = int(np.searchsorted(self._cumulative_dists, s)) + idx = min(max(idx, 0), len(self._path) - 2) + direction = self._path[idx + 1] - self._path[idx] + return float(np.arctan2(direction[1], direction[0])) + + def project(self, pos: NDArray[np.float64]) -> PolylineProjection: + return project_to_polyline(float(pos[0]), float(pos[1]), self._path) + + def distance_to_goal(self, current_pos: NDArray[np.float64]) -> float: + return float(np.linalg.norm(self._path[-1] - current_pos)) + + def get_distance_to_path(self, pos: NDArray[np.float64]) -> float: + index = self.find_closest_point_index(pos) + return float(np.linalg.norm(self._path[index] - pos)) + + def find_closest_point_index(self, pos: NDArray[np.float64]) -> int: + """Find the index of the closest point on the path.""" + distances = np.linalg.norm(self._path - pos, axis=1) + return int(np.argmin(distances)) + + +def _make_cumulative_distance_array(array: NDArray[np.float64]) -> NDArray[np.float64]: + """ + For an array representing 2D points, create an array of all the distances + between the points. + """ + + if len(array) < 2: + return np.array([0.0]) + + segments = array[1:] - array[:-1] + segment_dists = np.linalg.norm(segments, axis=1) + return np.cumsum(segment_dists) diff --git a/dimos/navigation/dannav/rotation_clearance.py b/dimos/navigation/dannav/rotation_clearance.py new file mode 100644 index 0000000000..f3ee839392 --- /dev/null +++ b/dimos/navigation/dannav/rotation_clearance.py @@ -0,0 +1,64 @@ +# 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. + +"""Map-based checks for in-place rotation clearance in holonomic local planning.""" + +from __future__ import annotations + +import math + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid + +PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD = 50 + + +def can_rotate_in_place( + costmap: OccupancyGrid, + pose: PoseStamped, + rotation_radius_m: float, + *, + obstacle_cost_threshold: int = PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD, +) -> bool: + """Return True when a full in-place spin fits inside free map cells. + + Samples every grid cell whose center lies inside a circle of + ``rotation_radius_m`` around ``pose``. Any unknown or occupied cell + (cost >= ``obstacle_cost_threshold``) makes rotation unsafe. + """ + if rotation_radius_m <= 0.0 or not math.isfinite(rotation_radius_m): + return True + + grid_center = costmap.world_to_grid(pose.position) + center_x = int(round(float(grid_center.x))) + center_y = int(round(float(grid_center.y))) + radius_cells = int(math.ceil(rotation_radius_m / costmap.resolution)) + radius_cells_sq = radius_cells * radius_cells + + for dy in range(-radius_cells, radius_cells + 1): + for dx in range(-radius_cells, radius_cells + 1): + if dx * dx + dy * dy > radius_cells_sq: + continue + cell_x = center_x + dx + cell_y = center_y + dy + if not (0 <= cell_x < costmap.width and 0 <= cell_y < costmap.height): + return False + cell_value = int(costmap.grid[cell_y, cell_x]) + if cell_value == CostValues.UNKNOWN or cell_value >= obstacle_cost_threshold: + return False + + return True + + +__all__ = ["can_rotate_in_place"] diff --git a/dimos/navigation/dannav/simple_global_planner.py b/dimos/navigation/dannav/simple_global_planner.py new file mode 100644 index 0000000000..d424051a9c --- /dev/null +++ b/dimos/navigation/dannav/simple_global_planner.py @@ -0,0 +1,130 @@ +# 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. + +"""Intended for testing and experimenting only + +Loop A* global planner: replans on every new occupancy grid. + +``_on_costmap`` and ``_on_goal`` both call ``_replan``; the stored costmap, goal +(from ``clicked_point``), and robot start (from ``odom``) feed ``min_cost_astar`` +and the resulting ``Path`` is published on ``path``. There is no goal-following +state machine here: a fresh grid means a fresh A* run, and the downstream +``dannav.HolonomicController`` tracks whatever path is published. +""" + +from __future__ import annotations + +import threading +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import VectorLike +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class SimpleGlobalPlannerConfig(ModuleConfig): + goal_tolerance: float = 0.2 + + +class SimpleGlobalPlanner(Module): + config: SimpleGlobalPlannerConfig + + global_costmap: In[OccupancyGrid] + clicked_point: In[PointStamped] + odom: In[PoseStamped] + + path: Out[Path] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._lock = threading.Lock() + self._costmap: OccupancyGrid | None = None + self._goal: PoseStamped | None = None + self._odom: PoseStamped | None = None + + @rpc + def start(self) -> None: + super().start() + + self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) + self.register_disposable(Disposable(self.global_costmap.subscribe(self._on_costmap))) + self.register_disposable( + Disposable( + self.clicked_point.subscribe( + lambda point: self._on_goal(point.to_pose_stamped()) + ) + ) + ) + + @rpc + def stop(self) -> None: + super().stop() + + def plan( + self, + costmap: OccupancyGrid, + goal: PoseStamped, + start: VectorLike, + ) -> Path | None: + return min_cost_astar(costmap, goal.position, start) + + def _on_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._odom = msg + + def _on_costmap(self, costmap: OccupancyGrid) -> None: + with self._lock: + self._costmap = costmap + self._replan() + + def _on_goal(self, goal: PoseStamped) -> None: + logger.info("New goal", x=round(goal.x, 3), y=round(goal.y, 3)) + with self._lock: + self._goal = goal + self._replan() + + def _replan(self) -> None: + with self._lock: + costmap = self._costmap + goal = self._goal + odom = self._odom + + if costmap is None or goal is None or odom is None: + return + + start = (float(odom.position.x), float(odom.position.y)) + if goal.position.distance(odom.position) < self.config.goal_tolerance: + return + + path = self.plan(costmap, goal, start) + if path is None or not path.poses: + logger.warning( + "No path found.", + goal_x=round(goal.x, 3), + goal_y=round(goal.y, 3), + ) + return + + self.path.publish(path) diff --git a/dimos/navigation/dannav/test_local_planner_path_controller.py b/dimos/navigation/dannav/test_local_planner_path_controller.py new file mode 100644 index 0000000000..d3b5073e9d --- /dev/null +++ b/dimos/navigation/dannav/test_local_planner_path_controller.py @@ -0,0 +1,679 @@ +# 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. + +"""P3-3: ``LocalPlanner`` path controller switch and holonomic integration smoke.""" + +from __future__ import annotations + +import math +from pathlib import Path as FsPath +import time +from typing import Any + +import numpy as np +import pytest + +from dimos.core.global_config import GlobalConfig +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.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.dannav.controllers import ( + HolonomicPathController, + make_local_path_controller, +) +from dimos.navigation.dannav.local_planner import LocalPlanner +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.dannav.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.utils.trigonometry import angle_diff + + +def _planar_speed_m_s(cmd: Twist) -> float: + return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="map", + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append(_pose_stamped(point[0], point[1], yaw)) + return Path(frame_id="map", poses=poses) + + +def _free_navigation_map(global_config: GlobalConfig) -> NavigationMap: + nav = NavigationMap(global_config, "gradient") + nav.update( + OccupancyGrid( + grid=np.zeros((200, 200), dtype=np.int8), + resolution=0.05, + origin=Pose(-2.0, -2.0, 0.0), + frame_id="map", + ts=1.0, + ) + ) + return nav + + +def _integrate_holonomic_pose( + x_m: float, + y_m: float, + yaw_rad: float, + cmd_body: Twist, + dt_s: float, +) -> tuple[float, float, float]: + """Integrate body-frame cmd_vel in the world frame (test harness only).""" + vx = float(cmd_body.linear.x) + vy = float(cmd_body.linear.y) + wz = float(cmd_body.angular.z) + c = math.cos(yaw_rad) + s = math.sin(yaw_rad) + x_m += (c * vx - s * vy) * dt_s + y_m += (s * vx + c * vy) * dt_s + yaw_rad += wz * dt_s + yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) + return x_m, y_m, yaw_rad + + +class _LocalPlannerHarnessResult: + def __init__( + self, + *, + command_history: list[Twist], + stop_messages: list[str], + final_x_m: float, + final_y_m: float, + final_yaw_rad: float, + ) -> None: + self.command_history = command_history + self.stop_messages = stop_messages + self.final_x_m = final_x_m + self.final_y_m = final_y_m + self.final_yaw_rad = final_yaw_rad + + +class _RecordingHolonomicTrackingController(HolonomicTrackingController): + def __init__(self) -> None: + super().__init__(k_position_per_s=0.0, k_yaw_per_s=0.0) + self.measurements: list[TrajectoryMeasuredSample] = [] + + def control( + self, + reference: TrajectoryReferenceSample, + measurement: TrajectoryMeasuredSample, + ) -> Twist: + self.measurements.append(measurement) + return super().control(reference, measurement) + + +def _local_planner_with_recording_holonomic_controller() -> tuple[ + LocalPlanner, _RecordingHolonomicTrackingController +]: + g = GlobalConfig(local_planner_path_controller="holonomic") + nav = NavigationMap(g, "gradient") + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) + lp._path = path + lp._path_distancer = PathDistancer(path) + controller = lp._controller + assert isinstance(controller, HolonomicPathController) + recorder = _RecordingHolonomicTrackingController() + controller._inner = recorder + return lp, recorder + + +def _run_local_planner_harness( + tmp_path: FsPath, + *, + points: list[tuple[float, float]], + initial_yaw_rad: float = 0.0, + speed_m_s: float = 1.0, + max_normal_accel_m_s2: float = 0.6, + max_tangent_accel_m_s2: float = 1.0, + max_goal_decel_m_s2: float = 1.0, + max_ticks: int = 260, +) -> _LocalPlannerHarnessResult: + rate_hz = 60.0 + dt_s = 1.0 / rate_hz + global_config = GlobalConfig( + local_planner_path_controller="holonomic", + local_planner_control_rate_hz=rate_hz, + planner_robot_speed=speed_m_s, + local_planner_max_normal_accel_m_s2=max_normal_accel_m_s2, + local_planner_max_tangent_accel_m_s2=max_tangent_accel_m_s2, + local_planner_goal_decel_m_s2=max_goal_decel_m_s2, + local_planner_max_planar_cmd_accel_m_s2=8.0, + local_planner_max_yaw_accel_rad_s2=8.0, + ) + planner = LocalPlanner(global_config, _free_navigation_map(global_config), goal_tolerance=0.08) + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, initial_yaw_rad + latest_cmd = Twist() + command_history: list[Twist] = [] + stop_messages: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + command_history.append(Twist(cmd)) + + cmd_sub = planner.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = planner.stopped_navigating.subscribe(stop_messages.append) + sim_time_s = 1.0 + + try: + planner.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) + planner.start_planning(_path_from_points(points)) + for _ in range(max_ticks): + if "arrived" in stop_messages: + break + time.sleep(dt_s * 1.1) + plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( + plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s + ) + sim_time_s += dt_s + planner.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) + ) + finally: + planner.stop() + cmd_sub.dispose() + stop_sub.dispose() + + return _LocalPlannerHarnessResult( + command_history=command_history, + stop_messages=stop_messages, + final_x_m=plant_x_m, + final_y_m=plant_y_m, + final_yaw_rad=plant_yaw_rad, + ) + + +def test_holonomic_path_controller_slews_first_command_from_rest() -> None: + g = GlobalConfig(local_planner_path_controller="holonomic") + ctrl = HolonomicPathController( + g, + speed=0.55, + control_frequency=10.0, + k_position_per_s=2.0, + k_yaw_per_s=1.5, + ) + odom = PoseStamped(frame_id="map", position=[0.0, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)) + out = ctrl.advance(np.array([0.5, 0.0], dtype=np.float64), odom) + assert math.hypot(float(out.linear.x), float(out.linear.y)) == pytest.approx(0.5) + + +def test_local_planner_odom_delta_passes_estimated_measured_body_twist() -> None: + lp, recorder = _local_planner_with_recording_holonomic_controller() + lp._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) + lp._compute_path_following() + lp._current_odom = _pose_stamped(0.3, -0.1, 0.2, ts=1.5) + + lp._compute_path_following() + + measured = recorder.measurements[-1].twist_body + vx_w = 0.3 / 0.5 + vy_w = -0.1 / 0.5 + assert measured.linear.x == pytest.approx(math.cos(0.2) * vx_w + math.sin(0.2) * vy_w) + assert measured.linear.y == pytest.approx(-math.sin(0.2) * vx_w + math.cos(0.2) * vy_w) + assert measured.angular.z == pytest.approx(0.2 / 0.5) + + +def test_local_planner_rotation_passes_estimated_measured_body_twist() -> None: + lp, recorder = _local_planner_with_recording_holonomic_controller() + lp._current_odom = _pose_stamped(0.0, 0.0, 1.0, ts=1.0) + lp._compute_initial_rotation() + lp._current_odom = _pose_stamped(0.2, 0.1, 0.8, ts=1.4) + + lp._compute_initial_rotation() + + measured = recorder.measurements[-1].twist_body + vx_w = 0.2 / 0.4 + vy_w = 0.1 / 0.4 + assert measured.linear.x == pytest.approx(math.cos(0.8) * vx_w + math.sin(0.8) * vy_w) + assert measured.linear.y == pytest.approx(-math.sin(0.8) * vx_w + math.cos(0.8) * vy_w) + assert measured.angular.z == pytest.approx(angle_diff(0.8, 1.0) / 0.4) + + +def test_local_planner_invalid_dt_passes_zero_measured_body_twist() -> None: + lp, recorder = _local_planner_with_recording_holonomic_controller() + lp._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=5.0) + lp._compute_path_following() + lp._current_odom = _pose_stamped(0.2, 0.0, 0.0, ts=5.0) + + lp._compute_path_following() + + measured = recorder.measurements[-1].twist_body + assert measured.is_zero() + + +def test_local_planner_lookahead_reference_uses_path_end_progress() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + planner_robot_speed=0.5, + ) + nav = NavigationMap(g, "gradient") + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) + distancer = PathDistancer(path) + odom = _pose_stamped(0.2, 0.1, 0.0, ts=10.0) + current_pos = np.array([odom.position.x, odom.position.y], dtype=np.float64) + path_speed = lp._path_speed_for_index(distancer, 0, current_pos) + + reference = lp._lookahead_reference_sample( + distancer, + odom, + current_pos, + path_speed, + ) + + assert reference.pose_plan.position.x == pytest.approx(0.7) + assert reference.time_s == pytest.approx(11.0) + + +def test_local_planner_uses_curvature_speed_cap_for_holonomic_path() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + planner_robot_speed=1.2, + local_planner_max_normal_accel_m_s2=0.6, + ) + nav = NavigationMap(g, "gradient") + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = Path( + frame_id="map", + poses=[ + PoseStamped( + frame_id="map", + position=[0.0, 0.0, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ), + PoseStamped( + frame_id="map", + position=[0.5, 0.0, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ), + PoseStamped( + frame_id="map", + position=[0.5, 0.5, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ), + ], + ) + lp._path = path + lp._path_distancer = PathDistancer(path) + lp._current_odom = PoseStamped( + frame_id="map", + position=[0.5, 0.0, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ) + + lp._compute_path_following() + + assert isinstance(lp._controller, HolonomicPathController) + assert lp._controller._speed < 1.2 + + +def test_local_planner_uses_configured_goal_decel_for_path_speed_cap() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + planner_robot_speed=2.0, + local_planner_max_tangent_accel_m_s2=4.0, + local_planner_goal_decel_m_s2=0.5, + ) + nav = NavigationMap(g, "gradient") + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (1.0, 0.0)]) + distancer = PathDistancer(path) + current_pos = np.array([0.75, 0.0], dtype=np.float64) + + speed = lp._path_speed_for_index(distancer, 0, current_pos) + + assert speed == pytest.approx(0.5, abs=1e-3) + + +def test_local_planner_path_speed_uses_geometry_and_near_goal_decel_caps() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + planner_robot_speed=2.0, + local_planner_max_normal_accel_m_s2=0.5, + local_planner_goal_decel_m_s2=1.0, + ) + nav = NavigationMap(g, "gradient") + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (2.0, 1.0)]) + distancer = PathDistancer(path) + + corner_pos = np.array([1.0, 0.0], dtype=np.float64) + corner_index = distancer.find_closest_point_index(corner_pos) + corner_speed = lp._path_speed_for_index(distancer, corner_index, corner_pos) + + near_goal_pos = np.array([1.95, 1.0], dtype=np.float64) + near_goal_index = distancer.find_closest_point_index(near_goal_pos) + near_goal_speed = lp._path_speed_for_index(distancer, near_goal_index, near_goal_pos) + + right_angle_radius_m = math.sqrt(0.5) + geometry_cap_m_s = math.sqrt(g.local_planner_max_normal_accel_m_s2 * right_angle_radius_m) + goal_decel_cap_m_s = math.sqrt( + 2.0 * g.local_planner_goal_decel_m_s2 * distancer.distance_to_goal(near_goal_pos) + ) + assert corner_speed == pytest.approx(geometry_cap_m_s) + assert near_goal_speed == pytest.approx(goal_decel_cap_m_s, abs=1e-3) + assert near_goal_speed < corner_speed + + +def test_local_planner_path_speed_anticipates_corner_tangent_decel() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + planner_robot_speed=2.0, + local_planner_max_tangent_accel_m_s2=0.5, + local_planner_max_normal_accel_m_s2=0.1, + ) + nav = NavigationMap(g, "gradient") + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)]) + distancer = PathDistancer(path) + + before_corner_pos = np.array([0.85, 0.0], dtype=np.float64) + before_corner_speed = lp._path_speed_for_index( + distancer, + distancer.find_closest_point_index(before_corner_pos), + before_corner_pos, + ) + + assert before_corner_speed < g.planner_robot_speed + assert before_corner_speed < 1.0 + + +def test_local_planner_in_the_loop_harness_reaches_arrival_on_straight_line( + tmp_path: FsPath, +) -> None: + result = _run_local_planner_harness( + tmp_path, + points=[(0.1, 0.0), (1.2, 0.0)], + speed_m_s=1.0, + ) + + assert "arrived" in result.stop_messages + assert result.command_history + assert math.hypot(result.final_x_m - 1.2, result.final_y_m) < 0.15 + + +def test_local_planner_in_the_loop_harness_exercises_curvature_speed_cap( + tmp_path: FsPath, +) -> None: + result = _run_local_planner_harness( + tmp_path, + points=[(0.1, 0.0), (0.45, 0.0), (0.45, 0.45), (0.8, 0.45)], + speed_m_s=1.2, + max_normal_accel_m_s2=0.6, + max_ticks=320, + ) + + assert "arrived" in result.stop_messages + + +def test_local_planner_in_the_loop_harness_supports_2mps_right_angle_with_conservative_profile( + tmp_path: FsPath, +) -> None: + result = _run_local_planner_harness( + tmp_path, + points=[(0.1, 0.0), (0.55, 0.0), (0.55, 0.55), (0.9, 0.55)], + speed_m_s=2.0, + max_tangent_accel_m_s2=0.5, + max_normal_accel_m_s2=0.1, + max_goal_decel_m_s2=0.5, + max_ticks=420, + ) + commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] + + assert "arrived" in result.stop_messages + assert commanded_speeds + assert max(commanded_speeds) < 0.75 + + +def test_local_planner_in_the_loop_harness_decelerates_near_goal( + tmp_path: FsPath, +) -> None: + result = _run_local_planner_harness( + tmp_path, + points=[(0.1, 0.0), (1.0, 0.0)], + speed_m_s=1.2, + max_ticks=300, + ) + commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] + moving_speeds = [speed for speed in commanded_speeds if speed > 0.05] + + assert "arrived" in result.stop_messages + assert moving_speeds + assert max(moving_speeds[: len(moving_speeds) // 2]) > 0.7 + assert min(moving_speeds[-10:]) < 0.55 + assert min(moving_speeds[-10:]) < 0.6 * max(moving_speeds) + assert math.hypot(result.final_x_m - 1.0, result.final_y_m) < 0.15 + + +def test_local_planner_in_the_loop_harness_exercises_initial_rotation( + tmp_path: FsPath, +) -> None: + result = _run_local_planner_harness( + tmp_path, + points=[(0.1, 0.0), (1.0, 0.0)], + initial_yaw_rad=0.8, + speed_m_s=0.9, + max_ticks=320, + ) + + assert "arrived" in result.stop_messages + assert any( + abs(float(cmd.angular.z)) > 0.05 and _planar_speed_m_s(cmd) < 0.05 + for cmd in result.command_history[:20] + ) + assert abs(result.final_yaw_rad) < 0.35 + + +def test_make_local_path_controller_holonomic_velocity_damping_reduces_planar_command() -> None: + # Aligned pose so kp/ky contribute nothing; the only difference is the velocity + # damping gain wired from GlobalConfig through the factory into the inner law. + reference = TrajectoryReferenceSample( + time_s=1.0, + pose_plan=Pose(position=[0.0, 0.0, 0.0], orientation=_yaw_quaternion(0.0)), + twist_body=Twist(linear=[0.5, 0.2, 0.0]), + ) + odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) + measured_body_twist = Twist(linear=[1.1, 0.6, 0.0]) + + def _factory(k_velocity: float) -> Any: + return make_local_path_controller( + GlobalConfig( + local_planner_path_controller="holonomic", + local_planner_holonomic_kp=0.0, + local_planner_holonomic_ky=0.0, + local_planner_holonomic_kv=k_velocity, + local_planner_holonomic_kw=0.0, + # Generous slew headroom so the first-tick accel clamp does not mask the gain. + local_planner_max_planar_cmd_accel_m_s2=8.0, + ), + speed=1.0, + control_frequency=10.0, + ) + + undamped = _factory(0.0).advance_reference(reference, odom, measured_body_twist) + damped = _factory(0.5).advance_reference(reference, odom, measured_body_twist) + + assert undamped.linear.x == pytest.approx(0.5) + assert undamped.linear.y == pytest.approx(0.2) + # 0.5 * (ref - measured) damping pulls the command down to (0.2, 0.0). + assert damped.linear.x == pytest.approx(0.2) + assert damped.linear.y == pytest.approx(0.0) + assert math.hypot(damped.linear.x, damped.linear.y) < math.hypot( + undamped.linear.x, undamped.linear.y + ) + + +def _narrow_corridor_navigation_map(global_config: GlobalConfig) -> NavigationMap: + grid = np.zeros((80, 80), dtype=np.int8) + for row in range(grid.shape[0]): + world_y = -2.0 + row * 0.05 + if abs(world_y) > 0.15: + grid[row, :] = CostValues.OCCUPIED + nav = NavigationMap(global_config, "gradient") + nav.update( + OccupancyGrid( + grid=grid, + resolution=0.05, + origin=Pose(-2.0, -2.0, 0.0), + frame_id="map", + ts=1.0, + ) + ) + return nav + + +def test_holonomic_yaw_lock_reference_drives_along_backward_path() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + local_planner_holonomic_kp=2.0, + local_planner_holonomic_ky=1.0, + robot_rotation_diameter=0.6, + ) + nav = _narrow_corridor_navigation_map(g) + lp = LocalPlanner(g, nav, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (-0.8, 0.0)]) + distancer = PathDistancer(path) + odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) + current_pos = np.array([odom.position.x, odom.position.y], dtype=np.float64) + path_speed = lp._path_speed_for_index(distancer, 0, current_pos) + + reference = lp._lookahead_reference_sample( + distancer, + odom, + current_pos, + path_speed, + yaw_lock_rad=0.0, + ) + cmd = lp._controller.advance_reference( + reference, + odom, + Twist(), + ) + + assert reference.pose_plan.orientation.euler[2] == pytest.approx(0.0) + assert float(cmd.linear.x) < -0.05 + assert abs(float(cmd.angular.z)) < 0.05 + + +def test_local_planner_narrow_corridor_retreats_without_spinning(tmp_path: FsPath) -> None: + rate_hz = 60.0 + dt_s = 1.0 / rate_hz + global_config = GlobalConfig( + local_planner_path_controller="holonomic", + local_planner_control_rate_hz=rate_hz, + planner_robot_speed=0.45, + robot_width=0.25, + robot_rotation_diameter=0.6, + local_planner_holonomic_kp=2.0, + local_planner_holonomic_ky=1.0, + local_planner_max_planar_cmd_accel_m_s2=8.0, + local_planner_max_yaw_accel_rad_s2=8.0, + ) + planner = LocalPlanner( + global_config, + _narrow_corridor_navigation_map(global_config), + goal_tolerance=0.08, + ) + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 + latest_cmd = Twist() + command_history: list[Twist] = [] + stop_messages: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + command_history.append(Twist(cmd)) + + cmd_sub = planner.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = planner.stopped_navigating.subscribe(stop_messages.append) + sim_time_s = 1.0 + + try: + planner.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) + planner.start_planning(_path_from_points([(0.0, 0.0), (-0.8, 0.0)])) + for _ in range(420): + if "arrived" in stop_messages: + break + time.sleep(dt_s * 1.1) + plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( + plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s + ) + sim_time_s += dt_s + planner.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) + ) + finally: + planner.stop() + cmd_sub.dispose() + stop_sub.dispose() + + assert "arrived" in stop_messages + assert plant_x_m < -0.55 + assert abs(plant_yaw_rad) < 0.2 + assert all(abs(float(cmd.angular.z)) < 0.12 for cmd in command_history[:40]) + + +def test_make_local_path_controller_differential_ignores_measured_body_twist() -> None: + # The differential branch returns a PController, which has no velocity-damping term; + # large kv/kw in config must not change its output when measured twist is supplied. + controller = make_local_path_controller( + GlobalConfig( + local_planner_path_controller="differential", + local_planner_holonomic_kv=5.0, + local_planner_holonomic_kw=5.0, + ), + speed=1.0, + control_frequency=10.0, + ) + reference = TrajectoryReferenceSample( + time_s=1.0, + pose_plan=Pose(position=[1.0, 0.0, 0.0], orientation=_yaw_quaternion(0.0)), + twist_body=Twist(linear=[1.0, 0.0, 0.0]), + ) + odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) + measured_body_twist = Twist(linear=[1.1, 0.6, 0.0], angular=[0.0, 0.0, 0.9]) + + without_measurement = controller.advance_reference(reference, odom) + with_measurement = controller.advance_reference(reference, odom, measured_body_twist) + + assert math.hypot(without_measurement.linear.x, without_measurement.linear.y) > 0.0 + assert with_measurement.linear.x == pytest.approx(without_measurement.linear.x) + assert with_measurement.linear.y == pytest.approx(without_measurement.linear.y) + assert with_measurement.angular.z == pytest.approx(without_measurement.angular.z) diff --git a/dimos/navigation/dannav/test_local_planner_run_envelope.py b/dimos/navigation/dannav/test_local_planner_run_envelope.py new file mode 100644 index 0000000000..138776aec6 --- /dev/null +++ b/dimos/navigation/dannav/test_local_planner_run_envelope.py @@ -0,0 +1,272 @@ +# 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. + +"""Run-profile speed application in the local planner.""" + +from __future__ import annotations + +import math +from pathlib import Path as FsPath +import time + +import numpy as np +import pytest + +from dimos.core.global_config import GlobalConfig +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.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.dannav.local_planner import LocalPlanner +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.dannav.path_distancer import PathDistancer + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="map", + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append(_pose_stamped(point[0], point[1], yaw)) + return Path(frame_id="map", poses=poses) + + +def _free_navigation_map(global_config: GlobalConfig) -> NavigationMap: + nav = NavigationMap(global_config, "gradient") + nav.update( + OccupancyGrid( + grid=np.zeros((240, 240), dtype=np.int8), + resolution=0.05, + origin=Pose(-2.0, -2.0, 0.0), + frame_id="map", + ts=1.0, + ) + ) + return nav + + +def _straight_points(length_m: float, spacing_m: float = 0.1) -> list[tuple[float, float]]: + n = int(round(length_m / spacing_m)) + return [(i * spacing_m, 0.0) for i in range(n + 1)] + + +def _right_angle_points( + approach_m: float, exit_m: float, spacing_m: float = 0.1 +) -> list[tuple[float, float]]: + points = _straight_points(approach_m, spacing_m) + n_exit = int(round(exit_m / spacing_m)) + points.extend((approach_m, i * spacing_m) for i in range(1, n_exit + 1)) + return points + + +def _make_planner(g: GlobalConfig, *, goal_tolerance: float = 0.2) -> LocalPlanner: + return LocalPlanner(g, _free_navigation_map(g), goal_tolerance=goal_tolerance) + + +def _integrate_holonomic_pose( + x_m: float, + y_m: float, + yaw_rad: float, + cmd_body: Twist, + dt_s: float, +) -> tuple[float, float, float]: + """Integrate body-frame cmd_vel in the world frame (test harness only).""" + vx = float(cmd_body.linear.x) + vy = float(cmd_body.linear.y) + wz = float(cmd_body.angular.z) + c = math.cos(yaw_rad) + s = math.sin(yaw_rad) + x_m += (c * vx - s * vy) * dt_s + y_m += (s * vx + c * vy) * dt_s + yaw_rad += wz * dt_s + yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) + return x_m, y_m, yaw_rad + + +def _start_and_stop(lp: LocalPlanner, path: Path, run_profile_name: str | None = None) -> None: + """Start a goal, then stop and wait for the planner thread to exit.""" + lp.start_planning(path, run_profile_name=run_profile_name) + thread = lp._thread + lp.stop_planning() + if thread is not None: + thread.join(2.0) + + +def test_walk_default_keeps_legacy_speed() -> None: + """With the default profile, short sharp paths still plan, and the path speed + comes from planner_robot_speed exactly as before.""" + g = GlobalConfig( + local_planner_path_controller="holonomic", + planner_robot_speed=1.0, + ) + lp = _make_planner(g) + lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=1.0)) + stops: list[str] = [] + sub = lp.stopped_navigating.subscribe(stops.append) + + try: + _start_and_stop(lp, _path_from_points(_right_angle_points(0.3, 0.2))) + finally: + sub.dispose() + lp.stop() + + assert "run_envelope_rejected" not in stops + + distancer = PathDistancer(_path_from_points(_straight_points(6.0))) + mid_pos = np.array([3.0, 0.0], dtype=np.float64) + speed = lp._path_speed_for_index(distancer, distancer.find_closest_point_index(mid_pos), mid_pos) + assert speed == pytest.approx(1.0) + + +def test_run_conservative_drives_planner_speed_and_command_caps() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + go2_run_profile="run_conservative", + ) + lp = _make_planner(g) + lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=time.time())) + stops: list[str] = [] + sub = lp.stopped_navigating.subscribe(stops.append) + path = _path_from_points(_straight_points(6.0)) + + try: + _start_and_stop(lp, path) + finally: + sub.dispose() + lp.stop() + + assert "run_envelope_rejected" not in stops + + distancer = PathDistancer(path) + mid_pos = np.array([3.0, 0.0], dtype=np.float64) + near_goal_pos = np.array([5.7, 0.0], dtype=np.float64) + mid_speed = lp._path_speed_for_index( + distancer, distancer.find_closest_point_index(mid_pos), mid_pos + ) + near_goal_speed = lp._path_speed_for_index( + distancer, distancer.find_closest_point_index(near_goal_pos), near_goal_pos + ) + + assert mid_speed == pytest.approx(1.5) + assert near_goal_speed == pytest.approx(math.sqrt(2.0 * 1.5 * 0.3)) + + +def test_run_profile_speed_respects_global_nerf() -> None: + g = GlobalConfig( + local_planner_path_controller="holonomic", + go2_run_profile="run_conservative", + nerf_speed=0.5, + ) + lp = _make_planner(g) + lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=time.time())) + path = _path_from_points(_straight_points(6.0)) + + try: + _start_and_stop(lp, path) + finally: + lp.stop() + + distancer = PathDistancer(path) + mid_pos = np.array([3.0, 0.0], dtype=np.float64) + speed = lp._path_speed_for_index(distancer, distancer.find_closest_point_index(mid_pos), mid_pos) + assert speed == pytest.approx(0.75) + + +def test_per_goal_override_applies_and_does_not_leak_to_next_goal() -> None: + g = GlobalConfig(local_planner_path_controller="holonomic") + lp = _make_planner(g) + lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=time.time())) + path = _path_from_points(_straight_points(6.0)) + distancer = PathDistancer(path) + mid_pos = np.array([3.0, 0.0], dtype=np.float64) + mid_index = distancer.find_closest_point_index(mid_pos) + + try: + _start_and_stop(lp, path, run_profile_name="trot") + trot_speed = lp._path_speed_for_index(distancer, mid_index, mid_pos) + + _start_and_stop(lp, path) + default_speed = lp._path_speed_for_index(distancer, mid_index, mid_pos) + finally: + lp.stop() + + assert trot_speed == pytest.approx(1.0) + assert default_speed == pytest.approx(0.55) + + +def test_run_profile_commands_faster_than_walk_in_closed_loop() -> None: + rate_hz = 60.0 + dt_s = 1.0 / rate_hz + g = GlobalConfig( + local_planner_path_controller="holonomic", + local_planner_control_rate_hz=rate_hz, + go2_run_profile="run_conservative", + ) + planner = LocalPlanner(g, _free_navigation_map(g), goal_tolerance=0.1) + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 + latest_cmd = Twist() + commanded_speeds: list[float] = [] + stops: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + commanded_speeds.append(math.hypot(float(cmd.linear.x), float(cmd.linear.y))) + + cmd_sub = planner.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = planner.stopped_navigating.subscribe(stops.append) + + try: + planner.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=time.time()) + ) + planner.start_planning(_path_from_points([(0.1, 0.0), (3.1, 0.0)])) + for _ in range(420): + if stops: + break + time.sleep(dt_s * 1.1) + plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( + plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s + ) + planner.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=time.time()) + ) + finally: + planner.stop() + cmd_sub.dispose() + stop_sub.dispose() + + assert "arrived" in stops + assert commanded_speeds + assert max(commanded_speeds) > 1.0 + assert max(commanded_speeds) <= 1.5 + 1e-6 diff --git a/dimos/navigation/dannav/test_rotation_clearance.py b/dimos/navigation/dannav/test_rotation_clearance.py new file mode 100644 index 0000000000..9fd04256b2 --- /dev/null +++ b/dimos/navigation/dannav/test_rotation_clearance.py @@ -0,0 +1,72 @@ +# 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. + +from __future__ import annotations + +import numpy as np + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.navigation.dannav.rotation_clearance import can_rotate_in_place + + +def _pose_at(x: float, y: float) -> PoseStamped: + return PoseStamped(frame_id="map", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) + + +def test_can_rotate_in_place_on_open_map() -> None: + grid = np.zeros((80, 80), dtype=np.int8) + costmap = OccupancyGrid( + grid=grid, + resolution=0.05, + origin=Pose(-2.0, -2.0, 0.0), + frame_id="map", + ts=1.0, + ) + + assert can_rotate_in_place(costmap, _pose_at(0.0, 0.0), rotation_radius_m=0.3) + + +def test_can_rotate_in_place_rejects_narrow_corridor() -> None: + grid = np.zeros((80, 80), dtype=np.int8) + # Corridor width 0.30 m (robot can translate, 0.6 m rotation disk cannot fit). + for row in range(grid.shape[0]): + world_y = -2.0 + row * 0.05 + if abs(world_y) > 0.15: + grid[row, :] = CostValues.OCCUPIED + + costmap = OccupancyGrid( + grid=grid, + resolution=0.05, + origin=Pose(-2.0, -2.0, 0.0), + frame_id="map", + ts=1.0, + ) + + assert not can_rotate_in_place(costmap, _pose_at(0.0, 0.0), rotation_radius_m=0.3) + + +def test_can_rotate_in_place_rejects_unknown_cells() -> None: + grid = np.zeros((40, 40), dtype=np.int8) + grid[20, 21] = CostValues.UNKNOWN + costmap = OccupancyGrid( + grid=grid, + resolution=0.05, + origin=Pose(-1.0, -1.0, 0.0), + frame_id="map", + ts=1.0, + ) + + assert not can_rotate_in_place(costmap, _pose_at(0.0, 0.0), rotation_radius_m=0.1) diff --git a/dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py b/dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py new file mode 100644 index 0000000000..dce50ba587 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py @@ -0,0 +1,540 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Plot trajectory control tick JSONL exports. + +Reads UTF-8 JSONL as documented in ``dimos/navigation/trajectory_control_tick_jsonl.md``. + +Outputs: +- **Summary** (``*_plot.png``): cross-track vs time and speed scatter. +- **Path profile** (``*_path_profile.png``): measured arc length on the x-axis with + |cross-track error| (red), commanded speed, reference-path curvature, and heading error + stacked vertically so you can see where the route turns and where error spikes. +""" + +from __future__ import annotations + +import argparse +import json +import math +from pathlib import Path +import sys +from typing import Literal + +PLOT_REQUIRED_FIELDS = ( + "schema_version", + "ref_time_s", + "meas_time_s", + "e_cross_track_m", + "e_heading_rad", + "commanded_planar_speed_m_s", + "dt_s", +) + +PATH_PROFILE_REQUIRED_FIELDS = PLOT_REQUIRED_FIELDS + ( + "ref_x_m", + "ref_y_m", + "ref_yaw_rad", + "meas_x_m", + "meas_y_m", +) + +MOVING_SPEED_THRESHOLD_M_S = 0.05 + +# Reference-path |curvature| bins (1/m) for printed summaries on moving ticks. +CURVATURE_BIN_EDGES = (0.0, 0.05, 0.2, 0.5, float("inf")) +CURVATURE_BIN_LABELS = ( + "straight |kappa|<0.05", + "gentle 0.05-0.2", + "medium 0.2-0.5", + "tight |kappa|>=0.5", +) + + +def _load_ticks(path: Path, *, required_fields: tuple[str, ...] = PLOT_REQUIRED_FIELDS) -> list[dict[str, object]]: + rows: list[dict[str, object]] = [] + with path.open(encoding="utf-8") as f: + for line in f: + line = line.strip() + if not line: + continue + rows.append(json.loads(line)) + if not rows: + raise SystemExit(f"No JSON lines in {path}") + for i, row in enumerate(rows): + ver = row.get("schema_version") + if ver != 1: + raise SystemExit(f"Line {i + 1}: unsupported schema_version {ver!r} (expected 1)") + missing = [field for field in required_fields if field not in row] + if missing: + raise SystemExit(f"Line {i + 1}: missing required fields: {', '.join(missing)}") + return rows + + +def _angle_diff(from_rad: float, to_rad: float) -> float: + return math.atan2(math.sin(to_rad - from_rad), math.cos(to_rad - from_rad)) + + +def _col(rows: list[dict[str, object]], name: str) -> list[float]: + return [float(row[name]) for row in rows] + + +def _time_basis(rows: list[dict[str, object]], ref_t, meas_t) -> object: + import numpy as np + + return ( + ref_t + if np.any(np.diff(ref_t) != 0) or np.max(np.abs(ref_t)) > 0 + else meas_t + ) + + +def _print_cross_track_summary(rows: list[dict[str, object]], inp: Path) -> None: + moving = [ + row + for row in rows + if float(row["commanded_planar_speed_m_s"]) > MOVING_SPEED_THRESHOLD_M_S + ] + if not moving: + print(f"{inp.name}: no moving ticks (cmd speed > {MOVING_SPEED_THRESHOLD_M_S} m/s)") + return + + abs_ct = [abs(float(row["e_cross_track_m"])) for row in moving] + signed_ct = [float(row["e_cross_track_m"]) for row in moving] + abs_ct_sorted = sorted(abs_ct) + p95_index = max(0, int(math.ceil(0.95 * len(abs_ct_sorted))) - 1) + rms = math.sqrt(sum(v * v for v in signed_ct) / len(signed_ct)) + + print(f"Cross-track summary for {inp.name} ({len(moving)} moving / {len(rows)} ticks):") + print(f" mean |e_cross_track_m| = {sum(abs_ct) / len(abs_ct):.4f} m") + print(f" max |e_cross_track_m| = {max(abs_ct):.4f} m") + print(f" p95 |e_cross_track_m| = {abs_ct_sorted[p95_index]:.4f} m") + print(f" RMS e_cross_track_m = {rms:.4f} m (signed, industry reporting style)") + print( + " Note: e_cross_track is lateral offset vs the controller reference pose " + "(lookahead on path), not arc-length progress." + ) + + +def _plot_cross_track( + *, + inp: Path, + out: Path, + rows: list[dict[str, object]], + n: int, +) -> None: + import matplotlib.pyplot as plt + import numpy as np + + def col(name: str) -> np.ndarray: + return np.array([float(r[name]) for r in rows], dtype=np.float64) + + speed_m_s = col("commanded_planar_speed_m_s") + ref_t = col("ref_time_s") + meas_t = col("meas_time_s") + e_xt = col("e_cross_track_m") + e_h = col("e_heading_rad") + dt_s = col("dt_s") + abs_e_xt = np.abs(e_xt) + + t_plot = _time_basis(rows, ref_t, meas_t) + + fig, axes = plt.subplots(2, 2, figsize=(10, 8), constrained_layout=True) + ax_sc, ax_ts_ct, ax_ts_speed, ax_ts_h = axes[0, 0], axes[0, 1], axes[1, 0], axes[1, 1] + + ax_sc.scatter(abs_e_xt, speed_m_s, c=t_plot, cmap="viridis", s=36, zorder=2) + ax_sc.plot(abs_e_xt, speed_m_s, color="0.5", linewidth=0.8, alpha=0.7, zorder=1) + ax_sc.set_xlabel("|e_cross_track_m| (m)") + ax_sc.set_ylabel("Commanded planar speed (m/s)") + ax_sc.set_title("Speed vs |cross-track error| (color = time)") + ax_sc.grid(True, alpha=0.3) + + ax_ts_ct.plot(t_plot, e_xt, marker="o", markersize=3, linewidth=1, color="C1", label="e_cross_track_m") + ax_ts_ct.axhline(0.0, color="0.4", linewidth=0.8, linestyle=":") + ax_ts_ct.set_xlabel("ref_time_s or meas_time_s (s)") + ax_ts_ct.set_ylabel("e_cross_track_m (m)") + ax_ts_ct.set_title("Cross-track error over time (+ = left of reference)") + ax_ts_ct.grid(True, alpha=0.3) + ax_ts_ct.legend(loc="best", fontsize="small") + + ax_ts_speed.plot(t_plot, speed_m_s, marker="o", markersize=3, linewidth=1, color="C0") + ax_ts_speed.set_xlabel("ref_time_s or meas_time_s (s)") + ax_ts_speed.set_ylabel("commanded_planar_speed_m_s") + ax_ts_speed.set_title("Commanded speed over time") + ax_ts_speed.grid(True, alpha=0.3) + + ax_ts_h.plot(t_plot, e_h, marker="o", markersize=3, linewidth=1, color="C3", label="e_heading_rad") + ax_ts_h.set_xlabel("ref_time_s or meas_time_s (s)") + ax_ts_h.set_ylabel("e_heading_rad (rad)") + ax_ts_h.set_title("Heading error and control dt") + ax_ts_h.grid(True, alpha=0.3) + ax_dt = ax_ts_h.twinx() + ax_dt.plot(t_plot, dt_s, color="C2", linestyle="--", linewidth=1, alpha=0.85, label="dt_s") + ax_dt.set_ylabel("dt_s", color="C2") + ax_dt.tick_params(axis="y", labelcolor="C2") + h1, l1 = ax_ts_h.get_legend_handles_labels() + h2, l2 = ax_dt.get_legend_handles_labels() + ax_ts_h.legend(h1 + h2, l1 + l2, loc="best", fontsize="small") + + fig.suptitle( + f"Cross-track tracking: {inp.name} ({n} samples)", + fontsize=11, + ) + + out.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(out, dpi=150) + plt.close(fig) + + +def _derive_path_profile(rows: list[dict[str, object]]) -> dict[str, object]: + """Build arc-length, curvature, and segment-since-stop series from tick poses.""" + import numpy as np + + n = len(rows) + ref_x = np.array(_col(rows, "ref_x_m"), dtype=np.float64) + ref_y = np.array(_col(rows, "ref_y_m"), dtype=np.float64) + ref_yaw = np.array(_col(rows, "ref_yaw_rad"), dtype=np.float64) + meas_x = np.array(_col(rows, "meas_x_m"), dtype=np.float64) + meas_y = np.array(_col(rows, "meas_y_m"), dtype=np.float64) + speed = np.array(_col(rows, "commanded_planar_speed_m_s"), dtype=np.float64) + abs_ct = np.abs(np.array(_col(rows, "e_cross_track_m"), dtype=np.float64)) + e_h = np.array(_col(rows, "e_heading_rad"), dtype=np.float64) + + ds_meas = np.zeros(n, dtype=np.float64) + ds_ref = np.zeros(n, dtype=np.float64) + dpsi_ref = np.zeros(n, dtype=np.float64) + for i in range(1, n): + ds_meas[i] = math.hypot(meas_x[i] - meas_x[i - 1], meas_y[i] - meas_y[i - 1]) + ds_ref[i] = math.hypot(ref_x[i] - ref_x[i - 1], ref_y[i] - ref_y[i - 1]) + dpsi_ref[i] = _angle_diff(ref_yaw[i - 1], ref_yaw[i]) + + s_meas = np.cumsum(ds_meas) + kappa_ref = np.zeros(n, dtype=np.float64) + for i in range(1, n): + if ds_ref[i] > 1e-6: + kappa_ref[i] = dpsi_ref[i] / ds_ref[i] + + # Smooth reference curvature so single-tick lookahead jumps do not dominate the plot. + smooth_window = 5 + kappa_smooth = np.zeros(n, dtype=np.float64) + for i in range(n): + j0 = max(0, i - smooth_window + 1) + ds_sum = float(np.sum(ds_ref[j0 : i + 1])) + psi_sum = float(np.sum(np.abs(dpsi_ref[j0 : i + 1]))) + if ds_sum > 1e-6: + kappa_smooth[i] = psi_sum / ds_sum + + stopped = speed <= MOVING_SPEED_THRESHOLD_M_S + moving = ~stopped + + segment_s = np.zeros(n, dtype=np.float64) + turn_since_stop = np.zeros(n, dtype=np.float64) + for i in range(1, n): + if stopped[i]: + segment_s[i] = 0.0 + turn_since_stop[i] = 0.0 + else: + segment_s[i] = segment_s[i - 1] + ds_meas[i] + turn_since_stop[i] = turn_since_stop[i - 1] + abs(dpsi_ref[i]) + + return { + "s_meas_m": s_meas, + "segment_s_m": segment_s, + "turn_since_stop_rad": turn_since_stop, + "kappa_ref_rad_m": kappa_ref, + "abs_kappa_ref_rad_m": np.abs(kappa_ref), + "kappa_smooth_rad_m": kappa_smooth, + "abs_kappa_smooth_rad_m": kappa_smooth, + "commanded_speed_m_s": speed, + "abs_cross_track_m": abs_ct, + "e_heading_rad": e_h, + "stopped": stopped, + "moving": moving, + } + + +def _print_path_profile_summary(rows: list[dict[str, object]], inp: Path, profile: dict[str, object]) -> None: + import numpy as np + + moving = profile["moving"] + assert isinstance(moving, np.ndarray) + if not np.any(moving): + print(f"{inp.name}: path profile - no moving ticks") + return + + abs_ct = profile["abs_cross_track_m"] + kappa = profile["abs_kappa_smooth_rad_m"] + speed = profile["commanded_speed_m_s"] + s_meas = profile["s_meas_m"] + assert isinstance(abs_ct, np.ndarray) + assert isinstance(kappa, np.ndarray) + assert isinstance(speed, np.ndarray) + assert isinstance(s_meas, np.ndarray) + + print(f"Path-profile summary for {inp.name} ({int(moving.sum())} moving ticks):") + print(" By reference-path |curvature| (where error tends to concentrate):") + for lo, hi, label in zip( + CURVATURE_BIN_EDGES[:-1], + CURVATURE_BIN_EDGES[1:], + CURVATURE_BIN_LABELS, + strict=True, + ): + mask = moving & (kappa >= lo) & (kappa < hi) + count = int(mask.sum()) + if count == 0: + print(f" {label}: n=0") + continue + vals = abs_ct[mask] + print( + f" {label}: n={count}, mean|CTE|={float(vals.mean()):.4f} m, " + f"max|CTE|={float(vals.max()):.4f} m" + ) + + # Find worst 0.5 m windows along measured arc length (moving ticks only). + window_m = 0.5 + s_mov = s_meas[moving] + ct_mov_arr = abs_ct[moving] + if len(s_mov) >= 2: + worst: list[tuple[float, float, float]] = [] + i0 = 0 + for i1 in range(len(s_mov)): + while i0 < i1 and s_mov[i1] - s_mov[i0] > window_m: + i0 += 1 + if i1 > i0: + seg = ct_mov_arr[i0 : i1 + 1] + worst.append((float(seg.mean()), float(s_mov[i0]), float(s_mov[i1]))) + worst.sort(reverse=True) + print(f" Worst {window_m:.1f} m windows by mean |CTE| (measured arc length):") + for rank, (mean_ct, s0, s1) in enumerate(worst[:5], start=1): + print(f" #{rank}: s={s0:.2f}-{s1:.2f} m, mean|CTE|={mean_ct:.4f} m") + + i_max = int(np.argmax(abs_ct[moving])) + mov_idx = int(np.flatnonzero(moving)[i_max]) + print( + f" Global max |CTE|={float(abs_ct[mov_idx]):.4f} m at s={float(s_meas[mov_idx]):.2f} m " + f"(|kappa|={float(kappa[mov_idx]):.3f} 1/m, speed={float(speed[mov_idx]):.2f} m/s)" + ) + + +def _shade_stopped_regions(axes: list[object], s: object, stopped: object) -> None: + import numpy as np + + assert isinstance(s, np.ndarray) + assert isinstance(stopped, np.ndarray) + if len(s) == 0: + return + in_stop = False + start_s = 0.0 + for i in range(len(s)): + if stopped[i] and not in_stop: + in_stop = True + start_s = float(s[i]) + elif not stopped[i] and in_stop: + in_stop = False + end_s = float(s[i]) + for ax in axes: + ax.axvspan(start_s, end_s, color="0.92", zorder=0) # type: ignore[attr-defined] + if in_stop: + end_s = float(s[-1]) + for ax in axes: + ax.axvspan(start_s, end_s, color="0.92", zorder=0) # type: ignore[attr-defined] + + +def _plot_path_profile( + *, + inp: Path, + out: Path, + rows: list[dict[str, object]], + n: int, + profile: dict[str, object], +) -> None: + import matplotlib.pyplot as plt + import numpy as np + + s = profile["s_meas_m"] + abs_ct = profile["abs_cross_track_m"] + speed = profile["commanded_speed_m_s"] + kappa = profile["abs_kappa_smooth_rad_m"] + turn_stop = profile["turn_since_stop_rad"] + e_h = profile["e_heading_rad"] + stopped = profile["stopped"] + moving = profile["moving"] + assert isinstance(s, np.ndarray) + assert isinstance(abs_ct, np.ndarray) + assert isinstance(speed, np.ndarray) + assert isinstance(kappa, np.ndarray) + assert isinstance(turn_stop, np.ndarray) + assert isinstance(e_h, np.ndarray) + assert isinstance(stopped, np.ndarray) + assert isinstance(moving, np.ndarray) + + fig_h = max(12.0, 0.004 * float(s[-1]) if len(s) else 12.0) + fig = plt.figure(figsize=(11, fig_h)) + gs = fig.add_gridspec( + 5, + 1, + height_ratios=[2.2, 1.2, 1.2, 1.0, 1.1], + hspace=0.28, + ) + ax_ct = fig.add_subplot(gs[0]) + ax_speed = fig.add_subplot(gs[1], sharex=ax_ct) + ax_kappa = fig.add_subplot(gs[2], sharex=ax_ct) + ax_turn = fig.add_subplot(gs[3], sharex=ax_ct) + ax_scatter = fig.add_subplot(gs[4]) + axes = [ax_ct, ax_speed, ax_kappa, ax_turn] + + _shade_stopped_regions(axes, s, stopped) + + ax_ct.fill_between(s, 0.0, abs_ct, color="#d62728", alpha=0.35, zorder=1) + ax_ct.plot(s, abs_ct, color="#b01020", linewidth=1.2, label="|e_cross_track_m|", zorder=2) + i_peak = int(np.argmax(abs_ct)) + ax_ct.scatter([float(s[i_peak])], [float(abs_ct[i_peak])], color="#b01020", s=40, zorder=3) + ax_ct.annotate( + f"max {abs_ct[i_peak]:.3f} m @ {s[i_peak]:.1f} m", + xy=(float(s[i_peak]), float(abs_ct[i_peak])), + xytext=(8, 8), + textcoords="offset points", + fontsize=8, + color="#b01020", + ) + ax_ct.set_ylabel("|cross-track| (m)") + ax_ct.set_title("Cross-track error along measured path (red)") + ax_ct.grid(True, alpha=0.3) + ax_ct.legend(loc="upper right", fontsize="small") + + ax_speed.plot(s, speed, color="C0", linewidth=1.0) + ax_speed.set_ylabel("cmd speed (m/s)") + ax_speed.set_title("Commanded planar speed") + ax_speed.grid(True, alpha=0.3) + + ax_kappa.plot(s, kappa, color="C2", linewidth=1.0) + kappa_cap = float(np.percentile(kappa[moving], 95)) * 1.2 if np.any(moving) else 1.0 + if kappa_cap > 0.05: + ax_kappa.set_ylim(0.0, max(kappa_cap, 0.5)) + ax_kappa.set_ylabel("|kappa| (1/m)") + ax_kappa.set_title("Reference-path curvature, 5-tick smoothed (|d yaw| / ds on ref)") + ax_kappa.grid(True, alpha=0.3) + + ax_turn.plot(s, np.degrees(turn_stop), color="C4", linewidth=1.0) + ax_turn.set_ylabel("deg since stop") + ax_turn.set_title("Cumulative |ref heading change| since last stop") + ax_turn.grid(True, alpha=0.3) + + ax_turn.set_xlabel("Measured arc length s (m)") + + sc = ax_scatter.scatter( + kappa[moving], + abs_ct[moving], + c=speed[moving], + cmap="viridis", + s=14, + alpha=0.75, + ) + ax_scatter.set_xlabel("|kappa| (1/m)") + ax_scatter.set_ylabel("|cross-track| (m)") + ax_scatter.set_title("Curvature vs |cross-track| on moving ticks (color = cmd speed)") + ax_scatter.grid(True, alpha=0.3) + cbar = fig.colorbar(sc, ax=ax_scatter, fraction=0.046, pad=0.02) + cbar.set_label("cmd speed (m/s)") + + fig.suptitle( + f"Path profile: {inp.name} ({n} ticks, gray = stopped)", + fontsize=11, + ) + + out.parent.mkdir(parents=True, exist_ok=True) + fig.subplots_adjust(left=0.08, right=0.96, top=0.94, bottom=0.06, hspace=0.38) + fig.savefig(out, dpi=150) + plt.close(fig) + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Plot trajectory control tick JSONL (cross-track and path-profile views)." + ) + parser.add_argument( + "jsonl", + type=Path, + help="Path to trajectory control tick export (.jsonl).", + ) + parser.add_argument( + "-o", + "--output", + type=Path, + default=None, + help="Summary PNG path (default: _plot.png).", + ) + parser.add_argument( + "--profile-output", + type=Path, + default=None, + help="Path-profile PNG (default: _path_profile.png).", + ) + parser.add_argument( + "--plot", + choices=("summary", "profile", "both"), + default="both", + help="Which figures to generate (default: both).", + ) + args = parser.parse_args() + inp = args.jsonl.expanduser().resolve() + if not inp.is_file(): + raise SystemExit(f"Not a file: {inp}") + + try: + import matplotlib + + matplotlib.use("Agg") + except ImportError as e: + raise SystemExit( + "matplotlib and numpy are required. Example: " + "uv run --with matplotlib python " + "dimos/navigation/holonomic_trajectory_controller/docs/" + "plot_trajectory_control_ticks.py ..." + ) from e + + plot_mode: Literal["summary", "profile", "both"] = args.plot + need_profile = plot_mode in ("profile", "both") + required = PATH_PROFILE_REQUIRED_FIELDS if need_profile else PLOT_REQUIRED_FIELDS + rows = _load_ticks(inp, required_fields=required) + n = len(rows) + + if args.output is None: + summary_out = inp.parent / f"{inp.stem}_plot.png" + else: + summary_out = args.output.expanduser().resolve() + + if args.profile_output is None: + profile_out = inp.parent / f"{inp.stem}_path_profile.png" + else: + profile_out = args.profile_output.expanduser().resolve() + + if plot_mode in ("summary", "both"): + _print_cross_track_summary(rows, inp) + _plot_cross_track(inp=inp, out=summary_out, rows=rows, n=n) + print(f"Wrote {summary_out}") + + if need_profile: + profile = _derive_path_profile(rows) + _print_path_profile_summary(rows, inp, profile) + _plot_path_profile(inp=inp, out=profile_out, rows=rows, n=n, profile=profile) + print(f"Wrote {profile_out}") + + +if __name__ == "__main__": + try: + main() + except BrokenPipeError: + sys.exit(0) diff --git a/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md b/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md new file mode 100644 index 0000000000..b0668b6f96 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md @@ -0,0 +1,78 @@ +# Trajectory control tick JSONL export + +## Format + +- **Encoding:** UTF-8. +- **Layout:** one JSON object per line (JSON Lines, `.jsonl`). Empty lines are ignored when reading. +- **Schema version:** integer field `schema_version` on every object. Current value is `1`. Consumers should reject or quarantine unknown major versions. + +## Key order and names + +After `schema_version`, object keys follow the field order of `TrajectoryControlTick` in `trajectory_control_tick_log.py`. Producers use that order for stable diffs and stream inspection; parsers must not rely on key order. + +## Fields + +| Key | Type | Unit | Meaning | +|-----|------|------|---------| +| `schema_version` | int | - | Export schema; `1` for this revision. | +| `ref_time_s` | number | s | Reference sample time basis (producer-defined; see trajectory types). | +| `ref_x_m` | number | m | Reference pose x in plan horizontal frame. | +| `ref_y_m` | number | m | Reference pose y in plan horizontal frame. | +| `ref_yaw_rad` | number | rad | Reference yaw in plan frame. | +| `ref_twist_linear_x_m_s` | number | m/s | Reference twist linear x (body). | +| `ref_twist_linear_y_m_s` | number | m/s | Reference twist linear y (body). | +| `ref_twist_linear_z_m_s` | number | m/s | Reference twist linear z (body). | +| `ref_twist_angular_x_rad_s` | number | rad/s | Reference twist angular x (body). | +| `ref_twist_angular_y_rad_s` | number | rad/s | Reference twist angular y (body). | +| `ref_twist_angular_z_rad_s` | number | rad/s | Reference twist angular z (body). | +| `meas_time_s` | number | s | Measured sample time basis. | +| `meas_x_m` | number | m | Measured pose x (plan frame). | +| `meas_y_m` | number | m | Measured pose y (plan frame). | +| `meas_yaw_rad` | number | rad | Measured yaw (plan frame). | +| `meas_twist_linear_x_m_s` | number | m/s | Measured twist linear x (body). Live `LocalPlanner` logs estimate this from consecutive odom poses when timestamps advance. | +| `meas_twist_linear_y_m_s` | number | m/s | Measured twist linear y (body). Live `LocalPlanner` logs estimate this from consecutive odom poses when timestamps advance. | +| `meas_twist_linear_z_m_s` | number | m/s | Measured twist linear z (body). | +| `meas_twist_angular_x_rad_s` | number | rad/s | Measured twist angular x (body). | +| `meas_twist_angular_y_rad_s` | number | rad/s | Measured twist angular y (body). | +| `meas_twist_angular_z_rad_s` | number | rad/s | Measured twist angular z (body). Live `LocalPlanner` logs estimate this from consecutive odom yaws when timestamps advance. | +| `e_along_track_m` | number | m | Along-track error (see `trajectory_metrics`). | +| `e_cross_track_m` | number | m | Cross-track error. | +| `e_heading_rad` | number | rad | Heading error. | +| `planar_position_divergence_m` | number | m | Planar position divergence (speed-vs-divergence plots). | +| `cmd_linear_x_m_s` | number | m/s | Commanded linear x (published this tick). | +| `cmd_linear_y_m_s` | number | m/s | Commanded linear y. | +| `cmd_linear_z_m_s` | number | m/s | Commanded linear z. | +| `cmd_angular_x_rad_s` | number | rad/s | Commanded angular x. | +| `cmd_angular_y_rad_s` | number | rad/s | Commanded angular y. | +| `cmd_angular_z_rad_s` | number | rad/s | Commanded angular z. | +| `commanded_planar_speed_m_s` | number | m/s | Planar speed from command (`hypot(linear.x, linear.y)` in body). | +| `dt_s` | number | s | Control period for this tick. | +| `wall_time_s` | number or null | s | Optional wall clock. | +| `sim_time_s` | number or null | s | Optional simulation time. | + +## API + +- `trajectory_control_tick_to_jsonl_dict` and `JsonlTrajectoryControlTickSink` in `dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export`. +- `LocalPlanner` writes live JSONL when `GlobalConfig.local_planner_trajectory_tick_log_path` is set to a file path. + +## Plotting + +Use `planar_position_divergence_m` vs `commanded_planar_speed_m_s` (and time series on `ref_time_s` or `meas_time_s`); `pandas.read_json(..., lines=True)` accepts this format if you use pandas. + +**Control rate:** after you have exports, relate `dt_s` and optional `wall_time_s` to a sensible `local_planner_control_rate_hz` and to plant delay. + +**Live navigation export:** set `local_planner_trajectory_tick_log_path` in `GlobalConfig`, run the holonomic path follower, then plot the generated file. This is the normal path for comparing speed vs divergence on a robot or replay harness. + +**Built-in recipe:** from the repository root, run the plot script with `uv run --with matplotlib`. Example using the in-tree sample JSONL: + +```bash +uv run --with matplotlib python dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py \ + dimos/navigation/fixtures/trajectory_control_ticks_sample.jsonl \ + -o dimos/navigation/fixtures/trajectory_control_ticks_sample_plot.png +``` + +For your own export: + +```bash +uv run --with matplotlib python dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py path/to/ticks.jsonl -o out.png +``` diff --git a/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md b/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md new file mode 100644 index 0000000000..2c4ab472ec --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md @@ -0,0 +1,66 @@ +# Operator run-profile contract + +## Purpose + +- **What:** Named movement envelopes (`walk`, `trot`, `run_conservative`, + `run_verified`) that an operator, agent, CLI, or MCP caller can request + instead of a loose `planner_robot_speed` number plus a fistful of + `--local-planner-*` flags. +- **Where:** `dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles`. +- **Status:** data + validation, plus Go2 locomotion-mode wiring. The profile's + `required_locomotion_mode` is wired into `GO2Connection` start-up via + `GlobalConfig.go2_run_profile`. The live stack applies profile speed/accel/yaw + limits through `LocalPlanner`. + +## Data model — `RunProfile` + +All numeric fields are **upper bounds in SI units** in the same conventions as +the live limit types (`HolonomicCommandLimits`, `PathSpeedProfileLimits`): +planar speed is `hypot(vx, vy)` in the body frame; yaw rate is `wz`. + +| Field | Unit | Meaning | +|-------|------|---------| +| `name` | str | Profile identity (registry key must match). | +| `requested_planner_speed_m_s` | m/s | Requested cruise speed (still curvature/decel-capped downstream). | +| `max_tangent_accel_m_s2` | m/s² | Along-path acceleration cap for the speed profile. | +| `max_normal_accel_m_s2` | m/s² | Centripetal (curvature) acceleration cap. | +| `goal_decel_m_s2` | m/s² | Deceleration approaching the goal. | +| `max_planar_cmd_accel_m_s2` | m/s² | Command slew cap on planar `cmd_vel`. | +| `max_yaw_rate_rad_s` | rad/s | Yaw-rate cap. | +| `max_yaw_accel_rad_s2` | rad/s² | Yaw-acceleration cap. | +| `required_locomotion_mode` | str | Embodiment mode to activate before high-speed motion (Go2: `default` or `rage`). | +| `description` | str | Human-readable note. | + +Adapters onto the existing validated limit types: + +- `command_limits() -> HolonomicCommandLimits` +- `path_speed_profile_limits_at(max_speed_m_s) -> PathSpeedProfileLimits` + +### Validation (rejected at construction) + +- Every speed/acceleration/yaw field must be **finite and strictly positive**. +- `name` and `required_locomotion_mode` must be non-empty. + +## Profile resolution + +`GO2_RUN_PROFILES.get(name)` looks up a profile by name. Unknown names raise +`RunProfileError` with a message listing known profiles. + +Per-goal overrides are resolved in `LocalPlanner._resolve_run_envelope`: the goal +profile name wins over `GlobalConfig.go2_run_profile`. The registry default name +(`walk`) keeps legacy walking behavior via `_default_run_envelope()`. + +## Go2 profiles (`GO2_RUN_PROFILES`) + +Caps are **conservative nominal engineering envelopes, not measured hardware +performance**. `walk` reproduces today's `LocalPlanner` default (0.55 m/s and +the `local_planner_*` defaults). Default `relative_move(...)` stays `walk`. + +| Profile | Speed (m/s) | Go2 mode | +|---------|-------------|----------| +| `walk` | 0.55 | `default` | +| `trot` | 1.0 | `default` | +| `run_conservative` | 1.5 | `default` | +| `run_verified` | 2.5 | `rage` | + +Set `GO2_RUN_PROFILE=` to select any profile at startup. diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py new file mode 100644 index 0000000000..5f428f1b17 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py @@ -0,0 +1,209 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for ``trajectory_command_limits``.""" + +import math + +import pytest + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( + HolonomicCommandLimits, + clamp_holonomic_cmd_vel, +) + +_LIMITS = HolonomicCommandLimits( + max_planar_speed_m_s=1.0, + max_yaw_rate_rad_s=0.5, + max_planar_linear_accel_m_s2=2.0, + max_yaw_accel_rad_s2=1.0, +) + + +def test_limits_reject_non_finite_or_negative() -> None: + with pytest.raises(ValueError, match="max_planar_speed_m_s"): + HolonomicCommandLimits( + max_planar_speed_m_s=-1.0, + max_yaw_rate_rad_s=1.0, + max_planar_linear_accel_m_s2=1.0, + max_yaw_accel_rad_s2=1.0, + ) + + +def test_saturate_planar_speed() -> None: + prev = Twist() + raw = Twist(linear=Vector3(2.0, 0.0, 0.0), angular=Vector3(0.0, 0.0, 0.0)) + out = clamp_holonomic_cmd_vel( + prev, + raw, + _LIMITS, + 1.0, + ) + assert out.linear.x == pytest.approx(1.0) + assert out.linear.y == pytest.approx(0.0) + + +def test_saturate_planar_speed_holonomic_direction() -> None: + """Speed cap scales (vx, vy) together, preserving direction.""" + prev = Twist() + raw = Twist( + linear=Vector3(3.0, 4.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + out = clamp_holonomic_cmd_vel( + prev, + raw, + _LIMITS, + 1.0, + ) + v_max = 1.0 + n = math.hypot(3.0, 4.0) + assert out.linear.x == pytest.approx(3.0 / n * v_max) + assert out.linear.y == pytest.approx(4.0 / n * v_max) + + +def test_acceleration_limits_one_axis_from_rest() -> None: + prev = Twist() + raw = Twist(linear=Vector3(1.0, 0.0, 0.0), angular=Vector3(0.0, 0.0, 0.0)) + a_max = 2.0 + dt = 0.1 + limits = HolonomicCommandLimits( + max_planar_speed_m_s=10.0, + max_yaw_rate_rad_s=10.0, + max_planar_linear_accel_m_s2=a_max, + max_yaw_accel_rad_s2=10.0, + ) + out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) + assert out.linear.x == pytest.approx(a_max * dt) + + +def test_acceleration_limit_scales_planar_delta_vector() -> None: + prev = Twist(linear=Vector3(0.5, 0.0, 0.0)) + raw = Twist(linear=Vector3(0.5, 1.0, 0.0)) + a_max = 1.5 + dt = 0.2 + limits = HolonomicCommandLimits( + max_planar_speed_m_s=10.0, + max_yaw_rate_rad_s=10.0, + max_planar_linear_accel_m_s2=a_max, + max_yaw_accel_rad_s2=10.0, + ) + + out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) + + assert out.linear.x == pytest.approx(0.5) + assert out.linear.y == pytest.approx(a_max * dt) + assert math.hypot(out.linear.x - prev.linear.x, out.linear.y - prev.linear.y) == pytest.approx( + a_max * dt + ) + + +def test_acceleration_limit_bounds_planar_reversal() -> None: + prev = Twist(linear=Vector3(0.6, 0.8, 0.0)) + raw = Twist(linear=Vector3(-0.6, -0.8, 0.0)) + a_max = 2.0 + dt = 0.1 + limits = HolonomicCommandLimits( + max_planar_speed_m_s=10.0, + max_yaw_rate_rad_s=10.0, + max_planar_linear_accel_m_s2=a_max, + max_yaw_accel_rad_s2=10.0, + ) + + out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) + + delta_x = out.linear.x - prev.linear.x + delta_y = out.linear.y - prev.linear.y + assert math.hypot(delta_x, delta_y) == pytest.approx(a_max * dt) + assert math.hypot(out.linear.x, out.linear.y) < math.hypot(prev.linear.x, prev.linear.y) + + +def test_slew_uses_previous_command() -> None: + prev = Twist( + linear=Vector3(0.8, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + raw = Twist( + linear=Vector3(1.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + a_max = 1.0 + dt = 0.1 + limits = HolonomicCommandLimits( + max_planar_speed_m_s=2.0, + max_yaw_rate_rad_s=2.0, + max_planar_linear_accel_m_s2=a_max, + max_yaw_accel_rad_s2=10.0, + ) + out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) + assert out.linear.x == pytest.approx(0.8 + a_max * dt) + + +def test_yaw_rate_and_acceleration() -> None: + prev = Twist(angular=Vector3(0.0, 0.0, 0.0)) + raw = Twist(angular=Vector3(0.0, 0.0, 1.0)) + limits = HolonomicCommandLimits( + max_planar_speed_m_s=1.0, + max_yaw_rate_rad_s=0.5, + max_planar_linear_accel_m_s2=1.0, + max_yaw_accel_rad_s2=5.0, + ) + out = clamp_holonomic_cmd_vel(prev, raw, limits, 1.0) + assert out.angular.z == pytest.approx(0.5) + + +def test_yaw_acceleration_step() -> None: + prev = Twist(angular=Vector3(0.0, 0.0, 0.0)) + raw = Twist(angular=Vector3(0.0, 0.0, 1.0)) + limits = HolonomicCommandLimits( + max_planar_speed_m_s=1.0, + max_yaw_rate_rad_s=10.0, + max_planar_linear_accel_m_s2=1.0, + max_yaw_accel_rad_s2=2.0, + ) + out = clamp_holonomic_cmd_vel(prev, raw, limits, 0.1) + assert out.angular.z == pytest.approx(0.2) + + +def test_rejects_non_positive_dt() -> None: + with pytest.raises(ValueError, match="dt_s"): + clamp_holonomic_cmd_vel(Twist(), Twist(), _LIMITS, 0.0) + + +def test_passes_through_unmodeled_vector_components() -> None: + """linear.z, angular x/y come from raw_cmd, not from previous_cmd.""" + prev = Twist( + linear=Vector3(0.0, 0.0, 1.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + raw = Twist( + linear=Vector3(0.0, 0.0, 2.0), + angular=Vector3(0.1, -0.2, 0.0), + ) + out = clamp_holonomic_cmd_vel( + prev, + raw, + HolonomicCommandLimits( + max_planar_speed_m_s=1.0, + max_yaw_rate_rad_s=1.0, + max_planar_linear_accel_m_s2=10.0, + max_yaw_accel_rad_s2=10.0, + ), + 0.05, + ) + assert out.linear.z == pytest.approx(2.0) + assert out.angular.x == pytest.approx(0.1) + assert out.angular.y == pytest.approx(-0.2) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py new file mode 100644 index 0000000000..653bcff372 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py @@ -0,0 +1,210 @@ +# 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. + +"""Holonomic tracking controller unit tests.""" + +from __future__ import annotations + +import math + +import pytest + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( + HolonomicCommandLimits, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.utils.trigonometry import angle_diff + + +def _pose_xy_yaw(x: float, y: float, yaw: float) -> Pose: + return Pose( + x, + y, + 0.0, + 0.0, + 0.0, + math.sin(yaw / 2.0), + math.cos(yaw / 2.0), + ) + + +def _ref(time_s: float, pose: Pose, twist: Twist) -> TrajectoryReferenceSample: + return TrajectoryReferenceSample(time_s=time_s, pose_plan=pose, twist_body=twist) + + +def _meas(time_s: float, pose: Pose, twist: Twist) -> TrajectoryMeasuredSample: + return TrajectoryMeasuredSample(time_s=time_s, pose_plan=pose, twist_body=twist) + + +def test_tracking_feedforward_when_aligned() -> None: + ctrl = HolonomicTrackingController(k_position_per_s=2.0, k_yaw_per_s=1.5) + ref_twist = Twist(linear=Vector3(0.4, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.05)) + p = _pose_xy_yaw(1.0, -2.0, math.pi / 6) + out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, Twist())) + assert out.linear.x == pytest.approx(0.4) + assert out.linear.y == pytest.approx(-0.1) + assert out.angular.z == pytest.approx(0.05) + + +def test_default_damping_gains_ignore_measured_velocity() -> None: + ctrl = HolonomicTrackingController(k_position_per_s=2.0, k_yaw_per_s=1.5) + ref_twist = Twist(linear=Vector3(0.4, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.05)) + measured_twist = Twist(linear=Vector3(1.2, -0.8, 0.0), angular=Vector3(0.0, 0.0, 0.7)) + p = _pose_xy_yaw(1.0, -2.0, math.pi / 6) + out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) + assert out.linear.x == pytest.approx(0.4) + assert out.linear.y == pytest.approx(-0.1) + assert out.angular.z == pytest.approx(0.05) + + +def test_velocity_damping_reduces_planar_command_when_measured_velocity_exceeds_reference() -> None: + ctrl = HolonomicTrackingController( + k_position_per_s=0.0, + k_yaw_per_s=0.0, + k_velocity_per_s=0.5, + ) + ref_twist = Twist(linear=Vector3(0.5, 0.2, 0.0), angular=Vector3()) + measured_twist = Twist(linear=Vector3(1.1, 0.6, 0.0), angular=Vector3()) + p = _pose_xy_yaw(0.0, 0.0, 0.0) + + out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) + + assert out.linear.x == pytest.approx(0.2) + assert out.linear.y == pytest.approx(0.0) + assert math.hypot(out.linear.x, out.linear.y) < math.hypot( + ref_twist.linear.x, ref_twist.linear.y + ) + + +def test_yaw_rate_damping_reduces_command_when_measured_rate_exceeds_reference() -> None: + ctrl = HolonomicTrackingController( + k_position_per_s=0.0, + k_yaw_per_s=0.0, + k_yaw_rate_per_s=0.5, + ) + ref_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.4)) + measured_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.9)) + p = _pose_xy_yaw(0.0, 0.0, 0.0) + + out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) + + assert out.angular.z == pytest.approx(0.15) + assert abs(out.angular.z) < abs(ref_twist.angular.z) + + +def test_velocity_damping_does_not_increase_planar_command_when_measured_velocity_is_below_reference() -> ( + None +): + ctrl = HolonomicTrackingController( + k_position_per_s=0.0, + k_yaw_per_s=0.0, + k_velocity_per_s=0.5, + ) + ref_twist = Twist(linear=Vector3(0.5, -0.4, 0.0), angular=Vector3()) + measured_twist = Twist(linear=Vector3(0.2, -0.1, 0.0), angular=Vector3()) + p = _pose_xy_yaw(0.0, 0.0, 0.0) + + out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) + + assert out.linear.x == pytest.approx(0.5) + assert out.linear.y == pytest.approx(-0.4) + + +def test_yaw_rate_damping_does_not_increase_command_when_measured_rate_is_below_reference() -> None: + ctrl = HolonomicTrackingController( + k_position_per_s=0.0, + k_yaw_per_s=0.0, + k_yaw_rate_per_s=0.5, + ) + ref_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.4)) + measured_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.1)) + p = _pose_xy_yaw(0.0, 0.0, 0.0) + + out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) + + assert out.angular.z == pytest.approx(0.4) + + +@pytest.mark.parametrize("value", [-0.1, math.inf, math.nan]) +def test_velocity_damping_gain_validation_rejects_negative_or_non_finite(value: float) -> None: + with pytest.raises(ValueError, match="k_velocity_per_s"): + HolonomicTrackingController( + k_position_per_s=1.0, + k_yaw_per_s=1.0, + k_velocity_per_s=value, + ) + + +@pytest.mark.parametrize("value", [-0.1, math.inf, math.nan]) +def test_yaw_rate_damping_gain_validation_rejects_negative_or_non_finite(value: float) -> None: + with pytest.raises(ValueError, match="k_yaw_rate_per_s"): + HolonomicTrackingController( + k_position_per_s=1.0, + k_yaw_per_s=1.0, + k_yaw_rate_per_s=value, + ) + + +def test_tracking_rotates_reference_feedforward_into_measured_body_frame() -> None: + ctrl = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=0.0) + ref_p = _pose_xy_yaw(0.0, 1.0, math.pi / 2.0) + meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) + ref_twist = Twist(linear=Vector3(0.5, 0.0, 0.0), angular=Vector3(0.0, 0.0, 0.0)) + out = ctrl.control(_ref(0.0, ref_p, ref_twist), _meas(0.0, meas_p, Twist())) + assert out.linear.x == pytest.approx(0.0, abs=1e-12) + assert out.linear.y == pytest.approx(0.5) + + +def test_tracking_pushes_toward_reference_in_body_frame() -> None: + ctrl = HolonomicTrackingController(k_position_per_s=1.0, k_yaw_per_s=0.0) + ref_p = _pose_xy_yaw(1.0, 0.0, 0.0) + meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) + out = ctrl.control( + _ref(0.0, ref_p, Twist()), + _meas(0.0, meas_p, Twist()), + ) + assert out.linear.x == pytest.approx(1.0) + assert out.linear.y == pytest.approx(0.0) + + +def test_tracking_heading_correction_sign() -> None: + ctrl = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=2.0) + ref_p = _pose_xy_yaw(0.0, 0.0, 0.5) + meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) + out = ctrl.control(_ref(0.0, ref_p, Twist()), _meas(0.0, meas_p, Twist())) + e_psi = angle_diff(0.0, 0.5) + assert out.angular.z == pytest.approx(-2.0 * e_psi) + + +def test_configure_caps_planar_speed_and_yaw_rate() -> None: + ctrl = HolonomicTrackingController(k_position_per_s=100.0, k_yaw_per_s=100.0) + ctrl.configure( + HolonomicCommandLimits( + max_planar_speed_m_s=0.5, + max_yaw_rate_rad_s=0.2, + max_planar_linear_accel_m_s2=10.0, + max_yaw_accel_rad_s2=10.0, + ), + ) + ref_p = _pose_xy_yaw(10.0, 0.0, 0.0) + meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) + out = ctrl.control(_ref(0.0, ref_p, Twist()), _meas(0.0, meas_p, Twist())) + assert math.hypot(out.linear.x, out.linear.y) == pytest.approx(0.5) + ref_p2 = _pose_xy_yaw(0.0, 0.0, 10.0) + out2 = ctrl.control(_ref(0.0, ref_p2, Twist()), _meas(0.0, meas_p, Twist())) + assert abs(out2.angular.z) == pytest.approx(0.2) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py new file mode 100644 index 0000000000..e28acb9e17 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py @@ -0,0 +1,157 @@ +# 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. + +"""Polyline speed profile behavior tests.""" + +import math + +import numpy as np +import pytest + +from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( + PathSpeedProfileLimits, + profile_speed_along_polyline, + speed_at_progress_m, +) + + +def _straight_line_path(length_m: float) -> tuple[np.ndarray, np.ndarray]: + path_xy = np.array([[0.0, 0.0], [length_m, 0.0]], dtype=np.float64) + cumulative = np.array([length_m], dtype=np.float64) + return path_xy, cumulative + + +def test_limits_reject_bad_values() -> None: + with pytest.raises(ValueError, match="max_speed_m_s"): + PathSpeedProfileLimits( + max_speed_m_s=-1.0, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=1.0, + ) + + +def test_line_long_segment_reaches_speed_plateau() -> None: + """Straight 10 m line: enough length to hit ``v_max`` before braking.""" + limits = PathSpeedProfileLimits( + max_speed_m_s=2.0, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=10.0, + ) + path_xy, cumulative = _straight_line_path(10.0) + s, v = profile_speed_along_polyline( + path_xy, + cumulative, + limits, + goal_decel_m_s2=limits.max_tangent_accel_m_s2, + num_samples=2001, + start_speed_m_s=0.0, + ) + assert s[0] == pytest.approx(0.0) + assert s[-1] == pytest.approx(10.0) + assert v[0] == pytest.approx(0.0) + assert v[-1] == pytest.approx(0.0) + assert max(v) == pytest.approx(2.0, rel=0, abs=0.02) + mid = len(v) // 2 + assert v[mid] == pytest.approx(2.0, rel=0, abs=0.02) + + +def test_line_short_segment_triangular_peak() -> None: + """1 m line, high nominal ``v_max``: peak limited by ``sqrt(a * L)`` triangle.""" + limits = PathSpeedProfileLimits( + max_speed_m_s=10.0, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=10.0, + ) + length_m = 1.0 + path_xy, cumulative = _straight_line_path(length_m) + _, v = profile_speed_along_polyline( + path_xy, + cumulative, + limits, + goal_decel_m_s2=limits.max_tangent_accel_m_s2, + num_samples=4001, + start_speed_m_s=0.0, + ) + expected_peak = math.sqrt(limits.max_tangent_accel_m_s2 * length_m) + assert max(v) == pytest.approx(expected_peak, rel=0, abs=0.01) + + +def test_tight_corner_centripetal_cap_below_max_speed() -> None: + """90 deg corner: circumradius caps speed below ``max_speed_m_s``.""" + limits = PathSpeedProfileLimits( + max_speed_m_s=3.0, + max_tangent_accel_m_s2=2.0, + max_normal_accel_m_s2=1.0, + ) + leg_m = 0.25 + path_xy = np.array( + [[0.0, 0.0], [leg_m, 0.0], [leg_m, leg_m]], + dtype=np.float64, + ) + segments = path_xy[1:] - path_xy[:-1] + cumulative = np.cumsum(np.linalg.norm(segments, axis=1)) + corner_s = float(cumulative[0]) + s_profile, v_profile = profile_speed_along_polyline( + path_xy, + cumulative, + limits, + goal_decel_m_s2=limits.max_tangent_accel_m_s2, + num_samples=200, + start_speed_m_s=0.0, + ) + at_corner_speed = speed_at_progress_m(corner_s, s_profile, v_profile) + assert at_corner_speed < limits.max_speed_m_s + assert at_corner_speed < 0.6 + + +def test_zero_length_returns_origin() -> None: + limits = PathSpeedProfileLimits(1.0, 1.0, 1.0) + path_xy = np.array([[0.0, 0.0]], dtype=np.float64) + s, v = profile_speed_along_polyline( + path_xy, + np.array([], dtype=np.float64), + limits, + goal_decel_m_s2=1.0, + ) + assert s == [0.0] + assert v == [0.0] + + +def test_polyline_profile_decelerates_before_corner() -> None: + limits = PathSpeedProfileLimits( + max_speed_m_s=2.0, + max_tangent_accel_m_s2=0.5, + max_normal_accel_m_s2=0.1, + ) + path_xy = np.array( + [[0.0, 0.0], [1.0, 0.0], [1.0, 1.0], [2.0, 1.0]], + dtype=np.float64, + ) + segments = path_xy[1:] - path_xy[:-1] + cumulative = np.cumsum(np.linalg.norm(segments, axis=1)) + s_profile, v_profile = profile_speed_along_polyline( + path_xy, + cumulative, + limits, + goal_decel_m_s2=0.5, + num_samples=200, + ) + corner_s = 1.0 + before_corner_speed = speed_at_progress_m(0.85, s_profile, v_profile) + at_corner_speed = speed_at_progress_m(corner_s, s_profile, v_profile) + cruise_speed = speed_at_progress_m(0.4, s_profile, v_profile) + + assert cruise_speed > before_corner_speed + assert before_corner_speed > at_corner_speed + assert at_corner_speed < 0.6 diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py new file mode 100644 index 0000000000..af9d6307a4 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py @@ -0,0 +1,124 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for the operator run-profile contract.""" + +import math + +import pytest + +from dimos.navigation.dannav.controllers import command_envelope_overrides_for_profile +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( + GO2_RUN_PROFILES, + RunProfile, + RunProfileError, + RunProfileRegistry, +) + + +def _profile(**overrides: object) -> RunProfile: + base: dict[str, object] = dict( + name="probe", + requested_planner_speed_m_s=1.0, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=0.6, + goal_decel_m_s2=1.0, + max_planar_cmd_accel_m_s2=5.0, + max_yaw_rate_rad_s=1.0, + max_yaw_accel_rad_s2=5.0, + required_locomotion_mode="default", + ) + base.update(overrides) + return RunProfile(**base) # type: ignore[arg-type] + + +def test_go2_profiles_step_the_speed_envelope_up() -> None: + speeds = [ + GO2_RUN_PROFILES.get(name).requested_planner_speed_m_s + for name in ("walk", "trot", "run_conservative", "run_verified") + ] + assert speeds == sorted(speeds) + assert len(set(speeds)) == len(speeds) + + +@pytest.mark.parametrize( + "field_name", + [ + "requested_planner_speed_m_s", + "max_tangent_accel_m_s2", + "max_normal_accel_m_s2", + "goal_decel_m_s2", + "max_planar_cmd_accel_m_s2", + "max_yaw_rate_rad_s", + "max_yaw_accel_rad_s2", + ], +) +@pytest.mark.parametrize("bad_value", [0.0, -1.0, math.nan, math.inf]) +def test_envelope_fields_reject_non_positive_or_non_finite( + field_name: str, bad_value: float +) -> None: + with pytest.raises(RunProfileError, match=field_name): + _profile(**{field_name: bad_value}) + + +def test_empty_name_rejected() -> None: + with pytest.raises(RunProfileError, match="name"): + _profile(name=" ") + + +def test_empty_locomotion_mode_rejected() -> None: + with pytest.raises(RunProfileError, match="required_locomotion_mode"): + _profile(required_locomotion_mode="") + + +def test_profile_limit_helpers_match_fields() -> None: + profile = GO2_RUN_PROFILES.get("run_conservative") + limits = profile.command_limits() + assert limits.max_planar_speed_m_s == pytest.approx(profile.requested_planner_speed_m_s) + assert limits.max_yaw_rate_rad_s == pytest.approx(profile.max_yaw_rate_rad_s) + + path_limits = profile.path_speed_profile_limits_at(1.25) + assert path_limits.max_speed_m_s == pytest.approx(1.25) + assert path_limits.max_tangent_accel_m_s2 == pytest.approx(profile.max_tangent_accel_m_s2) + + overrides = command_envelope_overrides_for_profile(profile) + assert overrides.max_yaw_rate_rad_s == pytest.approx(profile.max_yaw_rate_rad_s) + assert overrides.max_planar_cmd_accel_m_s2 == pytest.approx(profile.max_planar_cmd_accel_m_s2) + + +def test_registry_rejects_key_name_mismatch() -> None: + walk = GO2_RUN_PROFILES.get("walk") + with pytest.raises(RunProfileError, match="does not match"): + RunProfileRegistry( + profiles={"stroll": walk}, + default_profile_name="stroll", + ) + + +def test_registry_rejects_missing_default() -> None: + walk = GO2_RUN_PROFILES.get("walk") + with pytest.raises(RunProfileError, match="default profile"): + RunProfileRegistry( + profiles={"walk": walk}, + default_profile_name="run_verified", + ) + + +def test_get_unknown_name_raises_with_known_names() -> None: + with pytest.raises(RunProfileError) as excinfo: + GO2_RUN_PROFILES.get("sprint") + message = str(excinfo.value) + assert "sprint" in message + for known in ("walk", "trot", "run_conservative", "run_verified"): + assert known in message diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py new file mode 100644 index 0000000000..10943ed384 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py @@ -0,0 +1,124 @@ +# 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. + +"""Holonomic body-frame command limits and saturation. + +Limits apply to the same planar ``cmd_vel`` convention as +``trajectory_metrics.commanded_planar_speed`` (``linear.x``, ``linear.y`` in +the body frame) and to yaw rate ``angular.z``. + +Non-planar components (``linear.z``, ``angular.x``, ``angular.y``) are not +slew-limited here; they are passed through from ``raw_cmd`` so a future +controller or stack can extend DOFs without this helper fighting it. + +Slewing: acceleration limits are applied in the 2D linear velocity plane, then +planar speed and yaw rate limits cap the result. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + +@dataclass(frozen=True) +class HolonomicCommandLimits: + """Upper bounds for holonomic body-frame command saturation.""" + + max_planar_speed_m_s: float + max_yaw_rate_rad_s: float + max_planar_linear_accel_m_s2: float + max_yaw_accel_rad_s2: float + + def __post_init__(self) -> None: + for name, value in ( + ("max_planar_speed_m_s", self.max_planar_speed_m_s), + ("max_yaw_rate_rad_s", self.max_yaw_rate_rad_s), + ("max_planar_linear_accel_m_s2", self.max_planar_linear_accel_m_s2), + ("max_yaw_accel_rad_s2", self.max_yaw_accel_rad_s2), + ): + if not math.isfinite(value) or value < 0.0: + raise ValueError(f"{name} must be a non-negative finite float, got {value!r}") + + +def clamp_holonomic_cmd_vel( + previous_cmd: Twist, + raw_cmd: Twist, + limits: HolonomicCommandLimits, + dt_s: float, +) -> Twist: + """Apply acceleration then rate limits; call each tick with the previous *published* command. + + Parameters + ---------- + previous_cmd + Last command actually applied or sent, used for acceleration bounding. + raw_cmd + Unsaturated request (for example a controller output). + limits + Speed and acceleration envelopes. + dt_s + Controller period in seconds. Must be positive and finite. + """ + if not (math.isfinite(dt_s) and dt_s > 0.0): + raise ValueError(f"dt_s must be a finite positive scalar, got {dt_s!r}") + + p_x, p_y = float(previous_cmd.linear.x), float(previous_cmd.linear.y) + p_wz = float(previous_cmd.angular.z) + r_x, r_y = float(raw_cmd.linear.x), float(raw_cmd.linear.y) + r_wz = float(raw_cmd.angular.z) + + d_x, d_y = r_x - p_x, r_y - p_y + mag = math.hypot(d_x, d_y) + max_d = limits.max_planar_linear_accel_m_s2 * dt_s + if mag > 1e-15 and mag > max_d: + s = max_d / mag + s_x, s_y = p_x + d_x * s, p_y + d_y * s + else: + s_x, s_y = r_x, r_y + + sp = math.hypot(s_x, s_y) + v_max = limits.max_planar_speed_m_s + if sp > v_max and sp > 1e-15: + f = v_max / sp + s_x, s_y = s_x * f, s_y * f + + w_delta = r_wz - p_wz + max_w_step = limits.max_yaw_accel_rad_s2 * dt_s + w_delta = max(-max_w_step, min(max_w_step, w_delta)) + wz = p_wz + w_delta + w_max = limits.max_yaw_rate_rad_s + wz = max(-w_max, min(w_max, wz)) + + return Twist( + linear=Vector3( + s_x, + s_y, + float(raw_cmd.linear.z), + ), + angular=Vector3( + float(raw_cmd.angular.x), + float(raw_cmd.angular.y), + wz, + ), + ) + + +__all__ = [ + "HolonomicCommandLimits", + "clamp_holonomic_cmd_vel", +] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py new file mode 100644 index 0000000000..b1b58ad637 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py @@ -0,0 +1,71 @@ +# 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. + +"""JSONL export for trajectory control tick logs. + +Each line is one UTF-8 JSON object. Normative field names and units are +documented in ``trajectory_control_tick_jsonl.md`` in this package. +""" + +from __future__ import annotations + +from dataclasses import asdict +import json +from pathlib import Path +from typing import Any + +from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import TrajectoryControlTick + +TRAJECTORY_CONTROL_TICK_JSONL_SCHEMA_VERSION = 1 + + +def trajectory_control_tick_to_jsonl_dict(tick: TrajectoryControlTick) -> dict[str, Any]: + """Map one tick to a JSON-ready dict (stable key order: schema, then dataclass fields).""" + return {"schema_version": TRAJECTORY_CONTROL_TICK_JSONL_SCHEMA_VERSION} | asdict(tick) + + +class JsonlTrajectoryControlTickSink: + """Append control ticks directly to a JSONL file during live runs.""" + + def __init__(self, path: Path | str) -> None: + self.path = Path(path).expanduser() + self.path.parent.mkdir(parents=True, exist_ok=True) + self._file = self.path.open("a", encoding="utf-8") + + def append(self, tick: TrajectoryControlTick) -> None: + self._file.write( + json.dumps( + trajectory_control_tick_to_jsonl_dict(tick), + separators=(",", ":"), + allow_nan=False, + ) + ) + self._file.write("\n") + self._file.flush() + + def close(self) -> None: + self._file.close() + + def __enter__(self) -> JsonlTrajectoryControlTickSink: + return self + + def __exit__(self, *_: object) -> None: + self.close() + + +__all__ = [ + "TRAJECTORY_CONTROL_TICK_JSONL_SCHEMA_VERSION", + "JsonlTrajectoryControlTickSink", + "trajectory_control_tick_to_jsonl_dict", +] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py new file mode 100644 index 0000000000..a2f1c66f2a --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py @@ -0,0 +1,204 @@ +# 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-control-tick telemetry record. + +Each tick captures reference and measured pose and body-frame velocity, pose +tracking error scalars, the commanded ``cmd_vel``, control period ``dt``, and +optional wall or simulation timestamps. Live runs pass ticks to +``JsonlTrajectoryControlTickSink`` when ``GlobalConfig.local_planner_trajectory_tick_log_path`` +is set. + +JSONL export (stable field names) lives in ``dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export`` +and ``trajectory_control_tick_jsonl.md``. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Protocol, runtime_checkable + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.navigation.holonomic_trajectory_controller.trajectory_metrics import ( + commanded_planar_speed, + planar_position_divergence, + pose_errors_vs_reference, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample + + +@dataclass(frozen=True) +class TrajectoryControlTick: + """One closed-loop trajectory control iteration (data only).""" + + # Reference: plan-frame pose and body-frame twist, trajectory time basis + ref_time_s: float + ref_x_m: float + ref_y_m: float + ref_yaw_rad: float + ref_twist_linear_x_m_s: float + ref_twist_linear_y_m_s: float + ref_twist_linear_z_m_s: float + ref_twist_angular_x_rad_s: float + ref_twist_angular_y_rad_s: float + ref_twist_angular_z_rad_s: float + + # Measured state in the same frames and time basis as ``trajectory_types`` + meas_time_s: float + meas_x_m: float + meas_y_m: float + meas_yaw_rad: float + meas_twist_linear_x_m_s: float + meas_twist_linear_y_m_s: float + meas_twist_linear_z_m_s: float + meas_twist_angular_x_rad_s: float + meas_twist_angular_y_rad_s: float + meas_twist_angular_z_rad_s: float + + # Error scalars (see ``trajectory_metrics``) + e_along_track_m: float + e_cross_track_m: float + e_heading_rad: float + planar_position_divergence_m: float + + # Command actually published this tick (body frame) + cmd_linear_x_m_s: float + cmd_linear_y_m_s: float + cmd_linear_z_m_s: float + cmd_angular_x_rad_s: float + cmd_angular_y_rad_s: float + cmd_angular_z_rad_s: float + commanded_planar_speed_m_s: float + + dt_s: float + wall_time_s: float | None = None + sim_time_s: float | None = None + + +def _planar_yaw_rad(pose_plan_pose: object) -> float: + return float(pose_plan_pose.orientation.euler.z) + + +def _twist_components(twist: Twist) -> tuple[float, float, float, float, float, float]: + return ( + float(twist.linear.x), + float(twist.linear.y), + float(twist.linear.z), + float(twist.angular.x), + float(twist.angular.y), + float(twist.angular.z), + ) + + +def trajectory_control_tick_from_samples( + reference: TrajectoryReferenceSample, + measurement: TrajectoryMeasuredSample, + command: Twist, + dt_s: float, + *, + wall_time_s: float | None = None, + sim_time_s: float | None = None, +) -> TrajectoryControlTick: + """Build a tick record from trajectory samples and the clamped command.""" + ref_p = reference.pose_plan + meas_p = measurement.pose_plan + x_ref, y_ref = float(ref_p.position.x), float(ref_p.position.y) + yaw_ref = _planar_yaw_rad(ref_p) + x_m, y_m = float(meas_p.position.x), float(meas_p.position.y) + yaw_m = _planar_yaw_rad(meas_p) + + e_at, e_ct, e_psi = pose_errors_vs_reference(x_m, y_m, yaw_m, x_ref, y_ref, yaw_ref) + div = planar_position_divergence(e_at, e_ct) + + rlin = _twist_components(reference.twist_body) + mlin = _twist_components(measurement.twist_body) + clin = _twist_components(command) + v_cmd = commanded_planar_speed(command) + + return TrajectoryControlTick( + ref_time_s=float(reference.time_s), + ref_x_m=x_ref, + ref_y_m=y_ref, + ref_yaw_rad=yaw_ref, + ref_twist_linear_x_m_s=rlin[0], + ref_twist_linear_y_m_s=rlin[1], + ref_twist_linear_z_m_s=rlin[2], + ref_twist_angular_x_rad_s=rlin[3], + ref_twist_angular_y_rad_s=rlin[4], + ref_twist_angular_z_rad_s=rlin[5], + meas_time_s=float(measurement.time_s), + meas_x_m=x_m, + meas_y_m=y_m, + meas_yaw_rad=yaw_m, + meas_twist_linear_x_m_s=mlin[0], + meas_twist_linear_y_m_s=mlin[1], + meas_twist_linear_z_m_s=mlin[2], + meas_twist_angular_x_rad_s=mlin[3], + meas_twist_angular_y_rad_s=mlin[4], + meas_twist_angular_z_rad_s=mlin[5], + e_along_track_m=e_at, + e_cross_track_m=e_ct, + e_heading_rad=e_psi, + planar_position_divergence_m=div, + cmd_linear_x_m_s=clin[0], + cmd_linear_y_m_s=clin[1], + cmd_linear_z_m_s=clin[2], + cmd_angular_x_rad_s=clin[3], + cmd_angular_y_rad_s=clin[4], + cmd_angular_z_rad_s=clin[5], + commanded_planar_speed_m_s=v_cmd, + dt_s=float(dt_s), + wall_time_s=wall_time_s, + sim_time_s=sim_time_s, + ) + + +@runtime_checkable +class TrajectoryControlTickSink(Protocol): + """Consumer of control ticks (in-memory buffer, exporter, metrics, etc.).""" + + def append(self, tick: TrajectoryControlTick) -> None: ... + + +def append_trajectory_control_tick( + sink: TrajectoryControlTickSink | None, + reference: TrajectoryReferenceSample, + measurement: TrajectoryMeasuredSample, + command: Twist, + dt_s: float, + *, + wall_time_s: float | None = None, + sim_time_s: float | None = None, +) -> TrajectoryControlTick | None: + """If ``sink`` is None, skip work and return None; else record and return the tick.""" + if sink is None: + return None + tick = trajectory_control_tick_from_samples( + reference, + measurement, + command, + dt_s, + wall_time_s=wall_time_s, + sim_time_s=sim_time_s, + ) + sink.append(tick) + return tick + + +__all__ = [ + "TrajectoryControlTick", + "TrajectoryControlTickSink", + "append_trajectory_control_tick", + "trajectory_control_tick_from_samples", +] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py new file mode 100644 index 0000000000..0516b33670 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py @@ -0,0 +1,166 @@ +# 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. + +"""Holonomic planar trajectory tracking. + +Cartesian law in the plan frame: position error ``(p_ref - p_meas)`` is +rotated into the measured body frame, then a proportional correction is added +to the reference body ``Twist`` (feedforward). Heading uses the same +``angle_diff`` convention as ``trajectory_metrics.pose_errors_vs_reference``. + +This is a standard omnidirectional tracking law, not a path-curvature or +lookahead car-style law (no Pure Pursuit). +""" + +from __future__ import annotations + +import math + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import HolonomicCommandLimits +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.utils.trigonometry import angle_diff + + +def _planar_yaw_rad(pose_plan: object) -> float: + return float(pose_plan.orientation.euler.z) + + +def _scale_planar_twist(cmd: Twist, max_planar_speed_m_s: float) -> Twist: + sp = math.hypot(float(cmd.linear.x), float(cmd.linear.y)) + if sp <= max_planar_speed_m_s or sp < 1e-15: + return Twist(cmd) + f = max_planar_speed_m_s / sp + return Twist( + linear=Vector3( + float(cmd.linear.x) * f, + float(cmd.linear.y) * f, + float(cmd.linear.z), + ), + angular=Vector3( + float(cmd.angular.x), + float(cmd.angular.y), + float(cmd.angular.z), + ), + ) + + +def _clamp_yaw_rate(cmd: Twist, max_abs_wz: float) -> Twist: + wz = float(cmd.angular.z) + wz = max(-max_abs_wz, min(max_abs_wz, wz)) + return Twist( + linear=Vector3( + float(cmd.linear.x), + float(cmd.linear.y), + float(cmd.linear.z), + ), + angular=Vector3( + float(cmd.angular.x), + float(cmd.angular.y), + wz, + ), + ) + + +def _signed_overspeed_error(measured: float, reference: float) -> float: + if reference > 0.0: + return max(0.0, measured - reference) + if reference < 0.0: + return min(0.0, measured - reference) + return measured + + +class HolonomicTrackingController: + """Feedforward body twist plus proportional pose tracking in the holonomic plane.""" + + def __init__( + self, + *, + k_position_per_s: float, + k_yaw_per_s: float, + k_velocity_per_s: float = 0.0, + k_yaw_rate_per_s: float = 0.0, + ) -> None: + if not math.isfinite(k_position_per_s) or k_position_per_s < 0.0: + raise ValueError("k_position_per_s must be a non-negative finite float") + if not math.isfinite(k_yaw_per_s) or k_yaw_per_s < 0.0: + raise ValueError("k_yaw_per_s must be a non-negative finite float") + if not math.isfinite(k_velocity_per_s) or k_velocity_per_s < 0.0: + raise ValueError("k_velocity_per_s must be a non-negative finite float") + if not math.isfinite(k_yaw_rate_per_s) or k_yaw_rate_per_s < 0.0: + raise ValueError("k_yaw_rate_per_s must be a non-negative finite float") + self._kp = float(k_position_per_s) + self._ky = float(k_yaw_per_s) + self._kv = float(k_velocity_per_s) + self._kw = float(k_yaw_rate_per_s) + self._limits: HolonomicCommandLimits | None = None + + def configure(self, limits: HolonomicCommandLimits) -> None: + self._limits = limits + + def reset(self) -> None: + pass + + def control( + self, + reference: TrajectoryReferenceSample, + measurement: TrajectoryMeasuredSample, + ) -> Twist: + x_r = float(reference.pose_plan.position.x) + y_r = float(reference.pose_plan.position.y) + yaw_r = _planar_yaw_rad(reference.pose_plan) + x_m = float(measurement.pose_plan.position.x) + y_m = float(measurement.pose_plan.position.y) + yaw_m = _planar_yaw_rad(measurement.pose_plan) + + dx_w = x_r - x_m + dy_w = y_r - y_m + c = math.cos(yaw_m) + s = math.sin(yaw_m) + ex_b = c * dx_w + s * dy_w + ey_b = -s * dx_w + c * dy_w + + ref = reference.twist_body + yaw_ref_to_meas = yaw_r - yaw_m + c_rm = math.cos(yaw_ref_to_meas) + s_rm = math.sin(yaw_ref_to_meas) + vx_ff = c_rm * float(ref.linear.x) - s_rm * float(ref.linear.y) + vy_ff = s_rm * float(ref.linear.x) + c_rm * float(ref.linear.y) + meas = measurement.twist_body + vx_err = _signed_overspeed_error(float(meas.linear.x), vx_ff) + vy_err = _signed_overspeed_error(float(meas.linear.y), vy_ff) + vx = vx_ff + self._kp * ex_b - self._kv * vx_err + vy = vy_ff + self._kp * ey_b - self._kv * vy_err + e_psi = angle_diff(yaw_m, yaw_r) + wz_ff = float(ref.angular.z) + wz_err = _signed_overspeed_error(float(meas.angular.z), wz_ff) + wz = wz_ff - self._ky * e_psi - self._kw * wz_err + + raw = Twist( + linear=Vector3(vx, vy, float(ref.linear.z)), + angular=Vector3( + float(ref.angular.x), + float(ref.angular.y), + wz, + ), + ) + lim = self._limits + if lim is None: + return raw + out = _scale_planar_twist(raw, lim.max_planar_speed_m_s) + return _clamp_yaw_rate(out, lim.max_yaw_rate_rad_s) + + +__all__ = ["HolonomicTrackingController"] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py new file mode 100644 index 0000000000..cd04c23d8e --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py @@ -0,0 +1,274 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on the "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. + +"""Planar trajectory tracking metrics. + +This module is the **normative** definition for telemetry, plots, and tests that +speak about along-track error, cross-track error, divergence, and commanded +planar speed. + +**Frames** + +- Path geometry and horizontal positions use the same **map / plan** horizontal + frame as ``Path`` and ``PathDistancer`` (x forward, y left in that frame). +- ``commanded_planar_speed`` uses **base-link** linear ``Twist`` components + (``linear.x`` forward, ``linear.y`` lateral) as emitted on ``cmd_vel`` for + omnidirectional bases. + +**Along-track and cross-track (reference pose)** + +Given a reference pose ``(x_ref, y_ref, yaw_ref)`` and measured +``(x_meas, y_meas)``, express the position error in a frame aligned with the +reference heading (x = forward along reference, y = left): + +- **Along-track error** ``e_at`` (m): signed component of ``(p_meas - p_ref)`` + on the reference forward axis. Positive means the measured point is **ahead** + of the reference in the reference forward direction. +- **Cross-track error** ``e_ct`` (m): signed component on the left axis. + Positive means the measured point is **to the left** of the reference when + facing along ``yaw_ref``. +- **Heading error** ``e_psi`` (rad): ``angle_diff(yaw_meas, yaw_ref)`` so it + matches ``dimos.utils.trigonometry.angle_diff`` conventions used in + navigation. + +**Divergence from target** + +- **Planar position divergence** (m): ``hypot(e_at, e_ct)``. This is the + default scalar for **speed vs planar position error** style plots when a + full reference pose exists. +- **Planar pose divergence** (mixed units unless scaled): ``sqrt(e_at**2 + + e_ct**2 + (yaw_weight_rad_to_m * e_psi)**2)``. ``yaw_weight_rad_to_m`` maps + heading into an equivalent meter scale for a single RMS; set to ``0`` to + recover position-only divergence. + +**Polyline reference (spatial path only)** + +For a piecewise-linear path, the closest point **on segments** (not only on +vertices) defines a foot point, tangent, and arc length ``s`` from the first +vertex to the foot. **Signed cross-track** uses the left-of-tangent rule +above. **Along-track error vs a time-parameterized reference** needs a scalar +``s_ref`` from the trajectory (arc length along the same polyline): ``e_at = +s_meas - s_ref``. Pure geometry without ``s_ref`` does not define longitudinal +error relative to a moving target; logging and controllers should supply +``s_ref`` once trajectories are timed. + +**Commanded speed for plots** + +At each control tick, sample ``commanded_planar_speed(cmd_twist)`` using the +``Twist`` actually published. This is the magnitude of the horizontal command +in the base frame, **not** a finite difference of odometry. + +**Context-dependent tolerance** + +Obstacle clearance and corridor width are **planner-side** inputs. Tracking +tolerances may be tightened in tight clearance and relaxed in open space. This +module exposes ``TrackingTolerance`` plus ``scale_tolerance_by_clearance`` as a +**deterministic example** of how clearance can modulate tolerances; production +planners may replace that mapping while keeping the same error definitions. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.utils.trigonometry import angle_diff + + +def pose_errors_vs_reference( + x_meas: float, + y_meas: float, + yaw_meas: float, + x_ref: float, + y_ref: float, + yaw_ref: float, +) -> tuple[float, float, float]: + """Along-track (m), cross-track (m), heading error (rad) vs a reference pose.""" + dx = x_meas - x_ref + dy = y_meas - y_ref + c = math.cos(yaw_ref) + s = math.sin(yaw_ref) + e_at = dx * c + dy * s + e_ct = -dx * s + dy * c + e_psi = angle_diff(yaw_meas, yaw_ref) + return (e_at, e_ct, e_psi) + + +def planar_position_divergence(e_at: float, e_ct: float) -> float: + """Euclidean planar position error (m) from along- and cross-track components.""" + return float(math.hypot(e_at, e_ct)) + + +def planar_pose_divergence( + e_at: float, + e_ct: float, + e_psi: float, + *, + yaw_weight_rad_to_m: float = 0.0, +) -> float: + """RMS of planar position and optional yaw, with yaw scaled to meters.""" + w = yaw_weight_rad_to_m + return float(math.sqrt(e_at * e_at + e_ct * e_ct + (w * e_psi) * (w * e_psi))) + + +def commanded_planar_speed(cmd: Twist) -> float: + """Horizontal speed (m/s) from a commanded ``Twist`` in the base link frame.""" + return float(math.hypot(cmd.linear.x, cmd.linear.y)) + + +@dataclass(frozen=True) +class PolylineProjection: + """Closest point on a polyline and path coordinates at the foot.""" + + foot_xy: tuple[float, float] + segment_start_index: int + tangent_yaw: float + s_along_path_m: float + signed_cross_track_m: float + + +def project_to_polyline(x: float, y: float, polyline_xy: NDArray[np.float64]) -> PolylineProjection: + """Project a point onto a ``(N, 2)`` polyline in the map frame.""" + if polyline_xy.ndim != 2 or polyline_xy.shape[1] != 2: + raise ValueError("polyline_xy must have shape (N, 2)") + n = polyline_xy.shape[0] + if n == 0: + raise ValueError("polyline_xy must be non-empty") + if n == 1: + fx = float(polyline_xy[0, 0]) + fy = float(polyline_xy[0, 1]) + lat = float(math.hypot(x - fx, y - fy)) + return PolylineProjection( + (fx, fy), + 0, + 0.0, + 0.0, + lat, + ) + + p = np.array([x, y], dtype=np.float64) + prefix_s = np.zeros(n, dtype=np.float64) + for i in range(1, n): + prefix_s[i] = prefix_s[i - 1] + float(np.linalg.norm(polyline_xy[i] - polyline_xy[i - 1])) + + best_d2 = math.inf + best_foot = polyline_xy[0].copy() + best_seg = 0 + best_t = 0.0 + best_seg_len = 0.0 + + for i in range(n - 1): + a = polyline_xy[i] + b = polyline_xy[i + 1] + ab = b - a + seg_len2 = float(ab @ ab) + if seg_len2 <= 1e-18: + foot = a + t_param = 0.0 + seg_len = 0.0 + else: + t_param = float(np.clip(((p - a) @ ab) / seg_len2, 0.0, 1.0)) + foot = a + t_param * ab + seg_len = math.sqrt(seg_len2) + d2 = float((p - foot) @ (p - foot)) + if d2 < best_d2: + best_d2 = d2 + best_foot = foot + best_seg = i + best_t = t_param + best_seg_len = seg_len + + s_along = float(prefix_s[best_seg] + best_t * best_seg_len) + + a = polyline_xy[best_seg] + b = polyline_xy[best_seg + 1] + ab = b - a + seg_len = float(np.linalg.norm(ab)) + if seg_len < 1e-9: + yaw = 0.0 + tx, ty = 1.0, 0.0 + else: + tx, ty = float(ab[0] / seg_len), float(ab[1] / seg_len) + yaw = math.atan2(ab[1], ab[0]) + nx, ny = -ty, tx + vx = x - float(best_foot[0]) + vy = y - float(best_foot[1]) + signed_ct = vx * nx + vy * ny + return PolylineProjection( + (float(best_foot[0]), float(best_foot[1])), + best_seg, + yaw, + s_along, + float(signed_ct), + ) + + +def along_track_progress_error(s_meas_m: float, s_ref_m: float) -> float: + """Signed arc-length error (m): measured minus reference along the same path.""" + return float(s_meas_m - s_ref_m) + + +@dataclass(frozen=True) +class TrackingTolerance: + """Absolute envelopes for tracking errors (planner or policy may supply these).""" + + along_track_m: float + cross_track_m: float + heading_rad: float + + def satisfied( + self, + e_at: float, + e_ct: float, + e_psi: float, + ) -> bool: + return ( + abs(e_at) <= self.along_track_m + and abs(e_ct) <= self.cross_track_m + and abs(e_psi) <= self.heading_rad + ) + + +def scale_tolerance_by_clearance( + base: TrackingTolerance, + clearance_m: float, + *, + tight_clearance_m: float = 0.35, + loose_clearance_m: float = 2.0, + tight_scale: float = 0.55, + loose_scale: float = 1.15, +) -> TrackingTolerance: + """Piecewise-linear scale of tolerances vs a scalar clearance (example policy). + + When ``clearance_m`` is small, tolerances shrink (tighter tracking in narrow + space). When clearance is large, tolerances grow modestly. Values are + illustrative; planners may substitute their own mapping. + """ + c = clearance_m + if c <= tight_clearance_m: + k = tight_scale + elif c >= loose_clearance_m: + k = loose_scale + else: + u = (c - tight_clearance_m) / (loose_clearance_m - tight_clearance_m) + k = tight_scale + u * (loose_scale - tight_scale) + return TrackingTolerance( + along_track_m=base.along_track_m * k, + cross_track_m=base.cross_track_m * k, + heading_rad=base.heading_rad * k, + ) diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py new file mode 100644 index 0000000000..362d205159 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py @@ -0,0 +1,227 @@ +# 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. + +"""Speed vs arc length for planar polylines. + +Builds a scalar speed profile along a polyline using per-sample geometry caps +and a forward-backward pass on arc length (``v^2 <= v_0^2 + 2 a \\Delta s``). + +- **Straight segments:** cap is ``limits.max_speed_m_s``. +- **Corners:** cap is ``min(max_speed_m_s, sqrt(max_normal_accel_m_s2 * |R|))`` + from the circumradius of the local vertex triangle. + +The live planner seeds the forward pass at cruise speed so replanned paths do not +force zero speed at the origin. +""" + +from __future__ import annotations + +import bisect +from collections.abc import Sequence +from dataclasses import dataclass +import math + +import numpy as np +from numpy.typing import NDArray + + +@dataclass(frozen=True) +class PathSpeedProfileLimits: + """Scalar limits for profiling speed along one planar path segment.""" + + max_speed_m_s: float + max_tangent_accel_m_s2: float + max_normal_accel_m_s2: float + + def __post_init__(self) -> None: + for name, value in ( + ("max_speed_m_s", self.max_speed_m_s), + ("max_tangent_accel_m_s2", self.max_tangent_accel_m_s2), + ("max_normal_accel_m_s2", self.max_normal_accel_m_s2), + ): + if not math.isfinite(value) or value < 0.0: + raise ValueError(f"{name} must be a non-negative finite float, got {value!r}") + + +def _circular_arc_geometry_speed_cap_m_s( + abs_radius_m: float, limits: PathSpeedProfileLimits +) -> float: + """Upper speed from centripetal bound ``v^2 / R <= a_n`` and ``max_speed_m_s``.""" + if not math.isfinite(abs_radius_m) or abs_radius_m <= 0.0: + raise ValueError(f"abs_radius_m must be finite and positive, got {abs_radius_m!r}") + v_curve = math.sqrt(limits.max_normal_accel_m_s2 * abs_radius_m) + return min(limits.max_speed_m_s, v_curve) + + +def _line_segment_geometry_speed_cap_m_s(limits: PathSpeedProfileLimits) -> float: + """Geometric cap for a straight segment (no curvature binding).""" + return float(limits.max_speed_m_s) + + +def _vertex_progress_along_polyline_m( + cumulative_segment_s_m: NDArray[np.float64], +) -> NDArray[np.float64]: + """Arc length to each vertex; segment cumulatives are PathDistancer-style.""" + vertex_s = np.zeros(len(cumulative_segment_s_m) + 1, dtype=np.float64) + vertex_s[1:] = cumulative_segment_s_m + return vertex_s + + +def _polyline_geometry_speed_cap_m_s( + path_xy: NDArray[np.float64], + vertex_s_m: Sequence[float], + progress_m: float, + limits: PathSpeedProfileLimits, +) -> float: + """Geometry speed cap at arc length ``progress_m`` on a planar polyline.""" + if len(path_xy) < 3: + return _line_segment_geometry_speed_cap_m_s(limits) + + for i in range(1, len(path_xy) - 1): + vertex_s = float(vertex_s_m[i]) + prev_s = float(vertex_s_m[i - 1]) + next_s = float(vertex_s_m[i + 1]) + local_scale = max(vertex_s - prev_s, next_s - vertex_s, 1.0) + if abs(float(progress_m) - vertex_s) > max(1e-9, local_scale * 1e-9): + continue + p0, p1, p2 = path_xy[i - 1], path_xy[i], path_xy[i + 1] + a = float(np.linalg.norm(p1 - p0)) + b = float(np.linalg.norm(p2 - p1)) + c = float(np.linalg.norm(p2 - p0)) + v10 = p1 - p0 + v20 = p2 - p0 + area2 = abs(float(v10[0] * v20[1] - v10[1] * v20[0])) + if min(a, b, c, area2) <= 1e-9: + return _line_segment_geometry_speed_cap_m_s(limits) + radius = (a * b * c) / (2.0 * area2) + return _circular_arc_geometry_speed_cap_m_s(radius, limits) + return _line_segment_geometry_speed_cap_m_s(limits) + + +def _profile_sample_distances_m( + total_length_m: float, + vertex_s_m: Sequence[float], + num_samples: int, +) -> list[float]: + """Uniform arc-length samples plus every polyline vertex.""" + if total_length_m <= 0.0: + return [0.0] + if num_samples < 3: + raise ValueError(f"num_samples must be at least 3, got {num_samples}") + distances = {0.0, float(total_length_m), *(float(s) for s in vertex_s_m)} + for i in range(num_samples): + distances.add(total_length_m * float(i) / float(num_samples - 1)) + return sorted(distances) + + +def _speed_profile_from_geometry_caps( + s_m: Sequence[float], + geometry_cap_m_s: Sequence[float], + *, + max_tangent_accel_m_s2: float, + goal_decel_m_s2: float, + start_speed_m_s: float = 0.0, +) -> list[float]: + """Forward tangent-accel and backward goal-decel envelope over geometry caps. + + ``start_speed_m_s`` seeds the forward pass at ``s_m[0]``. Use ``0`` for an + offline rest-to-rest segment profile. For the live planner, pass cruise speed + so acceleration along open path is not forced to zero at the path origin. + """ + if len(s_m) != len(geometry_cap_m_s): + raise ValueError("speed profile distances and caps must have the same length") + if not s_m: + return [] + + v_forward = [0.0] * len(s_m) + if start_speed_m_s > 0.0: + v_forward[0] = min(float(geometry_cap_m_s[0]), start_speed_m_s) + for i in range(len(s_m) - 1): + ds = max(0.0, float(s_m[i + 1]) - float(s_m[i])) + v_next_sq = v_forward[i] * v_forward[i] + 2.0 * max_tangent_accel_m_s2 * ds + v_forward[i + 1] = min(float(geometry_cap_m_s[i + 1]), math.sqrt(max(0.0, v_next_sq))) + + v_backward = [0.0] * len(s_m) + for i in range(len(s_m) - 1, 0, -1): + ds = max(0.0, float(s_m[i]) - float(s_m[i - 1])) + v_prev_sq = v_backward[i] * v_backward[i] + 2.0 * goal_decel_m_s2 * ds + v_backward[i - 1] = min(float(geometry_cap_m_s[i - 1]), math.sqrt(max(0.0, v_prev_sq))) + + return [ + min(float(geometry_cap_m_s[i]), v_forward[i], v_backward[i]) for i in range(len(s_m)) + ] + + +def profile_speed_along_polyline( + path_xy: NDArray[np.float64], + cumulative_segment_s_m: NDArray[np.float64], + limits: PathSpeedProfileLimits, + goal_decel_m_s2: float, + *, + num_samples: int = 200, + start_speed_m_s: float | None = None, +) -> tuple[list[float], list[float]]: + """Speed profile along a polyline with anticipatory corner decel.""" + if len(path_xy) < 2: + return [0.0], [0.0] + + if start_speed_m_s is None: + start_speed_m_s = limits.max_speed_m_s + + vertex_s_m = _vertex_progress_along_polyline_m(cumulative_segment_s_m) + total_length_m = float(vertex_s_m[-1]) + s_profile = _profile_sample_distances_m(total_length_m, vertex_s_m, num_samples) + caps = [ + _polyline_geometry_speed_cap_m_s(path_xy, vertex_s_m, s, limits) for s in s_profile + ] + v_profile = _speed_profile_from_geometry_caps( + s_profile, + caps, + max_tangent_accel_m_s2=limits.max_tangent_accel_m_s2, + goal_decel_m_s2=goal_decel_m_s2, + start_speed_m_s=start_speed_m_s, + ) + return s_profile, v_profile + + +def speed_at_progress_m( + progress_m: float, s_m: Sequence[float], v_m_s: Sequence[float] +) -> float: + """Linearly interpolate profile speed at arc length ``progress_m``.""" + if not s_m: + return 0.0 + if len(s_m) == 1: + return float(v_m_s[0]) + + s = float(np.clip(progress_m, s_m[0], s_m[-1])) + if s <= s_m[0]: + return float(v_m_s[0]) + if s >= s_m[-1]: + return float(v_m_s[-1]) + + idx = bisect.bisect_right(s_m, s) - 1 + idx = max(0, min(idx, len(s_m) - 2)) + s0, s1 = float(s_m[idx]), float(s_m[idx + 1]) + v0, v1 = float(v_m_s[idx]), float(v_m_s[idx + 1]) + if s1 <= s0: + return v0 + u = (s - s0) / (s1 - s0) + return v0 + (v1 - v0) * u + + +__all__ = [ + "PathSpeedProfileLimits", + "profile_speed_along_polyline", + "speed_at_progress_m", +] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py new file mode 100644 index 0000000000..d9bc690508 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py @@ -0,0 +1,187 @@ +# 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. + +"""Named Go2 movement envelopes (speed and limit caps). + +Data and validation only. Live wiring: ``GlobalConfig.go2_run_profile``, +``LocalPlanner._resolve_run_envelope``, and ``go2.connection`` locomotion mode. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math + +from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import HolonomicCommandLimits +from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import PathSpeedProfileLimits + +_POSITIVE_FIELDS: tuple[str, ...] = ( + "requested_planner_speed_m_s", + "max_tangent_accel_m_s2", + "max_normal_accel_m_s2", + "goal_decel_m_s2", + "max_planar_cmd_accel_m_s2", + "max_yaw_rate_rad_s", + "max_yaw_accel_rad_s2", +) + + +class RunProfileError(ValueError): + """Invalid run-profile definition (bad units or unknown name).""" + + +@dataclass(frozen=True) +class RunProfile: + """One named movement envelope an operator may request.""" + + name: str + requested_planner_speed_m_s: float + max_tangent_accel_m_s2: float + max_normal_accel_m_s2: float + goal_decel_m_s2: float + max_planar_cmd_accel_m_s2: float + max_yaw_rate_rad_s: float + max_yaw_accel_rad_s2: float + required_locomotion_mode: str + description: str = "" + + def __post_init__(self) -> None: + if not self.name.strip(): + raise RunProfileError("run profile name must be non-empty") + for field_name in _POSITIVE_FIELDS: + value = getattr(self, field_name) + if not math.isfinite(value) or value <= 0.0: + raise RunProfileError( + f"{self.name!r}.{field_name} must be a positive finite float, got {value!r}" + ) + if not self.required_locomotion_mode.strip(): + raise RunProfileError(f"{self.name!r}.required_locomotion_mode must be non-empty") + + def command_limits(self) -> HolonomicCommandLimits: + """Body-frame command saturation envelope for this profile.""" + return HolonomicCommandLimits( + max_planar_speed_m_s=self.requested_planner_speed_m_s, + max_yaw_rate_rad_s=self.max_yaw_rate_rad_s, + max_planar_linear_accel_m_s2=self.max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=self.max_yaw_accel_rad_s2, + ) + + def path_speed_profile_limits_at(self, max_speed_m_s: float) -> PathSpeedProfileLimits: + """Geometry-aware path speed limits at the given cruise cap (m/s).""" + return PathSpeedProfileLimits( + max_speed_m_s=max_speed_m_s, + max_tangent_accel_m_s2=self.max_tangent_accel_m_s2, + max_normal_accel_m_s2=self.max_normal_accel_m_s2, + ) + + +@dataclass(frozen=True) +class RunProfileRegistry: + """Named run profiles with a declared default profile name.""" + + profiles: Mapping[str, RunProfile] + default_profile_name: str + + def __post_init__(self) -> None: + if not self.profiles: + raise RunProfileError("registry must define at least one profile") + for key, profile in self.profiles.items(): + if key != profile.name: + raise RunProfileError( + f"registry key {key!r} does not match profile name {profile.name!r}" + ) + if self.default_profile_name not in self.profiles: + raise RunProfileError( + f"default profile {self.default_profile_name!r} is not in the registry" + ) + object.__setattr__(self, "profiles", dict(self.profiles)) + + def get(self, name: str) -> RunProfile: + """Look up a profile by name; unknown names list the known profiles.""" + try: + return self.profiles[name] + except KeyError as exc: + known = ", ".join(sorted(self.profiles)) + raise RunProfileError(f"unknown run profile {name!r}; known profiles: {known}") from exc + + def names(self) -> tuple[str, ...]: + """Profile names in registration order.""" + return tuple(self.profiles) + + +GO2_RUN_PROFILES = RunProfileRegistry( + default_profile_name="walk", + profiles={ + "walk": RunProfile( + name="walk", + requested_planner_speed_m_s=0.55, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=0.6, + goal_decel_m_s2=1.0, + max_planar_cmd_accel_m_s2=5.0, + max_yaw_rate_rad_s=1.0, + max_yaw_accel_rad_s2=5.0, + required_locomotion_mode="default", + description="Default cautious navigation; matches today's LocalPlanner walking behavior.", + ), + "trot": RunProfile( + name="trot", + requested_planner_speed_m_s=1.0, + max_tangent_accel_m_s2=1.5, + max_normal_accel_m_s2=0.8, + goal_decel_m_s2=1.2, + max_planar_cmd_accel_m_s2=5.0, + max_yaw_rate_rad_s=1.2, + max_yaw_accel_rad_s2=5.0, + required_locomotion_mode="default", + description="Faster trot within the default locomotion mode.", + ), + "run_conservative": RunProfile( + name="run_conservative", + requested_planner_speed_m_s=1.5, + max_tangent_accel_m_s2=2.0, + max_normal_accel_m_s2=1.0, + goal_decel_m_s2=1.5, + max_planar_cmd_accel_m_s2=6.0, + max_yaw_rate_rad_s=1.0, + max_yaw_accel_rad_s2=4.0, + required_locomotion_mode="default", + description=( + "Conservative run envelope at planner speed caps; stays in default " + "locomotion mode so onboard lidar and obstacle avoidance remain active." + ), + ), + "run_verified": RunProfile( + name="run_verified", + requested_planner_speed_m_s=2.5, + max_tangent_accel_m_s2=2.5, + max_normal_accel_m_s2=1.2, + goal_decel_m_s2=2.0, + max_planar_cmd_accel_m_s2=6.0, + max_yaw_rate_rad_s=0.8, + max_yaw_accel_rad_s2=3.0, + required_locomotion_mode="rage", + description="Highest envelope; switches Go2 to rage locomotion mode.", + ), + }, +) + + +__all__ = [ + "GO2_RUN_PROFILES", + "RunProfile", + "RunProfileError", + "RunProfileRegistry", +] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_types.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_types.py new file mode 100644 index 0000000000..cf272adc77 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_types.py @@ -0,0 +1,72 @@ +# 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-tick reference and measured samples for holonomic path following and JSONL logging. + +**Plan horizontal frame (``pose_plan``)** + +- Positions and yaw use the same plan horizontal convention as ``Path``, + ``PathDistancer``, and ``trajectory_metrics.pose_errors_vs_reference``. + +**Body command frame (``twist_body``)** + +- ``Twist.linear`` and ``Twist.angular`` follow the base / ``cmd_vel`` convention: + planar speed uses ``hypot(linear.x, linear.y)`` with x forward and y lateral. + +**Time (``time_s``)** + +- Scalar time in seconds for this sample (odom timestamp in live ``LocalPlanner`` runs). + +``Pose`` and ``Twist`` are copied in ``__post_init__`` so samples do not alias +caller-owned messages. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Twist import Twist + + +@dataclass(frozen=True) +class TrajectoryReferenceSample: + """One timed point on the reference trajectory (target).""" + + time_s: float + pose_plan: Pose + twist_body: Twist + + def __post_init__(self) -> None: + object.__setattr__(self, "pose_plan", Pose(self.pose_plan)) + object.__setattr__(self, "twist_body", Twist(self.twist_body)) + + +@dataclass(frozen=True) +class TrajectoryMeasuredSample: + """One timed measurement of actual robot state for tracking.""" + + time_s: float + pose_plan: Pose + twist_body: Twist + + def __post_init__(self) -> None: + object.__setattr__(self, "pose_plan", Pose(self.pose_plan)) + object.__setattr__(self, "twist_body", Twist(self.twist_body)) + + +__all__ = [ + "TrajectoryMeasuredSample", + "TrajectoryReferenceSample", +] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 39b26ff51b..babe073603 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -112,6 +112,7 @@ "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", + "unitree-go2-dannav": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_dannav:unitree_go2_dannav", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet", "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", @@ -179,6 +180,7 @@ "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", "gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule", + "holonomic-controller": "dimos.navigation.dannav.holonomic_controller.HolonomicController", "hosted-arm-teleop-module": "dimos.teleop.quest_hosted.hosted_extensions.HostedArmTeleopModule", "hosted-teleop-module": "dimos.teleop.quest_hosted.hosted_teleop_module.HostedTeleopModule", "hosted-teleop-recorder": "dimos.teleop.quest_hosted.blueprints.HostedTeleopRecorder", diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py new file mode 100644 index 0000000000..dd2a29bac8 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Go2 nav stack with holonomic trajectory control and hot replanning. + +Uses ``DannavPlanner`` with ``replan_on_costmap_update=True``. For the default +differential stack see ``unitree_go2``. +""" + +from dimos.core.coordination.blueprints import autoconnect +from dimos.mapping.costmapper import CostMapper +from dimos.mapping.pointclouds.occupancy import GeneralOccupancyConfig +from dimos.mapping.voxels import VoxelGridMapper +from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( + WavefrontFrontierExplorer, +) +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.patrolling.module import PatrollingModule +from dimos.navigation.dannav.module import DannavPlanner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic + +unitree_go2_dannav = autoconnect( + unitree_go2_basic, + VoxelGridMapper.blueprint(emit_every=1), + CostMapper.blueprint( + algo="general", + config=GeneralOccupancyConfig( + resolution=0.05, + min_height=0.08, + max_height=2.0, + mark_free_radius=0.2, + ), + ), + DannavPlanner.blueprint( + replan_on_costmap_update=True, + ), + MovementManager.blueprint(), +).global_config(n_workers=10, robot_model="unitree_go2", robot_width=0.25) + +__all__ = ["unitree_go2_dannav"] diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index 185546217b..c8bab179d7 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -67,6 +67,10 @@ class Go2Mode(str, Enum): class ConnectionConfig(ModuleConfig): ip: str = Field(default_factory=lambda m: m["g"].robot_ip) mode: Go2Mode = Go2Mode.DEFAULT + lidar: bool = True + camera: bool = True + # "mcf" for stair traversal, "normal" for basic, None to leave it as is + motion_mode: str | None = None # Per-device AES-128 key (Go2 fw >=1.1.15); defaults from GlobalConfig. aes_128_key: str | None = Field(default_factory=lambda m: m["g"].unitree_aes_128_key) @@ -177,6 +181,9 @@ def balance_stand(self) -> bool: def set_obstacle_avoidance(self, enabled: bool = True) -> None: pass + def set_motion_mode(self, name: str) -> None: + pass + def enable_rage_mode(self) -> bool: return True @@ -249,16 +256,21 @@ def onimage(image: Image) -> None: self.color_image.publish(image) self._latest_video_frame = image - self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) + if self.config.lidar: + self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) self.register_disposable(self.connection.odom_stream().subscribe(self._publish_tf)) - self.register_disposable(self.connection.video_stream().subscribe(onimage)) self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) - self._camera_info_thread = Thread( - target=self.publish_camera_info, - daemon=True, - ) - self._camera_info_thread.start() + if self.config.camera: + self.register_disposable(self.connection.video_stream().subscribe(onimage)) + self._camera_info_thread = Thread( + target=self.publish_camera_info, + daemon=True, + ) + self._camera_info_thread.start() + + if self.config.motion_mode and isinstance(self.connection, UnitreeWebRTCConnection): + self.connection.set_motion_mode(self.config.motion_mode) self.standup() time.sleep(3) @@ -269,8 +281,6 @@ def onimage(image: Image) -> None: self.connection.set_obstacle_avoidance(self.config.g.obstacle_avoidance) - # self.record("go2_bigoffice") - @rpc def stop(self) -> None: self.liedown() diff --git a/pyproject.toml b/pyproject.toml index e4f03e5406..cac7a453c5 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -505,6 +505,8 @@ module = [ "cyclonedds", "cyclonedds.*", "dimos_lcm.*", + "dimos_mls_planner", + "dimos_voxel_ray_tracing", "etils", "faster_whisper", "geometry_msgs.*", From 9b5ae14595de7ee0805083b880fd54d4ea75a621 Mon Sep 17 00:00:00 2001 From: danvi Date: Fri, 26 Jun 2026 15:16:35 +0800 Subject: [PATCH 2/9] Build DanHolonomicTC --- dimos/navigation/dannav/controllers.py | 189 +--- .../navigation/dannav/holonomic_controller.py | 190 ---- dimos/navigation/dannav/local_planner.py | 8 +- .../dannav/simple_global_planner.py | 130 --- .../test_local_planner_path_controller.py | 8 +- .../dannav/test_local_planner_run_envelope.py | 2 +- .../holonomic_path_controller.py | 218 +++++ .../holonomic_trajectory_controller/module.py | 836 ++++++++++++++++++ .../path_distancer.py | 0 .../test_dan_holonomic_tc.py | 257 ++++++ .../test_holonomic_path_follower.py | 555 ++++++++++++ .../test_run_envelope.py | 204 +++++ .../test_trajectory_run_profiles.py | 2 +- dimos/robot/all_blueprints.py | 1 - 14 files changed, 2082 insertions(+), 518 deletions(-) delete mode 100644 dimos/navigation/dannav/holonomic_controller.py delete mode 100644 dimos/navigation/dannav/simple_global_planner.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/module.py rename dimos/navigation/{dannav => holonomic_trajectory_controller}/path_distancer.py (100%) create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py create mode 100644 dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py diff --git a/dimos/navigation/dannav/controllers.py b/dimos/navigation/dannav/controllers.py index 5db33f55c9..e89e9b0adc 100644 --- a/dimos/navigation/dannav/controllers.py +++ b/dimos/navigation/dannav/controllers.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dataclasses import dataclass import math from typing import Protocol @@ -25,13 +24,8 @@ 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.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( - HolonomicCommandLimits, - clamp_holonomic_cmd_vel, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import RunProfile -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import HolonomicPathController +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryReferenceSample from dimos.utils.trigonometry import angle_diff @@ -42,34 +36,6 @@ def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: ) -def _pose_from_pose_stamped(odom: PoseStamped) -> Pose: - return Pose(odom.position, odom.orientation) - - -@dataclass(frozen=True) -class CommandEnvelopeOverrides: - """Run-profile command caps that replace the GlobalConfig defaults. - - The planar speed cap keeps tracking ``set_speed`` (the geometry-capped - path speed); these are the remaining saturation limits a movement - envelope owns. - """ - - max_yaw_rate_rad_s: float - max_planar_cmd_accel_m_s2: float - max_yaw_accel_rad_s2: float - - -def command_envelope_overrides_for_profile(profile: RunProfile) -> CommandEnvelopeOverrides: - """Map a :class:`RunProfile` to planner command caps (planar speed excluded).""" - limits = profile.command_limits() - return CommandEnvelopeOverrides( - max_yaw_rate_rad_s=limits.max_yaw_rate_rad_s, - max_planar_cmd_accel_m_s2=limits.max_planar_linear_accel_m_s2, - max_yaw_accel_rad_s2=limits.max_yaw_accel_rad_s2, - ) - - class Controller(Protocol): def advance( self, @@ -264,157 +230,6 @@ def _compute_angular_velocity(self, yaw_error: float) -> float: return float(angular_velocity) -class HolonomicPathController: - """Follow path segments using the holonomic tracking law (P3-3, issue 921). - - Wraps :class:`HolonomicTrackingController` in the :class:`Controller` seam - (lookahead + odom). Rotations in place use the same law with a fixed - position reference. Not a car-style or Pure Pursuit path law. - """ - - def __init__( - self, - global_config: GlobalConfig, - speed: float, - control_frequency: float, - k_position_per_s: float, - k_yaw_per_s: float, - k_velocity_per_s: float = 0.0, - k_yaw_rate_per_s: float = 0.0, - ) -> None: - self._global_config = global_config - self._speed = float(speed) - self._control_frequency = float(control_frequency) - self._inner = HolonomicTrackingController( - k_position_per_s=k_position_per_s, - k_yaw_per_s=k_yaw_per_s, - k_velocity_per_s=k_velocity_per_s, - k_yaw_rate_per_s=k_yaw_rate_per_s, - ) - self._envelope_overrides: CommandEnvelopeOverrides | None = None - self._limits = self._make_limits() - self._inner.configure(self._limits) - self._previous_cmd = Twist() - - def set_speed(self, speed_m_s: float) -> None: - self._speed = float(speed_m_s) - self._limits = self._make_limits() - self._inner.configure(self._limits) - - def set_command_envelope(self, overrides: CommandEnvelopeOverrides | None) -> None: - """Apply (or clear, with ``None``) a run profile's command caps.""" - self._envelope_overrides = overrides - self._limits = self._make_limits() - self._inner.configure(self._limits) - - def _make_limits(self) -> HolonomicCommandLimits: - overrides = self._envelope_overrides - if overrides is not None: - return HolonomicCommandLimits( - max_planar_speed_m_s=self._speed, - max_yaw_rate_rad_s=overrides.max_yaw_rate_rad_s, - max_planar_linear_accel_m_s2=overrides.max_planar_cmd_accel_m_s2, - max_yaw_accel_rad_s2=overrides.max_yaw_accel_rad_s2, - ) - max_yaw_rate = self._global_config.local_planner_max_yaw_rate_rad_s - return HolonomicCommandLimits( - max_planar_speed_m_s=self._speed, - max_yaw_rate_rad_s=self._speed if max_yaw_rate is None else float(max_yaw_rate), - max_planar_linear_accel_m_s2=self._global_config.local_planner_max_planar_cmd_accel_m_s2, - max_yaw_accel_rad_s2=self._global_config.local_planner_max_yaw_accel_rad_s2, - ) - - def advance( - self, - lookahead_point: NDArray[np.float64], - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: - current_pos = np.array([float(current_odom.position.x), float(current_odom.position.y)]) - direction = np.asarray(lookahead_point, dtype=np.float64) - current_pos - distance = float(np.linalg.norm(direction)) - - if distance < 1e-6 or not np.isfinite(distance): - return Twist() - - ref_yaw = float(np.arctan2(direction[1], direction[0])) - ref_pose = _pose_from_xy_yaw(float(lookahead_point[0]), float(lookahead_point[1]), ref_yaw) - # Feedforward along the reference heading in the body frame of the target pose. - ref_ff = Twist( - linear=Vector3(self._speed, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - ref = TrajectoryReferenceSample(0.0, ref_pose, ref_ff) - return self.advance_reference(ref, current_odom, measured_body_twist) - - def advance_reference( - self, - reference: TrajectoryReferenceSample, - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: - twist = Twist() if measured_body_twist is None else measured_body_twist - meas = TrajectoryMeasuredSample(0.0, _pose_from_pose_stamped(current_odom), twist) - return self._limit_output(self._inner.control(reference, meas)) - - def rotate( - self, - yaw_error: float, - current_odom: PoseStamped | None = None, - measured_body_twist: Twist | None = None, - ) -> Twist: - if current_odom is None: - # ``LocalPlanner`` should always pass odom; keep a safe fallback. - wz = float(0.5 * yaw_error) - wz = float(np.clip(wz, -self._speed, self._speed)) - if wz != 0.0 and abs(wz) < 0.2: - wz = 0.2 * (1.0 if wz > 0 else -1.0) - t = Twist( - linear=Vector3(0.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, wz), - ) - return self._limit_output(self._apply_sim_angular(t)) - - robot_yaw = float(current_odom.orientation.euler[2]) - target_yaw = float(np.arctan2(np.sin(robot_yaw + yaw_error), np.cos(robot_yaw + yaw_error))) - p = _pose_from_xy_yaw( - float(current_odom.position.x), - float(current_odom.position.y), - target_yaw, - ) - ref = TrajectoryReferenceSample(0.0, p, Twist()) - twist = Twist() if measured_body_twist is None else measured_body_twist - meas = TrajectoryMeasuredSample(0.0, _pose_from_pose_stamped(current_odom), twist) - out = self._inner.control(ref, meas) - return self._limit_output(self._apply_sim_angular(out)) - - def reset_errors(self) -> None: - self._inner.reset() - self._previous_cmd = Twist() - - def reset_yaw_error(self, value: float) -> None: - del value - - def _apply_sim_angular(self, t: Twist) -> Twist: - wz = float(t.angular.z) - if self._global_config.simulation and 1e-9 < abs(wz) < 0.8: - wz = 0.8 * (1.0 if wz > 0 else -1.0) - return Twist( - linear=Vector3(float(t.linear.x), float(t.linear.y), float(t.linear.z)), - angular=Vector3(0.0, 0.0, wz), - ) - - def _limit_output(self, raw: Twist) -> Twist: - out = clamp_holonomic_cmd_vel( - self._previous_cmd, - raw, - self._limits, - 1.0 / self._control_frequency, - ) - self._previous_cmd = Twist(out) - return out - - def make_local_path_controller( global_config: GlobalConfig, speed: float, diff --git a/dimos/navigation/dannav/holonomic_controller.py b/dimos/navigation/dannav/holonomic_controller.py deleted file mode 100644 index e1885156f9..0000000000 --- a/dimos/navigation/dannav/holonomic_controller.py +++ /dev/null @@ -1,190 +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. - -"""Continuous holonomic path follower for the dannav loop planner. - -Control ticks run on the odom subscription callback (not a background thread). -``_on_path`` only swaps the ``PathDistancer`` under ``_lock``; each throttled -odom update computes and publishes ``nav_cmd_vel``. When the dannav -``GlobalPlanner`` publishes a new path, the next tick tracks it without -stopping the robot. -""" - -from __future__ import annotations - -import math -from threading import RLock -import time -from typing import Any - -import numpy as np -from reactivex.disposable import Disposable - -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path -from dimos.core.global_config import GlobalConfig -from dimos.navigation.dannav.controllers import ( - HolonomicPathController, - command_envelope_overrides_for_profile, -) -from dimos.navigation.dannav.path_distancer import PathDistancer -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import GO2_RUN_PROFILES, RunProfileError -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -def _session_speed_and_envelope( - global_config: GlobalConfig, -) -> tuple[float, CommandEnvelopeOverrides | None, str]: - """Match ``LocalPlanner`` session envelope: ``go2_run_profile`` sets speed and caps.""" - profile_name = global_config.go2_run_profile - if profile_name == GO2_RUN_PROFILES.default_profile_name: - speed = ( - float(global_config.planner_robot_speed) - if global_config.planner_robot_speed is not None - else GO2_RUN_PROFILES.get("walk").requested_planner_speed_m_s - ) - return speed, None, profile_name - - profile = GO2_RUN_PROFILES.get(profile_name) - return profile.requested_planner_speed_m_s, command_envelope_overrides_for_profile(profile), profile_name - - -class HolonomicControllerConfig(ModuleConfig): - goal_tolerance: float = 0.2 - - -class HolonomicController(Module): - """Track the latest ``Path`` with the holonomic law, never stopping to replan.""" - - config: HolonomicControllerConfig - - path: In[Path] - odom: In[PoseStamped] - - nav_cmd_vel: Out[Twist] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._lock = RLock() - self._path_distancer: PathDistancer | None = None - self._current_odom: PoseStamped | None = None - self._controller: HolonomicPathController | None = None - self._speed: float = 0.0 - self._control_frequency: float = 10.0 - self._last_tick_monotonic = 0.0 - - @rpc - def start(self) -> None: - super().start() - - global_config = self.config.g - self._control_frequency = float(global_config.local_planner_control_rate_hz) - try: - speed, envelope_overrides, _ = _session_speed_and_envelope(global_config) - except RunProfileError as exc: - logger.warning( - "Unknown run profile; falling back to walk speed.", - profile=global_config.go2_run_profile, - reason=str(exc), - ) - profile_name = GO2_RUN_PROFILES.default_profile_name - speed = ( - float(global_config.planner_robot_speed) - if global_config.planner_robot_speed is not None - else GO2_RUN_PROFILES.get(profile_name).requested_planner_speed_m_s - ) - envelope_overrides = None - if not math.isfinite(speed) or speed <= 0.0: - raise ValueError(f"planner speed must be a positive finite float, got {speed!r}") - if global_config.nerf_speed < 1.0: - speed *= global_config.nerf_speed - self._speed = speed - self._controller = HolonomicPathController( - global_config, - speed, - self._control_frequency, - k_position_per_s=global_config.local_planner_holonomic_kp, - k_yaw_per_s=global_config.local_planner_holonomic_ky, - k_velocity_per_s=global_config.local_planner_holonomic_kv, - k_yaw_rate_per_s=global_config.local_planner_holonomic_kw, - ) - self._controller.set_command_envelope(envelope_overrides) - - self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) - self.register_disposable(Disposable(self.path.subscribe(self._on_path))) - - @rpc - def stop(self) -> None: - self.nav_cmd_vel.publish(Twist()) - super().stop() - - def _on_odom(self, msg: PoseStamped) -> None: - pose = self._as_pose_stamped(msg) - with self._lock: - self._current_odom = pose - self._tick() - - def _on_path(self, path: Path) -> None: - if not path.poses: - return - path_distancer = PathDistancer(path) - with self._lock: - self._path_distancer = path_distancer - self._tick() - - def _tick(self) -> None: - now = time.monotonic() - period = 1.0 / self._control_frequency - if now - self._last_tick_monotonic < period: - return - self._last_tick_monotonic = now - - with self._lock: - path_distancer = self._path_distancer - current_odom = self._current_odom - - if path_distancer is None or current_odom is None: - return - - cmd = self._compute_cmd(path_distancer, current_odom) - self.nav_cmd_vel.publish(cmd) - - def _compute_cmd( - self, path_distancer: PathDistancer, current_odom: PoseStamped - ) -> Twist: - assert self._controller is not None - current_pos = np.array( - [float(current_odom.position.x), float(current_odom.position.y)], - dtype=np.float64, - ) - - if path_distancer.distance_to_goal(current_pos) < self.config.goal_tolerance: - return Twist() - - closest_index = path_distancer.find_closest_point_index(current_pos) - lookahead_point = path_distancer.find_lookahead_point(closest_index) - return self._controller.advance(lookahead_point, current_odom) - - @staticmethod - def _as_pose_stamped(msg: PoseStamped) -> PoseStamped: - to_pose = getattr(msg, "to_pose_stamped", None) - if callable(to_pose): - return to_pose() - return msg diff --git a/dimos/navigation/dannav/local_planner.py b/dimos/navigation/dannav/local_planner.py index 476c25019c..c5dfecfac9 100644 --- a/dimos/navigation/dannav/local_planner.py +++ b/dimos/navigation/dannav/local_planner.py @@ -34,15 +34,17 @@ from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.base import NavigationState from dimos.navigation.dannav.controllers import ( - CommandEnvelopeOverrides, Controller, + make_local_path_controller, +) +from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import ( + CommandEnvelopeOverrides, HolonomicPathController, command_envelope_overrides_for_profile, - make_local_path_controller, ) from dimos.navigation.replanning_a_star.navigation_map import NavigationMap from dimos.navigation.dannav.path_clearance import PathClearance -from dimos.navigation.dannav.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer from dimos.navigation.dannav.rotation_clearance import can_rotate_in_place from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export import JsonlTrajectoryControlTickSink from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import ( diff --git a/dimos/navigation/dannav/simple_global_planner.py b/dimos/navigation/dannav/simple_global_planner.py deleted file mode 100644 index d424051a9c..0000000000 --- a/dimos/navigation/dannav/simple_global_planner.py +++ /dev/null @@ -1,130 +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. - -"""Intended for testing and experimenting only - -Loop A* global planner: replans on every new occupancy grid. - -``_on_costmap`` and ``_on_goal`` both call ``_replan``; the stored costmap, goal -(from ``clicked_point``), and robot start (from ``odom``) feed ``min_cost_astar`` -and the resulting ``Path`` is published on ``path``. There is no goal-following -state machine here: a fresh grid means a fresh A* run, and the downstream -``dannav.HolonomicController`` tracks whatever path is published. -""" - -from __future__ import annotations - -import threading -from typing import Any - -from reactivex.disposable import Disposable - -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Vector3 import VectorLike -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class SimpleGlobalPlannerConfig(ModuleConfig): - goal_tolerance: float = 0.2 - - -class SimpleGlobalPlanner(Module): - config: SimpleGlobalPlannerConfig - - global_costmap: In[OccupancyGrid] - clicked_point: In[PointStamped] - odom: In[PoseStamped] - - path: Out[Path] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._lock = threading.Lock() - self._costmap: OccupancyGrid | None = None - self._goal: PoseStamped | None = None - self._odom: PoseStamped | None = None - - @rpc - def start(self) -> None: - super().start() - - self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) - self.register_disposable(Disposable(self.global_costmap.subscribe(self._on_costmap))) - self.register_disposable( - Disposable( - self.clicked_point.subscribe( - lambda point: self._on_goal(point.to_pose_stamped()) - ) - ) - ) - - @rpc - def stop(self) -> None: - super().stop() - - def plan( - self, - costmap: OccupancyGrid, - goal: PoseStamped, - start: VectorLike, - ) -> Path | None: - return min_cost_astar(costmap, goal.position, start) - - def _on_odom(self, msg: PoseStamped) -> None: - with self._lock: - self._odom = msg - - def _on_costmap(self, costmap: OccupancyGrid) -> None: - with self._lock: - self._costmap = costmap - self._replan() - - def _on_goal(self, goal: PoseStamped) -> None: - logger.info("New goal", x=round(goal.x, 3), y=round(goal.y, 3)) - with self._lock: - self._goal = goal - self._replan() - - def _replan(self) -> None: - with self._lock: - costmap = self._costmap - goal = self._goal - odom = self._odom - - if costmap is None or goal is None or odom is None: - return - - start = (float(odom.position.x), float(odom.position.y)) - if goal.position.distance(odom.position) < self.config.goal_tolerance: - return - - path = self.plan(costmap, goal, start) - if path is None or not path.poses: - logger.warning( - "No path found.", - goal_x=round(goal.x, 3), - goal_y=round(goal.y, 3), - ) - return - - self.path.publish(path) diff --git a/dimos/navigation/dannav/test_local_planner_path_controller.py b/dimos/navigation/dannav/test_local_planner_path_controller.py index d3b5073e9d..b2f557fb02 100644 --- a/dimos/navigation/dannav/test_local_planner_path_controller.py +++ b/dimos/navigation/dannav/test_local_planner_path_controller.py @@ -31,13 +31,11 @@ from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.dannav.controllers import ( - HolonomicPathController, - make_local_path_controller, -) +from dimos.navigation.dannav.controllers import make_local_path_controller from dimos.navigation.dannav.local_planner import LocalPlanner from dimos.navigation.replanning_a_star.navigation_map import NavigationMap -from dimos.navigation.dannav.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import HolonomicPathController +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample from dimos.utils.trigonometry import angle_diff diff --git a/dimos/navigation/dannav/test_local_planner_run_envelope.py b/dimos/navigation/dannav/test_local_planner_run_envelope.py index 138776aec6..a682e61279 100644 --- a/dimos/navigation/dannav/test_local_planner_run_envelope.py +++ b/dimos/navigation/dannav/test_local_planner_run_envelope.py @@ -32,7 +32,7 @@ from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.dannav.local_planner import LocalPlanner from dimos.navigation.replanning_a_star.navigation_map import NavigationMap -from dimos.navigation.dannav.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer def _yaw_quaternion(yaw_rad: float) -> Quaternion: diff --git a/dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py b/dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py new file mode 100644 index 0000000000..9e375e15b2 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py @@ -0,0 +1,218 @@ +# 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. + +from dataclasses import dataclass + +import numpy as np +from numpy.typing import NDArray + +from dimos.core.global_config import GlobalConfig +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 +from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( + HolonomicCommandLimits, + clamp_holonomic_cmd_vel, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import RunProfile +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample + + +def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: + return Pose( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + ) + + +def _pose_from_pose_stamped(odom: PoseStamped) -> Pose: + return Pose(odom.position, odom.orientation) + + +@dataclass(frozen=True) +class CommandEnvelopeOverrides: + """Run-profile command caps that replace the GlobalConfig defaults. + + The planar speed cap keeps tracking ``set_speed`` (the geometry-capped + path speed); these are the remaining saturation limits a movement + envelope owns. + """ + + max_yaw_rate_rad_s: float + max_planar_cmd_accel_m_s2: float + max_yaw_accel_rad_s2: float + + +def command_envelope_overrides_for_profile(profile: RunProfile) -> CommandEnvelopeOverrides: + """Map a :class:`RunProfile` to planner command caps (planar speed excluded).""" + limits = profile.command_limits() + return CommandEnvelopeOverrides( + max_yaw_rate_rad_s=limits.max_yaw_rate_rad_s, + max_planar_cmd_accel_m_s2=limits.max_planar_linear_accel_m_s2, + max_yaw_accel_rad_s2=limits.max_yaw_accel_rad_s2, + ) + + +class HolonomicPathController: + """Follow path segments using the holonomic tracking law (P3-3, issue 921). + + Wraps :class:`HolonomicTrackingController` in the :class:`Controller` seam + (lookahead + odom). Rotations in place use the same law with a fixed + position reference. Not a car-style or Pure Pursuit path law. + """ + + def __init__( + self, + global_config: GlobalConfig, + speed: float, + control_frequency: float, + k_position_per_s: float, + k_yaw_per_s: float, + k_velocity_per_s: float = 0.0, + k_yaw_rate_per_s: float = 0.0, + ) -> None: + self._global_config = global_config + self._speed = float(speed) + self._control_frequency = float(control_frequency) + self._inner = HolonomicTrackingController( + k_position_per_s=k_position_per_s, + k_yaw_per_s=k_yaw_per_s, + k_velocity_per_s=k_velocity_per_s, + k_yaw_rate_per_s=k_yaw_rate_per_s, + ) + self._envelope_overrides: CommandEnvelopeOverrides | None = None + self._limits = self._make_limits() + self._inner.configure(self._limits) + self._previous_cmd = Twist() + + def set_speed(self, speed_m_s: float) -> None: + self._speed = float(speed_m_s) + self._limits = self._make_limits() + self._inner.configure(self._limits) + + def set_command_envelope(self, overrides: CommandEnvelopeOverrides | None) -> None: + """Apply (or clear, with ``None``) a run profile's command caps.""" + self._envelope_overrides = overrides + self._limits = self._make_limits() + self._inner.configure(self._limits) + + def _make_limits(self) -> HolonomicCommandLimits: + overrides = self._envelope_overrides + if overrides is not None: + return HolonomicCommandLimits( + max_planar_speed_m_s=self._speed, + max_yaw_rate_rad_s=overrides.max_yaw_rate_rad_s, + max_planar_linear_accel_m_s2=overrides.max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=overrides.max_yaw_accel_rad_s2, + ) + max_yaw_rate = self._global_config.local_planner_max_yaw_rate_rad_s + return HolonomicCommandLimits( + max_planar_speed_m_s=self._speed, + max_yaw_rate_rad_s=self._speed if max_yaw_rate is None else float(max_yaw_rate), + max_planar_linear_accel_m_s2=self._global_config.local_planner_max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=self._global_config.local_planner_max_yaw_accel_rad_s2, + ) + + def advance( + self, + lookahead_point: NDArray[np.float64], + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: + current_pos = np.array([float(current_odom.position.x), float(current_odom.position.y)]) + direction = np.asarray(lookahead_point, dtype=np.float64) - current_pos + distance = float(np.linalg.norm(direction)) + + if distance < 1e-6 or not np.isfinite(distance): + return Twist() + + ref_yaw = float(np.arctan2(direction[1], direction[0])) + ref_pose = _pose_from_xy_yaw(float(lookahead_point[0]), float(lookahead_point[1]), ref_yaw) + # Feedforward along the reference heading in the body frame of the target pose. + ref_ff = Twist( + linear=Vector3(self._speed, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + ref = TrajectoryReferenceSample(0.0, ref_pose, ref_ff) + return self.advance_reference(ref, current_odom, measured_body_twist) + + def advance_reference( + self, + reference: TrajectoryReferenceSample, + current_odom: PoseStamped, + measured_body_twist: Twist | None = None, + ) -> Twist: + twist = Twist() if measured_body_twist is None else measured_body_twist + meas = TrajectoryMeasuredSample(0.0, _pose_from_pose_stamped(current_odom), twist) + return self._limit_output(self._inner.control(reference, meas)) + + def rotate( + self, + yaw_error: float, + current_odom: PoseStamped | None = None, + measured_body_twist: Twist | None = None, + ) -> Twist: + if current_odom is None: + # ``LocalPlanner`` should always pass odom; keep a safe fallback. + wz = float(0.5 * yaw_error) + wz = float(np.clip(wz, -self._speed, self._speed)) + if wz != 0.0 and abs(wz) < 0.2: + wz = 0.2 * (1.0 if wz > 0 else -1.0) + t = Twist( + linear=Vector3(0.0, 0.0, 0.0), + angular=Vector3(0.0, 0.0, wz), + ) + return self._limit_output(self._apply_sim_angular(t)) + + robot_yaw = float(current_odom.orientation.euler[2]) + target_yaw = float(np.arctan2(np.sin(robot_yaw + yaw_error), np.cos(robot_yaw + yaw_error))) + p = _pose_from_xy_yaw( + float(current_odom.position.x), + float(current_odom.position.y), + target_yaw, + ) + ref = TrajectoryReferenceSample(0.0, p, Twist()) + twist = Twist() if measured_body_twist is None else measured_body_twist + meas = TrajectoryMeasuredSample(0.0, _pose_from_pose_stamped(current_odom), twist) + out = self._inner.control(ref, meas) + return self._limit_output(self._apply_sim_angular(out)) + + def reset_errors(self) -> None: + self._inner.reset() + self._previous_cmd = Twist() + + def reset_yaw_error(self, value: float) -> None: + del value + + def _apply_sim_angular(self, t: Twist) -> Twist: + wz = float(t.angular.z) + if self._global_config.simulation and 1e-9 < abs(wz) < 0.8: + wz = 0.8 * (1.0 if wz > 0 else -1.0) + return Twist( + linear=Vector3(float(t.linear.x), float(t.linear.y), float(t.linear.z)), + angular=Vector3(0.0, 0.0, wz), + ) + + def _limit_output(self, raw: Twist) -> Twist: + out = clamp_holonomic_cmd_vel( + self._previous_cmd, + raw, + self._limits, + 1.0 / self._control_frequency, + ) + self._previous_cmd = Twist(out) + return out diff --git a/dimos/navigation/holonomic_trajectory_controller/module.py b/dimos/navigation/holonomic_trajectory_controller/module.py new file mode 100644 index 0000000000..9eb2c119ce --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/module.py @@ -0,0 +1,836 @@ +# 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. + +"""Holonomic trajectory controller. + +``DanHolonomicTC`` follows a planned ``Path`` with the holonomic tracking law. +It owns trajectory control only: the planner (``MLSPlannerNative``) owns route +safety and emits the path, sending an empty ``Path`` when nothing ahead is +traversable. The costmap, obstacle, and replanning concerns of the old +``LocalPlanner`` are gone; the stripped control core (``_HolonomicPathFollower``) +keeps the state machine, holonomic tracking, and run-profile envelope. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from threading import Event, RLock, Thread +import time +import traceback +from typing import Any, Literal, TypeAlias + +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] +import numpy as np +from reactivex import Subject +from reactivex.disposable import Disposable + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.core import rpc +from dimos.core.global_config import GlobalConfig +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +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 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.base import NavigationState +from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import ( + CommandEnvelopeOverrides, + HolonomicPathController, + command_envelope_overrides_for_profile, +) +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export import ( + JsonlTrajectoryControlTickSink, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import ( + TrajectoryControlTickSink, + append_trajectory_control_tick, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( + PathSpeedProfileLimits, + profile_speed_along_polyline, + speed_at_progress_m, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( + GO2_RUN_PROFILES, + RunProfile, + RunProfileError, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) +from dimos.utils.logging_config import setup_logger +from dimos.utils.trigonometry import angle_diff + +PlannerState: TypeAlias = Literal[ + "idle", "initial_rotation", "path_following", "final_rotation", "arrived" +] +# Only the two terminal reasons the controller still owns survive; the +# planner-oriented reasons (obstacle_found, map_updated, run_envelope_rejected) +# are gone with the costmap. +StopMessage: TypeAlias = Literal["arrived", "error"] + +logger = setup_logger() + + +@dataclass(frozen=True) +class ActiveRunEnvelope: + """The movement envelope governing the current follow. + + The speed is the session profile's requested speed (or the optional cruise + override) after any global slowdown scaling, and the limits come from the + profile. ``GO2_RUN_PROFILES`` is the single envelope source, so even the + default profile carries ``command_overrides``. + """ + + profile_name: str + speed_m_s: float + path_limits: PathSpeedProfileLimits + goal_decel_m_s2: float + command_overrides: CommandEnvelopeOverrides + + +class _HolonomicPathFollower: + """Constructible control core for :class:`DanHolonomicTC`. + + Owns the follow state machine, the holonomic tracking law, and the + run-profile speed/accel envelope. Has no transport: ``DanHolonomicTC`` wires + the ``path`` / ``odometry`` / ``stop_movement`` streams to its methods and + forwards ``cmd_vel`` / ``stopped_navigating``. Unit-testable directly, the + way the old ``LocalPlanner`` was. + """ + + cmd_vel: Subject # Subject[Twist] + stopped_navigating: Subject # Subject[StopMessage] + + _thread: Thread | None = None + _path: Path | None = None + _path_distancer: PathDistancer | None = None + _current_odom: PoseStamped | None = None + + _pose_index: int + _lock: RLock + _stop_planning_event: Event + _state: PlannerState + _state_unique_id: int + _global_config: GlobalConfig + _goal_tolerance: float + _controller: HolonomicPathController + _trajectory_tick_sink: TrajectoryControlTickSink | None + _previous_odom_for_velocity: PoseStamped | None + + _run_profile: str + _cruise_speed_override: float | None + _active_envelope: ActiveRunEnvelope + _path_speed_profile_s: list[float] | None + _path_speed_profile_v: list[float] | None + _path_speed_profile_path_id: int | None + _control_frequency: float + _orientation_tolerance: float + _align_heading_before_move: bool + _align_goal_yaw: bool + _goal_reached: bool + + def __init__(self, config: DanHolonomicTCConfig) -> None: + self.cmd_vel = Subject() + self.stopped_navigating = Subject() + + self._config = config + self._global_config = config.g + self._pose_index = 0 + self._lock = RLock() + self._stop_planning_event = Event() + self._state = "idle" + self._state_unique_id = 0 + self._goal_reached = False + self._goal_tolerance = float(config.goal_tolerance) + self._orientation_tolerance = float(config.orientation_tolerance) + self._control_frequency = float(config.control_frequency) + self._align_heading_before_move = bool(config.align_heading_before_move) + self._align_goal_yaw = bool(config.align_goal_yaw) + self._run_profile = config.run_profile + self._trajectory_tick_sink = self._make_trajectory_tick_sink() + self._previous_odom_for_velocity = None + self._path_speed_profile_s = None + self._path_speed_profile_v = None + self._path_speed_profile_path_id = None + + override = config.speed_m_s + if override is not None and (not math.isfinite(override) or override <= 0.0): + raise ValueError( + f"speed_m_s must be a positive finite float, got {override!r}" + ) + self._cruise_speed_override = override + + envelope = self._resolve_run_envelope() + if envelope is None: + raise ValueError(f"invalid run profile {self._run_profile!r}") + self._active_envelope = envelope + + self._controller = HolonomicPathController( + self._global_config, + envelope.speed_m_s, + self._control_frequency, + k_position_per_s=config.k_position_per_s, + k_yaw_per_s=config.k_yaw_per_s, + k_velocity_per_s=config.k_velocity_per_s, + k_yaw_rate_per_s=config.k_yaw_rate_per_s, + ) + self._apply_run_envelope(envelope) + + # ---- lifecycle ---------------------------------------------------------- + + def close(self) -> None: + with self._lock: + thread = self._thread + self.stop_planning() + if thread is not None: + thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + self._close_trajectory_tick_sink() + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._current_odom = msg + + def start_planning(self, path: Path) -> None: + self.stop_planning() + + self._stop_planning_event = Event() + + with self._lock: + self._goal_reached = False + self._path = path + self._path_distancer = PathDistancer(path) + self._pose_index = 0 + self._previous_odom_for_velocity = None + self._rebuild_path_speed_profile(self._path_distancer) + self._thread = Thread(target=self._thread_entrypoint, daemon=True) + self._thread.start() + + def update_path(self, path: Path) -> bool: + """Swap the active path without stopping the control thread.""" + if not path.poses: + return False + + with self._lock: + if self._path is None or self._thread is None: + return False + + self._path = path + self._path_distancer = PathDistancer(path) + current_odom = self._current_odom + if current_odom is not None: + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + self._pose_index = self._path_distancer.find_closest_point_index(current_pos) + self._rebuild_path_speed_profile(self._path_distancer) + + return True + + def stop_planning(self) -> None: + self.cmd_vel.on_next(Twist()) + self._stop_planning_event.set() + + with self._lock: + self._thread = None + self._goal_reached = False + + self._reset_state() + + # ---- run-profile envelope ---------------------------------------------- + + def set_run_profile(self, profile: str) -> bool: + """Set the session-default movement envelope and apply it live. + + Validated against the run-profile registry so a bad name cannot poison + the controller. The new envelope takes effect mid-follow. + """ + try: + GO2_RUN_PROFILES.get(profile) + except RunProfileError as exc: + logger.warning("Rejected run profile.", profile=profile, reason=str(exc)) + return False + self._run_profile = profile + envelope = self._resolve_run_envelope() + if envelope is None: + return False + self._apply_run_envelope(envelope) + return True + + def _profile_run_envelope(self, profile: RunProfile) -> ActiveRunEnvelope: + speed = ( + self._cruise_speed_override + if self._cruise_speed_override is not None + else profile.requested_planner_speed_m_s + ) + if self._global_config.nerf_speed < 1.0: + speed *= self._global_config.nerf_speed + return ActiveRunEnvelope( + profile_name=profile.name, + speed_m_s=speed, + path_limits=profile.path_speed_profile_limits_at(speed), + goal_decel_m_s2=profile.goal_decel_m_s2, + command_overrides=command_envelope_overrides_for_profile(profile), + ) + + def _resolve_run_envelope(self) -> ActiveRunEnvelope | None: + """Resolve the movement envelope from the current session profile.""" + name = self._run_profile + try: + profile = GO2_RUN_PROFILES.get(name) + except RunProfileError as exc: + logger.warning("run profile rejected", profile=name, reason=str(exc)) + return None + + envelope = self._profile_run_envelope(profile) + logger.info( + "run envelope applied", + profile=profile.name, + speed_m_s=round(envelope.speed_m_s, 3), + goal_decel_m_s2=envelope.goal_decel_m_s2, + max_yaw_rate_rad_s=profile.max_yaw_rate_rad_s, + ) + return envelope + + def _apply_run_envelope(self, envelope: ActiveRunEnvelope) -> None: + self._active_envelope = envelope + self._controller.set_command_envelope(envelope.command_overrides) + self._controller.set_speed(envelope.speed_m_s) + with self._lock: + path_distancer = self._path_distancer + if path_distancer is not None: + self._rebuild_path_speed_profile(path_distancer) + + # ---- tick logging ------------------------------------------------------- + + def _make_trajectory_tick_sink(self) -> TrajectoryControlTickSink | None: + path = self._config.trajectory_tick_log_path + if path is None or str(path).strip() == "": + return None + return JsonlTrajectoryControlTickSink(path) + + def _close_trajectory_tick_sink(self) -> None: + sink = self._trajectory_tick_sink + close = getattr(sink, "close", None) + if callable(close): + close() + self._trajectory_tick_sink = None + + # ---- introspection ------------------------------------------------------ + + def get_state(self) -> NavigationState: + with self._lock: + state = self._state + + match state: + case "idle" | "arrived": + return NavigationState.IDLE + case "initial_rotation" | "path_following" | "final_rotation": + return NavigationState.FOLLOWING_PATH + case _: + raise ValueError(f"Unknown planner state: {state}") + + def is_goal_reached(self) -> bool: + with self._lock: + return self._goal_reached + + # ---- control loop ------------------------------------------------------- + + def _thread_entrypoint(self) -> None: + try: + self._loop() + except Exception as e: + traceback.print_exc() + logger.exception("Error in holonomic trajectory control", exc_info=e) + self.stopped_navigating.on_next("error") + finally: + self._reset_state() + self.cmd_vel.on_next(Twist()) + + def _change_state(self, new_state: PlannerState) -> None: + if new_state == self._state: + return + self._state = new_state + self._state_unique_id += 1 + logger.info("changed state", state=new_state) + + def _initial_state(self, path: Path, current_odom: PoseStamped | None) -> PlannerState: + """Decide where the follow starts. + + A holonomic base translates while turning toward the path tangent, so by + default a non-empty path goes straight to ``path_following``. Rotate + first only when ``align_heading_before_move`` is set and the start is + misaligned with the first path tangent. + """ + if ( + not self._align_heading_before_move + or current_odom is None + or len(path.poses) == 0 + ): + return "path_following" + + first_yaw = path.poses[0].orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + initial_yaw_error = angle_diff(first_yaw, robot_yaw) + self._controller.reset_yaw_error(initial_yaw_error) + if abs(initial_yaw_error) < self._orientation_tolerance: + return "path_following" + return "initial_rotation" + + def _loop(self) -> None: + stop_event = self._stop_planning_event + + with self._lock: + path = self._path + current_odom = self._current_odom + + if path is None: + raise RuntimeError("No path set for holonomic path follower.") + + with self._lock: + self._change_state(self._initial_state(path, current_odom)) + + while not stop_event.is_set(): + start_time = time.perf_counter() + + with self._lock: + state: PlannerState = self._state + + if state == "initial_rotation": + cmd_vel = self._compute_initial_rotation() + elif state == "path_following": + cmd_vel = self._compute_path_following() + elif state == "final_rotation": + cmd_vel = self._compute_final_rotation() + elif state == "arrived": + with self._lock: + self._goal_reached = True + # Stop motion before signalling arrival, matching the path + # followers downstream consumers expect. + self.cmd_vel.on_next(Twist()) + self.stopped_navigating.on_next("arrived") + break + else: # idle + cmd_vel = None + + if cmd_vel is not None: + self.cmd_vel.on_next(cmd_vel) + + elapsed = time.perf_counter() - start_time + sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) + stop_event.wait(sleep_time) + + if stop_event.is_set(): + logger.info("Holonomic path follower loop exited due to stop event.") + + def _compute_initial_rotation(self) -> Twist: + with self._lock: + path = self._path + current_odom = self._current_odom + + assert path is not None + assert current_odom is not None + + first_pose = path.poses[0] + first_yaw = first_pose.orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + yaw_error = angle_diff(first_yaw, robot_yaw) + + if abs(yaw_error) < self._orientation_tolerance: + with self._lock: + self._change_state("path_following") + return self._compute_path_following() + + self._controller.set_speed(self._active_envelope.speed_m_s) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) + ref_pose = _pose_from_xy_yaw( + float(current_odom.position.x), + float(current_odom.position.y), + float(first_yaw), + ) + self._append_trajectory_control_tick( + ref_pose, + Twist(), + current_odom, + measured_body_twist, + cmd, + ) + return cmd + + def _compute_path_following(self) -> Twist: + with self._lock: + path_distancer = self._path_distancer + current_odom = self._current_odom + + assert path_distancer is not None + assert current_odom is not None + + current_pos = np.array([current_odom.position.x, current_odom.position.y]) + + if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: + if self._align_goal_yaw: + logger.info("Reached goal position, starting final rotation") + with self._lock: + self._change_state("final_rotation") + return self._compute_final_rotation() + logger.info("Reached goal position") + with self._lock: + self._change_state("arrived") + return Twist() + + closest_index = path_distancer.find_closest_point_index(current_pos) + + with self._lock: + self._pose_index = closest_index + + path_speed = self._path_speed_for_index(path_distancer, closest_index, current_pos) + self._controller.set_speed(path_speed) + reference_sample = self._lookahead_reference_sample( + path_distancer, + current_odom, + current_pos, + path_speed, + ) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + cmd = self._controller.advance_reference( + reference_sample, + current_odom, + measured_body_twist, + ) + self._append_trajectory_control_sample( + reference_sample, + current_odom, + measured_body_twist, + cmd, + ) + return cmd + + def _lookahead_reference_sample( + self, + path_distancer: PathDistancer, + current_odom: PoseStamped, + current_pos: np.ndarray, + path_speed: float, + ) -> TrajectoryReferenceSample: + projection = path_distancer.project(current_pos) + s_start = float(projection.s_along_path_m) + s_end = min( + path_distancer.path_length_m, + s_start + path_distancer.lookahead_distance_m, + ) + now_s = float(current_odom.ts) + if not math.isfinite(now_s): + now_s = 0.0 + travel_s = max(0.0, s_end - s_start) + dt_s = 1.0 / self._control_frequency + duration_s = max(dt_s, travel_s / max(path_speed, 1e-6)) + return self._reference_sample_at_progress( + path_distancer, + s_end, + now_s + duration_s, + path_speed, + ) + + def _reference_sample_at_progress( + self, + path_distancer: PathDistancer, + progress_m: float, + time_s: float, + path_speed: float, + ) -> TrajectoryReferenceSample: + point = path_distancer.point_at_progress(progress_m) + # Body yaw tracks the path tangent; the holonomic law translates toward + # the reference while turning. No costmap yaw-lock in this stack. + path_yaw = path_distancer.yaw_at_progress(progress_m) + feedforward = Twist( + linear=Vector3(path_speed, 0.0, 0.0), + angular=Vector3(0.0, 0.0, 0.0), + ) + return TrajectoryReferenceSample( + time_s=time_s, + pose_plan=_pose_from_xy_yaw(float(point[0]), float(point[1]), path_yaw), + twist_body=feedforward, + ) + + def _path_speed_for_index( + self, + path_distancer: PathDistancer, + closest_index: int, + current_pos: np.ndarray, + ) -> float: + del closest_index + self._ensure_path_speed_profile(path_distancer) + envelope = self._active_envelope + progress_m = float(path_distancer.project(current_pos).s_along_path_m) + profile_speed = self._profiled_path_speed_m_s(progress_m) + distance_cap = math.sqrt( + max( + 0.0, + 2.0 * envelope.goal_decel_m_s2 * path_distancer.distance_to_goal(current_pos), + ) + ) + capped = min(envelope.speed_m_s, profile_speed, distance_cap) + return min(envelope.speed_m_s, max(0.05, capped)) + + def _profiled_path_speed_m_s(self, progress_m: float) -> float: + s_profile = self._path_speed_profile_s + v_profile = self._path_speed_profile_v + if s_profile is None or v_profile is None: + return self._active_envelope.speed_m_s + return speed_at_progress_m(progress_m, s_profile, v_profile) + + def _ensure_path_speed_profile(self, path_distancer: PathDistancer) -> None: + path_id = id(path_distancer._path) + if ( + self._path_speed_profile_s is None + or self._path_speed_profile_path_id != path_id + ): + self._rebuild_path_speed_profile(path_distancer) + self._path_speed_profile_path_id = path_id + + def _rebuild_path_speed_profile(self, path_distancer: PathDistancer) -> None: + envelope = self._active_envelope + s_profile, v_profile = profile_speed_along_polyline( + path_distancer._path, + path_distancer._cumulative_dists, + envelope.path_limits, + envelope.goal_decel_m_s2, + ) + self._path_speed_profile_s = s_profile + self._path_speed_profile_v = v_profile + self._path_speed_profile_path_id = id(path_distancer._path) + + def _compute_final_rotation(self) -> Twist: + with self._lock: + path = self._path + current_odom = self._current_odom + + assert path is not None + assert current_odom is not None + + goal_yaw = path.poses[-1].orientation.euler[2] + robot_yaw = current_odom.orientation.euler[2] + yaw_error = angle_diff(goal_yaw, robot_yaw) + + if abs(yaw_error) < self._orientation_tolerance: + logger.info("Final rotation complete, goal reached") + with self._lock: + self._change_state("arrived") + return Twist() + + self._controller.set_speed(self._active_envelope.speed_m_s) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) + ref_pose = _pose_from_xy_yaw( + float(current_odom.position.x), + float(current_odom.position.y), + float(goal_yaw), + ) + self._append_trajectory_control_tick( + ref_pose, + Twist(), + current_odom, + measured_body_twist, + cmd, + ) + return cmd + + def _reset_state(self) -> None: + with self._lock: + self._change_state("idle") + self._path = None + self._path_distancer = None + self._pose_index = 0 + self._previous_odom_for_velocity = None + self._controller.set_speed(self._active_envelope.speed_m_s) + self._controller.reset_errors() + + def _append_trajectory_control_tick( + self, + reference_pose: Pose, + reference_twist: Twist, + current_odom: PoseStamped, + measured_body_twist: Twist, + command: Twist, + ) -> None: + reference = TrajectoryReferenceSample( + time_s=float(current_odom.ts), + pose_plan=reference_pose, + twist_body=reference_twist, + ) + self._append_trajectory_control_sample( + reference, + current_odom, + measured_body_twist, + command, + ) + + def _append_trajectory_control_sample( + self, + reference: TrajectoryReferenceSample, + current_odom: PoseStamped, + measured_body_twist: Twist, + command: Twist, + ) -> None: + sink = self._trajectory_tick_sink + if sink is None: + return + measurement = TrajectoryMeasuredSample( + time_s=float(current_odom.ts), + pose_plan=Pose(current_odom.position, current_odom.orientation), + twist_body=measured_body_twist, + ) + append_trajectory_control_tick( + sink, + reference, + measurement, + command, + 1.0 / self._control_frequency, + wall_time_s=time.time(), + ) + + def _estimate_measured_body_twist(self, current_odom: PoseStamped) -> Twist: + previous = self._previous_odom_for_velocity + self._previous_odom_for_velocity = current_odom + if previous is None: + return Twist() + dt = float(current_odom.ts) - float(previous.ts) + if not math.isfinite(dt) or dt <= 0.0: + return Twist() + vx_w = (float(current_odom.position.x) - float(previous.position.x)) / dt + vy_w = (float(current_odom.position.y) - float(previous.position.y)) / dt + yaw = float(current_odom.orientation.euler[2]) + c = math.cos(yaw) + s = math.sin(yaw) + vx_b = c * vx_w + s * vy_w + vy_b = -s * vx_w + c * vy_w + wz = ( + angle_diff( + float(current_odom.orientation.euler[2]), + float(previous.orientation.euler[2]), + ) + / dt + ) + return Twist( + linear=Vector3(vx_b, vy_b, 0.0), + angular=Vector3(0.0, 0.0, wz), + ) + + +def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: + return Pose( + position=Vector3(x, y, 0.0), + orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + ) + + +class DanHolonomicTCConfig(ModuleConfig): + control_frequency: float = 10.0 + run_profile: str = "walk" + speed_m_s: float | None = None + goal_tolerance: float = 0.2 + orientation_tolerance: float = 0.35 + k_position_per_s: float = 2.0 + k_yaw_per_s: float = 1.5 + k_velocity_per_s: float = 0.0 + k_yaw_rate_per_s: float = 0.0 + align_heading_before_move: bool = False + align_goal_yaw: bool = False + trajectory_tick_log_path: str | None = None + + +class DanHolonomicTC(Module): + """Follow a planned ``Path`` with the holonomic tracking law. + + Mirrors ``BasicPathFollower``'s stream surface. The planner owns route + safety and emits the ``Path``: an empty path stops the follow, a non-empty + path updates the active route or starts a new follow. Publishes + ``nav_cmd_vel`` until the goal is within tolerance, then ``goal_reached``. + ``stop_movement`` cancels the current path. + """ + + config: DanHolonomicTCConfig + + path: In[Path] + odometry: In[Odometry] + stop_movement: In[Bool] + + nav_cmd_vel: Out[Twist] + goal_reached: Out[Bool] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._core = _HolonomicPathFollower(self.config) + + @rpc + def start(self) -> None: + super().start() + self.register_disposable( + self._core.cmd_vel.subscribe(self.nav_cmd_vel.publish) + ) + self.register_disposable( + self._core.stopped_navigating.subscribe(self._on_core_stopped) + ) + self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + self.register_disposable(Disposable(self.path.subscribe(self._on_path))) + if self.stop_movement.transport is not None: + self.register_disposable(Disposable(self.stop_movement.subscribe(self._on_stop))) + + @rpc + def stop(self) -> None: + self._core.close() + self.nav_cmd_vel.publish(Twist()) + super().stop() + + def _on_odometry(self, msg: Odometry) -> None: + self._core.handle_odom(msg.to_pose_stamped()) + + def _on_path(self, path: Path) -> None: + # The planner owns path safety: it sends the route as far as it is safe, + # or an empty path when nothing ahead is traversable. + if len(path.poses) == 0: + self._core.stop_planning() + return + if not self._core.update_path(path): + self._core.start_planning(path) + + def _on_stop(self, msg: Bool) -> None: + if msg.data: + self._core.stop_planning() + + def _on_core_stopped(self, msg: StopMessage) -> None: + if msg == "arrived": + self.goal_reached.publish(Bool(True)) + logger.info("Goal reached") + # On "error" the core has already published a zero Twist and cleared the + # route; there is no planner here to ask for a replan. + + @rpc + def set_run_profile(self, profile: str) -> bool: + """Set the session-default movement envelope, applied live.""" + return self._core.set_run_profile(profile) + + @rpc + def is_goal_reached(self) -> bool: + return self._core.is_goal_reached() + + @rpc + def get_state(self) -> NavigationState: + return self._core.get_state() diff --git a/dimos/navigation/dannav/path_distancer.py b/dimos/navigation/holonomic_trajectory_controller/path_distancer.py similarity index 100% rename from dimos/navigation/dannav/path_distancer.py rename to dimos/navigation/holonomic_trajectory_controller/path_distancer.py diff --git a/dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py b/dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py new file mode 100644 index 0000000000..ba7cfb68e8 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py @@ -0,0 +1,257 @@ +# 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. + +"""``DanHolonomicTC`` stream wiring. + +These exercise the module surface that routes the ``path`` / ``odometry`` / +``stop_movement`` inputs into the control core and forwards ``nav_cmd_vel`` / +``goal_reached``: an empty path stops, a non-empty path hot-swaps or starts, +``stop_movement`` cancels, and arrival publishes ``goal_reached(True)``. The +control law itself is covered in ``test_holonomic_path_follower``. +""" + +from __future__ import annotations + +from collections.abc import Callable, Iterator +from contextlib import contextmanager +from dataclasses import dataclass, field +import math +import time +from typing import Any + +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] + +from dimos.core.stream import Stream, Transport +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.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.base import NavigationState +from dimos.navigation.holonomic_trajectory_controller.module import DanHolonomicTC + + +class _DirectTransport(Transport): # type: ignore[type-arg] + """Synchronous in-process transport so ``start()`` can wire the inputs. + + Delivers each broadcast straight to the subscribed handlers on the calling + thread, which keeps the wiring assertions deterministic. + """ + + def __init__(self) -> None: + self._subscribers: list[Callable[[Any], Any]] = [] + + def broadcast(self, _selfstream: Any, value: Any) -> None: + for callback in list(self._subscribers): + callback(value) + + def subscribe( + self, callback: Callable[[Any], Any], _selfstream: Stream[Any] | None = None + ) -> Callable[[], None]: + self._subscribers.append(callback) + + def _unsubscribe() -> None: + if callback in self._subscribers: + self._subscribers.remove(callback) + + return _unsubscribe + + def start(self) -> None: ... + + def stop(self) -> None: + self._subscribers.clear() + + +@dataclass +class _Captured: + cmd_vel: list[Twist] = field(default_factory=list) + goal_reached: list[Bool] = field(default_factory=list) + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _odometry(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> Odometry: + return Odometry( + ts=ts, + frame_id="map", + pose=Pose(position=[x, y, 0.0], orientation=_yaw_quaternion(yaw_rad)), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append( + PoseStamped( + ts=1.0, + frame_id="map", + position=[point[0], point[1], 0.0], + orientation=_yaw_quaternion(yaw), + ) + ) + return Path(frame_id="map", poses=poses) + + +def _is_zero_twist(cmd: Twist) -> bool: + return ( + abs(float(cmd.linear.x)) < 1e-9 + and abs(float(cmd.linear.y)) < 1e-9 + and abs(float(cmd.angular.z)) < 1e-9 + ) + + +def _wait_until(predicate: Callable[[], bool], timeout: float = 2.0) -> bool: + deadline = time.time() + timeout + while time.time() < deadline: + if predicate(): + return True + time.sleep(0.01) + return predicate() + + +class _ModuleHarness: + def __init__( + self, module: DanHolonomicTC, captured: _Captured, unsubs: list[Callable[[], None]] + ) -> None: + self.module = module + self.captured = captured + self._unsubs = unsubs + + @property + def core(self) -> Any: + return self.module._core + + def feed_odom(self, x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> None: + self.module.odometry.transport.broadcast(None, _odometry(x, y, yaw_rad, ts=ts)) + + def feed_path(self, path: Path) -> None: + self.module.path.transport.broadcast(None, path) + + def feed_empty_path(self) -> None: + self.module.path.transport.broadcast(None, Path(frame_id="map", poses=[])) + + def feed_stop(self, value: bool = True) -> None: + self.module.stop_movement.transport.broadcast(None, Bool(value)) + + def close(self) -> None: + for unsub in self._unsubs: + unsub() + self.module.stop() + + +@contextmanager +def _running_module(**config: Any) -> Iterator[_ModuleHarness]: + module = DanHolonomicTC(**config) + module.path.transport = _DirectTransport() + module.odometry.transport = _DirectTransport() + module.stop_movement.transport = _DirectTransport() + captured = _Captured() + unsubs = [ + module.nav_cmd_vel.subscribe(captured.cmd_vel.append), + module.goal_reached.subscribe(captured.goal_reached.append), + ] + module.start() + harness = _ModuleHarness(module, captured, unsubs) + try: + yield harness + finally: + harness.close() + + +def test_empty_path_publishes_zero_twist_and_clears_route() -> None: + with _running_module(goal_tolerance=0.2) as h: + h.feed_odom(0.0, 0.0, 0.0) + h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) + thread = h.core._thread + assert thread is not None # a route is active + + h.feed_empty_path() + thread.join(timeout=1.0) + + assert not thread.is_alive() + assert h.core._path is None + assert h.core._thread is None + assert h.core.get_state() == NavigationState.IDLE + assert h.captured.cmd_vel + assert _is_zero_twist(h.captured.cmd_vel[-1]) + assert not h.captured.goal_reached + + +def test_stop_movement_publishes_zero_twist_and_clears_route() -> None: + with _running_module(goal_tolerance=0.2) as h: + h.feed_odom(0.0, 0.0, 0.0) + h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) + thread = h.core._thread + assert thread is not None + + h.feed_stop(True) + thread.join(timeout=1.0) + + assert not thread.is_alive() + assert h.core._path is None + assert h.core._thread is None + assert h.core.get_state() == NavigationState.IDLE + assert h.captured.cmd_vel + assert _is_zero_twist(h.captured.cmd_vel[-1]) + + +def test_stop_movement_false_does_not_clear_route() -> None: + with _running_module(goal_tolerance=0.2) as h: + h.feed_odom(0.0, 0.0, 0.0) + h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) + thread = h.core._thread + + h.feed_stop(False) + + assert h.core._thread is thread + assert h.core._path is not None + + +def test_hot_update_path_swaps_route_without_restarting_thread() -> None: + with _running_module(goal_tolerance=0.2) as h: + h.feed_odom(0.0, 0.0, 0.0) + h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) + thread_before = h.core._thread + assert thread_before is not None + + h.feed_path(_path_from_points([(0.0, 0.0), (0.0, 2.0)])) + + # Same control thread: the route was hot-swapped, not restarted. + assert h.core._thread is thread_before + swapped = [(float(p.position.x), float(p.position.y)) for p in h.core._path.poses] + assert swapped == [(0.0, 0.0), (0.0, 2.0)] + + +def test_arrival_publishes_goal_reached_without_final_spin() -> None: + # Odom sits on the goal with a misaligned heading; align_goal_yaw=False must + # report arrival on position alone, never spinning to align the final yaw. + with _running_module(goal_tolerance=0.2, align_goal_yaw=False) as h: + h.feed_odom(1.0, 0.0, 1.2) + h.feed_path(_path_from_points([(0.0, 0.0), (1.0, 0.0)])) + + assert _wait_until(lambda: bool(h.captured.goal_reached)) + assert [bool(b.data) for b in h.captured.goal_reached] == [True] + assert h.core.is_goal_reached() is True + # No final rotation: every command published was a body-yaw rate of zero. + assert h.captured.cmd_vel + assert all(abs(float(cmd.angular.z)) < 1e-6 for cmd in h.captured.cmd_vel) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py b/dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py new file mode 100644 index 0000000000..8f3c739d19 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py @@ -0,0 +1,555 @@ +# 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. + +"""``_HolonomicPathFollower`` tracking law, path-speed caps, and closed loop. + +Relocated from ``dannav/test_local_planner_path_controller.py`` for the +``DanHolonomicTC`` control core. The costmap/yaw-lock and differential cases are +gone with the planner; the holonomic tracking law, path-speed envelope, and +arrival behavior remain. Movement envelopes that the old tests expressed through +``GlobalConfig`` accel fields now come from the run-profile registry, so +``_install_envelope`` drives the core with an explicit ``ActiveRunEnvelope`` for +the geometry-cap unit tests (the one seam that needs arbitrary accel values). +""" + +from __future__ import annotations + +import math +import time + +import numpy as np +import pytest + +from dimos.core.global_config import GlobalConfig +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.nav_msgs.Path import Path +from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import ( + CommandEnvelopeOverrides, + HolonomicPathController, +) +from dimos.navigation.holonomic_trajectory_controller.module import ( + ActiveRunEnvelope, + DanHolonomicTCConfig, + _HolonomicPathFollower, +) +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer +from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import ( + HolonomicTrackingController, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( + PathSpeedProfileLimits, +) +from dimos.navigation.holonomic_trajectory_controller.trajectory_types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) +from dimos.utils.trigonometry import angle_diff + + +def _planar_speed_m_s(cmd: Twist) -> float: + return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="map", + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append(_pose_stamped(point[0], point[1], yaw)) + return Path(frame_id="map", poses=poses) + + +def _make_follower(**overrides: object) -> _HolonomicPathFollower: + return _HolonomicPathFollower(DanHolonomicTCConfig(**overrides)) + + +def _install_envelope( + core: _HolonomicPathFollower, + *, + speed_m_s: float, + max_tangent_accel_m_s2: float, + max_normal_accel_m_s2: float, + goal_decel_m_s2: float, + max_planar_cmd_accel_m_s2: float = 8.0, + max_yaw_accel_rad_s2: float = 8.0, + max_yaw_rate_rad_s: float | None = None, +) -> None: + """Drive the core with an explicit movement envelope. + + The run-profile registry is the only production envelope source, so this is + the test seam for the arbitrary accel values the old ``GlobalConfig`` accel + fields used to provide. ``_apply_run_envelope`` re-seats the controller and + invalidates the cached path-speed profile, exactly as ``set_run_profile``. + """ + core._apply_run_envelope( + ActiveRunEnvelope( + profile_name="test", + speed_m_s=speed_m_s, + path_limits=PathSpeedProfileLimits( + max_speed_m_s=speed_m_s, + max_tangent_accel_m_s2=max_tangent_accel_m_s2, + max_normal_accel_m_s2=max_normal_accel_m_s2, + ), + goal_decel_m_s2=goal_decel_m_s2, + command_overrides=CommandEnvelopeOverrides( + max_yaw_rate_rad_s=speed_m_s if max_yaw_rate_rad_s is None else max_yaw_rate_rad_s, + max_planar_cmd_accel_m_s2=max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=max_yaw_accel_rad_s2, + ), + ) + ) + + +def _follower_with_envelope( + *, + speed_m_s: float, + max_tangent_accel_m_s2: float, + max_normal_accel_m_s2: float, + goal_decel_m_s2: float, + goal_tolerance: float = 0.01, +) -> _HolonomicPathFollower: + core = _make_follower(goal_tolerance=goal_tolerance) + _install_envelope( + core, + speed_m_s=speed_m_s, + max_tangent_accel_m_s2=max_tangent_accel_m_s2, + max_normal_accel_m_s2=max_normal_accel_m_s2, + goal_decel_m_s2=goal_decel_m_s2, + ) + return core + + +def _integrate_holonomic_pose( + x_m: float, + y_m: float, + yaw_rad: float, + cmd_body: Twist, + dt_s: float, +) -> tuple[float, float, float]: + """Integrate body-frame cmd_vel in the world frame (test harness only).""" + vx = float(cmd_body.linear.x) + vy = float(cmd_body.linear.y) + wz = float(cmd_body.angular.z) + c = math.cos(yaw_rad) + s = math.sin(yaw_rad) + x_m += (c * vx - s * vy) * dt_s + y_m += (s * vx + c * vy) * dt_s + yaw_rad += wz * dt_s + yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) + return x_m, y_m, yaw_rad + + +class _HarnessResult: + def __init__( + self, + *, + command_history: list[Twist], + stop_messages: list[str], + final_x_m: float, + final_y_m: float, + final_yaw_rad: float, + ) -> None: + self.command_history = command_history + self.stop_messages = stop_messages + self.final_x_m = final_x_m + self.final_y_m = final_y_m + self.final_yaw_rad = final_yaw_rad + + +class _RecordingHolonomicTrackingController(HolonomicTrackingController): + def __init__(self) -> None: + super().__init__(k_position_per_s=0.0, k_yaw_per_s=0.0) + self.measurements: list[TrajectoryMeasuredSample] = [] + + def control( + self, + reference: TrajectoryReferenceSample, + measurement: TrajectoryMeasuredSample, + ) -> Twist: + self.measurements.append(measurement) + return super().control(reference, measurement) + + +def _follower_with_recording_holonomic_controller() -> tuple[ + _HolonomicPathFollower, _RecordingHolonomicTrackingController +]: + core = _make_follower(goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) + core._path = path + core._path_distancer = PathDistancer(path) + controller = core._controller + assert isinstance(controller, HolonomicPathController) + recorder = _RecordingHolonomicTrackingController() + controller._inner = recorder + return core, recorder + + +def _run_follower_harness( + core: _HolonomicPathFollower, + *, + points: list[tuple[float, float]], + initial_yaw_rad: float = 0.0, + max_ticks: int = 260, + rate_hz: float = 60.0, +) -> _HarnessResult: + dt_s = 1.0 / rate_hz + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, initial_yaw_rad + latest_cmd = Twist() + command_history: list[Twist] = [] + stop_messages: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + command_history.append(Twist(cmd)) + + cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = core.stopped_navigating.subscribe(stop_messages.append) + sim_time_s = 1.0 + + try: + core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) + core.start_planning(_path_from_points(points)) + for _ in range(max_ticks): + if "arrived" in stop_messages: + break + time.sleep(dt_s * 1.1) + plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( + plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s + ) + sim_time_s += dt_s + core.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) + ) + finally: + core.close() + cmd_sub.dispose() + stop_sub.dispose() + + return _HarnessResult( + command_history=command_history, + stop_messages=stop_messages, + final_x_m=plant_x_m, + final_y_m=plant_y_m, + final_yaw_rad=plant_yaw_rad, + ) + + +def test_holonomic_path_controller_slews_first_command_from_rest() -> None: + ctrl = HolonomicPathController( + GlobalConfig(), + speed=0.55, + control_frequency=10.0, + k_position_per_s=2.0, + k_yaw_per_s=1.5, + ) + odom = PoseStamped(frame_id="map", position=[0.0, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)) + out = ctrl.advance(np.array([0.5, 0.0], dtype=np.float64), odom) + assert math.hypot(float(out.linear.x), float(out.linear.y)) == pytest.approx(0.5) + + +def test_path_following_passes_estimated_measured_body_twist() -> None: + core, recorder = _follower_with_recording_holonomic_controller() + core._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) + core._compute_path_following() + core._current_odom = _pose_stamped(0.3, -0.1, 0.2, ts=1.5) + + core._compute_path_following() + + measured = recorder.measurements[-1].twist_body + vx_w = 0.3 / 0.5 + vy_w = -0.1 / 0.5 + assert measured.linear.x == pytest.approx(math.cos(0.2) * vx_w + math.sin(0.2) * vy_w) + assert measured.linear.y == pytest.approx(-math.sin(0.2) * vx_w + math.cos(0.2) * vy_w) + assert measured.angular.z == pytest.approx(0.2 / 0.5) + + +def test_rotation_passes_estimated_measured_body_twist() -> None: + core, recorder = _follower_with_recording_holonomic_controller() + core._current_odom = _pose_stamped(0.0, 0.0, 1.0, ts=1.0) + core._compute_initial_rotation() + core._current_odom = _pose_stamped(0.2, 0.1, 0.8, ts=1.4) + + core._compute_initial_rotation() + + measured = recorder.measurements[-1].twist_body + vx_w = 0.2 / 0.4 + vy_w = 0.1 / 0.4 + assert measured.linear.x == pytest.approx(math.cos(0.8) * vx_w + math.sin(0.8) * vy_w) + assert measured.linear.y == pytest.approx(-math.sin(0.8) * vx_w + math.cos(0.8) * vy_w) + assert measured.angular.z == pytest.approx(angle_diff(0.8, 1.0) / 0.4) + + +def test_invalid_dt_passes_zero_measured_body_twist() -> None: + core, recorder = _follower_with_recording_holonomic_controller() + core._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=5.0) + core._compute_path_following() + core._current_odom = _pose_stamped(0.2, 0.0, 0.0, ts=5.0) + + core._compute_path_following() + + measured = recorder.measurements[-1].twist_body + assert measured.is_zero() + + +def test_lookahead_reference_uses_path_end_progress() -> None: + core = _make_follower(speed_m_s=0.5, goal_tolerance=0.01) + path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) + distancer = PathDistancer(path) + odom = _pose_stamped(0.2, 0.1, 0.0, ts=10.0) + current_pos = np.array([odom.position.x, odom.position.y], dtype=np.float64) + path_speed = core._path_speed_for_index(distancer, 0, current_pos) + + reference = core._lookahead_reference_sample( + distancer, + odom, + current_pos, + path_speed, + ) + + assert reference.pose_plan.position.x == pytest.approx(0.7) + assert reference.time_s == pytest.approx(11.0) + + +def test_uses_curvature_speed_cap_for_holonomic_path() -> None: + core = _follower_with_envelope( + speed_m_s=1.2, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=0.6, + goal_decel_m_s2=1.0, + ) + path = Path( + frame_id="map", + poses=[ + PoseStamped(frame_id="map", position=[0.0, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)), + PoseStamped(frame_id="map", position=[0.5, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)), + PoseStamped(frame_id="map", position=[0.5, 0.5, 0.0], orientation=Quaternion(0, 0, 0, 1)), + ], + ) + core._path = path + core._path_distancer = PathDistancer(path) + core._current_odom = PoseStamped( + frame_id="map", + position=[0.5, 0.0, 0.0], + orientation=Quaternion(0, 0, 0, 1), + ) + + core._compute_path_following() + + assert core._controller._speed < 1.2 + + +def test_uses_configured_goal_decel_for_path_speed_cap() -> None: + core = _follower_with_envelope( + speed_m_s=2.0, + max_tangent_accel_m_s2=4.0, + max_normal_accel_m_s2=0.6, + goal_decel_m_s2=0.5, + ) + distancer = PathDistancer(_path_from_points([(0.0, 0.0), (1.0, 0.0)])) + current_pos = np.array([0.75, 0.0], dtype=np.float64) + + speed = core._path_speed_for_index(distancer, 0, current_pos) + + assert speed == pytest.approx(0.5, abs=1e-3) + + +def test_path_speed_uses_geometry_and_near_goal_decel_caps() -> None: + max_normal_accel_m_s2 = 0.5 + goal_decel_m_s2 = 1.0 + core = _follower_with_envelope( + speed_m_s=2.0, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=max_normal_accel_m_s2, + goal_decel_m_s2=goal_decel_m_s2, + ) + distancer = PathDistancer( + _path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (2.0, 1.0)]) + ) + + corner_pos = np.array([1.0, 0.0], dtype=np.float64) + corner_index = distancer.find_closest_point_index(corner_pos) + corner_speed = core._path_speed_for_index(distancer, corner_index, corner_pos) + + near_goal_pos = np.array([1.95, 1.0], dtype=np.float64) + near_goal_index = distancer.find_closest_point_index(near_goal_pos) + near_goal_speed = core._path_speed_for_index(distancer, near_goal_index, near_goal_pos) + + right_angle_radius_m = math.sqrt(0.5) + geometry_cap_m_s = math.sqrt(max_normal_accel_m_s2 * right_angle_radius_m) + goal_decel_cap_m_s = math.sqrt( + 2.0 * goal_decel_m_s2 * distancer.distance_to_goal(near_goal_pos) + ) + assert corner_speed == pytest.approx(geometry_cap_m_s) + assert near_goal_speed == pytest.approx(goal_decel_cap_m_s, abs=1e-3) + assert near_goal_speed < corner_speed + + +def test_path_speed_anticipates_corner_tangent_decel() -> None: + speed_m_s = 2.0 + core = _follower_with_envelope( + speed_m_s=speed_m_s, + max_tangent_accel_m_s2=0.5, + max_normal_accel_m_s2=0.1, + goal_decel_m_s2=1.0, + ) + distancer = PathDistancer(_path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)])) + + before_corner_pos = np.array([0.85, 0.0], dtype=np.float64) + before_corner_speed = core._path_speed_for_index( + distancer, + distancer.find_closest_point_index(before_corner_pos), + before_corner_pos, + ) + + assert before_corner_speed < speed_m_s + assert before_corner_speed < 1.0 + + +def test_in_the_loop_harness_reaches_arrival_on_straight_line() -> None: + result = _run_follower_harness( + _make_follower(speed_m_s=1.0, control_frequency=60.0, goal_tolerance=0.08), + points=[(0.1, 0.0), (1.2, 0.0)], + ) + + assert "arrived" in result.stop_messages + assert result.command_history + assert math.hypot(result.final_x_m - 1.2, result.final_y_m) < 0.15 + + +def test_in_the_loop_harness_exercises_curvature_speed_cap() -> None: + result = _run_follower_harness( + _make_follower(speed_m_s=1.2, control_frequency=60.0, goal_tolerance=0.08), + points=[(0.1, 0.0), (0.45, 0.0), (0.45, 0.45), (0.8, 0.45)], + max_ticks=320, + ) + + assert "arrived" in result.stop_messages + + +def test_in_the_loop_harness_supports_2mps_right_angle_with_conservative_profile() -> None: + core = _make_follower(control_frequency=60.0, goal_tolerance=0.08) + _install_envelope( + core, + speed_m_s=2.0, + max_tangent_accel_m_s2=0.5, + max_normal_accel_m_s2=0.1, + goal_decel_m_s2=0.5, + ) + result = _run_follower_harness( + core, + points=[(0.1, 0.0), (0.55, 0.0), (0.55, 0.55), (0.9, 0.55)], + max_ticks=420, + ) + commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] + + assert "arrived" in result.stop_messages + assert commanded_speeds + assert max(commanded_speeds) < 0.75 + + +def test_in_the_loop_harness_decelerates_near_goal() -> None: + result = _run_follower_harness( + _make_follower(speed_m_s=1.2, control_frequency=60.0, goal_tolerance=0.08), + points=[(0.1, 0.0), (1.0, 0.0)], + max_ticks=300, + ) + commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] + moving_speeds = [speed for speed in commanded_speeds if speed > 0.05] + + assert "arrived" in result.stop_messages + assert moving_speeds + assert max(moving_speeds[: len(moving_speeds) // 2]) > 0.7 + assert min(moving_speeds[-10:]) < 0.55 + assert min(moving_speeds[-10:]) < 0.6 * max(moving_speeds) + assert math.hypot(result.final_x_m - 1.0, result.final_y_m) < 0.15 + + +def test_in_the_loop_harness_exercises_initial_rotation() -> None: + # Rotate-first is off by default for the holonomic base; opt in to exercise + # the initial_rotation state machine branch. + result = _run_follower_harness( + _make_follower( + speed_m_s=0.9, + control_frequency=60.0, + goal_tolerance=0.08, + align_heading_before_move=True, + ), + points=[(0.1, 0.0), (1.0, 0.0)], + initial_yaw_rad=0.8, + max_ticks=320, + ) + + assert "arrived" in result.stop_messages + assert any( + abs(float(cmd.angular.z)) > 0.05 and _planar_speed_m_s(cmd) < 0.05 + for cmd in result.command_history[:20] + ) + assert abs(result.final_yaw_rad) < 0.35 + + +def test_holonomic_velocity_damping_reduces_planar_command() -> None: + # Aligned pose so k_position/k_yaw contribute nothing; the only difference is + # the velocity-damping gain wired into the inner law via the controller ctor. + reference = TrajectoryReferenceSample( + time_s=1.0, + pose_plan=Pose(position=[0.0, 0.0, 0.0], orientation=_yaw_quaternion(0.0)), + twist_body=Twist(linear=[0.5, 0.2, 0.0]), + ) + odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) + measured_body_twist = Twist(linear=[1.1, 0.6, 0.0]) + + def _factory(k_velocity_per_s: float) -> HolonomicPathController: + return HolonomicPathController( + # Generous slew headroom so the first-tick accel clamp does not mask the gain. + GlobalConfig(local_planner_max_planar_cmd_accel_m_s2=8.0), + speed=1.0, + control_frequency=10.0, + k_position_per_s=0.0, + k_yaw_per_s=0.0, + k_velocity_per_s=k_velocity_per_s, + k_yaw_rate_per_s=0.0, + ) + + undamped = _factory(0.0).advance_reference(reference, odom, measured_body_twist) + damped = _factory(0.5).advance_reference(reference, odom, measured_body_twist) + + assert undamped.linear.x == pytest.approx(0.5) + assert undamped.linear.y == pytest.approx(0.2) + # 0.5 * (ref - measured) damping pulls the command down to (0.2, 0.0). + assert damped.linear.x == pytest.approx(0.2) + assert damped.linear.y == pytest.approx(0.0) + assert math.hypot(damped.linear.x, damped.linear.y) < math.hypot( + undamped.linear.x, undamped.linear.y + ) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py b/dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py new file mode 100644 index 0000000000..5f83d20577 --- /dev/null +++ b/dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py @@ -0,0 +1,204 @@ +# 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. + +"""Run-profile speed application in ``_HolonomicPathFollower``. + +Relocated from ``dannav/test_local_planner_run_envelope.py``. The session +profile is resolved from ``DanHolonomicTCConfig.run_profile`` at construction and +re-resolved live by ``set_run_profile``; the per-goal ``run_profile_name`` +override is gone, so the leak test is recast to session-profile switching. +""" + +from __future__ import annotations + +import math +import time + +import numpy as np +import pytest + +from dimos.core.global_config import GlobalConfig +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.nav_msgs.Path import Path +from dimos.navigation.holonomic_trajectory_controller.module import ( + DanHolonomicTCConfig, + _HolonomicPathFollower, +) +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="map", + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append(_pose_stamped(point[0], point[1], yaw)) + return Path(frame_id="map", poses=poses) + + +def _straight_points(length_m: float, spacing_m: float = 0.1) -> list[tuple[float, float]]: + n = round(length_m / spacing_m) + return [(i * spacing_m, 0.0) for i in range(n + 1)] + + +def _make_follower(**overrides: object) -> _HolonomicPathFollower: + return _HolonomicPathFollower(DanHolonomicTCConfig(**overrides)) + + +def _path_speed_at(core: _HolonomicPathFollower, path: Path, pos: tuple[float, float]) -> float: + distancer = PathDistancer(path) + current_pos = np.array(pos, dtype=np.float64) + return core._path_speed_for_index( + distancer, distancer.find_closest_point_index(current_pos), current_pos + ) + + +def _integrate_holonomic_pose( + x_m: float, + y_m: float, + yaw_rad: float, + cmd_body: Twist, + dt_s: float, +) -> tuple[float, float, float]: + """Integrate body-frame cmd_vel in the world frame (test harness only).""" + vx = float(cmd_body.linear.x) + vy = float(cmd_body.linear.y) + wz = float(cmd_body.angular.z) + c = math.cos(yaw_rad) + s = math.sin(yaw_rad) + x_m += (c * vx - s * vy) * dt_s + y_m += (s * vx + c * vy) * dt_s + yaw_rad += wz * dt_s + yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) + return x_m, y_m, yaw_rad + + +def test_walk_default_profile_speed() -> None: + """No override: the path speed is the walk profile cruise (0.55 m/s).""" + core = _make_follower() + speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) + assert speed == pytest.approx(0.55) + + +def test_cruise_override_drives_path_speed() -> None: + """``speed_m_s`` overrides the profile cruise (the old planner_robot_speed).""" + core = _make_follower(speed_m_s=1.0) + speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) + assert speed == pytest.approx(1.0) + + +def test_run_conservative_drives_planner_speed_and_command_caps() -> None: + core = _make_follower(run_profile="run_conservative") + path = _path_from_points(_straight_points(6.0)) + + mid_speed = _path_speed_at(core, path, (3.0, 0.0)) + near_goal_speed = _path_speed_at(core, path, (5.7, 0.0)) + + assert mid_speed == pytest.approx(1.5) + assert near_goal_speed == pytest.approx(math.sqrt(2.0 * 1.5 * 0.3)) + + +def test_run_profile_speed_respects_global_nerf() -> None: + core = _make_follower(run_profile="run_conservative", g=GlobalConfig(nerf_speed=0.5)) + speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) + assert speed == pytest.approx(0.75) + + +def test_set_run_profile_switches_session_speed() -> None: + """``set_run_profile`` replaces the dropped per-goal override: switching the + session profile re-resolves the envelope live, and switching back restores + the previous speed (no leak).""" + core = _make_follower() + path = _path_from_points(_straight_points(6.0)) + + assert core.set_run_profile("trot") is True + trot_speed = _path_speed_at(core, path, (3.0, 0.0)) + + assert core.set_run_profile("walk") is True + walk_speed = _path_speed_at(core, path, (3.0, 0.0)) + + assert trot_speed == pytest.approx(1.0) + assert walk_speed == pytest.approx(0.55) + + +def test_set_run_profile_rejects_unknown_profile() -> None: + core = _make_follower() + assert core.set_run_profile("does-not-exist") is False + # The rejected name does not poison the live envelope. + speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) + assert speed == pytest.approx(0.55) + + +def test_run_profile_commands_faster_than_walk_in_closed_loop() -> None: + rate_hz = 60.0 + dt_s = 1.0 / rate_hz + core = _make_follower( + run_profile="run_conservative", control_frequency=rate_hz, goal_tolerance=0.1 + ) + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 + latest_cmd = Twist() + commanded_speeds: list[float] = [] + stops: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + commanded_speeds.append(math.hypot(float(cmd.linear.x), float(cmd.linear.y))) + + cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = core.stopped_navigating.subscribe(stops.append) + sim_time_s = 1.0 + + try: + core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) + core.start_planning(_path_from_points([(0.1, 0.0), (3.1, 0.0)])) + for _ in range(420): + if "arrived" in stops: + break + time.sleep(dt_s * 1.1) + plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( + plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s + ) + sim_time_s += dt_s + core.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) + ) + finally: + core.close() + cmd_sub.dispose() + stop_sub.dispose() + + assert "arrived" in stops + assert commanded_speeds + assert max(commanded_speeds) > 1.0 + assert max(commanded_speeds) <= 1.5 + 1e-6 diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py index af9d6307a4..a8b725802b 100644 --- a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py +++ b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py @@ -18,7 +18,7 @@ import pytest -from dimos.navigation.dannav.controllers import command_envelope_overrides_for_profile +from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import command_envelope_overrides_for_profile from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( GO2_RUN_PROFILES, RunProfile, diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index e676e6741d..41fe773ff1 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -187,7 +187,6 @@ "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", "gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule", - "holonomic-controller": "dimos.navigation.dannav.holonomic_controller.HolonomicController", "hosted-arm-teleop-module": "dimos.teleop.quest_hosted.hosted_extensions.HostedArmTeleopModule", "hosted-teleop-module": "dimos.teleop.quest_hosted.hosted_teleop_module.HostedTeleopModule", "hosted-teleop-recorder": "dimos.teleop.quest_hosted.blueprints.HostedTeleopRecorder", From bc7cce2e0b7e8f69f6d49536a89ccbb6c73f9ceb Mon Sep 17 00:00:00 2001 From: danvi Date: Fri, 26 Jun 2026 19:08:12 +0800 Subject: [PATCH 3/9] New blueprint. Create the MLS + HTC blueprint --- .../holonomic_trajectory_controller/module.py | 8 +- .../nav_3d/mls_planner/goal_relay.py | 28 +++++ dimos/robot/all_blueprints.py | 4 + .../navigation/unitree_go2_mls_htc.py | 101 ++++++++++++++++++ 4 files changed, 137 insertions(+), 4 deletions(-) create mode 100644 dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py diff --git a/dimos/navigation/holonomic_trajectory_controller/module.py b/dimos/navigation/holonomic_trajectory_controller/module.py index 9eb2c119ce..ce14f4e6c6 100644 --- a/dimos/navigation/holonomic_trajectory_controller/module.py +++ b/dimos/navigation/holonomic_trajectory_controller/module.py @@ -743,14 +743,14 @@ def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: class DanHolonomicTCConfig(ModuleConfig): control_frequency: float = 10.0 - run_profile: str = "walk" + run_profile: str = "trot" speed_m_s: float | None = None goal_tolerance: float = 0.2 orientation_tolerance: float = 0.35 k_position_per_s: float = 2.0 - k_yaw_per_s: float = 1.5 - k_velocity_per_s: float = 0.0 - k_yaw_rate_per_s: float = 0.0 + k_yaw_per_s: float = 1.0 + k_velocity_per_s: float = 0.5 + k_yaw_rate_per_s: float = 1.0 align_heading_before_move: bool = False align_goal_yaw: bool = False trajectory_tick_log_path: str | None = None diff --git a/dimos/navigation/nav_3d/mls_planner/goal_relay.py b/dimos/navigation/nav_3d/mls_planner/goal_relay.py index 905c4f8973..ef2c312694 100644 --- a/dimos/navigation/nav_3d/mls_planner/goal_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/goal_relay.py @@ -28,6 +28,34 @@ class GoalRelayConfig(ModuleConfig): pass +class PoseOdomRelayConfig(ModuleConfig): + child_frame_id: str = "base_link" + + +class PoseOdomRelay(Module): + """Convert GO2Connection's PoseStamped odom to nav_msgs.Odometry.""" + + config: PoseOdomRelayConfig + + odom: In[PoseStamped] + odometry: Out[Odometry] + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) + + def _on_odom(self, msg: PoseStamped) -> None: + self.odometry.publish( + Odometry( + ts=msg.ts, + frame_id=msg.frame_id, + child_frame_id=self.config.child_frame_id, + pose=msg, + ) + ) + + class GoalRelay(Module): """Adapt odometry and goal points to the planner's PoseStamped inputs.""" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 41fe773ff1..cf5d57c6d6 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -121,6 +121,7 @@ "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers", "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", "unitree-go2-mid360-record": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_mid360_record:unitree_go2_mid360_record", + "unitree-go2-mls-htc": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_mls_htc:unitree_go2_mls_htc", "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", @@ -153,6 +154,8 @@ "collection-recorder": "dimos.learning.collection.recorder.CollectionRecorder", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", + "dan-holonomic-tc": "dimos.navigation.holonomic_trajectory_controller.module.DanHolonomicTC", + "dannav-planner": "dimos.navigation.dannav.module.DannavPlanner", "demo-calculator-skill": "dimos.agents.skills.demo_calculator_skill.DemoCalculatorSkill", "demo-monitoring": "dimos.agents.demos.demo_capabilities.DemoMonitoring", "demo-robot": "dimos.agents.skills.demo_robot.DemoRobot", @@ -231,6 +234,7 @@ "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", "pointlio-pose-recorder": "dimos.hardware.sensors.lidar.pointlio.pose_recorder.PointlioPoseRecorder", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", + "pose-odom-relay": "dimos.navigation.nav_3d.mls_planner.goal_relay.PoseOdomRelay", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py new file mode 100644 index 0000000000..2e22eaa153 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +# 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. + +"""3D navigation on Go2 with voxel-grid mapping, MLS planning, and holonomic +trajectory control over WebRTC. + +Decouples planning from control: ``MLSPlannerNative`` owns route safety and +emits the ``Path`` (empty when nothing ahead is traversable), while +``DanHolonomicTC`` follows that path with the holonomic tracking law. Compared +to ``unitree_go2_nav_3d`` this swaps ``PointLio`` + ``RayTracingVoxelMap`` for +``VoxelGridMapper``, adds ``PoseOdomRelay``, and replaces ``BasicPathFollower`` +with ``DanHolonomicTC``. +""" + +from typing import Any + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.mapping.voxels import VoxelGridMapper +from dimos.navigation.holonomic_trajectory_controller.module import DanHolonomicTC +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay, PoseOdomRelay +from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.visualization.vis_module import vis_module + +voxel_size = 0.08 +# Height of the head-mounted lidar above the ground while standing. +go2_lidar_height = 0.5 + + +def _render_global_map(msg: Any) -> Any: + return msg.to_rerun() + + +def _render_path(msg: Any) -> Any: + # The planner emits an empty path when it finds no route to the goal. + # Logging those would blank the line, so drop them and keep the last path. + if len(msg.poses) == 0: + return None + return msg + + +_nav_rerun_config = { + **rerun_config, + "max_hz": { + **rerun_config["max_hz"], + "world/global_map": 1.0, + }, + "memory_limit": "2096MB", + "visual_override": { + **rerun_config["visual_override"], + "world/global_map": _render_global_map, + "world/path": _render_path, + "world/surface_map": None, + "world/nodes": None, + "world/node_edges": None, + }, +} + +unitree_go2_mls_htc = autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), + # "mcf" for stair traversal + GO2Connection.blueprint(motion_mode="mcf"), + PoseOdomRelay.blueprint(), + VoxelGridMapper.blueprint( + voxel_size=voxel_size, + frame_id="world", + carve_columns=False, + emit_every=1, + ), + MLSPlannerNative.blueprint( + world_frame="world", + voxel_size=voxel_size, + robot_height=go2_lidar_height, + wall_clearance_m=0.2, + wall_buffer_m=0.75, + wall_buffer_weight=100.0, + step_threshold_m=0.16, + step_penalty_weight=1.0, + viz_publish_hz=0.0, + ), + GoalRelay.blueprint(), + DanHolonomicTC.blueprint(), + MovementManager.blueprint(), +).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) + +__all__ = ["unitree_go2_mls_htc"] From 3c3826fbde95eb7534620ab8d51905f07ebded65 Mon Sep 17 00:00:00 2001 From: danvi Date: Sat, 27 Jun 2026 15:25:57 +0800 Subject: [PATCH 4/9] Create DanLocalPlanner and dannav stack. - Create DanLocalPlanner: module, placement, wiring - Create lock_replan commit window - Add path smoothing and uniform resampling - Tune configs for testing - Remove progress markers - Remove the monolith: delete unitree_go2_dannav.py --- dimos/navigation/dannav/controllers.py | 248 ------ dimos/navigation/dannav/global_planner.py | 512 ----------- dimos/navigation/dannav/local_planner.py | 820 ------------------ .../navigation/dannav/local_planner/module.py | 257 ++++++ .../local_planner/test_dan_local_planner.py | 421 +++++++++ dimos/navigation/dannav/module.py | 190 ---- dimos/navigation/dannav/path_clearance.py | 125 --- dimos/navigation/dannav/rotation_clearance.py | 64 -- .../test_local_planner_path_controller.py | 677 --------------- .../dannav/test_local_planner_run_envelope.py | 272 ------ .../dannav/test_rotation_clearance.py | 72 -- .../trajectory_path_speed_profile.py | 2 +- dimos/robot/all_blueprints.py | 3 +- .../navigation/unitree_go2_mls_htc.py | 16 +- .../blueprints/smart/unitree_go2_dannav.py | 52 -- 15 files changed, 690 insertions(+), 3041 deletions(-) delete mode 100644 dimos/navigation/dannav/controllers.py delete mode 100644 dimos/navigation/dannav/global_planner.py delete mode 100644 dimos/navigation/dannav/local_planner.py create mode 100644 dimos/navigation/dannav/local_planner/module.py create mode 100644 dimos/navigation/dannav/local_planner/test_dan_local_planner.py delete mode 100644 dimos/navigation/dannav/module.py delete mode 100644 dimos/navigation/dannav/path_clearance.py delete mode 100644 dimos/navigation/dannav/rotation_clearance.py delete mode 100644 dimos/navigation/dannav/test_local_planner_path_controller.py delete mode 100644 dimos/navigation/dannav/test_local_planner_run_envelope.py delete mode 100644 dimos/navigation/dannav/test_rotation_clearance.py delete mode 100644 dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py diff --git a/dimos/navigation/dannav/controllers.py b/dimos/navigation/dannav/controllers.py deleted file mode 100644 index e89e9b0adc..0000000000 --- a/dimos/navigation/dannav/controllers.py +++ /dev/null @@ -1,248 +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. - -import math -from typing import Protocol - -import numpy as np -from numpy.typing import NDArray - -from dimos.core.global_config import GlobalConfig -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 -from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import HolonomicPathController -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryReferenceSample -from dimos.utils.trigonometry import angle_diff - - -def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: - return Pose( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), - ) - - -class Controller(Protocol): - def advance( - self, - lookahead_point: NDArray[np.float64], - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: ... - - def advance_reference( - self, - reference: TrajectoryReferenceSample, - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: ... - - def rotate( - self, - yaw_error: float, - current_odom: PoseStamped | None = None, - measured_body_twist: Twist | None = None, - ) -> Twist: ... - - def set_speed(self, speed_m_s: float) -> None: ... - - def reset_errors(self) -> None: ... - - def reset_yaw_error(self, value: float) -> None: ... - - -class PController: - _global_config: GlobalConfig - _speed: float - _control_frequency: float - - _min_linear_velocity: float = 0.2 - _min_angular_velocity: float = 0.2 - _k_angular: float = 0.5 - _max_angular_accel: float = 2.0 - _rotation_threshold: float = 90 * (math.pi / 180) - - def __init__(self, global_config: GlobalConfig, speed: float, control_frequency: float) -> None: - self._global_config = global_config - self._speed = speed - self._control_frequency = control_frequency - - def set_speed(self, speed_m_s: float) -> None: - self._speed = float(speed_m_s) - - def advance( - self, - lookahead_point: NDArray[np.float64], - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: - reference = TrajectoryReferenceSample( - time_s=float(current_odom.ts), - pose_plan=_pose_from_xy_yaw( - float(lookahead_point[0]), - float(lookahead_point[1]), - 0.0, - ), - twist_body=Twist( - linear=Vector3(self._speed, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ), - ) - return self.advance_reference(reference, current_odom, measured_body_twist) - - def advance_reference( - self, - reference: TrajectoryReferenceSample, - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: - del measured_body_twist - current_pos = np.array([current_odom.position.x, current_odom.position.y]) - lookahead_point = np.array( - [ - float(reference.pose_plan.position.x), - float(reference.pose_plan.position.y), - ], - dtype=np.float64, - ) - direction = lookahead_point - current_pos - distance = np.linalg.norm(direction) - - if distance < 1e-6: - # Robot is coincidentally at the lookahead point; skip this cycle. - return Twist() - - robot_yaw = current_odom.orientation.euler[2] - desired_yaw = np.arctan2(direction[1], direction[0]) - yaw_error = angle_diff(desired_yaw, robot_yaw) - - angular_velocity = self._compute_angular_velocity(yaw_error) - - # Rotate-then-drive: if heading error is large, rotate in place first - if abs(yaw_error) > self._rotation_threshold: - return self._angular_twist(angular_velocity) - - # When aligned, drive forward with proportional angular correction - linear_velocity = self._speed * (1.0 - abs(yaw_error) / self._rotation_threshold) - linear_velocity = self._apply_min_velocity(linear_velocity, self._min_linear_velocity) - - return Twist( - linear=Vector3(linear_velocity, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), - ) - - def rotate( - self, - yaw_error: float, - current_odom: PoseStamped | None = None, - measured_body_twist: Twist | None = None, - ) -> Twist: - del measured_body_twist - del current_odom - angular_velocity = self._compute_angular_velocity(yaw_error) - return self._angular_twist(angular_velocity) - - def _compute_angular_velocity(self, yaw_error: float) -> float: - angular_velocity = self._k_angular * yaw_error - angular_velocity = np.clip(angular_velocity, -self._speed, self._speed) - angular_velocity = self._apply_min_velocity(angular_velocity, self._min_angular_velocity) - return float(angular_velocity) - - def reset_errors(self) -> None: - pass - - def reset_yaw_error(self, value: float) -> None: - pass - - def _apply_min_velocity(self, velocity: float, min_velocity: float) -> float: - """Apply minimum velocity threshold, preserving sign. Returns 0 if velocity is 0.""" - if velocity == 0.0: - return 0.0 - if abs(velocity) < min_velocity: - return min_velocity if velocity > 0 else -min_velocity - return velocity - - def _angular_twist(self, angular_velocity: float) -> Twist: - # In simulation, we need stroger values - if self._global_config.simulation and abs(angular_velocity) < 0.8: - angular_velocity = 0.8 * np.sign(angular_velocity) - - return Twist( - linear=Vector3(0.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), - ) - - -class PdController(PController): - _k_derivative: float = 0.15 - - _prev_yaw_error: float - _prev_angular_velocity: float - - def __init__(self, global_config: GlobalConfig, speed: float, control_frequency: float) -> None: - super().__init__(global_config, speed, control_frequency) - - self._prev_yaw_error = 0.0 - self._prev_angular_velocity = 0.0 - - def reset_errors(self) -> None: - self._prev_yaw_error = 0.0 - self._prev_angular_velocity = 0.0 - - def reset_yaw_error(self, value: float) -> None: - self._prev_yaw_error = value - - def _compute_angular_velocity(self, yaw_error: float) -> float: - dt = 1.0 / self._control_frequency - - # PD control: proportional + derivative damping - yaw_error_derivative = (yaw_error - self._prev_yaw_error) / dt - angular_velocity = self._k_angular * yaw_error - self._k_derivative * yaw_error_derivative - - # Rate limiting: limit angular acceleration to prevent jerky corrections - max_delta = self._max_angular_accel * dt - angular_velocity = np.clip( - angular_velocity, - self._prev_angular_velocity - max_delta, - self._prev_angular_velocity + max_delta, - ) - - angular_velocity = np.clip(angular_velocity, -self._speed, self._speed) - angular_velocity = self._apply_min_velocity(angular_velocity, self._min_angular_velocity) - - self._prev_yaw_error = yaw_error - self._prev_angular_velocity = angular_velocity - - return float(angular_velocity) - - -def make_local_path_controller( - global_config: GlobalConfig, - speed: float, - control_frequency: float, -) -> Controller: - if global_config.local_planner_path_controller == "holonomic": - return HolonomicPathController( - global_config, - speed, - control_frequency, - k_position_per_s=global_config.local_planner_holonomic_kp, - k_yaw_per_s=global_config.local_planner_holonomic_ky, - k_velocity_per_s=global_config.local_planner_holonomic_kv, - k_yaw_rate_per_s=global_config.local_planner_holonomic_kw, - ) - return PController(global_config, speed, control_frequency) diff --git a/dimos/navigation/dannav/global_planner.py b/dimos/navigation/dannav/global_planner.py deleted file mode 100644 index 26af08326b..0000000000 --- a/dimos/navigation/dannav/global_planner.py +++ /dev/null @@ -1,512 +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. - -import math -from threading import Event, RLock, Thread, current_thread -import time - -from dimos_lcm.std_msgs import Bool -from reactivex import Subject -from reactivex.disposable import CompositeDisposable - -from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.core.global_config import GlobalConfig -from dimos.core.resource import Resource -from dimos.mapping.occupancy.path_resampling import smooth_resample_path -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.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.base import NavigationState -from dimos.navigation.replanning_a_star.goal_validator import find_safe_goal -from dimos.navigation.dannav.local_planner import LocalPlanner, StopMessage -from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar -from dimos.navigation.replanning_a_star.navigation_map import NavigationMap -from dimos.navigation.replanning_a_star.position_tracker import PositionTracker -from dimos.navigation.replanning_a_star.replan_limiter import ReplanLimiter -from dimos.utils.logging_config import setup_logger -from dimos.utils.trigonometry import angle_diff - -logger = setup_logger() - - -class GlobalPlanner(Resource): - path: Subject[Path] - goal_reached: Subject[Bool] - - _current_odom: PoseStamped | None = None - _current_goal: PoseStamped | None = None - _goal_run_profile: str | None = None - _goal_reached: bool = False - _thread: Thread | None = None - - _global_config: GlobalConfig - _navigation_map: NavigationMap - _navigation_map_near: NavigationMap - _local_planner: LocalPlanner - _position_tracker: PositionTracker - _replan_limiter: ReplanLimiter - _disposables: CompositeDisposable - _stop_planner: Event - _replan_event: Event - _replan_reason: StopMessage | None - _lock: RLock - _safe_goal_clearance: float - - _safe_goal_tolerance: float = 4.0 - _goal_tolerance: float = 0.2 - _rotation_tolerance: float = math.radians(15) - _replan_goal_tolerance: float = 0.5 - _stuck_time_window: float = 8.0 - _stuck_threshold: float = 0.4 - _max_path_deviation: float = 0.9 - _replanning_enabled: bool = True - _replan_on_costmap_update: bool - # Target ~0.14 m between hot replans; interval = spacing / speed (clamped). - _inplace_replan_spacing_m: float = 0.14 - _inplace_replan_min_interval_floor_s: float = 0.05 - _inplace_replan_min_interval_ceil_s: float = 0.25 - _last_inplace_replan_monotonic: float = 0.0 - - def __init__(self, global_config: GlobalConfig, *, replan_on_costmap_update: bool = False) -> None: - self.path = Subject() - self.goal_reached = Subject() - - self._global_config = global_config - self._replan_on_costmap_update = replan_on_costmap_update - self._navigation_map = NavigationMap(self._global_config, "voronoi") - self._navigation_map_near = NavigationMap(self._global_config, "gradient") - self._local_planner = LocalPlanner( - self._global_config, - self._navigation_map, - self._goal_tolerance, - hot_replan=replan_on_costmap_update, - ) - - stuck_threshold = self._stuck_threshold - if global_config.simulation: - stuck_threshold = 1.0 - - self._position_tracker = PositionTracker(self._stuck_time_window, stuck_threshold) - self._replan_limiter = ReplanLimiter() - self._disposables = CompositeDisposable() - self._stop_planner = Event() - self._replan_event = Event() - self._replan_reason = None - self._lock = RLock() - self._reset_safe_goal_clearance() - - def start(self) -> None: - self._local_planner.start() - self._disposables.add( - self._local_planner.stopped_navigating.subscribe(self._on_stopped_navigating) - ) - self._stop_planner.clear() - self._thread = Thread(target=self._thread_entrypoint, daemon=True) - self._thread.start() - - def stop(self) -> None: - self.cancel_goal() - self._local_planner.stop() - self._disposables.dispose() - self._stop_planner.set() - self._replan_event.set() - - if self._thread is not None and self._thread is not current_thread(): - self._thread.join(DEFAULT_THREAD_JOIN_TIMEOUT) - if self._thread.is_alive(): - logger.error("GlobalPlanner thread did not stop in time.") - self._thread = None - - def handle_odom(self, msg: PoseStamped) -> None: - with self._lock: - self._current_odom = msg - - self._local_planner.handle_odom(msg) - self._position_tracker.add_position(msg) - - def handle_global_costmap(self, msg: OccupancyGrid) -> None: - self._navigation_map.update(msg) - self._navigation_map_near.update(msg) - - if not self._replan_on_costmap_update: - return - - with self._lock: - has_goal = self._current_goal is not None - - if not has_goal: - return - - with self._lock: - self._replan_reason = "map_updated" - self._replan_event.set() - - def handle_goal_request(self, goal: PoseStamped, run_profile: str | None = None) -> None: - """Plan toward ``goal``; ``run_profile`` overrides the session envelope for it. - - The per-goal profile sticks to the goal across replans and is cleared - when the goal is cancelled or reached, so the next goal falls back to - the session default. - """ - logger.info("Got new goal", goal=str(goal), run_profile=run_profile) - with self._lock: - self._current_goal = goal - self._goal_run_profile = run_profile - self._goal_reached = False - self._replan_limiter.reset() - self._plan_path() - - def set_run_profile(self, profile: str) -> None: - """Set the session-default run profile used for subsequent goals. - - Module configs are per-process copies, so the operator surface pushes - the chosen profile here instead of relying on its own GlobalConfig - write being visible to the planner. - """ - self._global_config.go2_run_profile = profile - - def set_safe_goal_clearance(self, clearance: float) -> None: - with self._lock: - self._safe_goal_clearance = clearance - - def reset_safe_goal_clearance(self) -> None: - self._reset_safe_goal_clearance() - - def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False) -> None: - # return silently so we don't flood the logs. - with self._lock: - no_goal = self._current_goal is None - if no_goal and self._local_planner.get_state() == NavigationState.IDLE: - return - - logger.info("Cancelling goal.", but_will_try_again=but_will_try_again, arrived=arrived) - - with self._lock: - self._position_tracker.reset_data() - - if not but_will_try_again: - self._current_goal = None - self._goal_run_profile = None - self._goal_reached = arrived - self._replan_limiter.reset() - - self.path.on_next(Path()) - self._local_planner.stop_planning() - - if not but_will_try_again: - self.goal_reached.on_next(Bool(arrived)) - - def set_replanning_enabled(self, enabled: bool) -> None: - with self._lock: - self._replanning_enabled = enabled - - def get_state(self) -> NavigationState: - return self._local_planner.get_state() - - def is_goal_reached(self) -> bool: - with self._lock: - return self._goal_reached - - @property - def cmd_vel(self) -> Subject[Twist]: - return self._local_planner.cmd_vel - - @property - def navigation_costmap(self) -> Subject[OccupancyGrid]: - return self._local_planner.navigation_costmap - - def _thread_entrypoint(self) -> None: - """Monitor if the robot is stuck, veers off track, or stopped navigating.""" - - last_id = -1 - last_stuck_check = time.perf_counter() - - while not self._stop_planner.is_set(): - # Wait for either timeout or replan signal from local planner. - replanning_wanted = self._replan_event.wait(timeout=0.1) - - if self._stop_planner.is_set(): - break - - # Handle stop message from local planner (priority) - if replanning_wanted: - self._replan_event.clear() - with self._lock: - reason = self._replan_reason - self._replan_reason = None - - if reason is not None: - self._handle_stop_message(reason) - last_stuck_check = time.perf_counter() - continue - - with self._lock: - current_goal = self._current_goal - current_odom = self._current_odom - - if not current_goal or not current_odom: - continue - - if ( - current_goal.position.distance(current_odom.position) < self._goal_tolerance - and abs( - angle_diff(current_goal.orientation.euler[2], current_odom.orientation.euler[2]) - ) - < self._rotation_tolerance - ): - logger.info("Close enough to goal. Accepting as arrived.") - self.cancel_goal(arrived=True) - continue - - # Check if robot has veered too far off the path - deviation = self._local_planner.get_distance_to_path() - if deviation is not None and deviation > self._max_path_deviation: - logger.info( - "Robot veered off track. Replanning.", - deviation=round(deviation, 2), - threshold=self._max_path_deviation, - ) - self._replan_path() - last_stuck_check = time.perf_counter() - continue - - _, new_id = self._local_planner.get_unique_state() - - if new_id != last_id: - last_id = new_id - last_stuck_check = time.perf_counter() - continue - - if ( - time.perf_counter() - last_stuck_check > self._stuck_time_window - and self._position_tracker.is_stuck() - ): - logger.info("Robot is stuck. Replanning.") - self._replan_path() - last_stuck_check = time.perf_counter() - - def _on_stopped_navigating(self, stop_message: StopMessage) -> None: - with self._lock: - self._replan_reason = stop_message - # Signal the monitoring thread to do the replanning. This is so we don't have two - # threads which could be replanning at the same time. - self._replan_event.set() - - def _handle_stop_message(self, stop_message: StopMessage) -> None: - # Note, this runs in the monitoring thread. - - if stop_message == "arrived": - self.path.on_next(Path()) - logger.info("Arrived at goal.") - self.cancel_goal(arrived=True) - return - - if stop_message in ("map_updated", "obstacle_found") and self._replan_on_costmap_update: - if stop_message == "obstacle_found": - logger.info("Hot replanning path due to obstacle found.") - else: - logger.info("Hot replanning path due to costmap update.") - self._replan_path_inplace() - return - - self.path.on_next(Path()) - - if stop_message == "obstacle_found": - logger.info("Replanning path due to obstacle found.") - self._replan_path() - elif stop_message == "error": - logger.info("Failure in navigation.") - self._replan_path() - elif stop_message == "run_envelope_rejected": - # Unknown run profile or other envelope resolution failure; replanning - # would not change the outcome, so cancel the goal. - logger.warning("Run profile could not be applied; cancelling goal.") - self.cancel_goal() - else: - logger.error(f"No code to handle '{stop_message}'.") - self.cancel_goal() - - def _replan_path(self) -> None: - with self._lock: - current_odom = self._current_odom - current_goal = self._current_goal - - logger.info("Replanning.", attempt=self._replan_limiter.get_attempt()) - - assert current_odom is not None - assert current_goal is not None - - if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: - self.cancel_goal(arrived=True) - return - - if not self._replanning_enabled: - self.cancel_goal() - return - - if not self._replan_limiter.can_retry(current_odom.position): - self.cancel_goal() - return - - self._replan_limiter.will_retry() - - self._plan_path() - - def _inplace_replan_min_interval_for_speed(self, speed_m_s: float) -> float: - """Shorter throttle at higher planner speed (~fixed spatial replan spacing).""" - if not math.isfinite(speed_m_s) or speed_m_s <= 0.0: - return self._inplace_replan_min_interval_ceil_s - interval = self._inplace_replan_spacing_m / speed_m_s - return max( - self._inplace_replan_min_interval_floor_s, - min(self._inplace_replan_min_interval_ceil_s, interval), - ) - - def _replan_path_inplace(self) -> None: - now = time.monotonic() - min_interval = self._inplace_replan_min_interval_for_speed( - self._local_planner.planner_speed_m_s() - ) - if now - self._last_inplace_replan_monotonic < min_interval: - return - self._last_inplace_replan_monotonic = now - - with self._lock: - current_odom = self._current_odom - current_goal = self._current_goal - goal_run_profile = self._goal_run_profile - - if current_odom is None or current_goal is None: - return - - if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: - self.cancel_goal(arrived=True) - return - - if not self._replanning_enabled: - return - - safe_goal = self._find_safe_goal(current_goal.position) - if not safe_goal: - logger.warning( - "No safe goal found during hot replan.", - x=round(current_goal.x, 3), - y=round(current_goal.y, 3), - ) - return - - path = self._find_wide_path(safe_goal, current_odom.position) - if not path: - logger.warning( - "No path found during hot replan.", - x=round(safe_goal.x, 3), - y=round(safe_goal.y, 3), - ) - return - - resampled_path = smooth_resample_path(path, current_goal, 0.1) - self.path.on_next(resampled_path) - - if self._local_planner.get_state() == NavigationState.IDLE: - self._local_planner.start_planning(resampled_path, run_profile_name=goal_run_profile) - return - - if not self._local_planner.update_path(resampled_path): - self._local_planner.start_planning(resampled_path, run_profile_name=goal_run_profile) - - def _plan_path(self) -> None: - self.cancel_goal(but_will_try_again=True) - - with self._lock: - current_odom = self._current_odom - current_goal = self._current_goal - goal_run_profile = self._goal_run_profile - - assert current_goal is not None - - if current_odom is None: - logger.warning("Cannot handle goal request: missing odometry.") - return - - safe_goal = self._find_safe_goal(current_goal.position) - - if not safe_goal: - logger.warning( - "No safe goal found.", x=round(current_goal.x, 3), y=round(current_goal.y, 3) - ) - self.cancel_goal() - return - - path = self._find_wide_path(safe_goal, current_odom.position) - - if not path: - logger.warning( - "No path found to the goal.", x=round(safe_goal.x, 3), y=round(safe_goal.y, 3) - ) - self.cancel_goal() - return - - resampled_path = smooth_resample_path(path, current_goal, 0.1) - - self.path.on_next(resampled_path) - - self._local_planner.start_planning(resampled_path, run_profile_name=goal_run_profile) - - def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: - # sizes_to_try: list[float] = [2.2, 1.7, 1.3, 1] - sizes_to_try: list[float] = [1.1] - - for size in sizes_to_try: - distance = robot_pos.distance(goal) - navigation_map = self._navigation_map if distance > 1.5 else self._navigation_map_near - costmap = navigation_map.make_gradient_costmap(size) - path = min_cost_astar(costmap, goal, robot_pos) - if path and path.poses: - logger.info(f"Found path {size}x robot width.") - return path - - return None - - def _find_safe_goal(self, goal: Vector3) -> Vector3 | None: - costmap = self._navigation_map.binary_costmap - - if costmap.cell_value(goal) == CostValues.UNKNOWN: - return goal - - safe_goal = find_safe_goal( - costmap, - goal, - algorithm="bfs_contiguous", - cost_threshold=CostValues.OCCUPIED, - min_clearance=self._safe_goal_clearance, - max_search_distance=self._safe_goal_tolerance, - ) - - if safe_goal is None: - logger.warning("No safe goal found near requested target.") - return None - - goals_distance = safe_goal.distance(goal) - if goals_distance > 0.2: - logger.warning(f"Travelling to goal {goals_distance}m away from requested goal.") - - logger.info("Found safe goal.", x=round(safe_goal.x, 2), y=round(safe_goal.y, 2)) - - return safe_goal - - def _reset_safe_goal_clearance(self) -> None: - with self._lock: - self._safe_goal_clearance = self._global_config.robot_rotation_diameter / 2 diff --git a/dimos/navigation/dannav/local_planner.py b/dimos/navigation/dannav/local_planner.py deleted file mode 100644 index c5dfecfac9..0000000000 --- a/dimos/navigation/dannav/local_planner.py +++ /dev/null @@ -1,820 +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. - -from dataclasses import dataclass -import math -import os -from threading import Event, RLock, Thread -import time -import traceback -from typing import Literal, TypeAlias - -import numpy as np -from reactivex import Subject - -from dimos.core.global_config import GlobalConfig -from dimos.core.resource import Resource -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 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.base import NavigationState -from dimos.navigation.dannav.controllers import ( - Controller, - make_local_path_controller, -) -from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import ( - CommandEnvelopeOverrides, - HolonomicPathController, - command_envelope_overrides_for_profile, -) -from dimos.navigation.replanning_a_star.navigation_map import NavigationMap -from dimos.navigation.dannav.path_clearance import PathClearance -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer -from dimos.navigation.dannav.rotation_clearance import can_rotate_in_place -from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export import JsonlTrajectoryControlTickSink -from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import ( - TrajectoryControlTickSink, - append_trajectory_control_tick, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( - PathSpeedProfileLimits, - profile_speed_along_polyline, - speed_at_progress_m, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( - GO2_RUN_PROFILES, - RunProfile, - RunProfileError, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import ( - TrajectoryMeasuredSample, - TrajectoryReferenceSample, -) -from dimos.utils.logging_config import setup_logger -from dimos.utils.trigonometry import angle_diff - -PlannerState: TypeAlias = Literal[ - "idle", "initial_rotation", "path_following", "final_rotation", "arrived" -] -StopMessage: TypeAlias = Literal[ - "arrived", - "obstacle_found", - "error", - "run_envelope_rejected", - "map_updated", -] - -logger = setup_logger() - - -@dataclass(frozen=True) -class ActiveRunEnvelope: - """The movement envelope governing the current goal. - - ``profile_name`` is ``None`` for the default walking behavior, where the - speed and limits come from ``planner_robot_speed`` and the - ``local_planner_*`` config fields exactly as before run profiles existed. - For a named run profile, the speed is the profile's requested speed after - any global slowdown scaling, and the limits come from the profile. - """ - - profile_name: str | None - speed_m_s: float - path_limits: PathSpeedProfileLimits - goal_decel_m_s2: float - command_overrides: CommandEnvelopeOverrides | None - - -class LocalPlanner(Resource): - cmd_vel: Subject[Twist] - stopped_navigating: Subject[StopMessage] - navigation_costmap: Subject[OccupancyGrid] - - _thread: Thread | None = None - _path: Path | None = None - _path_clearance: PathClearance | None = None - _path_distancer: PathDistancer | None = None - _current_odom: PoseStamped | None = None - - _pose_index: int - _lock: RLock - _stop_planning_event: Event - _state: PlannerState - _state_unique_id: int - _global_config: GlobalConfig - _navigation_map: NavigationMap - _goal_tolerance: float - _controller: Controller - _trajectory_tick_sink: TrajectoryControlTickSink | None - _previous_odom_for_velocity: PoseStamped | None - - _speed: float = 0.55 - _active_envelope: ActiveRunEnvelope - _path_speed_profile_s: list[float] | None - _path_speed_profile_v: list[float] | None - _path_speed_profile_path_id: int | None - _control_frequency: float - _orientation_tolerance: float = 0.35 - _navigation_costmap_interval: float = 1.0 - _navigation_costmap_last: float = 0.0 - _hot_replan: bool - - def __init__( - self, - global_config: GlobalConfig, - navigation_map: NavigationMap, - goal_tolerance: float, - *, - hot_replan: bool = False, - ) -> None: - self.cmd_vel = Subject() - self.stopped_navigating = Subject() - self.navigation_costmap = Subject() - - self._pose_index = 0 - self._lock = RLock() - self._stop_planning_event = Event() - self._state = "idle" - self._state_unique_id = 0 - self._global_config = global_config - self._navigation_map = navigation_map - self._goal_tolerance = goal_tolerance - self._hot_replan = hot_replan - self._control_frequency = float(global_config.local_planner_control_rate_hz) - self._trajectory_tick_sink = self._make_trajectory_tick_sink(global_config) - self._previous_odom_for_velocity = None - - speed = ( - float(global_config.planner_robot_speed) - if global_config.planner_robot_speed is not None - else self._speed - ) - if not math.isfinite(speed) or speed <= 0.0: - raise ValueError(f"planner speed must be a positive finite float, got {speed!r}") - if global_config.nerf_speed < 1.0: - speed *= global_config.nerf_speed - self._speed = speed - self._active_envelope = self._default_run_envelope() - self._path_speed_profile_s = None - self._path_speed_profile_v = None - self._path_speed_profile_path_id = None - - self._controller = make_local_path_controller( - self._global_config, - speed, - self._control_frequency, - ) - - def start(self) -> None: - pass - - def stop(self) -> None: - self.stop_planning() - self._close_trajectory_tick_sink() - - def handle_odom(self, msg: PoseStamped) -> None: - with self._lock: - self._current_odom = msg - - def start_planning(self, path: Path, run_profile_name: str | None = None) -> None: - self.stop_planning() - - envelope = self._resolve_run_envelope(run_profile_name) - if envelope is None: - self.stopped_navigating.on_next("run_envelope_rejected") - return - self._apply_run_envelope(envelope) - - self._stop_planning_event = Event() - - with self._lock: - self._path = path - self._path_clearance = PathClearance(self._global_config, self._path) - self._path_distancer = PathDistancer(self._path) - self._pose_index = 0 - self._previous_odom_for_velocity = None - self._rebuild_path_speed_profile(self._path_distancer) - self._thread = Thread(target=self._thread_entrypoint, daemon=True) - self._thread.start() - - def update_path(self, path: Path) -> bool: - """Swap the active path without stopping the local planner thread.""" - if not path.poses: - return False - - with self._lock: - if self._path is None or self._thread is None: - return False - - self._path = path - self._path_clearance = PathClearance(self._global_config, self._path) - self._path_distancer = PathDistancer(self._path) - current_odom = self._current_odom - if current_odom is not None: - current_pos = np.array([current_odom.position.x, current_odom.position.y]) - self._pose_index = self._path_distancer.find_closest_point_index(current_pos) - self._rebuild_path_speed_profile(self._path_distancer) - - return True - - def _default_run_envelope(self) -> ActiveRunEnvelope: - """Walking behavior: speed and limits exactly as configured today.""" - return ActiveRunEnvelope( - profile_name=None, - speed_m_s=self._speed, - path_limits=PathSpeedProfileLimits( - max_speed_m_s=self._speed, - max_tangent_accel_m_s2=self._global_config.local_planner_max_tangent_accel_m_s2, - max_normal_accel_m_s2=self._global_config.local_planner_max_normal_accel_m_s2, - ), - goal_decel_m_s2=self._global_config.local_planner_goal_decel_m_s2, - command_overrides=None, - ) - - def _profile_run_envelope(self, profile: RunProfile) -> ActiveRunEnvelope: - speed = profile.requested_planner_speed_m_s - if self._global_config.nerf_speed < 1.0: - speed *= self._global_config.nerf_speed - return ActiveRunEnvelope( - profile_name=profile.name, - speed_m_s=speed, - path_limits=profile.path_speed_profile_limits_at(speed), - goal_decel_m_s2=profile.goal_decel_m_s2, - command_overrides=command_envelope_overrides_for_profile(profile), - ) - - def _resolve_run_envelope(self, run_profile_name: str | None) -> ActiveRunEnvelope | None: - """Resolve the movement envelope for one goal, before motion. - - A per-goal profile name wins over the session default - (``GlobalConfig.go2_run_profile``). The registry default profile keeps - the unchanged walking behavior. Any other named profile resolves its - speed and limits from the registry. - """ - name = ( - run_profile_name - if run_profile_name is not None - else self._global_config.go2_run_profile - ) - if name == GO2_RUN_PROFILES.default_profile_name: - return self._default_run_envelope() - - try: - profile = GO2_RUN_PROFILES.get(name) - except RunProfileError as exc: - logger.warning( - "run profile rejected", - profile=name, - reason=str(exc), - ) - return None - - envelope = self._profile_run_envelope(profile) - - logger.info( - "run envelope applied", - profile=profile.name, - speed_m_s=round(envelope.speed_m_s, 3), - goal_decel_m_s2=envelope.goal_decel_m_s2, - max_yaw_rate_rad_s=profile.max_yaw_rate_rad_s, - ) - return envelope - - def _apply_run_envelope(self, envelope: ActiveRunEnvelope) -> None: - self._active_envelope = envelope - controller = self._controller - if isinstance(controller, HolonomicPathController): - controller.set_command_envelope(envelope.command_overrides) - controller.set_speed(envelope.speed_m_s) - with self._lock: - path_distancer = self._path_distancer - if path_distancer is not None: - self._rebuild_path_speed_profile(path_distancer) - - def stop_planning(self) -> None: - self.cmd_vel.on_next(Twist()) - self._stop_planning_event.set() - - with self._lock: - self._thread = None - - self._reset_state() - - def _make_trajectory_tick_sink( - self, global_config: GlobalConfig - ) -> TrajectoryControlTickSink | None: - path = global_config.local_planner_trajectory_tick_log_path - if path is None or str(path).strip() == "": - return None - return JsonlTrajectoryControlTickSink(path) - - def _close_trajectory_tick_sink(self) -> None: - sink = self._trajectory_tick_sink - close = getattr(sink, "close", None) - if callable(close): - close() - self._trajectory_tick_sink = None - - def get_state(self) -> NavigationState: - with self._lock: - state = self._state - - match state: - case "idle" | "arrived": - return NavigationState.IDLE - case "initial_rotation" | "path_following" | "final_rotation": - return NavigationState.FOLLOWING_PATH - case _: - raise ValueError(f"Unknown planner state: {state}") - - def planner_speed_m_s(self) -> float: - with self._lock: - return self._active_envelope.speed_m_s - - def get_unique_state(self) -> tuple[PlannerState, int]: - with self._lock: - return (self._state, self._state_unique_id) - - def _thread_entrypoint(self) -> None: - try: - self._loop() - except Exception as e: - traceback.print_exc() - logger.exception("Error in local planning", exc_info=e) - self.stopped_navigating.on_next("error") - finally: - self._reset_state() - self.cmd_vel.on_next(Twist()) - - def _change_state(self, new_state: PlannerState) -> None: - if new_state == self._state: - return - self._state = new_state - self._state_unique_id += 1 - logger.info("changed state", state=new_state) - - def _loop(self) -> None: - stop_event = self._stop_planning_event - - with self._lock: - path = self._path - path_clearance = self._path_clearance - current_odom = self._current_odom - - if path is None or path_clearance is None: - raise RuntimeError("No path set for local planner.") - - # Determine initial state: skip initial_rotation if already aligned. - new_state: PlannerState = "initial_rotation" - if current_odom is not None and len(path.poses) > 0: - first_yaw = path.poses[0].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] - initial_yaw_error = angle_diff(first_yaw, robot_yaw) - self._controller.reset_yaw_error(initial_yaw_error) - angle_in_tolerance = abs(initial_yaw_error) < self._orientation_tolerance - if angle_in_tolerance: - position_in_tolerance = ( - path.poses[0].position.distance(current_odom.position) < 0.01 - ) - if position_in_tolerance: - new_state = "final_rotation" - else: - new_state = "path_following" - elif self._holonomic_yaw_lock_active(current_odom): - # Holonomic can translate without aligning first; in narrow - # corridors an initial spin would sweep the body into walls. - new_state = "path_following" - - with self._lock: - self._change_state(new_state) - - while not stop_event.is_set(): - start_time = time.perf_counter() - - with self._lock: - path_clearance.set_planner_speed(self._active_envelope.speed_m_s) - path_clearance.update_costmap(self._navigation_map.binary_costmap) - path_clearance.update_pose_index(self._pose_index) - - self._send_navigation_costmap(path, path_clearance) - - if path_clearance.is_obstacle_ahead(): - if self._hot_replan: - logger.info("Obstacle detected ahead, requesting hot replan.") - self.stopped_navigating.on_next("obstacle_found") - self.cmd_vel.on_next(Twist()) - else: - logger.info("Obstacle detected ahead, stopping local planner.") - self.stopped_navigating.on_next("obstacle_found") - break - elapsed = time.perf_counter() - start_time - sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) - stop_event.wait(sleep_time) - continue - - with self._lock: - state: PlannerState = self._state - - if state == "initial_rotation": - cmd_vel = self._compute_initial_rotation() - elif state == "path_following": - cmd_vel = self._compute_path_following() - elif state == "final_rotation": - cmd_vel = self._compute_final_rotation() - elif state == "arrived": - self.stopped_navigating.on_next("arrived") - break - elif state == "idle": - cmd_vel = None - - if cmd_vel is not None: - self.cmd_vel.on_next(cmd_vel) - - elapsed = time.perf_counter() - start_time - sleep_time = max(0.0, (1.0 / self._control_frequency) - elapsed) - stop_event.wait(sleep_time) - - if stop_event.is_set(): - logger.info("Local planner loop exited due to stop event.") - - def _compute_initial_rotation(self) -> Twist: - with self._lock: - path = self._path - current_odom = self._current_odom - - assert path is not None - assert current_odom is not None - - if self._holonomic_yaw_lock_active(current_odom): - with self._lock: - self._change_state("path_following") - return self._compute_path_following() - - first_pose = path.poses[0] - first_yaw = first_pose.orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] - yaw_error = angle_diff(first_yaw, robot_yaw) - - if abs(yaw_error) < self._orientation_tolerance: - with self._lock: - self._change_state("path_following") - return self._compute_path_following() - - self._controller.set_speed(self._active_envelope.speed_m_s) - measured_body_twist = self._estimate_measured_body_twist(current_odom) - cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) - ref_pose = _pose_from_xy_yaw( - float(current_odom.position.x), - float(current_odom.position.y), - float(first_yaw), - ) - self._append_trajectory_control_tick( - ref_pose, - Twist(), - current_odom, - measured_body_twist, - cmd, - ) - return cmd - - def get_distance_to_path(self) -> float | None: - with self._lock: - path_distancer = self._path_distancer - current_odom = self._current_odom - - if path_distancer is None or current_odom is None: - return None - - current_pos = np.array([current_odom.position.x, current_odom.position.y]) - - return path_distancer.get_distance_to_path(current_pos) - - def _compute_path_following(self) -> Twist: - with self._lock: - path_distancer = self._path_distancer - current_odom = self._current_odom - - assert path_distancer is not None - assert current_odom is not None - - current_pos = np.array([current_odom.position.x, current_odom.position.y]) - - if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: - logger.info("Reached goal position, starting final rotation") - with self._lock: - self._change_state("final_rotation") - return self._compute_final_rotation() - - closest_index = path_distancer.find_closest_point_index(current_pos) - - with self._lock: - self._pose_index = closest_index - - path_speed = self._path_speed_for_index(path_distancer, closest_index, current_pos) - self._controller.set_speed(path_speed) - reference_sample = self._lookahead_reference_sample( - path_distancer, - current_odom, - current_pos, - path_speed, - yaw_lock_rad=self._holonomic_yaw_lock_yaw_rad(current_odom), - ) - measured_body_twist = self._estimate_measured_body_twist(current_odom) - cmd = self._controller.advance_reference( - reference_sample, - current_odom, - measured_body_twist, - ) - self._append_trajectory_control_sample( - reference_sample, - current_odom, - measured_body_twist, - cmd, - ) - return cmd - - def _lookahead_reference_sample( - self, - path_distancer: PathDistancer, - current_odom: PoseStamped, - current_pos: np.ndarray, - path_speed: float, - *, - yaw_lock_rad: float | None = None, - ) -> TrajectoryReferenceSample: - projection = path_distancer.project(current_pos) - s_start = float(projection.s_along_path_m) - s_end = min( - path_distancer.path_length_m, - s_start + path_distancer.lookahead_distance_m, - ) - now_s = float(current_odom.ts) - if not math.isfinite(now_s): - now_s = 0.0 - travel_s = max(0.0, s_end - s_start) - dt_s = 1.0 / self._control_frequency - duration_s = max(dt_s, travel_s / max(path_speed, 1e-6)) - return self._reference_sample_at_progress( - path_distancer, - s_end, - now_s + duration_s, - path_speed, - yaw_lock_rad=yaw_lock_rad, - ) - - def _reference_sample_at_progress( - self, - path_distancer: PathDistancer, - progress_m: float, - time_s: float, - path_speed: float, - *, - yaw_lock_rad: float | None = None, - ) -> TrajectoryReferenceSample: - point = path_distancer.point_at_progress(progress_m) - path_yaw = path_distancer.yaw_at_progress(progress_m) - ref_yaw = path_yaw if yaw_lock_rad is None else float(yaw_lock_rad) - if yaw_lock_rad is None: - feedforward = Twist( - linear=Vector3(path_speed, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - else: - delta = angle_diff(path_yaw, ref_yaw) - feedforward = Twist( - linear=Vector3( - path_speed * math.cos(delta), - path_speed * math.sin(delta), - 0.0, - ), - angular=Vector3(0.0, 0.0, 0.0), - ) - return TrajectoryReferenceSample( - time_s=time_s, - pose_plan=_pose_from_xy_yaw(float(point[0]), float(point[1]), ref_yaw), - twist_body=feedforward, - ) - - def _path_speed_for_index( - self, - path_distancer: PathDistancer, - closest_index: int, - current_pos: np.ndarray, - ) -> float: - del closest_index - self._ensure_path_speed_profile(path_distancer) - envelope = self._active_envelope - progress_m = float(path_distancer.project(current_pos).s_along_path_m) - profile_speed = self._profiled_path_speed_m_s(progress_m) - distance_cap = math.sqrt( - max( - 0.0, - 2.0 * envelope.goal_decel_m_s2 * path_distancer.distance_to_goal(current_pos), - ) - ) - capped = min(envelope.speed_m_s, profile_speed, distance_cap) - return min(envelope.speed_m_s, max(0.05, capped)) - - def _profiled_path_speed_m_s(self, progress_m: float) -> float: - s_profile = self._path_speed_profile_s - v_profile = self._path_speed_profile_v - if s_profile is None or v_profile is None: - return self._active_envelope.speed_m_s - return speed_at_progress_m(progress_m, s_profile, v_profile) - - def _ensure_path_speed_profile(self, path_distancer: PathDistancer) -> None: - path_id = id(path_distancer._path) - if ( - self._path_speed_profile_s is None - or self._path_speed_profile_path_id != path_id - ): - self._rebuild_path_speed_profile(path_distancer) - self._path_speed_profile_path_id = path_id - - def _rebuild_path_speed_profile(self, path_distancer: PathDistancer) -> None: - envelope = self._active_envelope - s_profile, v_profile = profile_speed_along_polyline( - path_distancer._path, - path_distancer._cumulative_dists, - envelope.path_limits, - envelope.goal_decel_m_s2, - ) - self._path_speed_profile_s = s_profile - self._path_speed_profile_v = v_profile - self._path_speed_profile_path_id = id(path_distancer._path) - - def _compute_final_rotation(self) -> Twist: - with self._lock: - path = self._path - current_odom = self._current_odom - - assert path is not None - assert current_odom is not None - - if self._holonomic_yaw_lock_active(current_odom): - logger.info( - "Final rotation deferred: position goal reached in narrow corridor", - ) - with self._lock: - self._change_state("arrived") - return Twist() - - goal_yaw = path.poses[-1].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] - yaw_error = angle_diff(goal_yaw, robot_yaw) - - if abs(yaw_error) < self._orientation_tolerance: - logger.info("Final rotation complete, goal reached") - with self._lock: - self._change_state("arrived") - return Twist() - - self._controller.set_speed(self._active_envelope.speed_m_s) - measured_body_twist = self._estimate_measured_body_twist(current_odom) - cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) - ref_pose = _pose_from_xy_yaw( - float(current_odom.position.x), - float(current_odom.position.y), - float(goal_yaw), - ) - self._append_trajectory_control_tick( - ref_pose, - Twist(), - current_odom, - measured_body_twist, - cmd, - ) - return cmd - - def _reset_state(self) -> None: - with self._lock: - self._change_state("idle") - self._path = None - self._path_clearance = None - self._path_distancer = None - self._pose_index = 0 - self._previous_odom_for_velocity = None - self._controller.set_speed(self._active_envelope.speed_m_s) - self._controller.reset_errors() - - def _append_trajectory_control_tick( - self, - reference_pose: Pose, - reference_twist: Twist, - current_odom: PoseStamped, - measured_body_twist: Twist, - command: Twist, - ) -> None: - reference = TrajectoryReferenceSample( - time_s=float(current_odom.ts), - pose_plan=reference_pose, - twist_body=reference_twist, - ) - self._append_trajectory_control_sample( - reference, - current_odom, - measured_body_twist, - command, - ) - - def _append_trajectory_control_sample( - self, - reference: TrajectoryReferenceSample, - current_odom: PoseStamped, - measured_body_twist: Twist, - command: Twist, - ) -> None: - sink = self._trajectory_tick_sink - if sink is None: - return - measurement = TrajectoryMeasuredSample( - time_s=float(current_odom.ts), - pose_plan=Pose(current_odom.position, current_odom.orientation), - twist_body=measured_body_twist, - ) - append_trajectory_control_tick( - sink, - reference, - measurement, - command, - 1.0 / self._control_frequency, - wall_time_s=time.time(), - ) - - def _estimate_measured_body_twist(self, current_odom: PoseStamped) -> Twist: - previous = self._previous_odom_for_velocity - self._previous_odom_for_velocity = current_odom - if previous is None: - return Twist() - dt = float(current_odom.ts) - float(previous.ts) - if not math.isfinite(dt) or dt <= 0.0: - return Twist() - vx_w = (float(current_odom.position.x) - float(previous.position.x)) / dt - vy_w = (float(current_odom.position.y) - float(previous.position.y)) / dt - yaw = float(current_odom.orientation.euler[2]) - c = math.cos(yaw) - s = math.sin(yaw) - vx_b = c * vx_w + s * vy_w - vy_b = -s * vx_w + c * vy_w - wz = ( - angle_diff( - float(current_odom.orientation.euler[2]), - float(previous.orientation.euler[2]), - ) - / dt - ) - return Twist( - linear=Vector3(vx_b, vy_b, 0.0), - angular=Vector3(0.0, 0.0, wz), - ) - - def _holonomic_yaw_lock_active(self, current_odom: PoseStamped) -> bool: - if self._global_config.local_planner_path_controller != "holonomic": - return False - return self._holonomic_yaw_lock_yaw_rad(current_odom) is not None - - def _holonomic_yaw_lock_yaw_rad(self, current_odom: PoseStamped) -> float | None: - if self._global_config.local_planner_path_controller != "holonomic": - return None - try: - costmap = self._navigation_map.binary_costmap - except ValueError: - return float(current_odom.orientation.euler[2]) - rotation_radius_m = self._global_config.robot_rotation_diameter / 2.0 - if can_rotate_in_place(costmap, current_odom, rotation_radius_m): - return None - return float(current_odom.orientation.euler[2]) - - def _send_navigation_costmap(self, path: Path, path_clearance: PathClearance) -> None: - if "DEBUG_NAVIGATION" not in os.environ: - return - - now = time.time() - if now - self._navigation_costmap_last < self._navigation_costmap_interval: - return - - self._navigation_costmap_last = now - - self.navigation_costmap.on_next(self._navigation_map.gradient_costmap) - - -def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: - return Pose( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), - ) diff --git a/dimos/navigation/dannav/local_planner/module.py b/dimos/navigation/dannav/local_planner/module.py new file mode 100644 index 0000000000..1ddc819066 --- /dev/null +++ b/dimos/navigation/dannav/local_planner/module.py @@ -0,0 +1,257 @@ +# 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. + +"""Path-stream middleware between ``MLSPlannerNative`` and ``DanHolonomicTC``. + +``MLSPlannerNative`` re-roots and re-emits a path on every lidar frame: the +stream is sparse, unsmoothed, and unthrottled. The old ``GlobalPlanner`` shaped +that stream before the tracker saw it (replan throttle, smoothing/resample, +veer-triggered replan); those were deleted with ``dannav`` and never reproduced. + +``DanLocalPlanner`` reinstates them in a transport-free core (``_ReplanGate``) +wrapped in a thin ``Module``, without touching the Rust planner. Every feature +lives in :class:`DanLocalPlannerConfig` and defaults to off, so the module is a +pass-through until configured: + +- ``lock_replan``: commit-window throttle (forward a path only at commit + moments, suppress in between so the follower keeps a stable lookahead). +- ``resample_spacing_m`` / ``smoothing_window``: smooth + uniform resample + the committed path (``smooth_resample_path``). +- ``max_path_deviation_m``: veer override (commit the latest replan when the + robot drifts off the committed path). + +:class:`_ReplanGate` tracks the committed path with a :class:`PathDistancer` +and forwards a fresh planner path only when the robot has advanced +``lock_replan`` metres along the committed path, when a clicked goal first +lands, on cold start, or on a stop. Between commits it suppresses replans so +``DanHolonomicTC`` keeps a stable lookahead. Each committed path is smoothed +and uniformly resampled inside :meth:`_ReplanGate._commit` before it is stored +and forwarded. +""" + +from __future__ import annotations + +import math +from typing import Any + +import numpy as np +from numpy.typing import NDArray +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class DanLocalPlannerConfig(ModuleConfig): + """Configuration for :class:`DanLocalPlanner`. + + Defaults make every feature off so the module is a pass-through until + configured. + """ + + lock_replan: float = 0.0 # commit-window length in m; 0 disables gating + goal_commit_tolerance_m: float = 0.3 # match a path end to the clicked goal + resample_spacing_m: float = 0.1 # resample spacing in m; 0 disables smoothing + smoothing_window: int = 100 # moving-average window + max_path_deviation_m: float | None = None # veer override; None disables + + +class _ReplanGate: + """Transport-free core deciding which planner paths reach the follower. + + Owns the gate state and the commit decision, so it is unit-testable directly + the way ``_HolonomicPathFollower`` is. It forwards a planner path only at + *commit* moments and suppresses replans in between, so ``DanHolonomicTC`` + follows the last committed path over a guaranteed-stable lookahead instead of + being yanked onto a freshly re-rooted chord every lidar frame. + + The committed path is tracked with a :class:`PathDistancer`; progress is + arc-length along it from the pose at commit time. :meth:`on_odom` and + :meth:`on_goal` record the robot pose and the armed click goal that the + commit triggers consume. Smoothing runs in :meth:`_commit` before the path + is stored. + """ + + def __init__(self, config: DanLocalPlannerConfig) -> None: + self._cfg = config + # Latest robot xy from odometry; progress and veer deviation are measured + # against the committed path with it. + self._robot_xy: NDArray[np.float64] | None = None + # xy of the most recent finite click, armed until a path ends at it + # (fresh-click commit) or a cancel disarms it. + self._armed_goal: NDArray[np.float64] | None = None + # PathDistancer over the path DanHolonomicTC is currently following, and + # the robot's arc-length on it at commit time (~0 since MLS roots the + # path at the robot). None means nothing committed (cold start / stop). + self._committed: PathDistancer | None = None + self._anchor_progress_m: float = 0.0 + + def on_odom(self, odom: Odometry) -> None: + self._robot_xy = np.array([odom.position.x, odom.position.y], dtype=float) + + def on_goal(self, goal: PointStamped) -> None: + """Arm or disarm the fresh-click commit. + + A finite ``PointStamped`` arms it and stores the goal xy; a NaN point is + the cancel from ``MovementManager._cancel_goal`` and disarms it (and + drops the committed path). ``DanLocalPlanner`` does not touch + ``stop_movement``. + """ + if math.isnan(goal.x) or math.isnan(goal.y): + self._armed_goal = None + self._drop_committed() + return + self._armed_goal = np.array([goal.x, goal.y], dtype=float) + + def on_planner_path(self, path: Path) -> Path | None: + """Return the path to forward, or ``None`` to suppress it. + + Commit triggers, first match wins: + + 1. Empty planner path -> forward immediately (a stop) and drop the + committed path. Safety override, always first. + 2. No committed path yet (cold start, or just after a stop/cancel) -> + commit this path. + 3. Fresh click: a finite goal arrived since the last commit and this + path ends within ``goal_commit_tolerance_m`` of it -> commit and + disarm. Rejects a stale in-flight replan for the previous goal. + 4. Lock released: the robot has advanced ``>= lock_replan`` along the + committed path since the last commit -> commit and re-anchor. + 5. Otherwise -> suppress (return ``None``, publish nothing). + """ + if len(path.poses) == 0: # 1. stop override + self._drop_committed() + return path + if self._committed is None: # 2. cold start + return self._commit(path) + if self._armed_goal is not None and _ends_near( + path, self._armed_goal, self._cfg.goal_commit_tolerance_m + ): # 3. clicked -> agree + self._armed_goal = None + return self._commit(path) + if self._lock_released(): # 4. window elapsed + return self._commit(path) + return None # 5. replan held + + def _commit(self, path: Path) -> Path: + """Adopt ``path`` as the committed path and return it for publishing. + + Smoothing reshapes ``path`` here before it is stored, so progress and + veer deviation are measured against the same polyline the tracker follows. + """ + smoothed = self._smooth(path) + self._committed = PathDistancer(smoothed) + self._anchor_progress_m = self._progress_on_committed() + return smoothed + + def _smooth(self, path: Path) -> Path: + """Smooth and uniformly resample a committed path. + + Restores ``GlobalPlanner``'s ``smooth_resample_path``: upsample, + moving-average smooth, resample at a fixed spacing, endpoints fixed. The + holonomic tracker was tuned against that dense, rounded path; the raw + string-pulled MLS path has piecewise-constant tangents that step at chord + junctions and swing ``PathDistancer.yaw_at_progress``. + + ``resample_spacing_m <= 0`` disables it (forward the raw path, unchanged + object). The MLS path already ends at the goal, so its last pose is the + natural ``goal_pose`` (``smooth_resample_path`` keeps endpoints fixed). + """ + if self._cfg.resample_spacing_m <= 0.0 or len(path.poses) == 0: + return path + last = path.poses[-1] + goal_pose = Pose(last.position, last.orientation) + return smooth_resample_path( + path, goal_pose, self._cfg.resample_spacing_m, self._cfg.smoothing_window + ) + + def _drop_committed(self) -> None: + self._committed = None + self._anchor_progress_m = 0.0 + + def _lock_released(self) -> bool: + """True when the robot has advanced ``>= lock_replan`` since the commit. + + ``lock_replan <= 0`` disables gating, so the lock is always released and + every non-empty path commits (current/pass-through behavior). + """ + if self._cfg.lock_replan <= 0.0: + return True + progress = self._progress_on_committed() - self._anchor_progress_m + return progress >= self._cfg.lock_replan + + def _progress_on_committed(self) -> float: + """Robot arc-length along the committed path, or 0 if it can't be measured.""" + if self._committed is None or self._robot_xy is None: + return 0.0 + return self._committed.project(self._robot_xy).s_along_path_m + + +def _ends_near(path: Path, goal_xy: NDArray[np.float64], tolerance_m: float) -> bool: + """True when ``path``'s last pose is within ``tolerance_m`` of ``goal_xy``.""" + end = path.poses[-1] + return float(math.hypot(end.x - goal_xy[0], end.y - goal_xy[1])) <= tolerance_m + + +class DanLocalPlanner(Module): + """Gate and shape ``MLSPlannerNative``'s path stream for ``DanHolonomicTC``. + + Sits between the planner output (remapped to ``planner_path``) and the + follower's ``path`` input. Forwards the paths :class:`_ReplanGate` commits; + ``DanHolonomicTC`` keeps its unchanged ``BasicPathFollower`` surface. + """ + + config: DanLocalPlannerConfig + + planner_path: In[Path] # from MLSPlannerNative.path (remapped) + odometry: In[Odometry] # from PoseOdomRelay.odometry + goal: In[PointStamped] # from MovementManager.goal; the click/cancel signal + + path: Out[Path] # to DanHolonomicTC.path + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._gate = _ReplanGate(self.config) + + @rpc + def start(self) -> None: + super().start() + # Subscribe odom and goal before paths so the gate has the latest robot + # pose and armed goal when a planner path arrives. + self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + self.register_disposable(Disposable(self.goal.subscribe(self._on_goal))) + self.register_disposable( + Disposable(self.planner_path.subscribe(self._on_planner_path)) + ) + + def _on_odometry(self, msg: Odometry) -> None: + self._gate.on_odom(msg) + + def _on_goal(self, msg: PointStamped) -> None: + self._gate.on_goal(msg) + + def _on_planner_path(self, msg: Path) -> None: + forwarded = self._gate.on_planner_path(msg) + if forwarded is not None: + self.path.publish(forwarded) diff --git a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py new file mode 100644 index 0000000000..9c3f8f4584 --- /dev/null +++ b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py @@ -0,0 +1,421 @@ +# 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. + +"""Tests for ``DanLocalPlanner`` module wiring and ``_ReplanGate`` behavior.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterator +from contextlib import contextmanager +import math +from typing import Any + +from dimos.core.stream import Stream, Transport +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.dannav.local_planner.module import ( + DanLocalPlanner, + DanLocalPlannerConfig, + _ReplanGate, +) + + +class _DirectTransport(Transport): # type: ignore[type-arg] + """Synchronous in-process transport so ``start()`` can wire the inputs. + + Delivers each broadcast straight to the subscribed handlers on the calling + thread, which keeps the wiring assertions deterministic. + """ + + def __init__(self) -> None: + self._subscribers: list[Callable[[Any], Any]] = [] + + def broadcast(self, _selfstream: Any, value: Any) -> None: + for callback in list(self._subscribers): + callback(value) + + def subscribe( + self, callback: Callable[[Any], Any], _selfstream: Stream[Any] | None = None + ) -> Callable[[], None]: + self._subscribers.append(callback) + + def _unsubscribe() -> None: + if callback in self._subscribers: + self._subscribers.remove(callback) + + return _unsubscribe + + def start(self) -> None: ... + + def stop(self) -> None: + self._subscribers.clear() + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses = [ + PoseStamped(ts=1.0, frame_id="world", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) + for x, y in points + ] + return Path(frame_id="world", poses=poses) + + +def _odometry(x: float, y: float, *, ts: float = 1.0) -> Odometry: + return Odometry( + ts=ts, + frame_id="world", + pose=PoseStamped(position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]), + ) + + +def _point(x: float, y: float) -> PointStamped: + return PointStamped(x=x, y=y, z=0.0, frame_id="world") + + +class _ModuleHarness: + def __init__(self, module: DanLocalPlanner, forwarded: list[Path]) -> None: + self.module = module + self.forwarded = forwarded + + @property + def gate(self) -> _ReplanGate: + return self.module._gate + + def feed_odom(self, x: float, y: float, *, ts: float = 1.0) -> None: + self.module.odometry.transport.broadcast(None, _odometry(x, y, ts=ts)) + + def feed_goal(self, x: float, y: float) -> None: + self.module.goal.transport.broadcast(None, _point(x, y)) + + def feed_planner_path(self, path: Path) -> None: + self.module.planner_path.transport.broadcast(None, path) + + def close(self) -> None: + self.module.stop() + + +@contextmanager +def _running_module(**config: Any) -> Iterator[_ModuleHarness]: + module = DanLocalPlanner(**config) + module.planner_path.transport = _DirectTransport() + module.odometry.transport = _DirectTransport() + module.goal.transport = _DirectTransport() + forwarded: list[Path] = [] + module.path.subscribe(forwarded.append) + module.start() + harness = _ModuleHarness(module, forwarded) + try: + yield harness + finally: + harness.close() + + +# ---- gate core (transport-free) ---------------------------------------------- + + +def test_gate_forwards_non_empty_path() -> None: + gate = _ReplanGate(DanLocalPlannerConfig()) + path = _path_from_points([(0.0, 0.0), (1.0, 0.0)]) + assert gate.on_planner_path(path) is path + + +def test_gate_forwards_empty_path_as_stop() -> None: + gate = _ReplanGate(DanLocalPlannerConfig()) + empty = Path(frame_id="world", poses=[]) + assert gate.on_planner_path(empty) is empty + + +def test_gate_on_odom_records_robot_xy() -> None: + gate = _ReplanGate(DanLocalPlannerConfig()) + assert gate._robot_xy is None + gate.on_odom(_odometry(1.5, -2.0)) + assert gate._robot_xy is not None + assert list(gate._robot_xy) == [1.5, -2.0] + + +def test_gate_on_goal_finite_arms_and_nan_disarms() -> None: + gate = _ReplanGate(DanLocalPlannerConfig()) + gate.on_goal(_point(3.0, 4.0)) + assert gate._armed_goal is not None + assert list(gate._armed_goal) == [3.0, 4.0] + + # A NaN PointStamped is MovementManager's cancel: it disarms the gate. + gate.on_goal(_point(math.nan, math.nan)) + assert gate._armed_goal is None + + +# ---- module wiring ----------------------------------------------------------- + + +def test_planner_path_is_forwarded_to_path_output() -> None: + with _running_module() as h: + path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)]) + h.feed_planner_path(path) + + assert h.forwarded == [path] + + +def test_empty_planner_path_is_forwarded() -> None: + with _running_module() as h: + empty = Path(frame_id="world", poses=[]) + h.feed_planner_path(empty) + + assert h.forwarded == [empty] + + +def test_passthrough_forwards_every_path() -> None: + # Default config (lock_replan=0) is a pass-through: each planner path + # produces exactly one forwarded path. + with _running_module() as h: + paths = [ + _path_from_points([(0.0, 0.0), (1.0, 0.0)]), + _path_from_points([(0.0, 0.0), (0.0, 1.0)]), + _path_from_points([(0.0, 0.0), (1.0, 1.0)]), + ] + for path in paths: + h.feed_planner_path(path) + + assert h.forwarded == paths + + +def test_odom_and_goal_do_not_publish_a_path() -> None: + with _running_module() as h: + h.feed_odom(0.0, 0.0) + h.feed_goal(2.0, 0.0) + + # Only planner paths flow to the path output; odom/goal update gate state. + assert h.forwarded == [] + assert h.gate._robot_xy is not None + assert h.gate._armed_goal is not None + + +# ---- commit window (gate core) ------------------------------------------------- + + +def _gate(**config: Any) -> _ReplanGate: + return _ReplanGate(DanLocalPlannerConfig(**config)) + + +def test_replan_within_lock_is_suppressed() -> None: + gate = _gate(lock_replan=0.5) + gate.on_odom(_odometry(0.0, 0.0)) + first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) + assert gate.on_planner_path(first) is first # cold-start commit + + # MLS re-roots and re-emits, but the robot has only advanced 0.2 m (< L), so + # the replan is held and DanHolonomicTC keeps following the committed path. + gate.on_odom(_odometry(0.2, 0.0)) + replan = _path_from_points([(0.2, 0.0), (5.0, 0.0)]) + assert gate.on_planner_path(replan) is None + + +def test_replan_after_advancing_lock_is_published() -> None: + gate = _gate(lock_replan=0.5) + gate.on_odom(_odometry(0.0, 0.0)) + gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) + + # A replan within the window is held... + gate.on_odom(_odometry(0.3, 0.0)) + assert gate.on_planner_path(_path_from_points([(0.3, 0.0), (5.0, 0.0)])) is None + + # ...then once the robot has advanced >= L the next replan commits and + # re-anchors, so a subsequent replan is held again until the new window. + gate.on_odom(_odometry(0.6, 0.0)) + committed = _path_from_points([(0.6, 0.0), (5.0, 0.0)]) + assert gate.on_planner_path(committed) is committed + gate.on_odom(_odometry(0.7, 0.0)) + assert gate.on_planner_path(_path_from_points([(0.7, 0.0), (5.0, 0.0)])) is None + + +def test_fresh_click_commits_within_lock() -> None: + gate = _gate(lock_replan=0.5) + gate.on_odom(_odometry(0.0, 0.0)) + gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) + + # The robot has only crept forward (< L), so the window has not released... + gate.on_odom(_odometry(0.1, 0.0)) + # ...but a fresh click whose path actually reaches the goal commits anyway + # ("the dog agrees when clicked"), and the arm is consumed. + gate.on_goal(_point(5.0, 0.0)) + clicked = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + assert gate.on_planner_path(clicked) is clicked + assert gate._armed_goal is None + + +def test_fresh_click_does_not_commit_a_stale_replan() -> None: + # The click arms a commit, but a path that does NOT end at the clicked goal + # (a stale in-flight replan for the previous goal) is still held within L, + # and the arm stays set until a path that reaches the goal arrives. + gate = _gate(lock_replan=0.5) + gate.on_odom(_odometry(0.0, 0.0)) + gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) + gate.on_odom(_odometry(0.1, 0.0)) + gate.on_goal(_point(9.0, 0.0)) + stale = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) # ends at the OLD goal + assert gate.on_planner_path(stale) is None + assert gate._armed_goal is not None + + +def test_empty_path_published_and_resets_gate() -> None: + gate = _gate(lock_replan=0.5) + gate.on_odom(_odometry(0.0, 0.0)) + gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) + + # An empty path (nothing safe ahead) forwards immediately as a stop and + # drops the committed path. + empty = Path(frame_id="world", poses=[]) + assert gate.on_planner_path(empty) is empty + assert gate._committed is None + + # The reset means the next path cold-starts a commit even though the robot + # has not advanced a window's worth. + gate.on_odom(_odometry(0.1, 0.0)) + restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + assert gate.on_planner_path(restart) is restart + + +def test_cancel_resets_committed_path() -> None: + # A NaN goal is MovementManager's cancel: it disarms AND drops the committed + # path, so the next planner path cold-starts a fresh commit within L. + gate = _gate(lock_replan=0.5) + gate.on_odom(_odometry(0.0, 0.0)) + gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) + gate.on_goal(_point(math.nan, math.nan)) + assert gate._committed is None + + gate.on_odom(_odometry(0.1, 0.0)) + restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + assert gate.on_planner_path(restart) is restart + + +def test_lock_replan_zero_commits_every_non_empty_path() -> None: + # L = 0 disables gating: every non-empty path commits, even with no advance. + gate = _gate(lock_replan=0.0) + gate.on_odom(_odometry(0.0, 0.0)) + for path in ( + _path_from_points([(0.0, 0.0), (1.0, 0.0)]), + _path_from_points([(0.0, 0.0), (0.0, 1.0)]), + _path_from_points([(0.0, 0.0), (1.0, 1.0)]), + ): + assert gate.on_planner_path(path) is path + + +# ---- commit window (module wiring) --------------------------------------------- + + +def test_module_suppresses_replan_within_lock_then_publishes_after_advance() -> None: + with _running_module(lock_replan=0.5) as h: + h.feed_odom(0.0, 0.0) + first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) + h.feed_planner_path(first) + assert h.forwarded == [first] # cold-start commit + + # Replans arriving while the robot is still within the window publish + # nothing. + h.feed_odom(0.2, 0.0) + h.feed_planner_path(_path_from_points([(0.2, 0.0), (5.0, 0.0)])) + h.feed_odom(0.4, 0.0) + h.feed_planner_path(_path_from_points([(0.4, 0.0), (5.0, 0.0)])) + assert h.forwarded == [first] + + # Once the robot has advanced >= L, the next replan is published. + h.feed_odom(0.6, 0.0) + committed = _path_from_points([(0.6, 0.0), (5.0, 0.0)]) + h.feed_planner_path(committed) + assert h.forwarded == [first, committed] + + +def test_module_fresh_click_commits_within_lock() -> None: + with _running_module(lock_replan=0.5) as h: + h.feed_odom(0.0, 0.0) + first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) + h.feed_planner_path(first) + + h.feed_odom(0.1, 0.0) + h.feed_goal(5.0, 0.0) + clicked = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + h.feed_planner_path(clicked) + assert h.forwarded == [first, clicked] + + +def test_module_empty_path_publishes_and_resets() -> None: + with _running_module(lock_replan=0.5) as h: + h.feed_odom(0.0, 0.0) + first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) + h.feed_planner_path(first) + + empty = Path(frame_id="world", poses=[]) + h.feed_planner_path(empty) + h.feed_odom(0.1, 0.0) + restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + h.feed_planner_path(restart) + assert h.forwarded == [first, empty, restart] + + +# ---- smoothing + uniform resampling -------------------------------------------- + + +def _spacings(path: Path) -> list[float]: + return [ + math.hypot(b.x - a.x, b.y - a.y) for a, b in zip(path.poses, path.poses[1:], strict=False) + ] + + +def test_smooth_resamples_straight_path_to_uniform_spacing() -> None: + # A 2-point straight MLS path is densified to points ~resample_spacing_m + # apart, with the endpoints left exactly where MLS put them. + gate = _gate(resample_spacing_m=0.1) + out = gate._smooth(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) + + assert len(out.poses) > 2 + assert (out.poses[0].x, out.poses[0].y) == (0.0, 0.0) + assert (out.poses[-1].x, out.poses[-1].y) == (5.0, 0.0) + assert all(abs(d - 0.1) < 1e-6 for d in _spacings(out)) + + +def test_smooth_rounds_a_corner() -> None: + # A right-angle corner is rounded: no waypoint sits on the exact corner, and + # cutting the corner makes the smoothed path shorter than the raw chords. + gate = _gate(resample_spacing_m=0.1) + out = gate._smooth(_path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)])) + + assert min(math.hypot(p.x - 1.0, p.y - 0.0) for p in out.poses) > 0.05 + assert sum(_spacings(out)) < 2.0 # 1.0 + 1.0 raw cornered length + + +def test_smooth_returns_empty_path_unchanged() -> None: + gate = _gate(resample_spacing_m=0.1) + empty = Path(frame_id="world", poses=[]) + assert gate._smooth(empty) is empty + + +def test_smooth_disabled_returns_path_unchanged() -> None: + # resample_spacing_m == 0 forwards the raw path (same object), no reshaping. + gate = _gate(resample_spacing_m=0.0) + path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)]) + assert gate._smooth(path) is path + + +def test_commit_forwards_and_tracks_the_smoothed_path() -> None: + # The path the gate commits and tracks is the smoothed one, not the raw + # planner chords. + gate = _gate(resample_spacing_m=0.1) + gate.on_odom(_odometry(0.0, 0.0)) + raw = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) + forwarded = gate.on_planner_path(raw) + + assert forwarded is not raw + assert len(forwarded.poses) > 2 + assert all(abs(d - 0.1) < 1e-6 for d in _spacings(forwarded)) diff --git a/dimos/navigation/dannav/module.py b/dimos/navigation/dannav/module.py deleted file mode 100644 index 28c5fd977d..0000000000 --- a/dimos/navigation/dannav/module.py +++ /dev/null @@ -1,190 +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. - -import os -from typing import Any, Literal - -from dimos_lcm.std_msgs import Bool, String -from reactivex.disposable import Disposable - -from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.base import NavigationInterface, NavigationState -from dimos.navigation.dannav.global_planner import GlobalPlanner -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import GO2_RUN_PROFILES, RunProfileError -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class DannavPlannerConfig(ModuleConfig): - robot_width: float | None = None - robot_rotation_diameter: float | None = None - replan_on_costmap_update: bool = False - path_controller: Literal["differential", "holonomic"] | None = None - - -class DannavPlanner(Module, NavigationInterface): - config: DannavPlannerConfig - - odom: In[PoseStamped] # TODO: Use TF. - odometry: In[Odometry] - global_costmap: In[OccupancyGrid] - goal_request: In[PoseStamped] - clicked_point: In[PointStamped] - target: In[PoseStamped] - stop_movement: In[Bool] - - goal_reached: Out[Bool] - navigation_state: Out[String] # TODO: set it - nav_cmd_vel: Out[Twist] - path: Out[Path] - navigation_costmap: Out[OccupancyGrid] - - _planner: GlobalPlanner - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - overrides = { - name: value - for name, value in ( - ("robot_width", self.config.robot_width), - ("robot_rotation_diameter", self.config.robot_rotation_diameter), - ("local_planner_path_controller", self.config.path_controller or "holonomic"), - ) - if value is not None - } - effective_global_config = ( - self.config.g.model_copy(update=overrides) if overrides else self.config.g - ) - self._planner = GlobalPlanner( - effective_global_config, - replan_on_costmap_update=self.config.replan_on_costmap_update, - ) - logger.info( - "DannavPlanner configured.", - path_controller=effective_global_config.local_planner_path_controller, - replan_on_costmap_update=self.config.replan_on_costmap_update, - ) - - @rpc - def start(self) -> None: - super().start() - - self.register_disposable(Disposable(self.odom.subscribe(self._planner.handle_odom))) - self.register_disposable( - Disposable( - self.odometry.subscribe( - lambda msg: self._planner.handle_odom(msg.to_pose_stamped()) - ) - ) - ) - self.register_disposable( - Disposable(self.global_costmap.subscribe(self._planner.handle_global_costmap)) - ) - self.register_disposable( - Disposable(self.goal_request.subscribe(self._planner.handle_goal_request)) - ) - self.register_disposable( - Disposable(self.target.subscribe(self._planner.handle_goal_request)) - ) - - self.register_disposable( - Disposable( - self.clicked_point.subscribe( - lambda pt: self._planner.handle_goal_request(pt.to_pose_stamped()) - ) - ) - ) - - if self.stop_movement.transport is not None: - self.register_disposable( - Disposable(self.stop_movement.subscribe(self._on_stop_movement)) - ) - - self.register_disposable(self._planner.path.subscribe(self.path.publish)) - - self.register_disposable(self._planner.cmd_vel.subscribe(self.nav_cmd_vel.publish)) - - self.register_disposable(self._planner.goal_reached.subscribe(self.goal_reached.publish)) - - if "DEBUG_NAVIGATION" in os.environ: - self.register_disposable( - self._planner.navigation_costmap.subscribe(self.navigation_costmap.publish) - ) - - self._planner.start() - - @rpc - def stop(self) -> None: - self.cancel_goal() - self._planner.stop() - - super().stop() - - def _on_stop_movement(self, msg: Bool) -> None: - if msg.data: - self.cancel_goal() - - @rpc - def set_goal(self, goal: PoseStamped, profile: str | None = None) -> bool: - self._planner.handle_goal_request(goal, run_profile=profile) - return True - - @rpc - def set_run_profile(self, profile: str) -> bool: - """Set the session-default movement envelope for subsequent goals. - - Validated against the run-profile registry so a bad name cannot poison - the planner config. - """ - try: - GO2_RUN_PROFILES.get(profile) - except RunProfileError as exc: - logger.warning("Rejected run profile.", profile=profile, reason=str(exc)) - return False - self._planner.set_run_profile(profile) - return True - - @rpc - def get_state(self) -> NavigationState: - return self._planner.get_state() - - @rpc - def is_goal_reached(self) -> bool: - return self._planner.is_goal_reached() - - @rpc - def cancel_goal(self) -> bool: - self._planner.cancel_goal() - return True - - @rpc - def set_replanning_enabled(self, enabled: bool) -> None: - self._planner.set_replanning_enabled(enabled) - - @rpc - def set_safe_goal_clearance(self, clearance: float) -> None: - self._planner.set_safe_goal_clearance(clearance) - - @rpc - def reset_safe_goal_clearance(self) -> None: - self._planner.reset_safe_goal_clearance() diff --git a/dimos/navigation/dannav/path_clearance.py b/dimos/navigation/dannav/path_clearance.py deleted file mode 100644 index 232c5e905b..0000000000 --- a/dimos/navigation/dannav/path_clearance.py +++ /dev/null @@ -1,125 +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. - -from threading import RLock -import math - -import numpy as np -from numpy.typing import NDArray - -from dimos.core.global_config import GlobalConfig -from dimos.mapping.occupancy.path_mask import make_path_mask -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path - -# Minimum corridor check length (walk-speed baseline). -PATH_CLEARANCE_LOOKUP_DISTANCE_M = 3.0 -# Scale lookahead so faster profiles keep ~constant reaction time. -PATH_CLEARANCE_LOOKUP_TIME_S = 5.0 -# Treat elevated costmap cells as blocking (matches navigation gradient threshold). -PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD = 50 - - -def path_clearance_lookup_distance_m(speed_m_s: float) -> float: - """Return speed-scaled path-corridor check distance in meters.""" - if not math.isfinite(speed_m_s) or speed_m_s <= 0.0: - return PATH_CLEARANCE_LOOKUP_DISTANCE_M - return max(PATH_CLEARANCE_LOOKUP_DISTANCE_M, PATH_CLEARANCE_LOOKUP_TIME_S * speed_m_s) - - -class PathClearance: - _costmap: OccupancyGrid | None = None - _last_costmap: OccupancyGrid | None = None - _path_lookup_distance: float - _obstacle_cost_threshold: int = PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD - _max_distance_cache: float = 1.0 - _last_used_shape: tuple[int, ...] | None = None - _last_mask: NDArray[np.bool_] | None = None - _last_used_pose: int | None = None - _global_config: GlobalConfig - _lock: RLock - _path: Path - _pose_index: int - - def __init__(self, global_config: GlobalConfig, path: Path) -> None: - self._global_config = global_config - self._path = path - self._pose_index = 0 - self._lock = RLock() - self._path_lookup_distance = PATH_CLEARANCE_LOOKUP_DISTANCE_M - - def set_planner_speed(self, speed_m_s: float) -> None: - lookup_distance = path_clearance_lookup_distance_m(speed_m_s) - with self._lock: - if abs(lookup_distance - self._path_lookup_distance) > 0.01: - self._path_lookup_distance = lookup_distance - self._last_mask = None - self._last_used_pose = None - self._last_used_shape = None - - def update_costmap(self, costmap: OccupancyGrid) -> None: - with self._lock: - self._costmap = costmap - - def update_pose_index(self, index: int) -> None: - with self._lock: - self._pose_index = index - - @property - def mask(self) -> NDArray[np.bool_]: - with self._lock: - costmap = self._costmap - pose_index = self._pose_index - - assert costmap is not None - - if ( - self._last_mask is not None - and self._last_used_pose is not None - and costmap.grid.shape == self._last_used_shape - and self._pose_distance(self._last_used_pose, pose_index) < self._max_distance_cache - ): - return self._last_mask - - self._last_mask = make_path_mask( - occupancy_grid=costmap, - path=self._path, - robot_width=self._global_config.robot_width, - pose_index=pose_index, - max_length=self._path_lookup_distance, - ) - - self._last_used_shape = costmap.grid.shape - self._last_used_pose = pose_index - - return self._last_mask - - def is_obstacle_ahead(self) -> bool: - with self._lock: - costmap = self._costmap - - if costmap is None: - return True - - cells = costmap.grid[self.mask] - return bool( - np.any( - (cells >= self._obstacle_cost_threshold) & (cells != CostValues.UNKNOWN) - ) - ) - - def _pose_distance(self, index1: int, index2: int) -> float: - p1 = self._path.poses[index1].position - p2 = self._path.poses[index2].position - return p1.distance(p2) diff --git a/dimos/navigation/dannav/rotation_clearance.py b/dimos/navigation/dannav/rotation_clearance.py deleted file mode 100644 index f3ee839392..0000000000 --- a/dimos/navigation/dannav/rotation_clearance.py +++ /dev/null @@ -1,64 +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. - -"""Map-based checks for in-place rotation clearance in holonomic local planning.""" - -from __future__ import annotations - -import math - -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid - -PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD = 50 - - -def can_rotate_in_place( - costmap: OccupancyGrid, - pose: PoseStamped, - rotation_radius_m: float, - *, - obstacle_cost_threshold: int = PATH_CLEARANCE_OBSTACLE_COST_THRESHOLD, -) -> bool: - """Return True when a full in-place spin fits inside free map cells. - - Samples every grid cell whose center lies inside a circle of - ``rotation_radius_m`` around ``pose``. Any unknown or occupied cell - (cost >= ``obstacle_cost_threshold``) makes rotation unsafe. - """ - if rotation_radius_m <= 0.0 or not math.isfinite(rotation_radius_m): - return True - - grid_center = costmap.world_to_grid(pose.position) - center_x = int(round(float(grid_center.x))) - center_y = int(round(float(grid_center.y))) - radius_cells = int(math.ceil(rotation_radius_m / costmap.resolution)) - radius_cells_sq = radius_cells * radius_cells - - for dy in range(-radius_cells, radius_cells + 1): - for dx in range(-radius_cells, radius_cells + 1): - if dx * dx + dy * dy > radius_cells_sq: - continue - cell_x = center_x + dx - cell_y = center_y + dy - if not (0 <= cell_x < costmap.width and 0 <= cell_y < costmap.height): - return False - cell_value = int(costmap.grid[cell_y, cell_x]) - if cell_value == CostValues.UNKNOWN or cell_value >= obstacle_cost_threshold: - return False - - return True - - -__all__ = ["can_rotate_in_place"] diff --git a/dimos/navigation/dannav/test_local_planner_path_controller.py b/dimos/navigation/dannav/test_local_planner_path_controller.py deleted file mode 100644 index b2f557fb02..0000000000 --- a/dimos/navigation/dannav/test_local_planner_path_controller.py +++ /dev/null @@ -1,677 +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. - -"""P3-3: ``LocalPlanner`` path controller switch and holonomic integration smoke.""" - -from __future__ import annotations - -import math -from pathlib import Path as FsPath -import time -from typing import Any - -import numpy as np -import pytest - -from dimos.core.global_config import GlobalConfig -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.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.dannav.controllers import make_local_path_controller -from dimos.navigation.dannav.local_planner import LocalPlanner -from dimos.navigation.replanning_a_star.navigation_map import NavigationMap -from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import HolonomicPathController -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer -from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample -from dimos.utils.trigonometry import angle_diff - - -def _planar_speed_m_s(cmd: Twist) -> float: - return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) - - -def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) - - -def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: - return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), - ) - - -def _path_from_points(points: list[tuple[float, float]]) -> Path: - poses: list[PoseStamped] = [] - for index, point in enumerate(points): - if index + 1 < len(points): - next_point = points[index + 1] - yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) - else: - prev_point = points[index - 1] - yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) - poses.append(_pose_stamped(point[0], point[1], yaw)) - return Path(frame_id="map", poses=poses) - - -def _free_navigation_map(global_config: GlobalConfig) -> NavigationMap: - nav = NavigationMap(global_config, "gradient") - nav.update( - OccupancyGrid( - grid=np.zeros((200, 200), dtype=np.int8), - resolution=0.05, - origin=Pose(-2.0, -2.0, 0.0), - frame_id="map", - ts=1.0, - ) - ) - return nav - - -def _integrate_holonomic_pose( - x_m: float, - y_m: float, - yaw_rad: float, - cmd_body: Twist, - dt_s: float, -) -> tuple[float, float, float]: - """Integrate body-frame cmd_vel in the world frame (test harness only).""" - vx = float(cmd_body.linear.x) - vy = float(cmd_body.linear.y) - wz = float(cmd_body.angular.z) - c = math.cos(yaw_rad) - s = math.sin(yaw_rad) - x_m += (c * vx - s * vy) * dt_s - y_m += (s * vx + c * vy) * dt_s - yaw_rad += wz * dt_s - yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) - return x_m, y_m, yaw_rad - - -class _LocalPlannerHarnessResult: - def __init__( - self, - *, - command_history: list[Twist], - stop_messages: list[str], - final_x_m: float, - final_y_m: float, - final_yaw_rad: float, - ) -> None: - self.command_history = command_history - self.stop_messages = stop_messages - self.final_x_m = final_x_m - self.final_y_m = final_y_m - self.final_yaw_rad = final_yaw_rad - - -class _RecordingHolonomicTrackingController(HolonomicTrackingController): - def __init__(self) -> None: - super().__init__(k_position_per_s=0.0, k_yaw_per_s=0.0) - self.measurements: list[TrajectoryMeasuredSample] = [] - - def control( - self, - reference: TrajectoryReferenceSample, - measurement: TrajectoryMeasuredSample, - ) -> Twist: - self.measurements.append(measurement) - return super().control(reference, measurement) - - -def _local_planner_with_recording_holonomic_controller() -> tuple[ - LocalPlanner, _RecordingHolonomicTrackingController -]: - g = GlobalConfig(local_planner_path_controller="holonomic") - nav = NavigationMap(g, "gradient") - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) - lp._path = path - lp._path_distancer = PathDistancer(path) - controller = lp._controller - assert isinstance(controller, HolonomicPathController) - recorder = _RecordingHolonomicTrackingController() - controller._inner = recorder - return lp, recorder - - -def _run_local_planner_harness( - tmp_path: FsPath, - *, - points: list[tuple[float, float]], - initial_yaw_rad: float = 0.0, - speed_m_s: float = 1.0, - max_normal_accel_m_s2: float = 0.6, - max_tangent_accel_m_s2: float = 1.0, - max_goal_decel_m_s2: float = 1.0, - max_ticks: int = 260, -) -> _LocalPlannerHarnessResult: - rate_hz = 60.0 - dt_s = 1.0 / rate_hz - global_config = GlobalConfig( - local_planner_path_controller="holonomic", - local_planner_control_rate_hz=rate_hz, - planner_robot_speed=speed_m_s, - local_planner_max_normal_accel_m_s2=max_normal_accel_m_s2, - local_planner_max_tangent_accel_m_s2=max_tangent_accel_m_s2, - local_planner_goal_decel_m_s2=max_goal_decel_m_s2, - local_planner_max_planar_cmd_accel_m_s2=8.0, - local_planner_max_yaw_accel_rad_s2=8.0, - ) - planner = LocalPlanner(global_config, _free_navigation_map(global_config), goal_tolerance=0.08) - plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, initial_yaw_rad - latest_cmd = Twist() - command_history: list[Twist] = [] - stop_messages: list[str] = [] - - def _on_cmd_vel(cmd: Twist) -> None: - nonlocal latest_cmd - latest_cmd = Twist(cmd) - command_history.append(Twist(cmd)) - - cmd_sub = planner.cmd_vel.subscribe(_on_cmd_vel) - stop_sub = planner.stopped_navigating.subscribe(stop_messages.append) - sim_time_s = 1.0 - - try: - planner.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) - planner.start_planning(_path_from_points(points)) - for _ in range(max_ticks): - if "arrived" in stop_messages: - break - time.sleep(dt_s * 1.1) - plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( - plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s - ) - sim_time_s += dt_s - planner.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) - ) - finally: - planner.stop() - cmd_sub.dispose() - stop_sub.dispose() - - return _LocalPlannerHarnessResult( - command_history=command_history, - stop_messages=stop_messages, - final_x_m=plant_x_m, - final_y_m=plant_y_m, - final_yaw_rad=plant_yaw_rad, - ) - - -def test_holonomic_path_controller_slews_first_command_from_rest() -> None: - g = GlobalConfig(local_planner_path_controller="holonomic") - ctrl = HolonomicPathController( - g, - speed=0.55, - control_frequency=10.0, - k_position_per_s=2.0, - k_yaw_per_s=1.5, - ) - odom = PoseStamped(frame_id="map", position=[0.0, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)) - out = ctrl.advance(np.array([0.5, 0.0], dtype=np.float64), odom) - assert math.hypot(float(out.linear.x), float(out.linear.y)) == pytest.approx(0.5) - - -def test_local_planner_odom_delta_passes_estimated_measured_body_twist() -> None: - lp, recorder = _local_planner_with_recording_holonomic_controller() - lp._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) - lp._compute_path_following() - lp._current_odom = _pose_stamped(0.3, -0.1, 0.2, ts=1.5) - - lp._compute_path_following() - - measured = recorder.measurements[-1].twist_body - vx_w = 0.3 / 0.5 - vy_w = -0.1 / 0.5 - assert measured.linear.x == pytest.approx(math.cos(0.2) * vx_w + math.sin(0.2) * vy_w) - assert measured.linear.y == pytest.approx(-math.sin(0.2) * vx_w + math.cos(0.2) * vy_w) - assert measured.angular.z == pytest.approx(0.2 / 0.5) - - -def test_local_planner_rotation_passes_estimated_measured_body_twist() -> None: - lp, recorder = _local_planner_with_recording_holonomic_controller() - lp._current_odom = _pose_stamped(0.0, 0.0, 1.0, ts=1.0) - lp._compute_initial_rotation() - lp._current_odom = _pose_stamped(0.2, 0.1, 0.8, ts=1.4) - - lp._compute_initial_rotation() - - measured = recorder.measurements[-1].twist_body - vx_w = 0.2 / 0.4 - vy_w = 0.1 / 0.4 - assert measured.linear.x == pytest.approx(math.cos(0.8) * vx_w + math.sin(0.8) * vy_w) - assert measured.linear.y == pytest.approx(-math.sin(0.8) * vx_w + math.cos(0.8) * vy_w) - assert measured.angular.z == pytest.approx(angle_diff(0.8, 1.0) / 0.4) - - -def test_local_planner_invalid_dt_passes_zero_measured_body_twist() -> None: - lp, recorder = _local_planner_with_recording_holonomic_controller() - lp._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=5.0) - lp._compute_path_following() - lp._current_odom = _pose_stamped(0.2, 0.0, 0.0, ts=5.0) - - lp._compute_path_following() - - measured = recorder.measurements[-1].twist_body - assert measured.is_zero() - - -def test_local_planner_lookahead_reference_uses_path_end_progress() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - planner_robot_speed=0.5, - ) - nav = NavigationMap(g, "gradient") - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) - distancer = PathDistancer(path) - odom = _pose_stamped(0.2, 0.1, 0.0, ts=10.0) - current_pos = np.array([odom.position.x, odom.position.y], dtype=np.float64) - path_speed = lp._path_speed_for_index(distancer, 0, current_pos) - - reference = lp._lookahead_reference_sample( - distancer, - odom, - current_pos, - path_speed, - ) - - assert reference.pose_plan.position.x == pytest.approx(0.7) - assert reference.time_s == pytest.approx(11.0) - - -def test_local_planner_uses_curvature_speed_cap_for_holonomic_path() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - planner_robot_speed=1.2, - local_planner_max_normal_accel_m_s2=0.6, - ) - nav = NavigationMap(g, "gradient") - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = Path( - frame_id="map", - poses=[ - PoseStamped( - frame_id="map", - position=[0.0, 0.0, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ), - PoseStamped( - frame_id="map", - position=[0.5, 0.0, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ), - PoseStamped( - frame_id="map", - position=[0.5, 0.5, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ), - ], - ) - lp._path = path - lp._path_distancer = PathDistancer(path) - lp._current_odom = PoseStamped( - frame_id="map", - position=[0.5, 0.0, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ) - - lp._compute_path_following() - - assert isinstance(lp._controller, HolonomicPathController) - assert lp._controller._speed < 1.2 - - -def test_local_planner_uses_configured_goal_decel_for_path_speed_cap() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - planner_robot_speed=2.0, - local_planner_max_tangent_accel_m_s2=4.0, - local_planner_goal_decel_m_s2=0.5, - ) - nav = NavigationMap(g, "gradient") - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (1.0, 0.0)]) - distancer = PathDistancer(path) - current_pos = np.array([0.75, 0.0], dtype=np.float64) - - speed = lp._path_speed_for_index(distancer, 0, current_pos) - - assert speed == pytest.approx(0.5, abs=1e-3) - - -def test_local_planner_path_speed_uses_geometry_and_near_goal_decel_caps() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - planner_robot_speed=2.0, - local_planner_max_normal_accel_m_s2=0.5, - local_planner_goal_decel_m_s2=1.0, - ) - nav = NavigationMap(g, "gradient") - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (2.0, 1.0)]) - distancer = PathDistancer(path) - - corner_pos = np.array([1.0, 0.0], dtype=np.float64) - corner_index = distancer.find_closest_point_index(corner_pos) - corner_speed = lp._path_speed_for_index(distancer, corner_index, corner_pos) - - near_goal_pos = np.array([1.95, 1.0], dtype=np.float64) - near_goal_index = distancer.find_closest_point_index(near_goal_pos) - near_goal_speed = lp._path_speed_for_index(distancer, near_goal_index, near_goal_pos) - - right_angle_radius_m = math.sqrt(0.5) - geometry_cap_m_s = math.sqrt(g.local_planner_max_normal_accel_m_s2 * right_angle_radius_m) - goal_decel_cap_m_s = math.sqrt( - 2.0 * g.local_planner_goal_decel_m_s2 * distancer.distance_to_goal(near_goal_pos) - ) - assert corner_speed == pytest.approx(geometry_cap_m_s) - assert near_goal_speed == pytest.approx(goal_decel_cap_m_s, abs=1e-3) - assert near_goal_speed < corner_speed - - -def test_local_planner_path_speed_anticipates_corner_tangent_decel() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - planner_robot_speed=2.0, - local_planner_max_tangent_accel_m_s2=0.5, - local_planner_max_normal_accel_m_s2=0.1, - ) - nav = NavigationMap(g, "gradient") - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)]) - distancer = PathDistancer(path) - - before_corner_pos = np.array([0.85, 0.0], dtype=np.float64) - before_corner_speed = lp._path_speed_for_index( - distancer, - distancer.find_closest_point_index(before_corner_pos), - before_corner_pos, - ) - - assert before_corner_speed < g.planner_robot_speed - assert before_corner_speed < 1.0 - - -def test_local_planner_in_the_loop_harness_reaches_arrival_on_straight_line( - tmp_path: FsPath, -) -> None: - result = _run_local_planner_harness( - tmp_path, - points=[(0.1, 0.0), (1.2, 0.0)], - speed_m_s=1.0, - ) - - assert "arrived" in result.stop_messages - assert result.command_history - assert math.hypot(result.final_x_m - 1.2, result.final_y_m) < 0.15 - - -def test_local_planner_in_the_loop_harness_exercises_curvature_speed_cap( - tmp_path: FsPath, -) -> None: - result = _run_local_planner_harness( - tmp_path, - points=[(0.1, 0.0), (0.45, 0.0), (0.45, 0.45), (0.8, 0.45)], - speed_m_s=1.2, - max_normal_accel_m_s2=0.6, - max_ticks=320, - ) - - assert "arrived" in result.stop_messages - - -def test_local_planner_in_the_loop_harness_supports_2mps_right_angle_with_conservative_profile( - tmp_path: FsPath, -) -> None: - result = _run_local_planner_harness( - tmp_path, - points=[(0.1, 0.0), (0.55, 0.0), (0.55, 0.55), (0.9, 0.55)], - speed_m_s=2.0, - max_tangent_accel_m_s2=0.5, - max_normal_accel_m_s2=0.1, - max_goal_decel_m_s2=0.5, - max_ticks=420, - ) - commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] - - assert "arrived" in result.stop_messages - assert commanded_speeds - assert max(commanded_speeds) < 0.75 - - -def test_local_planner_in_the_loop_harness_decelerates_near_goal( - tmp_path: FsPath, -) -> None: - result = _run_local_planner_harness( - tmp_path, - points=[(0.1, 0.0), (1.0, 0.0)], - speed_m_s=1.2, - max_ticks=300, - ) - commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] - moving_speeds = [speed for speed in commanded_speeds if speed > 0.05] - - assert "arrived" in result.stop_messages - assert moving_speeds - assert max(moving_speeds[: len(moving_speeds) // 2]) > 0.7 - assert min(moving_speeds[-10:]) < 0.55 - assert min(moving_speeds[-10:]) < 0.6 * max(moving_speeds) - assert math.hypot(result.final_x_m - 1.0, result.final_y_m) < 0.15 - - -def test_local_planner_in_the_loop_harness_exercises_initial_rotation( - tmp_path: FsPath, -) -> None: - result = _run_local_planner_harness( - tmp_path, - points=[(0.1, 0.0), (1.0, 0.0)], - initial_yaw_rad=0.8, - speed_m_s=0.9, - max_ticks=320, - ) - - assert "arrived" in result.stop_messages - assert any( - abs(float(cmd.angular.z)) > 0.05 and _planar_speed_m_s(cmd) < 0.05 - for cmd in result.command_history[:20] - ) - assert abs(result.final_yaw_rad) < 0.35 - - -def test_make_local_path_controller_holonomic_velocity_damping_reduces_planar_command() -> None: - # Aligned pose so kp/ky contribute nothing; the only difference is the velocity - # damping gain wired from GlobalConfig through the factory into the inner law. - reference = TrajectoryReferenceSample( - time_s=1.0, - pose_plan=Pose(position=[0.0, 0.0, 0.0], orientation=_yaw_quaternion(0.0)), - twist_body=Twist(linear=[0.5, 0.2, 0.0]), - ) - odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) - measured_body_twist = Twist(linear=[1.1, 0.6, 0.0]) - - def _factory(k_velocity: float) -> Any: - return make_local_path_controller( - GlobalConfig( - local_planner_path_controller="holonomic", - local_planner_holonomic_kp=0.0, - local_planner_holonomic_ky=0.0, - local_planner_holonomic_kv=k_velocity, - local_planner_holonomic_kw=0.0, - # Generous slew headroom so the first-tick accel clamp does not mask the gain. - local_planner_max_planar_cmd_accel_m_s2=8.0, - ), - speed=1.0, - control_frequency=10.0, - ) - - undamped = _factory(0.0).advance_reference(reference, odom, measured_body_twist) - damped = _factory(0.5).advance_reference(reference, odom, measured_body_twist) - - assert undamped.linear.x == pytest.approx(0.5) - assert undamped.linear.y == pytest.approx(0.2) - # 0.5 * (ref - measured) damping pulls the command down to (0.2, 0.0). - assert damped.linear.x == pytest.approx(0.2) - assert damped.linear.y == pytest.approx(0.0) - assert math.hypot(damped.linear.x, damped.linear.y) < math.hypot( - undamped.linear.x, undamped.linear.y - ) - - -def _narrow_corridor_navigation_map(global_config: GlobalConfig) -> NavigationMap: - grid = np.zeros((80, 80), dtype=np.int8) - for row in range(grid.shape[0]): - world_y = -2.0 + row * 0.05 - if abs(world_y) > 0.15: - grid[row, :] = CostValues.OCCUPIED - nav = NavigationMap(global_config, "gradient") - nav.update( - OccupancyGrid( - grid=grid, - resolution=0.05, - origin=Pose(-2.0, -2.0, 0.0), - frame_id="map", - ts=1.0, - ) - ) - return nav - - -def test_holonomic_yaw_lock_reference_drives_along_backward_path() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - local_planner_holonomic_kp=2.0, - local_planner_holonomic_ky=1.0, - robot_rotation_diameter=0.6, - ) - nav = _narrow_corridor_navigation_map(g) - lp = LocalPlanner(g, nav, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (-0.8, 0.0)]) - distancer = PathDistancer(path) - odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) - current_pos = np.array([odom.position.x, odom.position.y], dtype=np.float64) - path_speed = lp._path_speed_for_index(distancer, 0, current_pos) - - reference = lp._lookahead_reference_sample( - distancer, - odom, - current_pos, - path_speed, - yaw_lock_rad=0.0, - ) - cmd = lp._controller.advance_reference( - reference, - odom, - Twist(), - ) - - assert reference.pose_plan.orientation.euler[2] == pytest.approx(0.0) - assert float(cmd.linear.x) < -0.05 - assert abs(float(cmd.angular.z)) < 0.05 - - -def test_local_planner_narrow_corridor_retreats_without_spinning(tmp_path: FsPath) -> None: - rate_hz = 60.0 - dt_s = 1.0 / rate_hz - global_config = GlobalConfig( - local_planner_path_controller="holonomic", - local_planner_control_rate_hz=rate_hz, - planner_robot_speed=0.45, - robot_width=0.25, - robot_rotation_diameter=0.6, - local_planner_holonomic_kp=2.0, - local_planner_holonomic_ky=1.0, - local_planner_max_planar_cmd_accel_m_s2=8.0, - local_planner_max_yaw_accel_rad_s2=8.0, - ) - planner = LocalPlanner( - global_config, - _narrow_corridor_navigation_map(global_config), - goal_tolerance=0.08, - ) - plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 - latest_cmd = Twist() - command_history: list[Twist] = [] - stop_messages: list[str] = [] - - def _on_cmd_vel(cmd: Twist) -> None: - nonlocal latest_cmd - latest_cmd = Twist(cmd) - command_history.append(Twist(cmd)) - - cmd_sub = planner.cmd_vel.subscribe(_on_cmd_vel) - stop_sub = planner.stopped_navigating.subscribe(stop_messages.append) - sim_time_s = 1.0 - - try: - planner.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) - planner.start_planning(_path_from_points([(0.0, 0.0), (-0.8, 0.0)])) - for _ in range(420): - if "arrived" in stop_messages: - break - time.sleep(dt_s * 1.1) - plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( - plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s - ) - sim_time_s += dt_s - planner.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) - ) - finally: - planner.stop() - cmd_sub.dispose() - stop_sub.dispose() - - assert "arrived" in stop_messages - assert plant_x_m < -0.55 - assert abs(plant_yaw_rad) < 0.2 - assert all(abs(float(cmd.angular.z)) < 0.12 for cmd in command_history[:40]) - - -def test_make_local_path_controller_differential_ignores_measured_body_twist() -> None: - # The differential branch returns a PController, which has no velocity-damping term; - # large kv/kw in config must not change its output when measured twist is supplied. - controller = make_local_path_controller( - GlobalConfig( - local_planner_path_controller="differential", - local_planner_holonomic_kv=5.0, - local_planner_holonomic_kw=5.0, - ), - speed=1.0, - control_frequency=10.0, - ) - reference = TrajectoryReferenceSample( - time_s=1.0, - pose_plan=Pose(position=[1.0, 0.0, 0.0], orientation=_yaw_quaternion(0.0)), - twist_body=Twist(linear=[1.0, 0.0, 0.0]), - ) - odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) - measured_body_twist = Twist(linear=[1.1, 0.6, 0.0], angular=[0.0, 0.0, 0.9]) - - without_measurement = controller.advance_reference(reference, odom) - with_measurement = controller.advance_reference(reference, odom, measured_body_twist) - - assert math.hypot(without_measurement.linear.x, without_measurement.linear.y) > 0.0 - assert with_measurement.linear.x == pytest.approx(without_measurement.linear.x) - assert with_measurement.linear.y == pytest.approx(without_measurement.linear.y) - assert with_measurement.angular.z == pytest.approx(without_measurement.angular.z) diff --git a/dimos/navigation/dannav/test_local_planner_run_envelope.py b/dimos/navigation/dannav/test_local_planner_run_envelope.py deleted file mode 100644 index a682e61279..0000000000 --- a/dimos/navigation/dannav/test_local_planner_run_envelope.py +++ /dev/null @@ -1,272 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Run-profile speed application in the local planner.""" - -from __future__ import annotations - -import math -from pathlib import Path as FsPath -import time - -import numpy as np -import pytest - -from dimos.core.global_config import GlobalConfig -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.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.dannav.local_planner import LocalPlanner -from dimos.navigation.replanning_a_star.navigation_map import NavigationMap -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer - - -def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) - - -def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: - return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), - ) - - -def _path_from_points(points: list[tuple[float, float]]) -> Path: - poses: list[PoseStamped] = [] - for index, point in enumerate(points): - if index + 1 < len(points): - next_point = points[index + 1] - yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) - else: - prev_point = points[index - 1] - yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) - poses.append(_pose_stamped(point[0], point[1], yaw)) - return Path(frame_id="map", poses=poses) - - -def _free_navigation_map(global_config: GlobalConfig) -> NavigationMap: - nav = NavigationMap(global_config, "gradient") - nav.update( - OccupancyGrid( - grid=np.zeros((240, 240), dtype=np.int8), - resolution=0.05, - origin=Pose(-2.0, -2.0, 0.0), - frame_id="map", - ts=1.0, - ) - ) - return nav - - -def _straight_points(length_m: float, spacing_m: float = 0.1) -> list[tuple[float, float]]: - n = int(round(length_m / spacing_m)) - return [(i * spacing_m, 0.0) for i in range(n + 1)] - - -def _right_angle_points( - approach_m: float, exit_m: float, spacing_m: float = 0.1 -) -> list[tuple[float, float]]: - points = _straight_points(approach_m, spacing_m) - n_exit = int(round(exit_m / spacing_m)) - points.extend((approach_m, i * spacing_m) for i in range(1, n_exit + 1)) - return points - - -def _make_planner(g: GlobalConfig, *, goal_tolerance: float = 0.2) -> LocalPlanner: - return LocalPlanner(g, _free_navigation_map(g), goal_tolerance=goal_tolerance) - - -def _integrate_holonomic_pose( - x_m: float, - y_m: float, - yaw_rad: float, - cmd_body: Twist, - dt_s: float, -) -> tuple[float, float, float]: - """Integrate body-frame cmd_vel in the world frame (test harness only).""" - vx = float(cmd_body.linear.x) - vy = float(cmd_body.linear.y) - wz = float(cmd_body.angular.z) - c = math.cos(yaw_rad) - s = math.sin(yaw_rad) - x_m += (c * vx - s * vy) * dt_s - y_m += (s * vx + c * vy) * dt_s - yaw_rad += wz * dt_s - yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) - return x_m, y_m, yaw_rad - - -def _start_and_stop(lp: LocalPlanner, path: Path, run_profile_name: str | None = None) -> None: - """Start a goal, then stop and wait for the planner thread to exit.""" - lp.start_planning(path, run_profile_name=run_profile_name) - thread = lp._thread - lp.stop_planning() - if thread is not None: - thread.join(2.0) - - -def test_walk_default_keeps_legacy_speed() -> None: - """With the default profile, short sharp paths still plan, and the path speed - comes from planner_robot_speed exactly as before.""" - g = GlobalConfig( - local_planner_path_controller="holonomic", - planner_robot_speed=1.0, - ) - lp = _make_planner(g) - lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=1.0)) - stops: list[str] = [] - sub = lp.stopped_navigating.subscribe(stops.append) - - try: - _start_and_stop(lp, _path_from_points(_right_angle_points(0.3, 0.2))) - finally: - sub.dispose() - lp.stop() - - assert "run_envelope_rejected" not in stops - - distancer = PathDistancer(_path_from_points(_straight_points(6.0))) - mid_pos = np.array([3.0, 0.0], dtype=np.float64) - speed = lp._path_speed_for_index(distancer, distancer.find_closest_point_index(mid_pos), mid_pos) - assert speed == pytest.approx(1.0) - - -def test_run_conservative_drives_planner_speed_and_command_caps() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - go2_run_profile="run_conservative", - ) - lp = _make_planner(g) - lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=time.time())) - stops: list[str] = [] - sub = lp.stopped_navigating.subscribe(stops.append) - path = _path_from_points(_straight_points(6.0)) - - try: - _start_and_stop(lp, path) - finally: - sub.dispose() - lp.stop() - - assert "run_envelope_rejected" not in stops - - distancer = PathDistancer(path) - mid_pos = np.array([3.0, 0.0], dtype=np.float64) - near_goal_pos = np.array([5.7, 0.0], dtype=np.float64) - mid_speed = lp._path_speed_for_index( - distancer, distancer.find_closest_point_index(mid_pos), mid_pos - ) - near_goal_speed = lp._path_speed_for_index( - distancer, distancer.find_closest_point_index(near_goal_pos), near_goal_pos - ) - - assert mid_speed == pytest.approx(1.5) - assert near_goal_speed == pytest.approx(math.sqrt(2.0 * 1.5 * 0.3)) - - -def test_run_profile_speed_respects_global_nerf() -> None: - g = GlobalConfig( - local_planner_path_controller="holonomic", - go2_run_profile="run_conservative", - nerf_speed=0.5, - ) - lp = _make_planner(g) - lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=time.time())) - path = _path_from_points(_straight_points(6.0)) - - try: - _start_and_stop(lp, path) - finally: - lp.stop() - - distancer = PathDistancer(path) - mid_pos = np.array([3.0, 0.0], dtype=np.float64) - speed = lp._path_speed_for_index(distancer, distancer.find_closest_point_index(mid_pos), mid_pos) - assert speed == pytest.approx(0.75) - - -def test_per_goal_override_applies_and_does_not_leak_to_next_goal() -> None: - g = GlobalConfig(local_planner_path_controller="holonomic") - lp = _make_planner(g) - lp.handle_odom(_pose_stamped(0.0, 0.0, 0.0, ts=time.time())) - path = _path_from_points(_straight_points(6.0)) - distancer = PathDistancer(path) - mid_pos = np.array([3.0, 0.0], dtype=np.float64) - mid_index = distancer.find_closest_point_index(mid_pos) - - try: - _start_and_stop(lp, path, run_profile_name="trot") - trot_speed = lp._path_speed_for_index(distancer, mid_index, mid_pos) - - _start_and_stop(lp, path) - default_speed = lp._path_speed_for_index(distancer, mid_index, mid_pos) - finally: - lp.stop() - - assert trot_speed == pytest.approx(1.0) - assert default_speed == pytest.approx(0.55) - - -def test_run_profile_commands_faster_than_walk_in_closed_loop() -> None: - rate_hz = 60.0 - dt_s = 1.0 / rate_hz - g = GlobalConfig( - local_planner_path_controller="holonomic", - local_planner_control_rate_hz=rate_hz, - go2_run_profile="run_conservative", - ) - planner = LocalPlanner(g, _free_navigation_map(g), goal_tolerance=0.1) - plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 - latest_cmd = Twist() - commanded_speeds: list[float] = [] - stops: list[str] = [] - - def _on_cmd_vel(cmd: Twist) -> None: - nonlocal latest_cmd - latest_cmd = Twist(cmd) - commanded_speeds.append(math.hypot(float(cmd.linear.x), float(cmd.linear.y))) - - cmd_sub = planner.cmd_vel.subscribe(_on_cmd_vel) - stop_sub = planner.stopped_navigating.subscribe(stops.append) - - try: - planner.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=time.time()) - ) - planner.start_planning(_path_from_points([(0.1, 0.0), (3.1, 0.0)])) - for _ in range(420): - if stops: - break - time.sleep(dt_s * 1.1) - plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( - plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s - ) - planner.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=time.time()) - ) - finally: - planner.stop() - cmd_sub.dispose() - stop_sub.dispose() - - assert "arrived" in stops - assert commanded_speeds - assert max(commanded_speeds) > 1.0 - assert max(commanded_speeds) <= 1.5 + 1e-6 diff --git a/dimos/navigation/dannav/test_rotation_clearance.py b/dimos/navigation/dannav/test_rotation_clearance.py deleted file mode 100644 index 9fd04256b2..0000000000 --- a/dimos/navigation/dannav/test_rotation_clearance.py +++ /dev/null @@ -1,72 +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. - -from __future__ import annotations - -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.navigation.dannav.rotation_clearance import can_rotate_in_place - - -def _pose_at(x: float, y: float) -> PoseStamped: - return PoseStamped(frame_id="map", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) - - -def test_can_rotate_in_place_on_open_map() -> None: - grid = np.zeros((80, 80), dtype=np.int8) - costmap = OccupancyGrid( - grid=grid, - resolution=0.05, - origin=Pose(-2.0, -2.0, 0.0), - frame_id="map", - ts=1.0, - ) - - assert can_rotate_in_place(costmap, _pose_at(0.0, 0.0), rotation_radius_m=0.3) - - -def test_can_rotate_in_place_rejects_narrow_corridor() -> None: - grid = np.zeros((80, 80), dtype=np.int8) - # Corridor width 0.30 m (robot can translate, 0.6 m rotation disk cannot fit). - for row in range(grid.shape[0]): - world_y = -2.0 + row * 0.05 - if abs(world_y) > 0.15: - grid[row, :] = CostValues.OCCUPIED - - costmap = OccupancyGrid( - grid=grid, - resolution=0.05, - origin=Pose(-2.0, -2.0, 0.0), - frame_id="map", - ts=1.0, - ) - - assert not can_rotate_in_place(costmap, _pose_at(0.0, 0.0), rotation_radius_m=0.3) - - -def test_can_rotate_in_place_rejects_unknown_cells() -> None: - grid = np.zeros((40, 40), dtype=np.int8) - grid[20, 21] = CostValues.UNKNOWN - costmap = OccupancyGrid( - grid=grid, - resolution=0.05, - origin=Pose(-1.0, -1.0, 0.0), - frame_id="map", - ts=1.0, - ) - - assert not can_rotate_in_place(costmap, _pose_at(0.0, 0.0), rotation_radius_m=0.1) diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py index 362d205159..473c22cb6c 100644 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py +++ b/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py @@ -15,7 +15,7 @@ """Speed vs arc length for planar polylines. Builds a scalar speed profile along a polyline using per-sample geometry caps -and a forward-backward pass on arc length (``v^2 <= v_0^2 + 2 a \\Delta s``). +and a forward-backward pass on arc length (``v^2 <= v_0^2 + 2 a Delta s``). - **Straight segments:** cap is ``limits.max_speed_m_s``. - **Corners:** cap is ``min(max_speed_m_s, sqrt(max_normal_accel_m_s2 * |R|))`` diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index cf5d57c6d6..6f88d412b8 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -114,7 +114,6 @@ "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", - "unitree-go2-dannav": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_dannav:unitree_go2_dannav", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet", "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", @@ -155,7 +154,7 @@ "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", "dan-holonomic-tc": "dimos.navigation.holonomic_trajectory_controller.module.DanHolonomicTC", - "dannav-planner": "dimos.navigation.dannav.module.DannavPlanner", + "dan-local-planner": "dimos.navigation.dannav.local_planner.module.DanLocalPlanner", "demo-calculator-skill": "dimos.agents.skills.demo_calculator_skill.DemoCalculatorSkill", "demo-monitoring": "dimos.agents.demos.demo_capabilities.DemoMonitoring", "demo-robot": "dimos.agents.skills.demo_robot.DemoRobot", diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py index 2e22eaa153..0f5c119066 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -29,6 +29,7 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.mapping.voxels import VoxelGridMapper +from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.holonomic_trajectory_controller.module import DanHolonomicTC from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay, PoseOdomRelay @@ -37,9 +38,9 @@ from dimos.robot.unitree.go2.connection import GO2Connection from dimos.visualization.vis_module import vis_module -voxel_size = 0.08 +voxel_size = 0.05 # Height of the head-mounted lidar above the ground while standing. -go2_lidar_height = 0.5 +go2_lidar_height = 1.0 def _render_global_map(msg: Any) -> Any: @@ -58,12 +59,15 @@ def _render_path(msg: Any) -> Any: **rerun_config, "max_hz": { **rerun_config["max_hz"], - "world/global_map": 1.0, + "world/global_map": 0, }, - "memory_limit": "2096MB", + "memory_limit": "8192MB", "visual_override": { **rerun_config["visual_override"], "world/global_map": _render_global_map, + # MLS path is remapped to planner_path for DanLocalPlanner; suppress the + # hot re-rooted stream so rerun only shows the gated path on world/path. + "world/planner_path": None, "world/path": _render_path, "world/surface_map": None, "world/nodes": None, @@ -79,7 +83,6 @@ def _render_path(msg: Any) -> Any: VoxelGridMapper.blueprint( voxel_size=voxel_size, frame_id="world", - carve_columns=False, emit_every=1, ), MLSPlannerNative.blueprint( @@ -92,8 +95,9 @@ def _render_path(msg: Any) -> Any: step_threshold_m=0.16, step_penalty_weight=1.0, viz_publish_hz=0.0, - ), + ).remappings([(MLSPlannerNative, "path", "planner_path")]), GoalRelay.blueprint(), + DanLocalPlanner.blueprint(lock_replan=3.0), DanHolonomicTC.blueprint(), MovementManager.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py deleted file mode 100644 index dd2a29bac8..0000000000 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_dannav.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Go2 nav stack with holonomic trajectory control and hot replanning. - -Uses ``DannavPlanner`` with ``replan_on_costmap_update=True``. For the default -differential stack see ``unitree_go2``. -""" - -from dimos.core.coordination.blueprints import autoconnect -from dimos.mapping.costmapper import CostMapper -from dimos.mapping.pointclouds.occupancy import GeneralOccupancyConfig -from dimos.mapping.voxels import VoxelGridMapper -from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( - WavefrontFrontierExplorer, -) -from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.navigation.patrolling.module import PatrollingModule -from dimos.navigation.dannav.module import DannavPlanner -from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic - -unitree_go2_dannav = autoconnect( - unitree_go2_basic, - VoxelGridMapper.blueprint(emit_every=1), - CostMapper.blueprint( - algo="general", - config=GeneralOccupancyConfig( - resolution=0.05, - min_height=0.08, - max_height=2.0, - mark_free_radius=0.2, - ), - ), - DannavPlanner.blueprint( - replan_on_costmap_update=True, - ), - MovementManager.blueprint(), -).global_config(n_workers=10, robot_model="unitree_go2", robot_width=0.25) - -__all__ = ["unitree_go2_dannav"] From 5d377cebe1855bd400f677b27e9f17e8dc3c1553 Mon Sep 17 00:00:00 2001 From: danvi Date: Thu, 2 Jul 2026 20:55:36 +0800 Subject: [PATCH 5/9] refactor: reorganize and simplify holonomic trajectory controller stack Move holonomic_tc under dannav, relocate shared geometry helpers, strip stale trajectory logging and run-profile code, and tighten controller types, configs, tests, and unitree_go2_mls_htc blueprint wiring. --- dimos/core/global_config.py | 26 - .../dannav/geometry/path_distancer.py | 194 ++++++ .../geometry/path_speed_profile.py} | 134 ++--- .../geometry/test_path_speed_profile.py | 86 +++ .../holonomic_tc/command_limits.py} | 11 +- .../dannav/holonomic_tc/docs/run_profiles.md | 111 ++++ .../holonomic_path_controller.py | 96 +-- .../holonomic_tracking_controller.py} | 9 +- .../holonomic_tc}/module.py | 220 +------ .../holonomic_tc/run_profiles.py} | 61 +- .../holonomic_tc/test_command_limits.py | 103 ++++ .../holonomic_tc}/test_dan_holonomic_tc.py | 116 ++-- .../test_holonomic_path_follower.py | 224 +++++++ .../test_holonomic_tracking_controller.py | 148 +++++ .../dannav/holonomic_tc/test_run_envelope.py | 130 ++++ .../holonomic_tc/types.py} | 29 +- .../navigation/dannav/local_planner/module.py | 38 +- .../local_planner/test_dan_local_planner.py | 324 +--------- .../docs/plot_trajectory_control_ticks.py | 540 ----------------- .../docs/trajectory_control_tick_jsonl.md | 78 --- .../docs/trajectory_run_profiles.md | 66 --- .../path_distancer.py | 131 ----- .../test_holonomic_path_follower.py | 555 ------------------ .../test_run_envelope.py | 204 ------- .../test_trajectory_command_limits.py | 209 ------- ...rajectory_holonomic_tracking_controller.py | 210 ------- .../test_trajectory_path_speed_profile.py | 157 ----- .../test_trajectory_run_profiles.py | 124 ---- .../trajectory_control_tick_export.py | 71 --- .../trajectory_control_tick_log.py | 204 ------- .../trajectory_metrics.py | 274 --------- .../nav_3d/mls_planner/goal_relay.py | 28 - dimos/robot/all_blueprints.py | 3 +- .../navigation/unitree_go2_mls_htc.py | 33 +- docs/usage/configuration.md | 1 - 35 files changed, 1221 insertions(+), 3727 deletions(-) create mode 100644 dimos/navigation/dannav/geometry/path_distancer.py rename dimos/navigation/{holonomic_trajectory_controller/trajectory_path_speed_profile.py => dannav/geometry/path_speed_profile.py} (66%) create mode 100644 dimos/navigation/dannav/geometry/test_path_speed_profile.py rename dimos/navigation/{holonomic_trajectory_controller/trajectory_command_limits.py => dannav/holonomic_tc/command_limits.py} (93%) create mode 100644 dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md rename dimos/navigation/{holonomic_trajectory_controller => dannav/holonomic_tc}/holonomic_path_controller.py (58%) rename dimos/navigation/{holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py => dannav/holonomic_tc/holonomic_tracking_controller.py} (93%) rename dimos/navigation/{holonomic_trajectory_controller => dannav/holonomic_tc}/module.py (76%) rename dimos/navigation/{holonomic_trajectory_controller/trajectory_run_profiles.py => dannav/holonomic_tc/run_profiles.py} (62%) create mode 100644 dimos/navigation/dannav/holonomic_tc/test_command_limits.py rename dimos/navigation/{holonomic_trajectory_controller => dannav/holonomic_tc}/test_dan_holonomic_tc.py (60%) create mode 100644 dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py create mode 100644 dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py create mode 100644 dimos/navigation/dannav/holonomic_tc/test_run_envelope.py rename dimos/navigation/{holonomic_trajectory_controller/trajectory_types.py => dannav/holonomic_tc/types.py} (78%) delete mode 100644 dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md delete mode 100644 dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md delete mode 100644 dimos/navigation/holonomic_trajectory_controller/path_distancer.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py delete mode 100644 dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index e5e205cb91..06aa6ed2f9 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -13,9 +13,7 @@ # limitations under the License. import re -from typing import Literal -from pydantic import Field, FiniteFloat from pydantic_settings import BaseSettings, SettingsConfigDict from dimos.constants import DEFAULT_BUILD_NATIVE @@ -65,30 +63,6 @@ class GlobalConfig(BaseSettings): robot_width: float = 0.3 robot_rotation_diameter: float = 0.6 nerf_speed: float = 1.0 - # Local path follower in ``replanning_a_star.LocalPlanner`` (issue 921 / P3-3). - local_planner_path_controller: Literal["differential", "holonomic"] = "holonomic" - local_planner_holonomic_kp: float = 2.0 - local_planner_holonomic_ky: float = 1.5 - local_planner_holonomic_kv: FiniteFloat = Field(default=0.0, ge=0.0) - local_planner_holonomic_kw: FiniteFloat = Field(default=0.0, ge=0.0) - local_planner_max_tangent_accel_m_s2: float = Field(default=1.0, gt=0.0) - local_planner_max_normal_accel_m_s2: float = Field(default=0.6, gt=0.0) - local_planner_goal_decel_m_s2: float = Field(default=1.0, gt=0.0) - local_planner_max_planar_cmd_accel_m_s2: float = Field(default=5.0, gt=0.0) - local_planner_max_yaw_accel_rad_s2: float = Field(default=5.0, gt=0.0) - local_planner_max_yaw_rate_rad_s: float | None = Field(default=None, gt=0.0) - # Issue 921 P4-1: one knob for LocalPlanner sleep pacing and controller dt (e.g. PD). - local_planner_control_rate_hz: float = Field(default=10.0, ge=0.1, le=60.0) - # Optional issue 921 JSONL telemetry export. Set to a file path for live speed-vs-divergence logs. - local_planner_trajectory_tick_log_path: str | None = None - planner_robot_speed: float | None = None - # Session-default movement envelope (named run profile from - # ``dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles.GO2_RUN_PROFILES``: walk, trot, - # run_conservative, run_verified). The Go2 locomotion mode (default vs rage) - # is derived from the profile's required_locomotion_mode at connection - # start-up, so navigation/agentic blueprints activate the run mode from config - # instead of only the rage keyboard-teleop blueprint. Default stays "walk". - go2_run_profile: str = "walk" mcp_port: int = 9990 build_native: bool = DEFAULT_BUILD_NATIVE dtop: bool = False diff --git a/dimos/navigation/dannav/geometry/path_distancer.py b/dimos/navigation/dannav/geometry/path_distancer.py new file mode 100644 index 0000000000..8b10287fa0 --- /dev/null +++ b/dimos/navigation/dannav/geometry/path_distancer.py @@ -0,0 +1,194 @@ +# 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. + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import cast + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs.Path import Path + + +@dataclass(frozen=True) +class PolylineProjection: + """Foot of a query point on a polyline, in the polyline (map) frame. + + - ``foot_xy``: closest point on the polyline to the query point. + - ``segment_start_index``: index ``i`` of the segment ``[i, i + 1]`` holding + the foot. + - ``tangent_yaw``: heading of that segment, ``atan2(dy, dx)`` in radians. + - ``s_along_path_m``: arc length from the polyline start to the foot. + - ``signed_cross_track_m``: lateral offset from the foot to the query point, + positive when the point is left of the segment direction. + """ + + foot_xy: tuple[float, float] + segment_start_index: int + tangent_yaw: float + s_along_path_m: float + signed_cross_track_m: float + + +def project_to_polyline(x: float, y: float, polyline_xy: NDArray[np.float64]) -> PolylineProjection: + """Project a point onto a ``(N, 2)`` polyline in the map frame. + + Answers "given this fixed polyline, what arc-length s is the robot at, + and how far off to the side," returning a PolylineProjection + """ + if polyline_xy.ndim != 2 or polyline_xy.shape[1] != 2: + raise ValueError("polyline_xy must have shape (N, 2)") + n = polyline_xy.shape[0] + if n == 0: + raise ValueError("polyline_xy must be non-empty") + if n == 1: + fx = float(polyline_xy[0, 0]) + fy = float(polyline_xy[0, 1]) + lat = float(math.hypot(x - fx, y - fy)) + return PolylineProjection( + (fx, fy), + 0, + 0.0, + 0.0, + lat, + ) + + p = np.array([x, y], dtype=np.float64) + starts = polyline_xy[:-1] + seg_vecs = polyline_xy[1:] - starts + seg_len2 = np.einsum("ij,ij->i", seg_vecs, seg_vecs) + seg_lens = np.sqrt(seg_len2) + + prefix_s = np.zeros(n, dtype=np.float64) + np.cumsum(seg_lens, out=prefix_s[1:]) + + # Foot on each segment: projection parameter clipped to the segment, with + # zero-length segments pinned to their start point (t=0). + nondegenerate = seg_len2 > 1e-18 + t_params = np.zeros(n - 1, dtype=np.float64) + t_params[nondegenerate] = ( + np.einsum("ij,ij->i", (p - starts)[nondegenerate], seg_vecs[nondegenerate]) + / seg_len2[nondegenerate] + ) + np.clip(t_params, 0.0, 1.0, out=t_params) + feet = starts + t_params[:, None] * seg_vecs + d2 = np.einsum("ij,ij->i", p - feet, p - feet) + + best_seg = int(np.argmin(d2)) + best_foot = feet[best_seg] + best_t = float(t_params[best_seg]) + best_seg_len = float(seg_lens[best_seg]) if nondegenerate[best_seg] else 0.0 + s_along = float(prefix_s[best_seg] + best_t * best_seg_len) + + a = polyline_xy[best_seg] + b = polyline_xy[best_seg + 1] + ab = b - a + seg_len = float(np.linalg.norm(ab)) + if seg_len < 1e-9: + yaw = 0.0 + tx, ty = 1.0, 0.0 + else: + tx, ty = float(ab[0] / seg_len), float(ab[1] / seg_len) + yaw = math.atan2(ab[1], ab[0]) + nx, ny = -ty, tx + vx = x - float(best_foot[0]) + vy = y - float(best_foot[1]) + signed_ct = vx * nx + vy * ny + return PolylineProjection( + (float(best_foot[0]), float(best_foot[1])), + best_seg, + yaw, + s_along, + float(signed_ct), + ) + + +class PathDistancer: + """Arc-length queries over a fixed ``Path`` polyline. + + Built once per path: ``_path`` holds the pose xy as an ``(N, 2)`` array and + ``_cumulative_dists`` the cumulative arc length after each segment (length + ``N - 1``, so ``_cumulative_dists[-1]`` is the total path length). + ``_lookahead_dist`` is the fixed lookahead in metres used by the follower. + + Query methods run against that cached geometry: ``point_at_progress`` and + ``yaw_at_progress`` map an arc length to a position and heading, ``project`` + maps a position to its foot on the path, and ``distance_to_goal`` gives the + straight-line distance to the final pose. + """ + + _lookahead_dist: float = 0.5 + _path: NDArray[np.float64] + _cumulative_dists: NDArray[np.float64] + + def __init__(self, path: Path) -> None: + self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) + self._cumulative_dists = _make_cumulative_distance_array(self._path) + + @property + def lookahead_distance_m(self) -> float: + return self._lookahead_dist + + @property + def path_length_m(self) -> float: + if len(self._path) < 2: + return 0.0 + return float(self._cumulative_dists[-1]) + + def point_at_progress(self, progress_m: float) -> NDArray[np.float64]: + if len(self._path) == 0: + raise ValueError("path must be non-empty") + if len(self._path) == 1: + return cast("NDArray[np.float64]", self._path[0].copy()) + + s = float(np.clip(progress_m, 0.0, self.path_length_m)) + idx = int(np.searchsorted(self._cumulative_dists, s)) + if idx >= len(self._cumulative_dists): + return cast("NDArray[np.float64]", self._path[-1].copy()) + + prev_s = self._cumulative_dists[idx - 1] if idx > 0 else 0.0 + segment_s = self._cumulative_dists[idx] - prev_s + if segment_s <= 1e-12: + return cast("NDArray[np.float64]", self._path[idx].copy()) + alpha = (s - prev_s) / segment_s + point = self._path[idx] + alpha * (self._path[idx + 1] - self._path[idx]) + return cast("NDArray[np.float64]", point) + + def yaw_at_progress(self, progress_m: float) -> float: + if len(self._path) < 2: + return 0.0 + s = float(np.clip(progress_m, 0.0, self.path_length_m)) + idx = int(np.searchsorted(self._cumulative_dists, s)) + idx = min(max(idx, 0), len(self._path) - 2) + direction = self._path[idx + 1] - self._path[idx] + return float(np.arctan2(direction[1], direction[0])) + + def project(self, pos: NDArray[np.float64]) -> PolylineProjection: + return project_to_polyline(float(pos[0]), float(pos[1]), self._path) + + def distance_to_goal(self, current_pos: NDArray[np.float64]) -> float: + return float(np.linalg.norm(self._path[-1] - current_pos)) + + +def _make_cumulative_distance_array(array: NDArray[np.float64]) -> NDArray[np.float64]: + """For a 2D point array, return cumulative arc length at each vertex.""" + if len(array) < 2: + return np.array([0.0]) + + segments = array[1:] - array[:-1] + segment_dists = np.linalg.norm(segments, axis=1) + return np.cumsum(segment_dists) diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py b/dimos/navigation/dannav/geometry/path_speed_profile.py similarity index 66% rename from dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py rename to dimos/navigation/dannav/geometry/path_speed_profile.py index 473c22cb6c..683fa5bd3e 100644 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_path_speed_profile.py +++ b/dimos/navigation/dannav/geometry/path_speed_profile.py @@ -27,7 +27,6 @@ from __future__ import annotations -import bisect from collections.abc import Sequence from dataclasses import dataclass import math @@ -54,21 +53,6 @@ def __post_init__(self) -> None: raise ValueError(f"{name} must be a non-negative finite float, got {value!r}") -def _circular_arc_geometry_speed_cap_m_s( - abs_radius_m: float, limits: PathSpeedProfileLimits -) -> float: - """Upper speed from centripetal bound ``v^2 / R <= a_n`` and ``max_speed_m_s``.""" - if not math.isfinite(abs_radius_m) or abs_radius_m <= 0.0: - raise ValueError(f"abs_radius_m must be finite and positive, got {abs_radius_m!r}") - v_curve = math.sqrt(limits.max_normal_accel_m_s2 * abs_radius_m) - return min(limits.max_speed_m_s, v_curve) - - -def _line_segment_geometry_speed_cap_m_s(limits: PathSpeedProfileLimits) -> float: - """Geometric cap for a straight segment (no curvature binding).""" - return float(limits.max_speed_m_s) - - def _vertex_progress_along_polyline_m( cumulative_segment_s_m: NDArray[np.float64], ) -> NDArray[np.float64]: @@ -78,35 +62,70 @@ def _vertex_progress_along_polyline_m( return vertex_s -def _polyline_geometry_speed_cap_m_s( +def _polyline_geometry_speed_caps_m_s( path_xy: NDArray[np.float64], - vertex_s_m: Sequence[float], - progress_m: float, + vertex_s_m: NDArray[np.float64], + s_profile: Sequence[float], limits: PathSpeedProfileLimits, -) -> float: - """Geometry speed cap at arc length ``progress_m`` on a planar polyline.""" - if len(path_xy) < 3: - return _line_segment_geometry_speed_cap_m_s(limits) - - for i in range(1, len(path_xy) - 1): - vertex_s = float(vertex_s_m[i]) - prev_s = float(vertex_s_m[i - 1]) - next_s = float(vertex_s_m[i + 1]) - local_scale = max(vertex_s - prev_s, next_s - vertex_s, 1.0) - if abs(float(progress_m) - vertex_s) > max(1e-9, local_scale * 1e-9): - continue - p0, p1, p2 = path_xy[i - 1], path_xy[i], path_xy[i + 1] - a = float(np.linalg.norm(p1 - p0)) - b = float(np.linalg.norm(p2 - p1)) - c = float(np.linalg.norm(p2 - p0)) - v10 = p1 - p0 - v20 = p2 - p0 - area2 = abs(float(v10[0] * v20[1] - v10[1] * v20[0])) - if min(a, b, c, area2) <= 1e-9: - return _line_segment_geometry_speed_cap_m_s(limits) - radius = (a * b * c) / (2.0 * area2) - return _circular_arc_geometry_speed_cap_m_s(radius, limits) - return _line_segment_geometry_speed_cap_m_s(limits) +) -> list[float]: + """Geometry speed cap at every arc length in ``s_profile`` on a planar polyline. + + The cap is ``max_speed_m_s`` everywhere except at interior vertices, where a + corner binds it via the centripetal bound ``v^2 / R <= a_n`` to + ``sqrt(max_normal_accel_m_s2 * R)``. ``R = a b c / (2 * area)`` is the + circumradius of the local vertex triangle. A degenerate triangle + (near-collinear or a zero-length side) leaves the cap at ``max_speed_m_s``. + + Corner caps depend only on the fixed vertices, so they are computed once for + all interior vertices and scattered onto the matching ``s_profile`` samples + (each vertex arc length is itself a sample). Every other sample keeps the + straight-segment cap ``max_speed_m_s``. + """ + max_cap = float(limits.max_speed_m_s) + s_arr = np.asarray(s_profile, dtype=np.float64) + caps = np.full(s_arr.shape[0], max_cap, dtype=np.float64) + if len(path_xy) < 3 or s_arr.shape[0] == 0: + return caps.tolist() + + p0 = path_xy[:-2] + p1 = path_xy[1:-1] + p2 = path_xy[2:] + side_a = np.linalg.norm(p1 - p0, axis=1) + side_b = np.linalg.norm(p2 - p1, axis=1) + side_c = np.linalg.norm(p2 - p0, axis=1) + v10 = p1 - p0 + v20 = p2 - p0 + area2 = np.abs(v10[:, 0] * v20[:, 1] - v10[:, 1] * v20[:, 0]) + + degenerate = np.minimum(np.minimum(side_a, side_b), np.minimum(side_c, area2)) <= 1e-9 + with np.errstate(divide="ignore", invalid="ignore"): + radius = (side_a * side_b * side_c) / (2.0 * area2) + v_curve = np.sqrt(limits.max_normal_accel_m_s2 * radius) + corner = np.where(degenerate, max_cap, np.minimum(max_cap, v_curve)) + + vertex_s = np.asarray(vertex_s_m, dtype=np.float64) + interior_s = vertex_s[1:-1] + local_scale = np.maximum( + np.maximum(interior_s - vertex_s[:-2], vertex_s[2:] - interior_s), 1.0 + ) + tol = np.maximum(1e-9, local_scale * 1e-9) + + # Each interior vertex arc length is present as a sample, so match every + # vertex to the nearest sample; the match is exact within ``tol``. + m = s_arr.shape[0] + idx = np.searchsorted(s_arr, interior_s, side="left") + left = np.clip(idx - 1, 0, m - 1) + right = np.clip(idx, 0, m - 1) + dl = np.abs(s_arr[left] - interior_s) + dr = np.abs(s_arr[right] - interior_s) + nearest = np.where(dl <= dr, left, right) + within = np.minimum(dl, dr) <= tol + + # Lowest vertex index wins a shared sample: assign high-to-low so index 0 is + # written last (matches the original first-match-wins scan over vertices). + order = np.nonzero(within)[0][::-1] + caps[nearest[order]] = corner[order] + return caps.tolist() def _profile_sample_distances_m( @@ -182,9 +201,7 @@ def profile_speed_along_polyline( vertex_s_m = _vertex_progress_along_polyline_m(cumulative_segment_s_m) total_length_m = float(vertex_s_m[-1]) s_profile = _profile_sample_distances_m(total_length_m, vertex_s_m, num_samples) - caps = [ - _polyline_geometry_speed_cap_m_s(path_xy, vertex_s_m, s, limits) for s in s_profile - ] + caps = _polyline_geometry_speed_caps_m_s(path_xy, vertex_s_m, s_profile, limits) v_profile = _speed_profile_from_geometry_caps( s_profile, caps, @@ -201,27 +218,4 @@ def speed_at_progress_m( """Linearly interpolate profile speed at arc length ``progress_m``.""" if not s_m: return 0.0 - if len(s_m) == 1: - return float(v_m_s[0]) - - s = float(np.clip(progress_m, s_m[0], s_m[-1])) - if s <= s_m[0]: - return float(v_m_s[0]) - if s >= s_m[-1]: - return float(v_m_s[-1]) - - idx = bisect.bisect_right(s_m, s) - 1 - idx = max(0, min(idx, len(s_m) - 2)) - s0, s1 = float(s_m[idx]), float(s_m[idx + 1]) - v0, v1 = float(v_m_s[idx]), float(v_m_s[idx + 1]) - if s1 <= s0: - return v0 - u = (s - s0) / (s1 - s0) - return v0 + (v1 - v0) * u - - -__all__ = [ - "PathSpeedProfileLimits", - "profile_speed_along_polyline", - "speed_at_progress_m", -] + return float(np.interp(progress_m, s_m, v_m_s)) diff --git a/dimos/navigation/dannav/geometry/test_path_speed_profile.py b/dimos/navigation/dannav/geometry/test_path_speed_profile.py new file mode 100644 index 0000000000..1df47639d2 --- /dev/null +++ b/dimos/navigation/dannav/geometry/test_path_speed_profile.py @@ -0,0 +1,86 @@ +# 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. + +"""``profile_speed_along_polyline`` branch coverage in one chain. + +Curvature and goal decel show up in closed-loop ``cmd_vel`` via +``test_holonomic_path_follower.py``; here only profile shape decisions not +reduced to a single max-speed bound: cruise plateau, zero at ends, corner cap and +braking order along arc length. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from dimos.navigation.dannav.geometry.path_speed_profile import ( + PathSpeedProfileLimits, + profile_speed_along_polyline, + speed_at_progress_m, +) + + +def _straight_path(length_m: float) -> tuple[np.ndarray, np.ndarray]: + path_xy = np.array([[0.0, 0.0], [length_m, 0.0]], dtype=np.float64) + return path_xy, np.array([length_m], dtype=np.float64) + + +def _cumulative(path_xy: np.ndarray) -> np.ndarray: + segments = path_xy[1:] - path_xy[:-1] + return np.cumsum(np.linalg.norm(segments, axis=1)) + + +def test_profile_speed_respects_cruise_corners_and_goal_endpoints() -> None: + cruise_limits = PathSpeedProfileLimits( + max_speed_m_s=2.0, + max_tangent_accel_m_s2=1.0, + max_normal_accel_m_s2=10.0, + ) + straight_xy, straight_cum = _straight_path(10.0) + _, straight_v = profile_speed_along_polyline( + straight_xy, + straight_cum, + cruise_limits, + goal_decel_m_s2=cruise_limits.max_tangent_accel_m_s2, + num_samples=2001, + start_speed_m_s=0.0, + ) + assert straight_v[0] == pytest.approx(0.0) + assert straight_v[-1] == pytest.approx(0.0) + assert max(straight_v) == pytest.approx(cruise_limits.max_speed_m_s, abs=0.02) + + corner_limits = PathSpeedProfileLimits( + max_speed_m_s=2.0, + max_tangent_accel_m_s2=0.5, + max_normal_accel_m_s2=0.1, + ) + corner_xy = np.array( + [[0.0, 0.0], [1.0, 0.0], [1.0, 1.0], [2.0, 1.0]], + dtype=np.float64, + ) + corner_cum = _cumulative(corner_xy) + s_profile, v_profile = profile_speed_along_polyline( + corner_xy, + corner_cum, + corner_limits, + goal_decel_m_s2=0.5, + num_samples=200, + ) + cruise_speed = speed_at_progress_m(0.4, s_profile, v_profile) + before_corner_speed = speed_at_progress_m(0.85, s_profile, v_profile) + at_corner_speed = speed_at_progress_m(1.0, s_profile, v_profile) + + assert cruise_speed > before_corner_speed > at_corner_speed + assert at_corner_speed < corner_limits.max_speed_m_s diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py b/dimos/navigation/dannav/holonomic_tc/command_limits.py similarity index 93% rename from dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py rename to dimos/navigation/dannav/holonomic_tc/command_limits.py index 10943ed384..337c74cad3 100644 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_command_limits.py +++ b/dimos/navigation/dannav/holonomic_tc/command_limits.py @@ -14,9 +14,8 @@ """Holonomic body-frame command limits and saturation. -Limits apply to the same planar ``cmd_vel`` convention as -``trajectory_metrics.commanded_planar_speed`` (``linear.x``, ``linear.y`` in -the body frame) and to yaw rate ``angular.z``. +Limits apply to planar ``cmd_vel`` (``linear.x``, ``linear.y`` in the body +frame) and to yaw rate ``angular.z``. Non-planar components (``linear.z``, ``angular.x``, ``angular.y``) are not slew-limited here; they are passed through from ``raw_cmd`` so a future @@ -116,9 +115,3 @@ def clamp_holonomic_cmd_vel( wz, ), ) - - -__all__ = [ - "HolonomicCommandLimits", - "clamp_holonomic_cmd_vel", -] diff --git a/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md b/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md new file mode 100644 index 0000000000..e91be7f02e --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md @@ -0,0 +1,111 @@ +# Operator run-profile contract + +## Purpose + +Named movement envelopes (`walk`, `trot`, `run_conservative`) bundle +cruise speed and limit caps in one registry entry. `DanHolonomicTC` is the only live +consumer: it resolves a profile name into path-speed limits and command saturation +limits on each follow. + +Registry data and validation live in +`dimos.navigation.dannav.holonomic_tc.run_profiles`. +Tuning limits means editing `GO2_RUN_PROFILES` there, or picking a different profile +name at deploy time. + +## What is wired today + + +| Surface | How you set the profile | Status | Code | +| ------------------------------------- | --------------------------------------------------------------------------- | -------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------- | +| Blueprint | `DanHolonomicTC.blueprint(run_profile="walk")` on the `autoconnect` chain | Wired at deploy | e.g. `unitree_go2_mls_htc.py` | +| CLI | `dimos run -o danholonomictc.run_profile=trot` | Wired at deploy | `dimos/robot/cli/dimos.py` (`load_config_args` -> module `config`) | +| Module default | Omit `run_profile`; `DanHolonomicTCConfig` defaults to `"walk"` | Wired | `module.py` (`DanHolonomicTCConfig.run_profile`) | +| Live RPC | `DanHolonomicTC.set_run_profile("trot")` on a running module | Wired in module; no production caller yet | `module.py` (`set_run_profile` RPC -> `_HolonomicPathFollower.set_run_profile`) | +| Cruise override | `DanHolonomicTC.blueprint(speed_m_s=1.0)` or `danholonomictc.speed_m_s=1.0` | Wired; overrides profile cruise only, not accel/yaw caps | `module.py` (`_cruise_speed_override` in `_profile_run_envelope`) | +| Agent skills (`relative_move`, etc.) | N/A | Not wired to run profiles | Skills use `NavigationInterface`; MLS+HTC stack uses click goals via `MovementManager` | +| MCP tools | N/A | Not wired to run profiles | No MCP tool calls `set_run_profile` | +| `GO2_RUN_PROFILE` env var | N/A | Removed | Was `GlobalConfig.go2_run_profile`; use blueprint / CLI module config instead | +| Go2 locomotion mode | `GO2Connection.blueprint(motion_mode="mcf")` | Separate from run profiles | e.g. `unitree_go2_mls_htc.py`; not driven by `run_profile` | + + + + +### Limit flow (when wired) + +1. `DanHolonomicTCConfig.run_profile` names a registry entry. +2. `_HolonomicPathFollower._resolve_run_envelope()` calls `GO2_RUN_PROFILES.get(name)`. +3. `_apply_run_envelope()` sets path-speed limits and passes the `RunProfile` into + `HolonomicPathController` (`set_profile`, `set_speed`). +4. Each control tick: path reference speed from the profile's path limits; `cmd_vel` + slew from the profile's yaw/accel caps (`holonomic_path_controller.py`). + + + +## Data model - `RunProfile` + +All numeric fields are **upper bounds in SI units** in the same conventions as +the live limit types (`HolonomicCommandLimits`, `PathSpeedProfileLimits`): +planar speed is `hypot(vx, vy)` in the body frame; yaw rate is `wz`. + + +| Field | Unit | Meaning | +| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | +| `name` | str | Profile identity (registry key must match). | +| `requested_planner_speed_m_s` | m/s | Requested cruise speed (still curvature/decel-capped downstream). | +| `max_tangent_accel_m_s2` | m/s² | Along-path acceleration cap for the speed profile. | +| `max_normal_accel_m_s2` | m/s² | Centripetal (curvature) acceleration cap. | +| `goal_decel_m_s2` | m/s² | Deceleration approaching the goal. | +| `max_planar_cmd_accel_m_s2` | m/s² | Command slew cap on planar `cmd_vel`. | +| `max_yaw_rate_rad_s` | rad/s | Yaw-rate cap. | +| `max_yaw_accel_rad_s2` | rad/s² | Yaw-acceleration cap. | + + +Adapter onto the existing validated limit type: + +- `path_speed_profile_limits_at(max_speed_m_s) -> PathSpeedProfileLimits` + + + +### Validation (rejected at construction) + +- Every speed/acceleration/yaw field must be **finite and strictly positive**. +- `name` must be non-empty. + + + +## Profile resolution + +`GO2_RUN_PROFILES.get(name)` looks up a profile by name. Unknown names raise +`RunProfileError` with a message listing known profiles. + +The session profile is `DanHolonomicTCConfig.run_profile`, resolved in +`_HolonomicPathFollower._resolve_run_envelope` at module construction. The +`set_run_profile` RPC re-resolves and applies a new profile live. The registry is +the single envelope source: even `walk` reads its caps from `GO2_RUN_PROFILES`, +not from removed `GlobalConfig.local_planner_*` fields. + +## Go2 profiles (`GO2_RUN_PROFILES`) + +Caps are **conservative nominal engineering envelopes, not measured hardware +performance**. + + +| Profile | Cruise speed (m/s) | +| ------------------ | ------------------ | +| `walk` | 0.55 | +| `trot` | 1.0 | +| `run_conservative` | 1.5 | + + +Example blueprint line: + +```python +DanHolonomicTC.blueprint(run_profile="walk"), +``` + +Example CLI override: + +```bash +dimos run unitree-go2-mls-htc -o danholonomictc.run_profile=trot +``` + diff --git a/dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py similarity index 58% rename from dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py rename to dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py index 9e375e15b2..8796fe78f8 100644 --- a/dimos/navigation/holonomic_trajectory_controller/holonomic_path_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py @@ -12,10 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dataclasses import dataclass - import numpy as np -from numpy.typing import NDArray from dimos.core.global_config import GlobalConfig from dimos.msgs.geometry_msgs.Pose import Pose @@ -23,13 +20,19 @@ 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.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( +from dimos.navigation.dannav.holonomic_tc.command_limits import ( HolonomicCommandLimits, clamp_holonomic_cmd_vel, ) -from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import RunProfile -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.navigation.dannav.holonomic_tc.holonomic_tracking_controller import ( + HolonomicTrackingController, +) +from dimos.navigation.dannav.holonomic_tc.run_profiles import RunProfile +from dimos.navigation.dannav.holonomic_tc.types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) +from dimos.utils.transform_utils import normalize_angle def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: @@ -43,32 +46,8 @@ def _pose_from_pose_stamped(odom: PoseStamped) -> Pose: return Pose(odom.position, odom.orientation) -@dataclass(frozen=True) -class CommandEnvelopeOverrides: - """Run-profile command caps that replace the GlobalConfig defaults. - - The planar speed cap keeps tracking ``set_speed`` (the geometry-capped - path speed); these are the remaining saturation limits a movement - envelope owns. - """ - - max_yaw_rate_rad_s: float - max_planar_cmd_accel_m_s2: float - max_yaw_accel_rad_s2: float - - -def command_envelope_overrides_for_profile(profile: RunProfile) -> CommandEnvelopeOverrides: - """Map a :class:`RunProfile` to planner command caps (planar speed excluded).""" - limits = profile.command_limits() - return CommandEnvelopeOverrides( - max_yaw_rate_rad_s=limits.max_yaw_rate_rad_s, - max_planar_cmd_accel_m_s2=limits.max_planar_linear_accel_m_s2, - max_yaw_accel_rad_s2=limits.max_yaw_accel_rad_s2, - ) - - class HolonomicPathController: - """Follow path segments using the holonomic tracking law (P3-3, issue 921). + """Follow path segments using the holonomic tracking law. Wraps :class:`HolonomicTrackingController` in the :class:`Controller` seam (lookahead + odom). Rotations in place use the same law with a fixed @@ -78,6 +57,7 @@ class HolonomicPathController: def __init__( self, global_config: GlobalConfig, + profile: RunProfile, speed: float, control_frequency: float, k_position_per_s: float, @@ -86,6 +66,7 @@ def __init__( k_yaw_rate_per_s: float = 0.0, ) -> None: self._global_config = global_config + self._profile = profile self._speed = float(speed) self._control_frequency = float(control_frequency) self._inner = HolonomicTrackingController( @@ -94,7 +75,6 @@ def __init__( k_velocity_per_s=k_velocity_per_s, k_yaw_rate_per_s=k_yaw_rate_per_s, ) - self._envelope_overrides: CommandEnvelopeOverrides | None = None self._limits = self._make_limits() self._inner.configure(self._limits) self._previous_cmd = Twist() @@ -104,52 +84,21 @@ def set_speed(self, speed_m_s: float) -> None: self._limits = self._make_limits() self._inner.configure(self._limits) - def set_command_envelope(self, overrides: CommandEnvelopeOverrides | None) -> None: - """Apply (or clear, with ``None``) a run profile's command caps.""" - self._envelope_overrides = overrides + def set_profile(self, profile: RunProfile) -> None: + """Apply a run profile's command saturation caps.""" + self._profile = profile self._limits = self._make_limits() self._inner.configure(self._limits) def _make_limits(self) -> HolonomicCommandLimits: - overrides = self._envelope_overrides - if overrides is not None: - return HolonomicCommandLimits( - max_planar_speed_m_s=self._speed, - max_yaw_rate_rad_s=overrides.max_yaw_rate_rad_s, - max_planar_linear_accel_m_s2=overrides.max_planar_cmd_accel_m_s2, - max_yaw_accel_rad_s2=overrides.max_yaw_accel_rad_s2, - ) - max_yaw_rate = self._global_config.local_planner_max_yaw_rate_rad_s + profile = self._profile return HolonomicCommandLimits( max_planar_speed_m_s=self._speed, - max_yaw_rate_rad_s=self._speed if max_yaw_rate is None else float(max_yaw_rate), - max_planar_linear_accel_m_s2=self._global_config.local_planner_max_planar_cmd_accel_m_s2, - max_yaw_accel_rad_s2=self._global_config.local_planner_max_yaw_accel_rad_s2, + max_yaw_rate_rad_s=profile.max_yaw_rate_rad_s, + max_planar_linear_accel_m_s2=profile.max_planar_cmd_accel_m_s2, + max_yaw_accel_rad_s2=profile.max_yaw_accel_rad_s2, ) - def advance( - self, - lookahead_point: NDArray[np.float64], - current_odom: PoseStamped, - measured_body_twist: Twist | None = None, - ) -> Twist: - current_pos = np.array([float(current_odom.position.x), float(current_odom.position.y)]) - direction = np.asarray(lookahead_point, dtype=np.float64) - current_pos - distance = float(np.linalg.norm(direction)) - - if distance < 1e-6 or not np.isfinite(distance): - return Twist() - - ref_yaw = float(np.arctan2(direction[1], direction[0])) - ref_pose = _pose_from_xy_yaw(float(lookahead_point[0]), float(lookahead_point[1]), ref_yaw) - # Feedforward along the reference heading in the body frame of the target pose. - ref_ff = Twist( - linear=Vector3(self._speed, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - ref = TrajectoryReferenceSample(0.0, ref_pose, ref_ff) - return self.advance_reference(ref, current_odom, measured_body_twist) - def advance_reference( self, reference: TrajectoryReferenceSample, @@ -179,7 +128,7 @@ def rotate( return self._limit_output(self._apply_sim_angular(t)) robot_yaw = float(current_odom.orientation.euler[2]) - target_yaw = float(np.arctan2(np.sin(robot_yaw + yaw_error), np.cos(robot_yaw + yaw_error))) + target_yaw = float(normalize_angle(robot_yaw + yaw_error)) p = _pose_from_xy_yaw( float(current_odom.position.x), float(current_odom.position.y), @@ -195,9 +144,6 @@ def reset_errors(self) -> None: self._inner.reset() self._previous_cmd = Twist() - def reset_yaw_error(self, value: float) -> None: - del value - def _apply_sim_angular(self, t: Twist) -> Twist: wz = float(t.angular.z) if self._global_config.simulation and 1e-9 < abs(wz) < 0.8: diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py similarity index 93% rename from dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py rename to dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py index 0516b33670..320a8ef679 100644 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_holonomic_tracking_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py @@ -17,7 +17,7 @@ Cartesian law in the plan frame: position error ``(p_ref - p_meas)`` is rotated into the measured body frame, then a proportional correction is added to the reference body ``Twist`` (feedforward). Heading uses the same -``angle_diff`` convention as ``trajectory_metrics.pose_errors_vs_reference``. +``angle_diff`` convention used elsewhere in navigation. This is a standard omnidirectional tracking law, not a path-curvature or lookahead car-style law (no Pure Pursuit). @@ -29,8 +29,8 @@ from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import HolonomicCommandLimits -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.navigation.dannav.holonomic_tc.command_limits import HolonomicCommandLimits +from dimos.navigation.dannav.holonomic_tc.types import TrajectoryMeasuredSample, TrajectoryReferenceSample from dimos.utils.trigonometry import angle_diff @@ -161,6 +161,3 @@ def control( return raw out = _scale_planar_twist(raw, lim.max_planar_speed_m_s) return _clamp_yaw_rate(out, lim.max_yaw_rate_rad_s) - - -__all__ = ["HolonomicTrackingController"] diff --git a/dimos/navigation/holonomic_trajectory_controller/module.py b/dimos/navigation/dannav/holonomic_tc/module.py similarity index 76% rename from dimos/navigation/holonomic_trajectory_controller/module.py rename to dimos/navigation/dannav/holonomic_tc/module.py index ce14f4e6c6..e3f81d0856 100644 --- a/dimos/navigation/holonomic_trajectory_controller/module.py +++ b/dimos/navigation/dannav/holonomic_tc/module.py @@ -41,39 +41,27 @@ from dimos.core.global_config import GlobalConfig from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -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 -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.base import NavigationState -from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import ( - CommandEnvelopeOverrides, - HolonomicPathController, - command_envelope_overrides_for_profile, -) -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer -from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export import ( - JsonlTrajectoryControlTickSink, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import ( - TrajectoryControlTickSink, - append_trajectory_control_tick, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( +from dimos.navigation.dannav.geometry.path_distancer import PathDistancer +from dimos.navigation.dannav.geometry.path_speed_profile import ( PathSpeedProfileLimits, profile_speed_along_polyline, speed_at_progress_m, ) -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( +from dimos.navigation.dannav.holonomic_tc.holonomic_path_controller import ( + HolonomicPathController, + _pose_from_xy_yaw, +) +from dimos.navigation.dannav.holonomic_tc.run_profiles import ( GO2_RUN_PROFILES, RunProfile, RunProfileError, ) -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import ( - TrajectoryMeasuredSample, +from dimos.navigation.dannav.holonomic_tc.types import ( TrajectoryReferenceSample, ) from dimos.utils.logging_config import setup_logger @@ -92,19 +80,12 @@ @dataclass(frozen=True) class ActiveRunEnvelope: - """The movement envelope governing the current follow. + """Resolved movement envelope for the current session profile.""" - The speed is the session profile's requested speed (or the optional cruise - override) after any global slowdown scaling, and the limits come from the - profile. ``GO2_RUN_PROFILES`` is the single envelope source, so even the - default profile carries ``command_overrides``. - """ - - profile_name: str + profile: RunProfile speed_m_s: float path_limits: PathSpeedProfileLimits goal_decel_m_s2: float - command_overrides: CommandEnvelopeOverrides class _HolonomicPathFollower: @@ -113,27 +94,23 @@ class _HolonomicPathFollower: Owns the follow state machine, the holonomic tracking law, and the run-profile speed/accel envelope. Has no transport: ``DanHolonomicTC`` wires the ``path`` / ``odometry`` / ``stop_movement`` streams to its methods and - forwards ``cmd_vel`` / ``stopped_navigating``. Unit-testable directly, the - way the old ``LocalPlanner`` was. + forwards ``cmd_vel`` / ``stopped_navigating``. """ - cmd_vel: Subject # Subject[Twist] - stopped_navigating: Subject # Subject[StopMessage] + cmd_vel: Subject[Twist] + stopped_navigating: Subject[StopMessage] _thread: Thread | None = None _path: Path | None = None _path_distancer: PathDistancer | None = None _current_odom: PoseStamped | None = None - _pose_index: int _lock: RLock _stop_planning_event: Event _state: PlannerState - _state_unique_id: int _global_config: GlobalConfig _goal_tolerance: float _controller: HolonomicPathController - _trajectory_tick_sink: TrajectoryControlTickSink | None _previous_odom_for_velocity: PoseStamped | None _run_profile: str @@ -146,7 +123,6 @@ class _HolonomicPathFollower: _orientation_tolerance: float _align_heading_before_move: bool _align_goal_yaw: bool - _goal_reached: bool def __init__(self, config: DanHolonomicTCConfig) -> None: self.cmd_vel = Subject() @@ -154,19 +130,15 @@ def __init__(self, config: DanHolonomicTCConfig) -> None: self._config = config self._global_config = config.g - self._pose_index = 0 self._lock = RLock() self._stop_planning_event = Event() self._state = "idle" - self._state_unique_id = 0 - self._goal_reached = False self._goal_tolerance = float(config.goal_tolerance) self._orientation_tolerance = float(config.orientation_tolerance) self._control_frequency = float(config.control_frequency) self._align_heading_before_move = bool(config.align_heading_before_move) self._align_goal_yaw = bool(config.align_goal_yaw) self._run_profile = config.run_profile - self._trajectory_tick_sink = self._make_trajectory_tick_sink() self._previous_odom_for_velocity = None self._path_speed_profile_s = None self._path_speed_profile_v = None @@ -186,6 +158,7 @@ def __init__(self, config: DanHolonomicTCConfig) -> None: self._controller = HolonomicPathController( self._global_config, + envelope.profile, envelope.speed_m_s, self._control_frequency, k_position_per_s=config.k_position_per_s, @@ -195,7 +168,7 @@ def __init__(self, config: DanHolonomicTCConfig) -> None: ) self._apply_run_envelope(envelope) - # ---- lifecycle ---------------------------------------------------------- + # lifecycle def close(self) -> None: with self._lock: @@ -203,7 +176,6 @@ def close(self) -> None: self.stop_planning() if thread is not None: thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) - self._close_trajectory_tick_sink() def handle_odom(self, msg: PoseStamped) -> None: with self._lock: @@ -215,10 +187,8 @@ def start_planning(self, path: Path) -> None: self._stop_planning_event = Event() with self._lock: - self._goal_reached = False self._path = path self._path_distancer = PathDistancer(path) - self._pose_index = 0 self._previous_odom_for_velocity = None self._rebuild_path_speed_profile(self._path_distancer) self._thread = Thread(target=self._thread_entrypoint, daemon=True) @@ -235,10 +205,6 @@ def update_path(self, path: Path) -> bool: self._path = path self._path_distancer = PathDistancer(path) - current_odom = self._current_odom - if current_odom is not None: - current_pos = np.array([current_odom.position.x, current_odom.position.y]) - self._pose_index = self._path_distancer.find_closest_point_index(current_pos) self._rebuild_path_speed_profile(self._path_distancer) return True @@ -249,11 +215,10 @@ def stop_planning(self) -> None: with self._lock: self._thread = None - self._goal_reached = False self._reset_state() - # ---- run-profile envelope ---------------------------------------------- + # run-profile envelope def set_run_profile(self, profile: str) -> bool: """Set the session-default movement envelope and apply it live. @@ -279,14 +244,11 @@ def _profile_run_envelope(self, profile: RunProfile) -> ActiveRunEnvelope: if self._cruise_speed_override is not None else profile.requested_planner_speed_m_s ) - if self._global_config.nerf_speed < 1.0: - speed *= self._global_config.nerf_speed return ActiveRunEnvelope( - profile_name=profile.name, + profile=profile, speed_m_s=speed, path_limits=profile.path_speed_profile_limits_at(speed), goal_decel_m_s2=profile.goal_decel_m_s2, - command_overrides=command_envelope_overrides_for_profile(profile), ) def _resolve_run_envelope(self) -> ActiveRunEnvelope | None: @@ -310,29 +272,14 @@ def _resolve_run_envelope(self) -> ActiveRunEnvelope | None: def _apply_run_envelope(self, envelope: ActiveRunEnvelope) -> None: self._active_envelope = envelope - self._controller.set_command_envelope(envelope.command_overrides) + self._controller.set_profile(envelope.profile) self._controller.set_speed(envelope.speed_m_s) with self._lock: path_distancer = self._path_distancer if path_distancer is not None: self._rebuild_path_speed_profile(path_distancer) - # ---- tick logging ------------------------------------------------------- - - def _make_trajectory_tick_sink(self) -> TrajectoryControlTickSink | None: - path = self._config.trajectory_tick_log_path - if path is None or str(path).strip() == "": - return None - return JsonlTrajectoryControlTickSink(path) - - def _close_trajectory_tick_sink(self) -> None: - sink = self._trajectory_tick_sink - close = getattr(sink, "close", None) - if callable(close): - close() - self._trajectory_tick_sink = None - - # ---- introspection ------------------------------------------------------ + # introspection def get_state(self) -> NavigationState: with self._lock: @@ -346,11 +293,7 @@ def get_state(self) -> NavigationState: case _: raise ValueError(f"Unknown planner state: {state}") - def is_goal_reached(self) -> bool: - with self._lock: - return self._goal_reached - - # ---- control loop ------------------------------------------------------- + # control loop def _thread_entrypoint(self) -> None: try: @@ -367,7 +310,6 @@ def _change_state(self, new_state: PlannerState) -> None: if new_state == self._state: return self._state = new_state - self._state_unique_id += 1 logger.info("changed state", state=new_state) def _initial_state(self, path: Path, current_odom: PoseStamped | None) -> PlannerState: @@ -388,7 +330,6 @@ def _initial_state(self, path: Path, current_odom: PoseStamped | None) -> Planne first_yaw = path.poses[0].orientation.euler[2] robot_yaw = current_odom.orientation.euler[2] initial_yaw_error = angle_diff(first_yaw, robot_yaw) - self._controller.reset_yaw_error(initial_yaw_error) if abs(initial_yaw_error) < self._orientation_tolerance: return "path_following" return "initial_rotation" @@ -419,8 +360,6 @@ def _loop(self) -> None: elif state == "final_rotation": cmd_vel = self._compute_final_rotation() elif state == "arrived": - with self._lock: - self._goal_reached = True # Stop motion before signalling arrival, matching the path # followers downstream consumers expect. self.cmd_vel.on_next(Twist()) @@ -459,20 +398,7 @@ def _compute_initial_rotation(self) -> Twist: self._controller.set_speed(self._active_envelope.speed_m_s) measured_body_twist = self._estimate_measured_body_twist(current_odom) - cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) - ref_pose = _pose_from_xy_yaw( - float(current_odom.position.x), - float(current_odom.position.y), - float(first_yaw), - ) - self._append_trajectory_control_tick( - ref_pose, - Twist(), - current_odom, - measured_body_twist, - cmd, - ) - return cmd + return self._controller.rotate(yaw_error, current_odom, measured_body_twist) def _compute_path_following(self) -> Twist: with self._lock: @@ -495,12 +421,7 @@ def _compute_path_following(self) -> Twist: self._change_state("arrived") return Twist() - closest_index = path_distancer.find_closest_point_index(current_pos) - - with self._lock: - self._pose_index = closest_index - - path_speed = self._path_speed_for_index(path_distancer, closest_index, current_pos) + path_speed = self._path_speed_at_position(path_distancer, current_pos) self._controller.set_speed(path_speed) reference_sample = self._lookahead_reference_sample( path_distancer, @@ -509,18 +430,11 @@ def _compute_path_following(self) -> Twist: path_speed, ) measured_body_twist = self._estimate_measured_body_twist(current_odom) - cmd = self._controller.advance_reference( - reference_sample, - current_odom, - measured_body_twist, - ) - self._append_trajectory_control_sample( + return self._controller.advance_reference( reference_sample, current_odom, measured_body_twist, - cmd, ) - return cmd def _lookahead_reference_sample( self, @@ -569,13 +483,11 @@ def _reference_sample_at_progress( twist_body=feedforward, ) - def _path_speed_for_index( + def _path_speed_at_position( self, path_distancer: PathDistancer, - closest_index: int, current_pos: np.ndarray, ) -> float: - del closest_index self._ensure_path_speed_profile(path_distancer) envelope = self._active_envelope progress_m = float(path_distancer.project(current_pos).s_along_path_m) @@ -637,75 +549,17 @@ def _compute_final_rotation(self) -> Twist: self._controller.set_speed(self._active_envelope.speed_m_s) measured_body_twist = self._estimate_measured_body_twist(current_odom) - cmd = self._controller.rotate(yaw_error, current_odom, measured_body_twist) - ref_pose = _pose_from_xy_yaw( - float(current_odom.position.x), - float(current_odom.position.y), - float(goal_yaw), - ) - self._append_trajectory_control_tick( - ref_pose, - Twist(), - current_odom, - measured_body_twist, - cmd, - ) - return cmd + return self._controller.rotate(yaw_error, current_odom, measured_body_twist) def _reset_state(self) -> None: with self._lock: self._change_state("idle") self._path = None self._path_distancer = None - self._pose_index = 0 self._previous_odom_for_velocity = None self._controller.set_speed(self._active_envelope.speed_m_s) self._controller.reset_errors() - def _append_trajectory_control_tick( - self, - reference_pose: Pose, - reference_twist: Twist, - current_odom: PoseStamped, - measured_body_twist: Twist, - command: Twist, - ) -> None: - reference = TrajectoryReferenceSample( - time_s=float(current_odom.ts), - pose_plan=reference_pose, - twist_body=reference_twist, - ) - self._append_trajectory_control_sample( - reference, - current_odom, - measured_body_twist, - command, - ) - - def _append_trajectory_control_sample( - self, - reference: TrajectoryReferenceSample, - current_odom: PoseStamped, - measured_body_twist: Twist, - command: Twist, - ) -> None: - sink = self._trajectory_tick_sink - if sink is None: - return - measurement = TrajectoryMeasuredSample( - time_s=float(current_odom.ts), - pose_plan=Pose(current_odom.position, current_odom.orientation), - twist_body=measured_body_twist, - ) - append_trajectory_control_tick( - sink, - reference, - measurement, - command, - 1.0 / self._control_frequency, - wall_time_s=time.time(), - ) - def _estimate_measured_body_twist(self, current_odom: PoseStamped) -> Twist: previous = self._previous_odom_for_velocity self._previous_odom_for_velocity = current_odom @@ -734,16 +588,9 @@ def _estimate_measured_body_twist(self, current_odom: PoseStamped) -> Twist: ) -def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: - return Pose( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), - ) - - class DanHolonomicTCConfig(ModuleConfig): control_frequency: float = 10.0 - run_profile: str = "trot" + run_profile: str = "walk" speed_m_s: float | None = None goal_tolerance: float = 0.2 orientation_tolerance: float = 0.35 @@ -753,13 +600,12 @@ class DanHolonomicTCConfig(ModuleConfig): k_yaw_rate_per_s: float = 1.0 align_heading_before_move: bool = False align_goal_yaw: bool = False - trajectory_tick_log_path: str | None = None class DanHolonomicTC(Module): """Follow a planned ``Path`` with the holonomic tracking law. - Mirrors ``BasicPathFollower``'s stream surface. The planner owns route + Consumes the robot's ``PoseStamped`` odom directly. The planner owns route safety and emits the ``Path``: an empty path stops the follow, a non-empty path updates the active route or starts a new follow. Publishes ``nav_cmd_vel`` until the goal is within tolerance, then ``goal_reached``. @@ -769,7 +615,7 @@ class DanHolonomicTC(Module): config: DanHolonomicTCConfig path: In[Path] - odometry: In[Odometry] + odom: In[PoseStamped] stop_movement: In[Bool] nav_cmd_vel: Out[Twist] @@ -788,7 +634,7 @@ def start(self) -> None: self.register_disposable( self._core.stopped_navigating.subscribe(self._on_core_stopped) ) - self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) self.register_disposable(Disposable(self.path.subscribe(self._on_path))) if self.stop_movement.transport is not None: self.register_disposable(Disposable(self.stop_movement.subscribe(self._on_stop))) @@ -799,8 +645,8 @@ def stop(self) -> None: self.nav_cmd_vel.publish(Twist()) super().stop() - def _on_odometry(self, msg: Odometry) -> None: - self._core.handle_odom(msg.to_pose_stamped()) + def _on_odom(self, msg: PoseStamped) -> None: + self._core.handle_odom(msg) def _on_path(self, path: Path) -> None: # The planner owns path safety: it sends the route as far as it is safe, @@ -827,10 +673,6 @@ def set_run_profile(self, profile: str) -> bool: """Set the session-default movement envelope, applied live.""" return self._core.set_run_profile(profile) - @rpc - def is_goal_reached(self) -> bool: - return self._core.is_goal_reached() - @rpc def get_state(self) -> NavigationState: return self._core.get_state() diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py b/dimos/navigation/dannav/holonomic_tc/run_profiles.py similarity index 62% rename from dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py rename to dimos/navigation/dannav/holonomic_tc/run_profiles.py index d9bc690508..d202e82b15 100644 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_run_profiles.py +++ b/dimos/navigation/dannav/holonomic_tc/run_profiles.py @@ -14,8 +14,8 @@ """Named Go2 movement envelopes (speed and limit caps). -Data and validation only. Live wiring: ``GlobalConfig.go2_run_profile``, -``LocalPlanner._resolve_run_envelope``, and ``go2.connection`` locomotion mode. +Data and validation only. Live wiring: ``DanHolonomicTCConfig.run_profile`` and +``_HolonomicPathFollower._resolve_run_envelope``. """ from __future__ import annotations @@ -24,8 +24,7 @@ from dataclasses import dataclass import math -from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import HolonomicCommandLimits -from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import PathSpeedProfileLimits +from dimos.navigation.dannav.geometry.path_speed_profile import PathSpeedProfileLimits _POSITIVE_FIELDS: tuple[str, ...] = ( "requested_planner_speed_m_s", @@ -54,8 +53,6 @@ class RunProfile: max_planar_cmd_accel_m_s2: float max_yaw_rate_rad_s: float max_yaw_accel_rad_s2: float - required_locomotion_mode: str - description: str = "" def __post_init__(self) -> None: if not self.name.strip(): @@ -66,17 +63,6 @@ def __post_init__(self) -> None: raise RunProfileError( f"{self.name!r}.{field_name} must be a positive finite float, got {value!r}" ) - if not self.required_locomotion_mode.strip(): - raise RunProfileError(f"{self.name!r}.required_locomotion_mode must be non-empty") - - def command_limits(self) -> HolonomicCommandLimits: - """Body-frame command saturation envelope for this profile.""" - return HolonomicCommandLimits( - max_planar_speed_m_s=self.requested_planner_speed_m_s, - max_yaw_rate_rad_s=self.max_yaw_rate_rad_s, - max_planar_linear_accel_m_s2=self.max_planar_cmd_accel_m_s2, - max_yaw_accel_rad_s2=self.max_yaw_accel_rad_s2, - ) def path_speed_profile_limits_at(self, max_speed_m_s: float) -> PathSpeedProfileLimits: """Geometry-aware path speed limits at the given cruise cap (m/s).""" @@ -89,10 +75,9 @@ def path_speed_profile_limits_at(self, max_speed_m_s: float) -> PathSpeedProfile @dataclass(frozen=True) class RunProfileRegistry: - """Named run profiles with a declared default profile name.""" + """Named run profiles.""" profiles: Mapping[str, RunProfile] - default_profile_name: str def __post_init__(self) -> None: if not self.profiles: @@ -102,10 +87,6 @@ def __post_init__(self) -> None: raise RunProfileError( f"registry key {key!r} does not match profile name {profile.name!r}" ) - if self.default_profile_name not in self.profiles: - raise RunProfileError( - f"default profile {self.default_profile_name!r} is not in the registry" - ) object.__setattr__(self, "profiles", dict(self.profiles)) def get(self, name: str) -> RunProfile: @@ -116,13 +97,8 @@ def get(self, name: str) -> RunProfile: known = ", ".join(sorted(self.profiles)) raise RunProfileError(f"unknown run profile {name!r}; known profiles: {known}") from exc - def names(self) -> tuple[str, ...]: - """Profile names in registration order.""" - return tuple(self.profiles) - GO2_RUN_PROFILES = RunProfileRegistry( - default_profile_name="walk", profiles={ "walk": RunProfile( name="walk", @@ -133,8 +109,6 @@ def names(self) -> tuple[str, ...]: max_planar_cmd_accel_m_s2=5.0, max_yaw_rate_rad_s=1.0, max_yaw_accel_rad_s2=5.0, - required_locomotion_mode="default", - description="Default cautious navigation; matches today's LocalPlanner walking behavior.", ), "trot": RunProfile( name="trot", @@ -145,8 +119,6 @@ def names(self) -> tuple[str, ...]: max_planar_cmd_accel_m_s2=5.0, max_yaw_rate_rad_s=1.2, max_yaw_accel_rad_s2=5.0, - required_locomotion_mode="default", - description="Faster trot within the default locomotion mode.", ), "run_conservative": RunProfile( name="run_conservative", @@ -157,31 +129,6 @@ def names(self) -> tuple[str, ...]: max_planar_cmd_accel_m_s2=6.0, max_yaw_rate_rad_s=1.0, max_yaw_accel_rad_s2=4.0, - required_locomotion_mode="default", - description=( - "Conservative run envelope at planner speed caps; stays in default " - "locomotion mode so onboard lidar and obstacle avoidance remain active." - ), - ), - "run_verified": RunProfile( - name="run_verified", - requested_planner_speed_m_s=2.5, - max_tangent_accel_m_s2=2.5, - max_normal_accel_m_s2=1.2, - goal_decel_m_s2=2.0, - max_planar_cmd_accel_m_s2=6.0, - max_yaw_rate_rad_s=0.8, - max_yaw_accel_rad_s2=3.0, - required_locomotion_mode="rage", - description="Highest envelope; switches Go2 to rage locomotion mode.", ), }, ) - - -__all__ = [ - "GO2_RUN_PROFILES", - "RunProfile", - "RunProfileError", - "RunProfileRegistry", -] diff --git a/dimos/navigation/dannav/holonomic_tc/test_command_limits.py b/dimos/navigation/dannav/holonomic_tc/test_command_limits.py new file mode 100644 index 0000000000..00ca10453a --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_command_limits.py @@ -0,0 +1,103 @@ +# 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. + +"""``clamp_holonomic_cmd_vel`` branch coverage in two chains. + +Closed-loop followers exercise this in motion; here only the limiter decisions +that are hard to see from arrival alone: planar cap/direction/accel/slew, yaw +cap/accel. +""" + +from __future__ import annotations + +import math + +import pytest + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.navigation.dannav.holonomic_tc.command_limits import ( + HolonomicCommandLimits, + clamp_holonomic_cmd_vel, +) + + +def test_clamp_planar_speed_accel_and_slew() -> None: + dt_s = 0.1 + a_max_m_s2 = 2.0 + v_max_m_s = 1.0 + + speed_out = clamp_holonomic_cmd_vel( + Twist(), + Twist(linear=Vector3(3.0, 4.0, 0.0)), + HolonomicCommandLimits(v_max_m_s, 10.0, 10.0, 10.0), + 1.0, + ) + assert math.hypot(speed_out.linear.x, speed_out.linear.y) == pytest.approx(v_max_m_s) + assert speed_out.linear.x / speed_out.linear.y == pytest.approx(3.0 / 4.0) + + accel_limits = HolonomicCommandLimits(10.0, 10.0, a_max_m_s2, 10.0) + from_rest = clamp_holonomic_cmd_vel( + Twist(), + Twist(linear=Vector3(5.0, 0.0, 0.0)), + accel_limits, + dt_s, + ) + assert from_rest.linear.x == pytest.approx(a_max_m_s2 * dt_s) + + prev = Twist(linear=Vector3(0.8, 0.0, 0.0)) + slew = clamp_holonomic_cmd_vel( + prev, + Twist(linear=Vector3(5.0, 0.0, 0.0)), + HolonomicCommandLimits(10.0, 10.0, 1.0, 10.0), + dt_s, + ) + assert slew.linear.x == pytest.approx(0.8 + 1.0 * dt_s) + + prev_moving = Twist(linear=Vector3(0.6, 0.8, 0.0)) + reversal = clamp_holonomic_cmd_vel( + prev_moving, + Twist(linear=Vector3(-0.6, -0.8, 0.0)), + accel_limits, + dt_s, + ) + delta = math.hypot( + reversal.linear.x - prev_moving.linear.x, + reversal.linear.y - prev_moving.linear.y, + ) + assert delta == pytest.approx(a_max_m_s2 * dt_s) + assert math.hypot(reversal.linear.x, reversal.linear.y) < math.hypot( + prev_moving.linear.x, prev_moving.linear.y + ) + + +def test_clamp_yaw_rate_and_accel() -> None: + w_max_rad_s = 0.5 + rate_capped = clamp_holonomic_cmd_vel( + Twist(), + Twist(angular=Vector3(0.0, 0.0, 1.0)), + HolonomicCommandLimits(1.0, w_max_rad_s, 1.0, 5.0), + 1.0, + ) + assert abs(rate_capped.angular.z) == pytest.approx(w_max_rad_s) + + dt_s = 0.1 + w_accel_rad_s2 = 2.0 + accel_capped = clamp_holonomic_cmd_vel( + Twist(), + Twist(angular=Vector3(0.0, 0.0, 1.0)), + HolonomicCommandLimits(1.0, 10.0, 1.0, w_accel_rad_s2), + dt_s, + ) + assert accel_capped.angular.z == pytest.approx(w_accel_rad_s2 * dt_s) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py similarity index 60% rename from dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py rename to dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py index ba7cfb68e8..a279062a96 100644 --- a/dimos/navigation/holonomic_trajectory_controller/test_dan_holonomic_tc.py +++ b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py @@ -12,14 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""``DanHolonomicTC`` stream wiring. - -These exercise the module surface that routes the ``path`` / ``odometry`` / -``stop_movement`` inputs into the control core and forwards ``nav_cmd_vel`` / -``goal_reached``: an empty path stops, a non-empty path hot-swaps or starts, -``stop_movement`` cancels, and arrival publishes ``goal_reached(True)``. The -control law itself is covered in ``test_holonomic_path_follower``. -""" +"""``DanHolonomicTC`` module integration: cancel inputs, path hot-swap, arrival.""" from __future__ import annotations @@ -28,27 +21,25 @@ from dataclasses import dataclass, field import math import time -from typing import Any +from typing import Any, Literal + +import pytest from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] from dimos.core.stream import Stream, Transport -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.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.base import NavigationState -from dimos.navigation.holonomic_trajectory_controller.module import DanHolonomicTC +from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC +_CancelVia = Literal["empty_path", "stop_movement"] -class _DirectTransport(Transport): # type: ignore[type-arg] - """Synchronous in-process transport so ``start()`` can wire the inputs. - Delivers each broadcast straight to the subscribed handlers on the calling - thread, which keeps the wiring assertions deterministic. - """ +class _DirectTransport(Transport): # type: ignore[type-arg] + """Synchronous in-process transport so ``start()`` can wire the inputs.""" def __init__(self) -> None: self._subscribers: list[Callable[[Any], Any]] = [] @@ -84,11 +75,12 @@ def _yaw_quaternion(yaw_rad: float) -> Quaternion: return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) -def _odometry(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> Odometry: - return Odometry( +def _odom(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( ts=ts, frame_id="map", - pose=Pose(position=[x, y, 0.0], orientation=_yaw_quaternion(yaw_rad)), + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), ) @@ -137,12 +129,8 @@ def __init__( self.captured = captured self._unsubs = unsubs - @property - def core(self) -> Any: - return self.module._core - def feed_odom(self, x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> None: - self.module.odometry.transport.broadcast(None, _odometry(x, y, yaw_rad, ts=ts)) + self.module.odom.transport.broadcast(None, _odom(x, y, yaw_rad, ts=ts)) def feed_path(self, path: Path) -> None: self.module.path.transport.broadcast(None, path) @@ -163,7 +151,7 @@ def close(self) -> None: def _running_module(**config: Any) -> Iterator[_ModuleHarness]: module = DanHolonomicTC(**config) module.path.transport = _DirectTransport() - module.odometry.transport = _DirectTransport() + module.odom.transport = _DirectTransport() module.stop_movement.transport = _DirectTransport() captured = _Captured() unsubs = [ @@ -178,80 +166,48 @@ def _running_module(**config: Any) -> Iterator[_ModuleHarness]: harness.close() -def test_empty_path_publishes_zero_twist_and_clears_route() -> None: - with _running_module(goal_tolerance=0.2) as h: - h.feed_odom(0.0, 0.0, 0.0) - h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) - thread = h.core._thread - assert thread is not None # a route is active - - h.feed_empty_path() - thread.join(timeout=1.0) - - assert not thread.is_alive() - assert h.core._path is None - assert h.core._thread is None - assert h.core.get_state() == NavigationState.IDLE - assert h.captured.cmd_vel - assert _is_zero_twist(h.captured.cmd_vel[-1]) - assert not h.captured.goal_reached +def _assert_route_cleared(h: _ModuleHarness) -> None: + assert _wait_until(lambda: h.module.get_state() == NavigationState.IDLE) + assert _wait_until(lambda: bool(h.captured.cmd_vel) and _is_zero_twist(h.captured.cmd_vel[-1])) + assert not h.captured.goal_reached -def test_stop_movement_publishes_zero_twist_and_clears_route() -> None: +@pytest.mark.parametrize("cancel_via", ["empty_path", "stop_movement"]) +def test_cancel_publishes_zero_twist_and_clears_route(cancel_via: _CancelVia) -> None: with _running_module(goal_tolerance=0.2) as h: h.feed_odom(0.0, 0.0, 0.0) h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) - thread = h.core._thread - assert thread is not None - - h.feed_stop(True) - thread.join(timeout=1.0) - assert not thread.is_alive() - assert h.core._path is None - assert h.core._thread is None - assert h.core.get_state() == NavigationState.IDLE - assert h.captured.cmd_vel - assert _is_zero_twist(h.captured.cmd_vel[-1]) - - -def test_stop_movement_false_does_not_clear_route() -> None: - with _running_module(goal_tolerance=0.2) as h: - h.feed_odom(0.0, 0.0, 0.0) - h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) - thread = h.core._thread - - h.feed_stop(False) + if cancel_via == "empty_path": + h.feed_empty_path() + else: + h.feed_stop(True) - assert h.core._thread is thread - assert h.core._path is not None + _assert_route_cleared(h) -def test_hot_update_path_swaps_route_without_restarting_thread() -> None: +def test_hot_update_path_swaps_route_to_new_goal() -> None: + # Robot at (1, 0): old goal (2, 0) is still out of tolerance; after the + # swap the terminus is (1, 0), so arrival proves the new route took effect. with _running_module(goal_tolerance=0.2) as h: - h.feed_odom(0.0, 0.0, 0.0) + h.feed_odom(1.0, 0.0, 0.0) h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) - thread_before = h.core._thread - assert thread_before is not None + assert not _wait_until(lambda: bool(h.captured.goal_reached), timeout=0.2) - h.feed_path(_path_from_points([(0.0, 0.0), (0.0, 2.0)])) + h.feed_path(_path_from_points([(0.0, 0.0), (1.0, 0.0)])) - # Same control thread: the route was hot-swapped, not restarted. - assert h.core._thread is thread_before - swapped = [(float(p.position.x), float(p.position.y)) for p in h.core._path.poses] - assert swapped == [(0.0, 0.0), (0.0, 2.0)] + assert _wait_until(lambda: bool(h.captured.goal_reached)) + assert h.captured.goal_reached[-1].data is True def test_arrival_publishes_goal_reached_without_final_spin() -> None: - # Odom sits on the goal with a misaligned heading; align_goal_yaw=False must - # report arrival on position alone, never spinning to align the final yaw. + # Odom on the goal with misaligned heading; align_goal_yaw=False must report + # arrival on position alone, never spinning to align the final yaw. with _running_module(goal_tolerance=0.2, align_goal_yaw=False) as h: h.feed_odom(1.0, 0.0, 1.2) h.feed_path(_path_from_points([(0.0, 0.0), (1.0, 0.0)])) assert _wait_until(lambda: bool(h.captured.goal_reached)) - assert [bool(b.data) for b in h.captured.goal_reached] == [True] - assert h.core.is_goal_reached() is True - # No final rotation: every command published was a body-yaw rate of zero. + assert h.captured.goal_reached[-1].data is True assert h.captured.cmd_vel assert all(abs(float(cmd.angular.z)) < 1e-6 for cmd in h.captured.cmd_vel) diff --git a/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py new file mode 100644 index 0000000000..42d9108486 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py @@ -0,0 +1,224 @@ +# 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. + +"""Closed-loop ``_HolonomicPathFollower`` integration tests. + +Math for profiling, tracking, and command limits lives in sibling unit tests. +Here: path in -> cmd_vel out -> simulated plant -> arrival / caps / branches. +""" + +from __future__ import annotations + +import math +import time +from dataclasses import dataclass + +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.nav_msgs.Path import Path +from dimos.navigation.dannav.holonomic_tc.module import ( + ActiveRunEnvelope, + DanHolonomicTCConfig, + _HolonomicPathFollower, +) +from dimos.navigation.dannav.geometry.path_speed_profile import ( + PathSpeedProfileLimits, +) +from dimos.navigation.dannav.holonomic_tc.run_profiles import RunProfile + + +def _planar_speed_m_s(cmd: Twist) -> float: + return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="map", + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append(_pose_stamped(point[0], point[1], yaw)) + return Path(frame_id="map", poses=poses) + + +def _make_follower(**overrides: object) -> _HolonomicPathFollower: + return _HolonomicPathFollower(DanHolonomicTCConfig(**overrides)) + + +def _install_envelope( + core: _HolonomicPathFollower, + *, + speed_m_s: float, + max_tangent_accel_m_s2: float, + max_normal_accel_m_s2: float, + goal_decel_m_s2: float, +) -> None: + core._apply_run_envelope( + ActiveRunEnvelope( + profile=RunProfile( + name="test", + requested_planner_speed_m_s=speed_m_s, + max_tangent_accel_m_s2=max_tangent_accel_m_s2, + max_normal_accel_m_s2=max_normal_accel_m_s2, + goal_decel_m_s2=goal_decel_m_s2, + max_planar_cmd_accel_m_s2=8.0, + max_yaw_rate_rad_s=speed_m_s, + max_yaw_accel_rad_s2=8.0, + ), + speed_m_s=speed_m_s, + path_limits=PathSpeedProfileLimits( + max_speed_m_s=speed_m_s, + max_tangent_accel_m_s2=max_tangent_accel_m_s2, + max_normal_accel_m_s2=max_normal_accel_m_s2, + ), + goal_decel_m_s2=goal_decel_m_s2, + ) + ) + + +@dataclass +class _RunResult: + command_history: list[Twist] + stop_messages: list[str] + final_x_m: float + final_y_m: float + final_yaw_rad: float + + +def _run_follower( + core: _HolonomicPathFollower, + *, + points: list[tuple[float, float]], + initial_yaw_rad: float = 0.0, + max_ticks: int = 300, + rate_hz: float = 60.0, +) -> _RunResult: + dt_s = 1.0 / rate_hz + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, initial_yaw_rad + latest_cmd = Twist() + command_history: list[Twist] = [] + stop_messages: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + command_history.append(Twist(cmd)) + + cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = core.stopped_navigating.subscribe(stop_messages.append) + sim_time_s = 1.0 + + try: + core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) + core.start_planning(_path_from_points(points)) + for _ in range(max_ticks): + if "arrived" in stop_messages: + break + time.sleep(dt_s * 1.1) + vx = float(latest_cmd.linear.x) + vy = float(latest_cmd.linear.y) + wz = float(latest_cmd.angular.z) + c = math.cos(plant_yaw_rad) + s = math.sin(plant_yaw_rad) + plant_x_m += (c * vx - s * vy) * dt_s + plant_y_m += (s * vx + c * vy) * dt_s + plant_yaw_rad += wz * dt_s + plant_yaw_rad = math.atan2(math.sin(plant_yaw_rad), math.cos(plant_yaw_rad)) + sim_time_s += dt_s + core.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) + ) + finally: + core.close() + cmd_sub.dispose() + stop_sub.dispose() + + return _RunResult(command_history, stop_messages, plant_x_m, plant_y_m, plant_yaw_rad) + + +def test_closed_loop_straight_line_arrives_with_goal_decel() -> None: + cruise_speed_m_s = 1.2 + goal_tolerance_m = 0.08 + goal_x_m = 1.0 + result = _run_follower( + _make_follower(speed_m_s=cruise_speed_m_s, control_frequency=60.0, goal_tolerance=goal_tolerance_m), + points=[(0.1, 0.0), (goal_x_m, 0.0)], + ) + speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history if _planar_speed_m_s(cmd) > 0.05] + + assert "arrived" in result.stop_messages + assert speeds + peak = max(speeds) + assert peak > 0.5 * cruise_speed_m_s + assert min(speeds[-10:]) < 0.5 * peak + assert math.hypot(result.final_x_m - goal_x_m, result.final_y_m) < goal_tolerance_m + 0.07 + + +def test_closed_loop_right_angle_respects_curvature_speed_cap() -> None: + core = _make_follower(control_frequency=60.0, goal_tolerance=0.08) + _install_envelope( + core, + speed_m_s=2.0, + max_tangent_accel_m_s2=0.5, + max_normal_accel_m_s2=0.1, + goal_decel_m_s2=0.5, + ) + result = _run_follower( + core, + points=[(0.1, 0.0), (0.55, 0.0), (0.55, 0.55), (0.9, 0.55)], + max_ticks=420, + ) + speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] + + assert "arrived" in result.stop_messages + assert speeds + assert max(speeds) < 0.75 + + +def test_closed_loop_rotate_first_then_arrives() -> None: + result = _run_follower( + _make_follower( + speed_m_s=0.9, + control_frequency=60.0, + goal_tolerance=0.08, + align_heading_before_move=True, + ), + points=[(0.1, 0.0), (1.0, 0.0)], + initial_yaw_rad=0.8, + max_ticks=320, + ) + + assert "arrived" in result.stop_messages + assert any( + abs(float(cmd.angular.z)) > 0.05 and _planar_speed_m_s(cmd) < 0.05 + for cmd in result.command_history[:20] + ) + assert abs(result.final_yaw_rad) < 0.35 diff --git a/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py new file mode 100644 index 0000000000..cda275cc94 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py @@ -0,0 +1,148 @@ +# 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. + +"""``HolonomicTrackingController`` branch coverage in two chains. + +Closed-loop followers exercise this in motion; here only tracking-law decisions +not visible from arrival alone: frame rotation, pose P, heading sign, one-sided +damping, configured speed caps. +""" + +from __future__ import annotations + +import math + +import pytest + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.navigation.dannav.holonomic_tc.command_limits import ( + HolonomicCommandLimits, +) +from dimos.navigation.dannav.holonomic_tc.holonomic_tracking_controller import HolonomicTrackingController +from dimos.navigation.dannav.holonomic_tc.types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) +from dimos.utils.trigonometry import angle_diff + + +def _pose_xy_yaw(x: float, y: float, yaw: float) -> Pose: + return Pose( + x, + y, + 0.0, + 0.0, + 0.0, + math.sin(yaw / 2.0), + math.cos(yaw / 2.0), + ) + + +def _ref(pose: Pose, twist: Twist) -> TrajectoryReferenceSample: + return TrajectoryReferenceSample(time_s=0.0, pose_plan=pose, twist_body=twist) + + +def _meas(pose: Pose, twist: Twist) -> TrajectoryMeasuredSample: + return TrajectoryMeasuredSample(time_s=0.0, pose_plan=pose, twist_body=twist) + + +def _planar_speed(cmd: Twist) -> float: + return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) + + +def test_tracking_feedforward_frame_and_pose_correction() -> None: + origin = _pose_xy_yaw(0.0, 0.0, 0.0) + ref_twist = Twist(linear=Vector3(0.4, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.05)) + aligned_pose = _pose_xy_yaw(1.0, -2.0, math.pi / 6) + + feedforward = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=0.0) + out = feedforward.control(_ref(aligned_pose, ref_twist), _meas(aligned_pose, Twist())) + assert out.linear.x == pytest.approx(ref_twist.linear.x) + assert out.linear.y == pytest.approx(ref_twist.linear.y) + assert out.angular.z == pytest.approx(ref_twist.angular.z) + + ref_rotated = _pose_xy_yaw(0.0, 1.0, math.pi / 2.0) + frame_out = feedforward.control( + _ref(ref_rotated, Twist(linear=Vector3(0.5, 0.0, 0.0))), + _meas(origin, Twist()), + ) + assert frame_out.linear.x == pytest.approx(0.0, abs=1e-12) + assert frame_out.linear.y == pytest.approx(0.5) + + position = HolonomicTrackingController(k_position_per_s=1.0, k_yaw_per_s=0.0) + position_out = position.control( + _ref(_pose_xy_yaw(1.0, 0.0, 0.0), Twist()), + _meas(origin, Twist()), + ) + assert position_out.linear.x == pytest.approx(1.0) + assert position_out.linear.y == pytest.approx(0.0) + + heading = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=2.0) + heading_out = heading.control( + _ref(_pose_xy_yaw(0.0, 0.0, 0.5), Twist()), + _meas(origin, Twist()), + ) + e_psi = angle_diff(0.0, 0.5) + assert heading_out.angular.z == pytest.approx(-2.0 * e_psi) + + +def test_tracking_damping_is_one_sided_and_respects_limits() -> None: + origin = _pose_xy_yaw(0.0, 0.0, 0.0) + ref_twist = Twist(linear=Vector3(0.5, 0.2, 0.0), angular=Vector3(0.0, 0.0, 0.4)) + overspeed_meas = Twist(linear=Vector3(1.1, 0.6, 0.0), angular=Vector3(0.0, 0.0, 0.9)) + underspeed_meas = Twist(linear=Vector3(0.2, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.1)) + + undamped = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=0.0) + undamped_out = undamped.control(_ref(origin, ref_twist), _meas(origin, overspeed_meas)) + assert undamped_out.linear.x == pytest.approx(ref_twist.linear.x) + assert abs(undamped_out.angular.z) == pytest.approx(abs(ref_twist.angular.z)) + + damped = HolonomicTrackingController( + k_position_per_s=0.0, + k_yaw_per_s=0.0, + k_velocity_per_s=0.5, + k_yaw_rate_per_s=0.5, + ) + damped_over = damped.control(_ref(origin, ref_twist), _meas(origin, overspeed_meas)) + assert _planar_speed(damped_over) < _planar_speed(ref_twist) + assert abs(damped_over.angular.z) < abs(ref_twist.angular.z) + + damped_under = damped.control(_ref(origin, ref_twist), _meas(origin, underspeed_meas)) + assert damped_under.linear.x == pytest.approx(ref_twist.linear.x) + assert damped_under.linear.y == pytest.approx(ref_twist.linear.y) + assert damped_under.angular.z == pytest.approx(ref_twist.angular.z) + + max_planar_m_s = 0.5 + max_yaw_rad_s = 0.2 + capped = HolonomicTrackingController(k_position_per_s=100.0, k_yaw_per_s=100.0) + capped.configure( + HolonomicCommandLimits( + max_planar_speed_m_s=max_planar_m_s, + max_yaw_rate_rad_s=max_yaw_rad_s, + max_planar_linear_accel_m_s2=10.0, + max_yaw_accel_rad_s2=10.0, + ), + ) + planar_out = capped.control( + _ref(_pose_xy_yaw(10.0, 0.0, 0.0), Twist()), + _meas(origin, Twist()), + ) + yaw_out = capped.control( + _ref(_pose_xy_yaw(0.0, 0.0, 10.0), Twist()), + _meas(origin, Twist()), + ) + assert _planar_speed(planar_out) == pytest.approx(max_planar_m_s) + assert abs(yaw_out.angular.z) == pytest.approx(max_yaw_rad_s) diff --git a/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py new file mode 100644 index 0000000000..c4c1ca3bca --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py @@ -0,0 +1,130 @@ +# 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. + +"""Run-profile session envelope via closed-loop ``cmd_vel``. + +Profile resolution and live ``set_run_profile`` - observable as commanded speed +and arrival, not ``_path_speed_at_position`` math. +""" + +from __future__ import annotations + +import math +import time + +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.nav_msgs.Path import Path +from dimos.navigation.dannav.holonomic_tc.module import ( + DanHolonomicTCConfig, + _HolonomicPathFollower, +) + + +def _yaw_quaternion(yaw_rad: float) -> Quaternion: + return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + + +def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="map", + position=[x, y, 0.0], + orientation=_yaw_quaternion(yaw_rad), + ) + + +def _path_from_points(points: list[tuple[float, float]]) -> Path: + poses: list[PoseStamped] = [] + for index, point in enumerate(points): + if index + 1 < len(points): + next_point = points[index + 1] + yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) + else: + prev_point = points[index - 1] + yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) + poses.append(_pose_stamped(point[0], point[1], yaw)) + return Path(frame_id="map", poses=poses) + + +def _make_follower(**overrides: object) -> _HolonomicPathFollower: + return _HolonomicPathFollower(DanHolonomicTCConfig(**overrides)) + + +def _planar_speed_m_s(cmd: Twist) -> float: + return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) + + +def _closed_loop_max_planar_speed( + core: _HolonomicPathFollower, + *, + rate_hz: float = 60.0, + max_ticks: int = 420, +) -> float: + dt_s = 1.0 / rate_hz + plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 + latest_cmd = Twist() + commanded_speeds: list[float] = [] + stops: list[str] = [] + + def _on_cmd_vel(cmd: Twist) -> None: + nonlocal latest_cmd + latest_cmd = Twist(cmd) + commanded_speeds.append(_planar_speed_m_s(cmd)) + + cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) + stop_sub = core.stopped_navigating.subscribe(stops.append) + sim_time_s = 1.0 + + try: + core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) + core.start_planning(_path_from_points([(0.1, 0.0), (3.1, 0.0)])) + for _ in range(max_ticks): + if "arrived" in stops: + break + time.sleep(dt_s * 1.1) + vx = float(latest_cmd.linear.x) + vy = float(latest_cmd.linear.y) + wz = float(latest_cmd.angular.z) + c = math.cos(plant_yaw_rad) + s = math.sin(plant_yaw_rad) + plant_x_m += (c * vx - s * vy) * dt_s + plant_y_m += (s * vx + c * vy) * dt_s + plant_yaw_rad += wz * dt_s + plant_yaw_rad = math.atan2(math.sin(plant_yaw_rad), math.cos(plant_yaw_rad)) + sim_time_s += dt_s + core.handle_odom( + _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) + ) + finally: + core.close() + cmd_sub.dispose() + stop_sub.dispose() + + assert "arrived" in stops + assert commanded_speeds + return max(commanded_speeds) + + +def test_run_conservative_commands_faster_than_walk_in_closed_loop() -> None: + walk_max = _closed_loop_max_planar_speed(_make_follower(run_profile="walk")) + run_max = _closed_loop_max_planar_speed(_make_follower(run_profile="run_conservative")) + assert run_max > walk_max + + +def test_set_run_profile_rejects_unknown_profile() -> None: + core = _make_follower() + assert core.set_run_profile("does-not-exist") is False + assert core.set_run_profile("trot") is True diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_types.py b/dimos/navigation/dannav/holonomic_tc/types.py similarity index 78% rename from dimos/navigation/holonomic_trajectory_controller/trajectory_types.py rename to dimos/navigation/dannav/holonomic_tc/types.py index cf272adc77..20f0389764 100644 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_types.py +++ b/dimos/navigation/dannav/holonomic_tc/types.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Per-tick reference and measured samples for holonomic path following and JSONL logging. +"""Per-tick reference and measured samples for holonomic path following. **Plan horizontal frame (``pose_plan``)** -- Positions and yaw use the same plan horizontal convention as ``Path``, - ``PathDistancer``, and ``trajectory_metrics.pose_errors_vs_reference``. +- Positions and yaw use the same plan horizontal convention as ``Path`` and + ``PathDistancer``. **Body command frame (``twist_body``)** @@ -41,8 +41,8 @@ @dataclass(frozen=True) -class TrajectoryReferenceSample: - """One timed point on the reference trajectory (target).""" +class TrajectorySample: + """Timed pose + body twist sample (shared shape).""" time_s: float pose_plan: Pose @@ -54,19 +54,10 @@ def __post_init__(self) -> None: @dataclass(frozen=True) -class TrajectoryMeasuredSample: - """One timed measurement of actual robot state for tracking.""" - - time_s: float - pose_plan: Pose - twist_body: Twist - - def __post_init__(self) -> None: - object.__setattr__(self, "pose_plan", Pose(self.pose_plan)) - object.__setattr__(self, "twist_body", Twist(self.twist_body)) +class TrajectoryReferenceSample(TrajectorySample): + """One timed point on the reference trajectory (target).""" -__all__ = [ - "TrajectoryMeasuredSample", - "TrajectoryReferenceSample", -] +@dataclass(frozen=True) +class TrajectoryMeasuredSample(TrajectorySample): + """One timed measurement of actual robot state for tracking.""" diff --git a/dimos/navigation/dannav/local_planner/module.py b/dimos/navigation/dannav/local_planner/module.py index 1ddc819066..64f4aaf4c0 100644 --- a/dimos/navigation/dannav/local_planner/module.py +++ b/dimos/navigation/dannav/local_planner/module.py @@ -15,21 +15,12 @@ """Path-stream middleware between ``MLSPlannerNative`` and ``DanHolonomicTC``. ``MLSPlannerNative`` re-roots and re-emits a path on every lidar frame: the -stream is sparse, unsmoothed, and unthrottled. The old ``GlobalPlanner`` shaped -that stream before the tracker saw it (replan throttle, smoothing/resample, -veer-triggered replan); those were deleted with ``dannav`` and never reproduced. - -``DanLocalPlanner`` reinstates them in a transport-free core (``_ReplanGate``) -wrapped in a thin ``Module``, without touching the Rust planner. Every feature -lives in :class:`DanLocalPlannerConfig` and defaults to off, so the module is a -pass-through until configured: +stream is sparse, unsmoothed, and unthrottled. - ``lock_replan``: commit-window throttle (forward a path only at commit moments, suppress in between so the follower keeps a stable lookahead). - ``resample_spacing_m`` / ``smoothing_window``: smooth + uniform resample the committed path (``smooth_resample_path``). -- ``max_path_deviation_m``: veer override (commit the latest replan when the - robot drifts off the committed path). :class:`_ReplanGate` tracks the committed path with a :class:`PathDistancer` and forwards a fresh planner path only when the robot has advanced @@ -55,9 +46,9 @@ from dimos.mapping.occupancy.path_resampling import smooth_resample_path from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer +from dimos.navigation.dannav.geometry.path_distancer import PathDistancer from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -72,9 +63,8 @@ class DanLocalPlannerConfig(ModuleConfig): lock_replan: float = 0.0 # commit-window length in m; 0 disables gating goal_commit_tolerance_m: float = 0.3 # match a path end to the clicked goal - resample_spacing_m: float = 0.1 # resample spacing in m; 0 disables smoothing + resample_spacing_m: float = 0.0 # resample spacing in m; 0 disables smoothing smoothing_window: int = 100 # moving-average window - max_path_deviation_m: float | None = None # veer override; None disables class _ReplanGate: @@ -95,8 +85,8 @@ class _ReplanGate: def __init__(self, config: DanLocalPlannerConfig) -> None: self._cfg = config - # Latest robot xy from odometry; progress and veer deviation are measured - # against the committed path with it. + # Latest robot xy from odometry; arc-length progress is measured against + # the committed path with it. self._robot_xy: NDArray[np.float64] | None = None # xy of the most recent finite click, armed until a path ends at it # (fresh-click commit) or a cancel disarms it. @@ -107,7 +97,7 @@ def __init__(self, config: DanLocalPlannerConfig) -> None: self._committed: PathDistancer | None = None self._anchor_progress_m: float = 0.0 - def on_odom(self, odom: Odometry) -> None: + def on_odom(self, odom: PoseStamped) -> None: self._robot_xy = np.array([odom.position.x, odom.position.y], dtype=float) def on_goal(self, goal: PointStamped) -> None: @@ -157,8 +147,8 @@ def on_planner_path(self, path: Path) -> Path | None: def _commit(self, path: Path) -> Path: """Adopt ``path`` as the committed path and return it for publishing. - Smoothing reshapes ``path`` here before it is stored, so progress and - veer deviation are measured against the same polyline the tracker follows. + Smoothing reshapes ``path`` here before it is stored, so progress is + measured against the same polyline the tracker follows. """ smoothed = self._smooth(path) self._committed = PathDistancer(smoothed) @@ -218,14 +208,14 @@ class DanLocalPlanner(Module): """Gate and shape ``MLSPlannerNative``'s path stream for ``DanHolonomicTC``. Sits between the planner output (remapped to ``planner_path``) and the - follower's ``path`` input. Forwards the paths :class:`_ReplanGate` commits; - ``DanHolonomicTC`` keeps its unchanged ``BasicPathFollower`` surface. + follower's ``path`` input. Forwards the paths :class:`_ReplanGate` commits + unchanged to ``DanHolonomicTC``. """ config: DanLocalPlannerConfig planner_path: In[Path] # from MLSPlannerNative.path (remapped) - odometry: In[Odometry] # from PoseOdomRelay.odometry + odom: In[PoseStamped] # from GO2Connection.odom goal: In[PointStamped] # from MovementManager.goal; the click/cancel signal path: Out[Path] # to DanHolonomicTC.path @@ -239,13 +229,13 @@ def start(self) -> None: super().start() # Subscribe odom and goal before paths so the gate has the latest robot # pose and armed goal when a planner path arrives. - self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) self.register_disposable(Disposable(self.goal.subscribe(self._on_goal))) self.register_disposable( Disposable(self.planner_path.subscribe(self._on_planner_path)) ) - def _on_odometry(self, msg: Odometry) -> None: + def _on_odom(self, msg: PoseStamped) -> None: self._gate.on_odom(msg) def _on_goal(self, msg: PointStamped) -> None: diff --git a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py index 9c3f8f4584..a048ef390a 100644 --- a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py +++ b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py @@ -12,58 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Tests for ``DanLocalPlanner`` module wiring and ``_ReplanGate`` behavior.""" +"""Tests for ``_ReplanGate`` commit-window gating.""" from __future__ import annotations -from collections.abc import Callable, Iterator -from contextlib import contextmanager import math from typing import Any -from dimos.core.stream import Stream, Transport from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.dannav.local_planner.module import ( - DanLocalPlanner, DanLocalPlannerConfig, _ReplanGate, ) -class _DirectTransport(Transport): # type: ignore[type-arg] - """Synchronous in-process transport so ``start()`` can wire the inputs. - - Delivers each broadcast straight to the subscribed handlers on the calling - thread, which keeps the wiring assertions deterministic. - """ - - def __init__(self) -> None: - self._subscribers: list[Callable[[Any], Any]] = [] - - def broadcast(self, _selfstream: Any, value: Any) -> None: - for callback in list(self._subscribers): - callback(value) - - def subscribe( - self, callback: Callable[[Any], Any], _selfstream: Stream[Any] | None = None - ) -> Callable[[], None]: - self._subscribers.append(callback) - - def _unsubscribe() -> None: - if callback in self._subscribers: - self._subscribers.remove(callback) - - return _unsubscribe - - def start(self) -> None: ... - - def stop(self) -> None: - self._subscribers.clear() - - def _path_from_points(points: list[tuple[float, float]]) -> Path: poses = [ PoseStamped(ts=1.0, frame_id="world", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) @@ -72,11 +36,12 @@ def _path_from_points(points: list[tuple[float, float]]) -> Path: return Path(frame_id="world", poses=poses) -def _odometry(x: float, y: float, *, ts: float = 1.0) -> Odometry: - return Odometry( +def _odom(x: float, y: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( ts=ts, frame_id="world", - pose=PoseStamped(position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]), + position=[x, y, 0.0], + orientation=[0.0, 0.0, 0.0, 1.0], ) @@ -84,173 +49,41 @@ def _point(x: float, y: float) -> PointStamped: return PointStamped(x=x, y=y, z=0.0, frame_id="world") -class _ModuleHarness: - def __init__(self, module: DanLocalPlanner, forwarded: list[Path]) -> None: - self.module = module - self.forwarded = forwarded - - @property - def gate(self) -> _ReplanGate: - return self.module._gate - - def feed_odom(self, x: float, y: float, *, ts: float = 1.0) -> None: - self.module.odometry.transport.broadcast(None, _odometry(x, y, ts=ts)) - - def feed_goal(self, x: float, y: float) -> None: - self.module.goal.transport.broadcast(None, _point(x, y)) - - def feed_planner_path(self, path: Path) -> None: - self.module.planner_path.transport.broadcast(None, path) - - def close(self) -> None: - self.module.stop() - - -@contextmanager -def _running_module(**config: Any) -> Iterator[_ModuleHarness]: - module = DanLocalPlanner(**config) - module.planner_path.transport = _DirectTransport() - module.odometry.transport = _DirectTransport() - module.goal.transport = _DirectTransport() - forwarded: list[Path] = [] - module.path.subscribe(forwarded.append) - module.start() - harness = _ModuleHarness(module, forwarded) - try: - yield harness - finally: - harness.close() - - -# ---- gate core (transport-free) ---------------------------------------------- - - -def test_gate_forwards_non_empty_path() -> None: - gate = _ReplanGate(DanLocalPlannerConfig()) - path = _path_from_points([(0.0, 0.0), (1.0, 0.0)]) - assert gate.on_planner_path(path) is path - - -def test_gate_forwards_empty_path_as_stop() -> None: - gate = _ReplanGate(DanLocalPlannerConfig()) - empty = Path(frame_id="world", poses=[]) - assert gate.on_planner_path(empty) is empty - - -def test_gate_on_odom_records_robot_xy() -> None: - gate = _ReplanGate(DanLocalPlannerConfig()) - assert gate._robot_xy is None - gate.on_odom(_odometry(1.5, -2.0)) - assert gate._robot_xy is not None - assert list(gate._robot_xy) == [1.5, -2.0] - - -def test_gate_on_goal_finite_arms_and_nan_disarms() -> None: - gate = _ReplanGate(DanLocalPlannerConfig()) - gate.on_goal(_point(3.0, 4.0)) - assert gate._armed_goal is not None - assert list(gate._armed_goal) == [3.0, 4.0] - - # A NaN PointStamped is MovementManager's cancel: it disarms the gate. - gate.on_goal(_point(math.nan, math.nan)) - assert gate._armed_goal is None - - -# ---- module wiring ----------------------------------------------------------- - - -def test_planner_path_is_forwarded_to_path_output() -> None: - with _running_module() as h: - path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)]) - h.feed_planner_path(path) - - assert h.forwarded == [path] - - -def test_empty_planner_path_is_forwarded() -> None: - with _running_module() as h: - empty = Path(frame_id="world", poses=[]) - h.feed_planner_path(empty) - - assert h.forwarded == [empty] - - -def test_passthrough_forwards_every_path() -> None: - # Default config (lock_replan=0) is a pass-through: each planner path - # produces exactly one forwarded path. - with _running_module() as h: - paths = [ - _path_from_points([(0.0, 0.0), (1.0, 0.0)]), - _path_from_points([(0.0, 0.0), (0.0, 1.0)]), - _path_from_points([(0.0, 0.0), (1.0, 1.0)]), - ] - for path in paths: - h.feed_planner_path(path) - - assert h.forwarded == paths - - -def test_odom_and_goal_do_not_publish_a_path() -> None: - with _running_module() as h: - h.feed_odom(0.0, 0.0) - h.feed_goal(2.0, 0.0) - - # Only planner paths flow to the path output; odom/goal update gate state. - assert h.forwarded == [] - assert h.gate._robot_xy is not None - assert h.gate._armed_goal is not None - - -# ---- commit window (gate core) ------------------------------------------------- - - def _gate(**config: Any) -> _ReplanGate: + config.setdefault("resample_spacing_m", 0.0) return _ReplanGate(DanLocalPlannerConfig(**config)) -def test_replan_within_lock_is_suppressed() -> None: - gate = _gate(lock_replan=0.5) - gate.on_odom(_odometry(0.0, 0.0)) - first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) - assert gate.on_planner_path(first) is first # cold-start commit - - # MLS re-roots and re-emits, but the robot has only advanced 0.2 m (< L), so - # the replan is held and DanHolonomicTC keeps following the committed path. - gate.on_odom(_odometry(0.2, 0.0)) - replan = _path_from_points([(0.2, 0.0), (5.0, 0.0)]) - assert gate.on_planner_path(replan) is None - - def test_replan_after_advancing_lock_is_published() -> None: gate = _gate(lock_replan=0.5) - gate.on_odom(_odometry(0.0, 0.0)) + gate.on_odom(_odom(0.0, 0.0)) gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) # A replan within the window is held... - gate.on_odom(_odometry(0.3, 0.0)) + gate.on_odom(_odom(0.3, 0.0)) assert gate.on_planner_path(_path_from_points([(0.3, 0.0), (5.0, 0.0)])) is None # ...then once the robot has advanced >= L the next replan commits and # re-anchors, so a subsequent replan is held again until the new window. - gate.on_odom(_odometry(0.6, 0.0)) + gate.on_odom(_odom(0.6, 0.0)) committed = _path_from_points([(0.6, 0.0), (5.0, 0.0)]) - assert gate.on_planner_path(committed) is committed - gate.on_odom(_odometry(0.7, 0.0)) + assert gate.on_planner_path(committed) is not None + gate.on_odom(_odom(0.7, 0.0)) assert gate.on_planner_path(_path_from_points([(0.7, 0.0), (5.0, 0.0)])) is None def test_fresh_click_commits_within_lock() -> None: gate = _gate(lock_replan=0.5) - gate.on_odom(_odometry(0.0, 0.0)) + gate.on_odom(_odom(0.0, 0.0)) gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) # The robot has only crept forward (< L), so the window has not released... - gate.on_odom(_odometry(0.1, 0.0)) + gate.on_odom(_odom(0.1, 0.0)) # ...but a fresh click whose path actually reaches the goal commits anyway # ("the dog agrees when clicked"), and the arm is consumed. gate.on_goal(_point(5.0, 0.0)) clicked = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) - assert gate.on_planner_path(clicked) is clicked + assert gate.on_planner_path(clicked) is not None assert gate._armed_goal is None @@ -259,9 +92,9 @@ def test_fresh_click_does_not_commit_a_stale_replan() -> None: # (a stale in-flight replan for the previous goal) is still held within L, # and the arm stays set until a path that reaches the goal arrives. gate = _gate(lock_replan=0.5) - gate.on_odom(_odometry(0.0, 0.0)) + gate.on_odom(_odom(0.0, 0.0)) gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) - gate.on_odom(_odometry(0.1, 0.0)) + gate.on_odom(_odom(0.1, 0.0)) gate.on_goal(_point(9.0, 0.0)) stale = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) # ends at the OLD goal assert gate.on_planner_path(stale) is None @@ -270,7 +103,7 @@ def test_fresh_click_does_not_commit_a_stale_replan() -> None: def test_empty_path_published_and_resets_gate() -> None: gate = _gate(lock_replan=0.5) - gate.on_odom(_odometry(0.0, 0.0)) + gate.on_odom(_odom(0.0, 0.0)) gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) # An empty path (nothing safe ahead) forwards immediately as a stop and @@ -281,141 +114,32 @@ def test_empty_path_published_and_resets_gate() -> None: # The reset means the next path cold-starts a commit even though the robot # has not advanced a window's worth. - gate.on_odom(_odometry(0.1, 0.0)) + gate.on_odom(_odom(0.1, 0.0)) restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) - assert gate.on_planner_path(restart) is restart + assert gate.on_planner_path(restart) is not None def test_cancel_resets_committed_path() -> None: # A NaN goal is MovementManager's cancel: it disarms AND drops the committed # path, so the next planner path cold-starts a fresh commit within L. gate = _gate(lock_replan=0.5) - gate.on_odom(_odometry(0.0, 0.0)) + gate.on_odom(_odom(0.0, 0.0)) gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) gate.on_goal(_point(math.nan, math.nan)) assert gate._committed is None - gate.on_odom(_odometry(0.1, 0.0)) + gate.on_odom(_odom(0.1, 0.0)) restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) - assert gate.on_planner_path(restart) is restart + assert gate.on_planner_path(restart) is not None def test_lock_replan_zero_commits_every_non_empty_path() -> None: # L = 0 disables gating: every non-empty path commits, even with no advance. gate = _gate(lock_replan=0.0) - gate.on_odom(_odometry(0.0, 0.0)) + gate.on_odom(_odom(0.0, 0.0)) for path in ( _path_from_points([(0.0, 0.0), (1.0, 0.0)]), _path_from_points([(0.0, 0.0), (0.0, 1.0)]), _path_from_points([(0.0, 0.0), (1.0, 1.0)]), ): - assert gate.on_planner_path(path) is path - - -# ---- commit window (module wiring) --------------------------------------------- - - -def test_module_suppresses_replan_within_lock_then_publishes_after_advance() -> None: - with _running_module(lock_replan=0.5) as h: - h.feed_odom(0.0, 0.0) - first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) - h.feed_planner_path(first) - assert h.forwarded == [first] # cold-start commit - - # Replans arriving while the robot is still within the window publish - # nothing. - h.feed_odom(0.2, 0.0) - h.feed_planner_path(_path_from_points([(0.2, 0.0), (5.0, 0.0)])) - h.feed_odom(0.4, 0.0) - h.feed_planner_path(_path_from_points([(0.4, 0.0), (5.0, 0.0)])) - assert h.forwarded == [first] - - # Once the robot has advanced >= L, the next replan is published. - h.feed_odom(0.6, 0.0) - committed = _path_from_points([(0.6, 0.0), (5.0, 0.0)]) - h.feed_planner_path(committed) - assert h.forwarded == [first, committed] - - -def test_module_fresh_click_commits_within_lock() -> None: - with _running_module(lock_replan=0.5) as h: - h.feed_odom(0.0, 0.0) - first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) - h.feed_planner_path(first) - - h.feed_odom(0.1, 0.0) - h.feed_goal(5.0, 0.0) - clicked = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) - h.feed_planner_path(clicked) - assert h.forwarded == [first, clicked] - - -def test_module_empty_path_publishes_and_resets() -> None: - with _running_module(lock_replan=0.5) as h: - h.feed_odom(0.0, 0.0) - first = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) - h.feed_planner_path(first) - - empty = Path(frame_id="world", poses=[]) - h.feed_planner_path(empty) - h.feed_odom(0.1, 0.0) - restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) - h.feed_planner_path(restart) - assert h.forwarded == [first, empty, restart] - - -# ---- smoothing + uniform resampling -------------------------------------------- - - -def _spacings(path: Path) -> list[float]: - return [ - math.hypot(b.x - a.x, b.y - a.y) for a, b in zip(path.poses, path.poses[1:], strict=False) - ] - - -def test_smooth_resamples_straight_path_to_uniform_spacing() -> None: - # A 2-point straight MLS path is densified to points ~resample_spacing_m - # apart, with the endpoints left exactly where MLS put them. - gate = _gate(resample_spacing_m=0.1) - out = gate._smooth(_path_from_points([(0.0, 0.0), (5.0, 0.0)])) - - assert len(out.poses) > 2 - assert (out.poses[0].x, out.poses[0].y) == (0.0, 0.0) - assert (out.poses[-1].x, out.poses[-1].y) == (5.0, 0.0) - assert all(abs(d - 0.1) < 1e-6 for d in _spacings(out)) - - -def test_smooth_rounds_a_corner() -> None: - # A right-angle corner is rounded: no waypoint sits on the exact corner, and - # cutting the corner makes the smoothed path shorter than the raw chords. - gate = _gate(resample_spacing_m=0.1) - out = gate._smooth(_path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)])) - - assert min(math.hypot(p.x - 1.0, p.y - 0.0) for p in out.poses) > 0.05 - assert sum(_spacings(out)) < 2.0 # 1.0 + 1.0 raw cornered length - - -def test_smooth_returns_empty_path_unchanged() -> None: - gate = _gate(resample_spacing_m=0.1) - empty = Path(frame_id="world", poses=[]) - assert gate._smooth(empty) is empty - - -def test_smooth_disabled_returns_path_unchanged() -> None: - # resample_spacing_m == 0 forwards the raw path (same object), no reshaping. - gate = _gate(resample_spacing_m=0.0) - path = _path_from_points([(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)]) - assert gate._smooth(path) is path - - -def test_commit_forwards_and_tracks_the_smoothed_path() -> None: - # The path the gate commits and tracks is the smoothed one, not the raw - # planner chords. - gate = _gate(resample_spacing_m=0.1) - gate.on_odom(_odometry(0.0, 0.0)) - raw = _path_from_points([(0.0, 0.0), (5.0, 0.0)]) - forwarded = gate.on_planner_path(raw) - - assert forwarded is not raw - assert len(forwarded.poses) > 2 - assert all(abs(d - 0.1) < 1e-6 for d in _spacings(forwarded)) + assert gate.on_planner_path(path) is not None diff --git a/dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py b/dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py deleted file mode 100644 index dce50ba587..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py +++ /dev/null @@ -1,540 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Plot trajectory control tick JSONL exports. - -Reads UTF-8 JSONL as documented in ``dimos/navigation/trajectory_control_tick_jsonl.md``. - -Outputs: -- **Summary** (``*_plot.png``): cross-track vs time and speed scatter. -- **Path profile** (``*_path_profile.png``): measured arc length on the x-axis with - |cross-track error| (red), commanded speed, reference-path curvature, and heading error - stacked vertically so you can see where the route turns and where error spikes. -""" - -from __future__ import annotations - -import argparse -import json -import math -from pathlib import Path -import sys -from typing import Literal - -PLOT_REQUIRED_FIELDS = ( - "schema_version", - "ref_time_s", - "meas_time_s", - "e_cross_track_m", - "e_heading_rad", - "commanded_planar_speed_m_s", - "dt_s", -) - -PATH_PROFILE_REQUIRED_FIELDS = PLOT_REQUIRED_FIELDS + ( - "ref_x_m", - "ref_y_m", - "ref_yaw_rad", - "meas_x_m", - "meas_y_m", -) - -MOVING_SPEED_THRESHOLD_M_S = 0.05 - -# Reference-path |curvature| bins (1/m) for printed summaries on moving ticks. -CURVATURE_BIN_EDGES = (0.0, 0.05, 0.2, 0.5, float("inf")) -CURVATURE_BIN_LABELS = ( - "straight |kappa|<0.05", - "gentle 0.05-0.2", - "medium 0.2-0.5", - "tight |kappa|>=0.5", -) - - -def _load_ticks(path: Path, *, required_fields: tuple[str, ...] = PLOT_REQUIRED_FIELDS) -> list[dict[str, object]]: - rows: list[dict[str, object]] = [] - with path.open(encoding="utf-8") as f: - for line in f: - line = line.strip() - if not line: - continue - rows.append(json.loads(line)) - if not rows: - raise SystemExit(f"No JSON lines in {path}") - for i, row in enumerate(rows): - ver = row.get("schema_version") - if ver != 1: - raise SystemExit(f"Line {i + 1}: unsupported schema_version {ver!r} (expected 1)") - missing = [field for field in required_fields if field not in row] - if missing: - raise SystemExit(f"Line {i + 1}: missing required fields: {', '.join(missing)}") - return rows - - -def _angle_diff(from_rad: float, to_rad: float) -> float: - return math.atan2(math.sin(to_rad - from_rad), math.cos(to_rad - from_rad)) - - -def _col(rows: list[dict[str, object]], name: str) -> list[float]: - return [float(row[name]) for row in rows] - - -def _time_basis(rows: list[dict[str, object]], ref_t, meas_t) -> object: - import numpy as np - - return ( - ref_t - if np.any(np.diff(ref_t) != 0) or np.max(np.abs(ref_t)) > 0 - else meas_t - ) - - -def _print_cross_track_summary(rows: list[dict[str, object]], inp: Path) -> None: - moving = [ - row - for row in rows - if float(row["commanded_planar_speed_m_s"]) > MOVING_SPEED_THRESHOLD_M_S - ] - if not moving: - print(f"{inp.name}: no moving ticks (cmd speed > {MOVING_SPEED_THRESHOLD_M_S} m/s)") - return - - abs_ct = [abs(float(row["e_cross_track_m"])) for row in moving] - signed_ct = [float(row["e_cross_track_m"]) for row in moving] - abs_ct_sorted = sorted(abs_ct) - p95_index = max(0, int(math.ceil(0.95 * len(abs_ct_sorted))) - 1) - rms = math.sqrt(sum(v * v for v in signed_ct) / len(signed_ct)) - - print(f"Cross-track summary for {inp.name} ({len(moving)} moving / {len(rows)} ticks):") - print(f" mean |e_cross_track_m| = {sum(abs_ct) / len(abs_ct):.4f} m") - print(f" max |e_cross_track_m| = {max(abs_ct):.4f} m") - print(f" p95 |e_cross_track_m| = {abs_ct_sorted[p95_index]:.4f} m") - print(f" RMS e_cross_track_m = {rms:.4f} m (signed, industry reporting style)") - print( - " Note: e_cross_track is lateral offset vs the controller reference pose " - "(lookahead on path), not arc-length progress." - ) - - -def _plot_cross_track( - *, - inp: Path, - out: Path, - rows: list[dict[str, object]], - n: int, -) -> None: - import matplotlib.pyplot as plt - import numpy as np - - def col(name: str) -> np.ndarray: - return np.array([float(r[name]) for r in rows], dtype=np.float64) - - speed_m_s = col("commanded_planar_speed_m_s") - ref_t = col("ref_time_s") - meas_t = col("meas_time_s") - e_xt = col("e_cross_track_m") - e_h = col("e_heading_rad") - dt_s = col("dt_s") - abs_e_xt = np.abs(e_xt) - - t_plot = _time_basis(rows, ref_t, meas_t) - - fig, axes = plt.subplots(2, 2, figsize=(10, 8), constrained_layout=True) - ax_sc, ax_ts_ct, ax_ts_speed, ax_ts_h = axes[0, 0], axes[0, 1], axes[1, 0], axes[1, 1] - - ax_sc.scatter(abs_e_xt, speed_m_s, c=t_plot, cmap="viridis", s=36, zorder=2) - ax_sc.plot(abs_e_xt, speed_m_s, color="0.5", linewidth=0.8, alpha=0.7, zorder=1) - ax_sc.set_xlabel("|e_cross_track_m| (m)") - ax_sc.set_ylabel("Commanded planar speed (m/s)") - ax_sc.set_title("Speed vs |cross-track error| (color = time)") - ax_sc.grid(True, alpha=0.3) - - ax_ts_ct.plot(t_plot, e_xt, marker="o", markersize=3, linewidth=1, color="C1", label="e_cross_track_m") - ax_ts_ct.axhline(0.0, color="0.4", linewidth=0.8, linestyle=":") - ax_ts_ct.set_xlabel("ref_time_s or meas_time_s (s)") - ax_ts_ct.set_ylabel("e_cross_track_m (m)") - ax_ts_ct.set_title("Cross-track error over time (+ = left of reference)") - ax_ts_ct.grid(True, alpha=0.3) - ax_ts_ct.legend(loc="best", fontsize="small") - - ax_ts_speed.plot(t_plot, speed_m_s, marker="o", markersize=3, linewidth=1, color="C0") - ax_ts_speed.set_xlabel("ref_time_s or meas_time_s (s)") - ax_ts_speed.set_ylabel("commanded_planar_speed_m_s") - ax_ts_speed.set_title("Commanded speed over time") - ax_ts_speed.grid(True, alpha=0.3) - - ax_ts_h.plot(t_plot, e_h, marker="o", markersize=3, linewidth=1, color="C3", label="e_heading_rad") - ax_ts_h.set_xlabel("ref_time_s or meas_time_s (s)") - ax_ts_h.set_ylabel("e_heading_rad (rad)") - ax_ts_h.set_title("Heading error and control dt") - ax_ts_h.grid(True, alpha=0.3) - ax_dt = ax_ts_h.twinx() - ax_dt.plot(t_plot, dt_s, color="C2", linestyle="--", linewidth=1, alpha=0.85, label="dt_s") - ax_dt.set_ylabel("dt_s", color="C2") - ax_dt.tick_params(axis="y", labelcolor="C2") - h1, l1 = ax_ts_h.get_legend_handles_labels() - h2, l2 = ax_dt.get_legend_handles_labels() - ax_ts_h.legend(h1 + h2, l1 + l2, loc="best", fontsize="small") - - fig.suptitle( - f"Cross-track tracking: {inp.name} ({n} samples)", - fontsize=11, - ) - - out.parent.mkdir(parents=True, exist_ok=True) - fig.savefig(out, dpi=150) - plt.close(fig) - - -def _derive_path_profile(rows: list[dict[str, object]]) -> dict[str, object]: - """Build arc-length, curvature, and segment-since-stop series from tick poses.""" - import numpy as np - - n = len(rows) - ref_x = np.array(_col(rows, "ref_x_m"), dtype=np.float64) - ref_y = np.array(_col(rows, "ref_y_m"), dtype=np.float64) - ref_yaw = np.array(_col(rows, "ref_yaw_rad"), dtype=np.float64) - meas_x = np.array(_col(rows, "meas_x_m"), dtype=np.float64) - meas_y = np.array(_col(rows, "meas_y_m"), dtype=np.float64) - speed = np.array(_col(rows, "commanded_planar_speed_m_s"), dtype=np.float64) - abs_ct = np.abs(np.array(_col(rows, "e_cross_track_m"), dtype=np.float64)) - e_h = np.array(_col(rows, "e_heading_rad"), dtype=np.float64) - - ds_meas = np.zeros(n, dtype=np.float64) - ds_ref = np.zeros(n, dtype=np.float64) - dpsi_ref = np.zeros(n, dtype=np.float64) - for i in range(1, n): - ds_meas[i] = math.hypot(meas_x[i] - meas_x[i - 1], meas_y[i] - meas_y[i - 1]) - ds_ref[i] = math.hypot(ref_x[i] - ref_x[i - 1], ref_y[i] - ref_y[i - 1]) - dpsi_ref[i] = _angle_diff(ref_yaw[i - 1], ref_yaw[i]) - - s_meas = np.cumsum(ds_meas) - kappa_ref = np.zeros(n, dtype=np.float64) - for i in range(1, n): - if ds_ref[i] > 1e-6: - kappa_ref[i] = dpsi_ref[i] / ds_ref[i] - - # Smooth reference curvature so single-tick lookahead jumps do not dominate the plot. - smooth_window = 5 - kappa_smooth = np.zeros(n, dtype=np.float64) - for i in range(n): - j0 = max(0, i - smooth_window + 1) - ds_sum = float(np.sum(ds_ref[j0 : i + 1])) - psi_sum = float(np.sum(np.abs(dpsi_ref[j0 : i + 1]))) - if ds_sum > 1e-6: - kappa_smooth[i] = psi_sum / ds_sum - - stopped = speed <= MOVING_SPEED_THRESHOLD_M_S - moving = ~stopped - - segment_s = np.zeros(n, dtype=np.float64) - turn_since_stop = np.zeros(n, dtype=np.float64) - for i in range(1, n): - if stopped[i]: - segment_s[i] = 0.0 - turn_since_stop[i] = 0.0 - else: - segment_s[i] = segment_s[i - 1] + ds_meas[i] - turn_since_stop[i] = turn_since_stop[i - 1] + abs(dpsi_ref[i]) - - return { - "s_meas_m": s_meas, - "segment_s_m": segment_s, - "turn_since_stop_rad": turn_since_stop, - "kappa_ref_rad_m": kappa_ref, - "abs_kappa_ref_rad_m": np.abs(kappa_ref), - "kappa_smooth_rad_m": kappa_smooth, - "abs_kappa_smooth_rad_m": kappa_smooth, - "commanded_speed_m_s": speed, - "abs_cross_track_m": abs_ct, - "e_heading_rad": e_h, - "stopped": stopped, - "moving": moving, - } - - -def _print_path_profile_summary(rows: list[dict[str, object]], inp: Path, profile: dict[str, object]) -> None: - import numpy as np - - moving = profile["moving"] - assert isinstance(moving, np.ndarray) - if not np.any(moving): - print(f"{inp.name}: path profile - no moving ticks") - return - - abs_ct = profile["abs_cross_track_m"] - kappa = profile["abs_kappa_smooth_rad_m"] - speed = profile["commanded_speed_m_s"] - s_meas = profile["s_meas_m"] - assert isinstance(abs_ct, np.ndarray) - assert isinstance(kappa, np.ndarray) - assert isinstance(speed, np.ndarray) - assert isinstance(s_meas, np.ndarray) - - print(f"Path-profile summary for {inp.name} ({int(moving.sum())} moving ticks):") - print(" By reference-path |curvature| (where error tends to concentrate):") - for lo, hi, label in zip( - CURVATURE_BIN_EDGES[:-1], - CURVATURE_BIN_EDGES[1:], - CURVATURE_BIN_LABELS, - strict=True, - ): - mask = moving & (kappa >= lo) & (kappa < hi) - count = int(mask.sum()) - if count == 0: - print(f" {label}: n=0") - continue - vals = abs_ct[mask] - print( - f" {label}: n={count}, mean|CTE|={float(vals.mean()):.4f} m, " - f"max|CTE|={float(vals.max()):.4f} m" - ) - - # Find worst 0.5 m windows along measured arc length (moving ticks only). - window_m = 0.5 - s_mov = s_meas[moving] - ct_mov_arr = abs_ct[moving] - if len(s_mov) >= 2: - worst: list[tuple[float, float, float]] = [] - i0 = 0 - for i1 in range(len(s_mov)): - while i0 < i1 and s_mov[i1] - s_mov[i0] > window_m: - i0 += 1 - if i1 > i0: - seg = ct_mov_arr[i0 : i1 + 1] - worst.append((float(seg.mean()), float(s_mov[i0]), float(s_mov[i1]))) - worst.sort(reverse=True) - print(f" Worst {window_m:.1f} m windows by mean |CTE| (measured arc length):") - for rank, (mean_ct, s0, s1) in enumerate(worst[:5], start=1): - print(f" #{rank}: s={s0:.2f}-{s1:.2f} m, mean|CTE|={mean_ct:.4f} m") - - i_max = int(np.argmax(abs_ct[moving])) - mov_idx = int(np.flatnonzero(moving)[i_max]) - print( - f" Global max |CTE|={float(abs_ct[mov_idx]):.4f} m at s={float(s_meas[mov_idx]):.2f} m " - f"(|kappa|={float(kappa[mov_idx]):.3f} 1/m, speed={float(speed[mov_idx]):.2f} m/s)" - ) - - -def _shade_stopped_regions(axes: list[object], s: object, stopped: object) -> None: - import numpy as np - - assert isinstance(s, np.ndarray) - assert isinstance(stopped, np.ndarray) - if len(s) == 0: - return - in_stop = False - start_s = 0.0 - for i in range(len(s)): - if stopped[i] and not in_stop: - in_stop = True - start_s = float(s[i]) - elif not stopped[i] and in_stop: - in_stop = False - end_s = float(s[i]) - for ax in axes: - ax.axvspan(start_s, end_s, color="0.92", zorder=0) # type: ignore[attr-defined] - if in_stop: - end_s = float(s[-1]) - for ax in axes: - ax.axvspan(start_s, end_s, color="0.92", zorder=0) # type: ignore[attr-defined] - - -def _plot_path_profile( - *, - inp: Path, - out: Path, - rows: list[dict[str, object]], - n: int, - profile: dict[str, object], -) -> None: - import matplotlib.pyplot as plt - import numpy as np - - s = profile["s_meas_m"] - abs_ct = profile["abs_cross_track_m"] - speed = profile["commanded_speed_m_s"] - kappa = profile["abs_kappa_smooth_rad_m"] - turn_stop = profile["turn_since_stop_rad"] - e_h = profile["e_heading_rad"] - stopped = profile["stopped"] - moving = profile["moving"] - assert isinstance(s, np.ndarray) - assert isinstance(abs_ct, np.ndarray) - assert isinstance(speed, np.ndarray) - assert isinstance(kappa, np.ndarray) - assert isinstance(turn_stop, np.ndarray) - assert isinstance(e_h, np.ndarray) - assert isinstance(stopped, np.ndarray) - assert isinstance(moving, np.ndarray) - - fig_h = max(12.0, 0.004 * float(s[-1]) if len(s) else 12.0) - fig = plt.figure(figsize=(11, fig_h)) - gs = fig.add_gridspec( - 5, - 1, - height_ratios=[2.2, 1.2, 1.2, 1.0, 1.1], - hspace=0.28, - ) - ax_ct = fig.add_subplot(gs[0]) - ax_speed = fig.add_subplot(gs[1], sharex=ax_ct) - ax_kappa = fig.add_subplot(gs[2], sharex=ax_ct) - ax_turn = fig.add_subplot(gs[3], sharex=ax_ct) - ax_scatter = fig.add_subplot(gs[4]) - axes = [ax_ct, ax_speed, ax_kappa, ax_turn] - - _shade_stopped_regions(axes, s, stopped) - - ax_ct.fill_between(s, 0.0, abs_ct, color="#d62728", alpha=0.35, zorder=1) - ax_ct.plot(s, abs_ct, color="#b01020", linewidth=1.2, label="|e_cross_track_m|", zorder=2) - i_peak = int(np.argmax(abs_ct)) - ax_ct.scatter([float(s[i_peak])], [float(abs_ct[i_peak])], color="#b01020", s=40, zorder=3) - ax_ct.annotate( - f"max {abs_ct[i_peak]:.3f} m @ {s[i_peak]:.1f} m", - xy=(float(s[i_peak]), float(abs_ct[i_peak])), - xytext=(8, 8), - textcoords="offset points", - fontsize=8, - color="#b01020", - ) - ax_ct.set_ylabel("|cross-track| (m)") - ax_ct.set_title("Cross-track error along measured path (red)") - ax_ct.grid(True, alpha=0.3) - ax_ct.legend(loc="upper right", fontsize="small") - - ax_speed.plot(s, speed, color="C0", linewidth=1.0) - ax_speed.set_ylabel("cmd speed (m/s)") - ax_speed.set_title("Commanded planar speed") - ax_speed.grid(True, alpha=0.3) - - ax_kappa.plot(s, kappa, color="C2", linewidth=1.0) - kappa_cap = float(np.percentile(kappa[moving], 95)) * 1.2 if np.any(moving) else 1.0 - if kappa_cap > 0.05: - ax_kappa.set_ylim(0.0, max(kappa_cap, 0.5)) - ax_kappa.set_ylabel("|kappa| (1/m)") - ax_kappa.set_title("Reference-path curvature, 5-tick smoothed (|d yaw| / ds on ref)") - ax_kappa.grid(True, alpha=0.3) - - ax_turn.plot(s, np.degrees(turn_stop), color="C4", linewidth=1.0) - ax_turn.set_ylabel("deg since stop") - ax_turn.set_title("Cumulative |ref heading change| since last stop") - ax_turn.grid(True, alpha=0.3) - - ax_turn.set_xlabel("Measured arc length s (m)") - - sc = ax_scatter.scatter( - kappa[moving], - abs_ct[moving], - c=speed[moving], - cmap="viridis", - s=14, - alpha=0.75, - ) - ax_scatter.set_xlabel("|kappa| (1/m)") - ax_scatter.set_ylabel("|cross-track| (m)") - ax_scatter.set_title("Curvature vs |cross-track| on moving ticks (color = cmd speed)") - ax_scatter.grid(True, alpha=0.3) - cbar = fig.colorbar(sc, ax=ax_scatter, fraction=0.046, pad=0.02) - cbar.set_label("cmd speed (m/s)") - - fig.suptitle( - f"Path profile: {inp.name} ({n} ticks, gray = stopped)", - fontsize=11, - ) - - out.parent.mkdir(parents=True, exist_ok=True) - fig.subplots_adjust(left=0.08, right=0.96, top=0.94, bottom=0.06, hspace=0.38) - fig.savefig(out, dpi=150) - plt.close(fig) - - -def main() -> None: - parser = argparse.ArgumentParser( - description="Plot trajectory control tick JSONL (cross-track and path-profile views)." - ) - parser.add_argument( - "jsonl", - type=Path, - help="Path to trajectory control tick export (.jsonl).", - ) - parser.add_argument( - "-o", - "--output", - type=Path, - default=None, - help="Summary PNG path (default: _plot.png).", - ) - parser.add_argument( - "--profile-output", - type=Path, - default=None, - help="Path-profile PNG (default: _path_profile.png).", - ) - parser.add_argument( - "--plot", - choices=("summary", "profile", "both"), - default="both", - help="Which figures to generate (default: both).", - ) - args = parser.parse_args() - inp = args.jsonl.expanduser().resolve() - if not inp.is_file(): - raise SystemExit(f"Not a file: {inp}") - - try: - import matplotlib - - matplotlib.use("Agg") - except ImportError as e: - raise SystemExit( - "matplotlib and numpy are required. Example: " - "uv run --with matplotlib python " - "dimos/navigation/holonomic_trajectory_controller/docs/" - "plot_trajectory_control_ticks.py ..." - ) from e - - plot_mode: Literal["summary", "profile", "both"] = args.plot - need_profile = plot_mode in ("profile", "both") - required = PATH_PROFILE_REQUIRED_FIELDS if need_profile else PLOT_REQUIRED_FIELDS - rows = _load_ticks(inp, required_fields=required) - n = len(rows) - - if args.output is None: - summary_out = inp.parent / f"{inp.stem}_plot.png" - else: - summary_out = args.output.expanduser().resolve() - - if args.profile_output is None: - profile_out = inp.parent / f"{inp.stem}_path_profile.png" - else: - profile_out = args.profile_output.expanduser().resolve() - - if plot_mode in ("summary", "both"): - _print_cross_track_summary(rows, inp) - _plot_cross_track(inp=inp, out=summary_out, rows=rows, n=n) - print(f"Wrote {summary_out}") - - if need_profile: - profile = _derive_path_profile(rows) - _print_path_profile_summary(rows, inp, profile) - _plot_path_profile(inp=inp, out=profile_out, rows=rows, n=n, profile=profile) - print(f"Wrote {profile_out}") - - -if __name__ == "__main__": - try: - main() - except BrokenPipeError: - sys.exit(0) diff --git a/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md b/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md deleted file mode 100644 index b0668b6f96..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_control_tick_jsonl.md +++ /dev/null @@ -1,78 +0,0 @@ -# Trajectory control tick JSONL export - -## Format - -- **Encoding:** UTF-8. -- **Layout:** one JSON object per line (JSON Lines, `.jsonl`). Empty lines are ignored when reading. -- **Schema version:** integer field `schema_version` on every object. Current value is `1`. Consumers should reject or quarantine unknown major versions. - -## Key order and names - -After `schema_version`, object keys follow the field order of `TrajectoryControlTick` in `trajectory_control_tick_log.py`. Producers use that order for stable diffs and stream inspection; parsers must not rely on key order. - -## Fields - -| Key | Type | Unit | Meaning | -|-----|------|------|---------| -| `schema_version` | int | - | Export schema; `1` for this revision. | -| `ref_time_s` | number | s | Reference sample time basis (producer-defined; see trajectory types). | -| `ref_x_m` | number | m | Reference pose x in plan horizontal frame. | -| `ref_y_m` | number | m | Reference pose y in plan horizontal frame. | -| `ref_yaw_rad` | number | rad | Reference yaw in plan frame. | -| `ref_twist_linear_x_m_s` | number | m/s | Reference twist linear x (body). | -| `ref_twist_linear_y_m_s` | number | m/s | Reference twist linear y (body). | -| `ref_twist_linear_z_m_s` | number | m/s | Reference twist linear z (body). | -| `ref_twist_angular_x_rad_s` | number | rad/s | Reference twist angular x (body). | -| `ref_twist_angular_y_rad_s` | number | rad/s | Reference twist angular y (body). | -| `ref_twist_angular_z_rad_s` | number | rad/s | Reference twist angular z (body). | -| `meas_time_s` | number | s | Measured sample time basis. | -| `meas_x_m` | number | m | Measured pose x (plan frame). | -| `meas_y_m` | number | m | Measured pose y (plan frame). | -| `meas_yaw_rad` | number | rad | Measured yaw (plan frame). | -| `meas_twist_linear_x_m_s` | number | m/s | Measured twist linear x (body). Live `LocalPlanner` logs estimate this from consecutive odom poses when timestamps advance. | -| `meas_twist_linear_y_m_s` | number | m/s | Measured twist linear y (body). Live `LocalPlanner` logs estimate this from consecutive odom poses when timestamps advance. | -| `meas_twist_linear_z_m_s` | number | m/s | Measured twist linear z (body). | -| `meas_twist_angular_x_rad_s` | number | rad/s | Measured twist angular x (body). | -| `meas_twist_angular_y_rad_s` | number | rad/s | Measured twist angular y (body). | -| `meas_twist_angular_z_rad_s` | number | rad/s | Measured twist angular z (body). Live `LocalPlanner` logs estimate this from consecutive odom yaws when timestamps advance. | -| `e_along_track_m` | number | m | Along-track error (see `trajectory_metrics`). | -| `e_cross_track_m` | number | m | Cross-track error. | -| `e_heading_rad` | number | rad | Heading error. | -| `planar_position_divergence_m` | number | m | Planar position divergence (speed-vs-divergence plots). | -| `cmd_linear_x_m_s` | number | m/s | Commanded linear x (published this tick). | -| `cmd_linear_y_m_s` | number | m/s | Commanded linear y. | -| `cmd_linear_z_m_s` | number | m/s | Commanded linear z. | -| `cmd_angular_x_rad_s` | number | rad/s | Commanded angular x. | -| `cmd_angular_y_rad_s` | number | rad/s | Commanded angular y. | -| `cmd_angular_z_rad_s` | number | rad/s | Commanded angular z. | -| `commanded_planar_speed_m_s` | number | m/s | Planar speed from command (`hypot(linear.x, linear.y)` in body). | -| `dt_s` | number | s | Control period for this tick. | -| `wall_time_s` | number or null | s | Optional wall clock. | -| `sim_time_s` | number or null | s | Optional simulation time. | - -## API - -- `trajectory_control_tick_to_jsonl_dict` and `JsonlTrajectoryControlTickSink` in `dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export`. -- `LocalPlanner` writes live JSONL when `GlobalConfig.local_planner_trajectory_tick_log_path` is set to a file path. - -## Plotting - -Use `planar_position_divergence_m` vs `commanded_planar_speed_m_s` (and time series on `ref_time_s` or `meas_time_s`); `pandas.read_json(..., lines=True)` accepts this format if you use pandas. - -**Control rate:** after you have exports, relate `dt_s` and optional `wall_time_s` to a sensible `local_planner_control_rate_hz` and to plant delay. - -**Live navigation export:** set `local_planner_trajectory_tick_log_path` in `GlobalConfig`, run the holonomic path follower, then plot the generated file. This is the normal path for comparing speed vs divergence on a robot or replay harness. - -**Built-in recipe:** from the repository root, run the plot script with `uv run --with matplotlib`. Example using the in-tree sample JSONL: - -```bash -uv run --with matplotlib python dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py \ - dimos/navigation/fixtures/trajectory_control_ticks_sample.jsonl \ - -o dimos/navigation/fixtures/trajectory_control_ticks_sample_plot.png -``` - -For your own export: - -```bash -uv run --with matplotlib python dimos/navigation/holonomic_trajectory_controller/docs/plot_trajectory_control_ticks.py path/to/ticks.jsonl -o out.png -``` diff --git a/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md b/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md deleted file mode 100644 index 2c4ab472ec..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/docs/trajectory_run_profiles.md +++ /dev/null @@ -1,66 +0,0 @@ -# Operator run-profile contract - -## Purpose - -- **What:** Named movement envelopes (`walk`, `trot`, `run_conservative`, - `run_verified`) that an operator, agent, CLI, or MCP caller can request - instead of a loose `planner_robot_speed` number plus a fistful of - `--local-planner-*` flags. -- **Where:** `dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles`. -- **Status:** data + validation, plus Go2 locomotion-mode wiring. The profile's - `required_locomotion_mode` is wired into `GO2Connection` start-up via - `GlobalConfig.go2_run_profile`. The live stack applies profile speed/accel/yaw - limits through `LocalPlanner`. - -## Data model — `RunProfile` - -All numeric fields are **upper bounds in SI units** in the same conventions as -the live limit types (`HolonomicCommandLimits`, `PathSpeedProfileLimits`): -planar speed is `hypot(vx, vy)` in the body frame; yaw rate is `wz`. - -| Field | Unit | Meaning | -|-------|------|---------| -| `name` | str | Profile identity (registry key must match). | -| `requested_planner_speed_m_s` | m/s | Requested cruise speed (still curvature/decel-capped downstream). | -| `max_tangent_accel_m_s2` | m/s² | Along-path acceleration cap for the speed profile. | -| `max_normal_accel_m_s2` | m/s² | Centripetal (curvature) acceleration cap. | -| `goal_decel_m_s2` | m/s² | Deceleration approaching the goal. | -| `max_planar_cmd_accel_m_s2` | m/s² | Command slew cap on planar `cmd_vel`. | -| `max_yaw_rate_rad_s` | rad/s | Yaw-rate cap. | -| `max_yaw_accel_rad_s2` | rad/s² | Yaw-acceleration cap. | -| `required_locomotion_mode` | str | Embodiment mode to activate before high-speed motion (Go2: `default` or `rage`). | -| `description` | str | Human-readable note. | - -Adapters onto the existing validated limit types: - -- `command_limits() -> HolonomicCommandLimits` -- `path_speed_profile_limits_at(max_speed_m_s) -> PathSpeedProfileLimits` - -### Validation (rejected at construction) - -- Every speed/acceleration/yaw field must be **finite and strictly positive**. -- `name` and `required_locomotion_mode` must be non-empty. - -## Profile resolution - -`GO2_RUN_PROFILES.get(name)` looks up a profile by name. Unknown names raise -`RunProfileError` with a message listing known profiles. - -Per-goal overrides are resolved in `LocalPlanner._resolve_run_envelope`: the goal -profile name wins over `GlobalConfig.go2_run_profile`. The registry default name -(`walk`) keeps legacy walking behavior via `_default_run_envelope()`. - -## Go2 profiles (`GO2_RUN_PROFILES`) - -Caps are **conservative nominal engineering envelopes, not measured hardware -performance**. `walk` reproduces today's `LocalPlanner` default (0.55 m/s and -the `local_planner_*` defaults). Default `relative_move(...)` stays `walk`. - -| Profile | Speed (m/s) | Go2 mode | -|---------|-------------|----------| -| `walk` | 0.55 | `default` | -| `trot` | 1.0 | `default` | -| `run_conservative` | 1.5 | `default` | -| `run_verified` | 2.5 | `rage` | - -Set `GO2_RUN_PROFILE=` to select any profile at startup. diff --git a/dimos/navigation/holonomic_trajectory_controller/path_distancer.py b/dimos/navigation/holonomic_trajectory_controller/path_distancer.py deleted file mode 100644 index 23f373180a..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/path_distancer.py +++ /dev/null @@ -1,131 +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. - -from typing import cast - -import numpy as np -from numpy.typing import NDArray - -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.holonomic_trajectory_controller.trajectory_metrics import PolylineProjection, project_to_polyline - - -class PathDistancer: - _lookahead_dist: float = 0.5 - _path: NDArray[np.float64] - _cumulative_dists: NDArray[np.float64] - - def __init__(self, path: Path) -> None: - self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) - self._cumulative_dists = _make_cumulative_distance_array(self._path) - - def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: - """ - Given a path, and a precomputed array of cumulative distances, find the - point which is `lookahead_dist` ahead of the current point. - """ - - if start_idx >= len(self._path) - 1: - return cast("NDArray[np.float64]", self._path[-1]) - - # Distance from path[0] to path[start_idx]. - base_dist = self._cumulative_dists[start_idx - 1] if start_idx > 0 else 0.0 - target_dist = base_dist + self._lookahead_dist - - # Binary search: cumulative_dists[i] = distance from path[0] to path[i+1] - idx = int(np.searchsorted(self._cumulative_dists, target_dist)) - - if idx >= len(self._cumulative_dists): - return cast("NDArray[np.float64]", self._path[-1]) - - # Interpolate within segment from path[idx] to path[idx+1]. - prev_cum_dist = self._cumulative_dists[idx - 1] if idx > 0 else 0.0 - segment_dist = self._cumulative_dists[idx] - prev_cum_dist - remaining_dist = target_dist - prev_cum_dist - - if segment_dist > 0: - t = remaining_dist / segment_dist - return cast( - "NDArray[np.float64]", - self._path[idx] + t * (self._path[idx + 1] - self._path[idx]), - ) - - return cast("NDArray[np.float64]", self._path[idx]) - - @property - def lookahead_distance_m(self) -> float: - return self._lookahead_dist - - @property - def path_length_m(self) -> float: - if len(self._path) < 2: - return 0.0 - return float(self._cumulative_dists[-1]) - - def point_at_progress(self, progress_m: float) -> NDArray[np.float64]: - if len(self._path) == 0: - raise ValueError("path must be non-empty") - if len(self._path) == 1: - return cast("NDArray[np.float64]", self._path[0].copy()) - - s = float(np.clip(progress_m, 0.0, self.path_length_m)) - idx = int(np.searchsorted(self._cumulative_dists, s)) - if idx >= len(self._cumulative_dists): - return cast("NDArray[np.float64]", self._path[-1].copy()) - - prev_s = self._cumulative_dists[idx - 1] if idx > 0 else 0.0 - segment_s = self._cumulative_dists[idx] - prev_s - if segment_s <= 1e-12: - return cast("NDArray[np.float64]", self._path[idx].copy()) - alpha = (s - prev_s) / segment_s - point = self._path[idx] + alpha * (self._path[idx + 1] - self._path[idx]) - return cast("NDArray[np.float64]", point) - - def yaw_at_progress(self, progress_m: float) -> float: - if len(self._path) < 2: - return 0.0 - s = float(np.clip(progress_m, 0.0, self.path_length_m)) - idx = int(np.searchsorted(self._cumulative_dists, s)) - idx = min(max(idx, 0), len(self._path) - 2) - direction = self._path[idx + 1] - self._path[idx] - return float(np.arctan2(direction[1], direction[0])) - - def project(self, pos: NDArray[np.float64]) -> PolylineProjection: - return project_to_polyline(float(pos[0]), float(pos[1]), self._path) - - def distance_to_goal(self, current_pos: NDArray[np.float64]) -> float: - return float(np.linalg.norm(self._path[-1] - current_pos)) - - def get_distance_to_path(self, pos: NDArray[np.float64]) -> float: - index = self.find_closest_point_index(pos) - return float(np.linalg.norm(self._path[index] - pos)) - - def find_closest_point_index(self, pos: NDArray[np.float64]) -> int: - """Find the index of the closest point on the path.""" - distances = np.linalg.norm(self._path - pos, axis=1) - return int(np.argmin(distances)) - - -def _make_cumulative_distance_array(array: NDArray[np.float64]) -> NDArray[np.float64]: - """ - For an array representing 2D points, create an array of all the distances - between the points. - """ - - if len(array) < 2: - return np.array([0.0]) - - segments = array[1:] - array[:-1] - segment_dists = np.linalg.norm(segments, axis=1) - return np.cumsum(segment_dists) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py b/dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py deleted file mode 100644 index 8f3c739d19..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/test_holonomic_path_follower.py +++ /dev/null @@ -1,555 +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. - -"""``_HolonomicPathFollower`` tracking law, path-speed caps, and closed loop. - -Relocated from ``dannav/test_local_planner_path_controller.py`` for the -``DanHolonomicTC`` control core. The costmap/yaw-lock and differential cases are -gone with the planner; the holonomic tracking law, path-speed envelope, and -arrival behavior remain. Movement envelopes that the old tests expressed through -``GlobalConfig`` accel fields now come from the run-profile registry, so -``_install_envelope`` drives the core with an explicit ``ActiveRunEnvelope`` for -the geometry-cap unit tests (the one seam that needs arbitrary accel values). -""" - -from __future__ import annotations - -import math -import time - -import numpy as np -import pytest - -from dimos.core.global_config import GlobalConfig -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.nav_msgs.Path import Path -from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import ( - CommandEnvelopeOverrides, - HolonomicPathController, -) -from dimos.navigation.holonomic_trajectory_controller.module import ( - ActiveRunEnvelope, - DanHolonomicTCConfig, - _HolonomicPathFollower, -) -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer -from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import ( - HolonomicTrackingController, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( - PathSpeedProfileLimits, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import ( - TrajectoryMeasuredSample, - TrajectoryReferenceSample, -) -from dimos.utils.trigonometry import angle_diff - - -def _planar_speed_m_s(cmd: Twist) -> float: - return math.hypot(float(cmd.linear.x), float(cmd.linear.y)) - - -def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) - - -def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: - return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), - ) - - -def _path_from_points(points: list[tuple[float, float]]) -> Path: - poses: list[PoseStamped] = [] - for index, point in enumerate(points): - if index + 1 < len(points): - next_point = points[index + 1] - yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) - else: - prev_point = points[index - 1] - yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) - poses.append(_pose_stamped(point[0], point[1], yaw)) - return Path(frame_id="map", poses=poses) - - -def _make_follower(**overrides: object) -> _HolonomicPathFollower: - return _HolonomicPathFollower(DanHolonomicTCConfig(**overrides)) - - -def _install_envelope( - core: _HolonomicPathFollower, - *, - speed_m_s: float, - max_tangent_accel_m_s2: float, - max_normal_accel_m_s2: float, - goal_decel_m_s2: float, - max_planar_cmd_accel_m_s2: float = 8.0, - max_yaw_accel_rad_s2: float = 8.0, - max_yaw_rate_rad_s: float | None = None, -) -> None: - """Drive the core with an explicit movement envelope. - - The run-profile registry is the only production envelope source, so this is - the test seam for the arbitrary accel values the old ``GlobalConfig`` accel - fields used to provide. ``_apply_run_envelope`` re-seats the controller and - invalidates the cached path-speed profile, exactly as ``set_run_profile``. - """ - core._apply_run_envelope( - ActiveRunEnvelope( - profile_name="test", - speed_m_s=speed_m_s, - path_limits=PathSpeedProfileLimits( - max_speed_m_s=speed_m_s, - max_tangent_accel_m_s2=max_tangent_accel_m_s2, - max_normal_accel_m_s2=max_normal_accel_m_s2, - ), - goal_decel_m_s2=goal_decel_m_s2, - command_overrides=CommandEnvelopeOverrides( - max_yaw_rate_rad_s=speed_m_s if max_yaw_rate_rad_s is None else max_yaw_rate_rad_s, - max_planar_cmd_accel_m_s2=max_planar_cmd_accel_m_s2, - max_yaw_accel_rad_s2=max_yaw_accel_rad_s2, - ), - ) - ) - - -def _follower_with_envelope( - *, - speed_m_s: float, - max_tangent_accel_m_s2: float, - max_normal_accel_m_s2: float, - goal_decel_m_s2: float, - goal_tolerance: float = 0.01, -) -> _HolonomicPathFollower: - core = _make_follower(goal_tolerance=goal_tolerance) - _install_envelope( - core, - speed_m_s=speed_m_s, - max_tangent_accel_m_s2=max_tangent_accel_m_s2, - max_normal_accel_m_s2=max_normal_accel_m_s2, - goal_decel_m_s2=goal_decel_m_s2, - ) - return core - - -def _integrate_holonomic_pose( - x_m: float, - y_m: float, - yaw_rad: float, - cmd_body: Twist, - dt_s: float, -) -> tuple[float, float, float]: - """Integrate body-frame cmd_vel in the world frame (test harness only).""" - vx = float(cmd_body.linear.x) - vy = float(cmd_body.linear.y) - wz = float(cmd_body.angular.z) - c = math.cos(yaw_rad) - s = math.sin(yaw_rad) - x_m += (c * vx - s * vy) * dt_s - y_m += (s * vx + c * vy) * dt_s - yaw_rad += wz * dt_s - yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) - return x_m, y_m, yaw_rad - - -class _HarnessResult: - def __init__( - self, - *, - command_history: list[Twist], - stop_messages: list[str], - final_x_m: float, - final_y_m: float, - final_yaw_rad: float, - ) -> None: - self.command_history = command_history - self.stop_messages = stop_messages - self.final_x_m = final_x_m - self.final_y_m = final_y_m - self.final_yaw_rad = final_yaw_rad - - -class _RecordingHolonomicTrackingController(HolonomicTrackingController): - def __init__(self) -> None: - super().__init__(k_position_per_s=0.0, k_yaw_per_s=0.0) - self.measurements: list[TrajectoryMeasuredSample] = [] - - def control( - self, - reference: TrajectoryReferenceSample, - measurement: TrajectoryMeasuredSample, - ) -> Twist: - self.measurements.append(measurement) - return super().control(reference, measurement) - - -def _follower_with_recording_holonomic_controller() -> tuple[ - _HolonomicPathFollower, _RecordingHolonomicTrackingController -]: - core = _make_follower(goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) - core._path = path - core._path_distancer = PathDistancer(path) - controller = core._controller - assert isinstance(controller, HolonomicPathController) - recorder = _RecordingHolonomicTrackingController() - controller._inner = recorder - return core, recorder - - -def _run_follower_harness( - core: _HolonomicPathFollower, - *, - points: list[tuple[float, float]], - initial_yaw_rad: float = 0.0, - max_ticks: int = 260, - rate_hz: float = 60.0, -) -> _HarnessResult: - dt_s = 1.0 / rate_hz - plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, initial_yaw_rad - latest_cmd = Twist() - command_history: list[Twist] = [] - stop_messages: list[str] = [] - - def _on_cmd_vel(cmd: Twist) -> None: - nonlocal latest_cmd - latest_cmd = Twist(cmd) - command_history.append(Twist(cmd)) - - cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) - stop_sub = core.stopped_navigating.subscribe(stop_messages.append) - sim_time_s = 1.0 - - try: - core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) - core.start_planning(_path_from_points(points)) - for _ in range(max_ticks): - if "arrived" in stop_messages: - break - time.sleep(dt_s * 1.1) - plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( - plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s - ) - sim_time_s += dt_s - core.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) - ) - finally: - core.close() - cmd_sub.dispose() - stop_sub.dispose() - - return _HarnessResult( - command_history=command_history, - stop_messages=stop_messages, - final_x_m=plant_x_m, - final_y_m=plant_y_m, - final_yaw_rad=plant_yaw_rad, - ) - - -def test_holonomic_path_controller_slews_first_command_from_rest() -> None: - ctrl = HolonomicPathController( - GlobalConfig(), - speed=0.55, - control_frequency=10.0, - k_position_per_s=2.0, - k_yaw_per_s=1.5, - ) - odom = PoseStamped(frame_id="map", position=[0.0, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)) - out = ctrl.advance(np.array([0.5, 0.0], dtype=np.float64), odom) - assert math.hypot(float(out.linear.x), float(out.linear.y)) == pytest.approx(0.5) - - -def test_path_following_passes_estimated_measured_body_twist() -> None: - core, recorder = _follower_with_recording_holonomic_controller() - core._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) - core._compute_path_following() - core._current_odom = _pose_stamped(0.3, -0.1, 0.2, ts=1.5) - - core._compute_path_following() - - measured = recorder.measurements[-1].twist_body - vx_w = 0.3 / 0.5 - vy_w = -0.1 / 0.5 - assert measured.linear.x == pytest.approx(math.cos(0.2) * vx_w + math.sin(0.2) * vy_w) - assert measured.linear.y == pytest.approx(-math.sin(0.2) * vx_w + math.cos(0.2) * vy_w) - assert measured.angular.z == pytest.approx(0.2 / 0.5) - - -def test_rotation_passes_estimated_measured_body_twist() -> None: - core, recorder = _follower_with_recording_holonomic_controller() - core._current_odom = _pose_stamped(0.0, 0.0, 1.0, ts=1.0) - core._compute_initial_rotation() - core._current_odom = _pose_stamped(0.2, 0.1, 0.8, ts=1.4) - - core._compute_initial_rotation() - - measured = recorder.measurements[-1].twist_body - vx_w = 0.2 / 0.4 - vy_w = 0.1 / 0.4 - assert measured.linear.x == pytest.approx(math.cos(0.8) * vx_w + math.sin(0.8) * vy_w) - assert measured.linear.y == pytest.approx(-math.sin(0.8) * vx_w + math.cos(0.8) * vy_w) - assert measured.angular.z == pytest.approx(angle_diff(0.8, 1.0) / 0.4) - - -def test_invalid_dt_passes_zero_measured_body_twist() -> None: - core, recorder = _follower_with_recording_holonomic_controller() - core._current_odom = _pose_stamped(0.0, 0.0, 0.0, ts=5.0) - core._compute_path_following() - core._current_odom = _pose_stamped(0.2, 0.0, 0.0, ts=5.0) - - core._compute_path_following() - - measured = recorder.measurements[-1].twist_body - assert measured.is_zero() - - -def test_lookahead_reference_uses_path_end_progress() -> None: - core = _make_follower(speed_m_s=0.5, goal_tolerance=0.01) - path = _path_from_points([(0.0, 0.0), (2.0, 0.0)]) - distancer = PathDistancer(path) - odom = _pose_stamped(0.2, 0.1, 0.0, ts=10.0) - current_pos = np.array([odom.position.x, odom.position.y], dtype=np.float64) - path_speed = core._path_speed_for_index(distancer, 0, current_pos) - - reference = core._lookahead_reference_sample( - distancer, - odom, - current_pos, - path_speed, - ) - - assert reference.pose_plan.position.x == pytest.approx(0.7) - assert reference.time_s == pytest.approx(11.0) - - -def test_uses_curvature_speed_cap_for_holonomic_path() -> None: - core = _follower_with_envelope( - speed_m_s=1.2, - max_tangent_accel_m_s2=1.0, - max_normal_accel_m_s2=0.6, - goal_decel_m_s2=1.0, - ) - path = Path( - frame_id="map", - poses=[ - PoseStamped(frame_id="map", position=[0.0, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)), - PoseStamped(frame_id="map", position=[0.5, 0.0, 0.0], orientation=Quaternion(0, 0, 0, 1)), - PoseStamped(frame_id="map", position=[0.5, 0.5, 0.0], orientation=Quaternion(0, 0, 0, 1)), - ], - ) - core._path = path - core._path_distancer = PathDistancer(path) - core._current_odom = PoseStamped( - frame_id="map", - position=[0.5, 0.0, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ) - - core._compute_path_following() - - assert core._controller._speed < 1.2 - - -def test_uses_configured_goal_decel_for_path_speed_cap() -> None: - core = _follower_with_envelope( - speed_m_s=2.0, - max_tangent_accel_m_s2=4.0, - max_normal_accel_m_s2=0.6, - goal_decel_m_s2=0.5, - ) - distancer = PathDistancer(_path_from_points([(0.0, 0.0), (1.0, 0.0)])) - current_pos = np.array([0.75, 0.0], dtype=np.float64) - - speed = core._path_speed_for_index(distancer, 0, current_pos) - - assert speed == pytest.approx(0.5, abs=1e-3) - - -def test_path_speed_uses_geometry_and_near_goal_decel_caps() -> None: - max_normal_accel_m_s2 = 0.5 - goal_decel_m_s2 = 1.0 - core = _follower_with_envelope( - speed_m_s=2.0, - max_tangent_accel_m_s2=1.0, - max_normal_accel_m_s2=max_normal_accel_m_s2, - goal_decel_m_s2=goal_decel_m_s2, - ) - distancer = PathDistancer( - _path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (2.0, 1.0)]) - ) - - corner_pos = np.array([1.0, 0.0], dtype=np.float64) - corner_index = distancer.find_closest_point_index(corner_pos) - corner_speed = core._path_speed_for_index(distancer, corner_index, corner_pos) - - near_goal_pos = np.array([1.95, 1.0], dtype=np.float64) - near_goal_index = distancer.find_closest_point_index(near_goal_pos) - near_goal_speed = core._path_speed_for_index(distancer, near_goal_index, near_goal_pos) - - right_angle_radius_m = math.sqrt(0.5) - geometry_cap_m_s = math.sqrt(max_normal_accel_m_s2 * right_angle_radius_m) - goal_decel_cap_m_s = math.sqrt( - 2.0 * goal_decel_m_s2 * distancer.distance_to_goal(near_goal_pos) - ) - assert corner_speed == pytest.approx(geometry_cap_m_s) - assert near_goal_speed == pytest.approx(goal_decel_cap_m_s, abs=1e-3) - assert near_goal_speed < corner_speed - - -def test_path_speed_anticipates_corner_tangent_decel() -> None: - speed_m_s = 2.0 - core = _follower_with_envelope( - speed_m_s=speed_m_s, - max_tangent_accel_m_s2=0.5, - max_normal_accel_m_s2=0.1, - goal_decel_m_s2=1.0, - ) - distancer = PathDistancer(_path_from_points([(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)])) - - before_corner_pos = np.array([0.85, 0.0], dtype=np.float64) - before_corner_speed = core._path_speed_for_index( - distancer, - distancer.find_closest_point_index(before_corner_pos), - before_corner_pos, - ) - - assert before_corner_speed < speed_m_s - assert before_corner_speed < 1.0 - - -def test_in_the_loop_harness_reaches_arrival_on_straight_line() -> None: - result = _run_follower_harness( - _make_follower(speed_m_s=1.0, control_frequency=60.0, goal_tolerance=0.08), - points=[(0.1, 0.0), (1.2, 0.0)], - ) - - assert "arrived" in result.stop_messages - assert result.command_history - assert math.hypot(result.final_x_m - 1.2, result.final_y_m) < 0.15 - - -def test_in_the_loop_harness_exercises_curvature_speed_cap() -> None: - result = _run_follower_harness( - _make_follower(speed_m_s=1.2, control_frequency=60.0, goal_tolerance=0.08), - points=[(0.1, 0.0), (0.45, 0.0), (0.45, 0.45), (0.8, 0.45)], - max_ticks=320, - ) - - assert "arrived" in result.stop_messages - - -def test_in_the_loop_harness_supports_2mps_right_angle_with_conservative_profile() -> None: - core = _make_follower(control_frequency=60.0, goal_tolerance=0.08) - _install_envelope( - core, - speed_m_s=2.0, - max_tangent_accel_m_s2=0.5, - max_normal_accel_m_s2=0.1, - goal_decel_m_s2=0.5, - ) - result = _run_follower_harness( - core, - points=[(0.1, 0.0), (0.55, 0.0), (0.55, 0.55), (0.9, 0.55)], - max_ticks=420, - ) - commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] - - assert "arrived" in result.stop_messages - assert commanded_speeds - assert max(commanded_speeds) < 0.75 - - -def test_in_the_loop_harness_decelerates_near_goal() -> None: - result = _run_follower_harness( - _make_follower(speed_m_s=1.2, control_frequency=60.0, goal_tolerance=0.08), - points=[(0.1, 0.0), (1.0, 0.0)], - max_ticks=300, - ) - commanded_speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history] - moving_speeds = [speed for speed in commanded_speeds if speed > 0.05] - - assert "arrived" in result.stop_messages - assert moving_speeds - assert max(moving_speeds[: len(moving_speeds) // 2]) > 0.7 - assert min(moving_speeds[-10:]) < 0.55 - assert min(moving_speeds[-10:]) < 0.6 * max(moving_speeds) - assert math.hypot(result.final_x_m - 1.0, result.final_y_m) < 0.15 - - -def test_in_the_loop_harness_exercises_initial_rotation() -> None: - # Rotate-first is off by default for the holonomic base; opt in to exercise - # the initial_rotation state machine branch. - result = _run_follower_harness( - _make_follower( - speed_m_s=0.9, - control_frequency=60.0, - goal_tolerance=0.08, - align_heading_before_move=True, - ), - points=[(0.1, 0.0), (1.0, 0.0)], - initial_yaw_rad=0.8, - max_ticks=320, - ) - - assert "arrived" in result.stop_messages - assert any( - abs(float(cmd.angular.z)) > 0.05 and _planar_speed_m_s(cmd) < 0.05 - for cmd in result.command_history[:20] - ) - assert abs(result.final_yaw_rad) < 0.35 - - -def test_holonomic_velocity_damping_reduces_planar_command() -> None: - # Aligned pose so k_position/k_yaw contribute nothing; the only difference is - # the velocity-damping gain wired into the inner law via the controller ctor. - reference = TrajectoryReferenceSample( - time_s=1.0, - pose_plan=Pose(position=[0.0, 0.0, 0.0], orientation=_yaw_quaternion(0.0)), - twist_body=Twist(linear=[0.5, 0.2, 0.0]), - ) - odom = _pose_stamped(0.0, 0.0, 0.0, ts=1.0) - measured_body_twist = Twist(linear=[1.1, 0.6, 0.0]) - - def _factory(k_velocity_per_s: float) -> HolonomicPathController: - return HolonomicPathController( - # Generous slew headroom so the first-tick accel clamp does not mask the gain. - GlobalConfig(local_planner_max_planar_cmd_accel_m_s2=8.0), - speed=1.0, - control_frequency=10.0, - k_position_per_s=0.0, - k_yaw_per_s=0.0, - k_velocity_per_s=k_velocity_per_s, - k_yaw_rate_per_s=0.0, - ) - - undamped = _factory(0.0).advance_reference(reference, odom, measured_body_twist) - damped = _factory(0.5).advance_reference(reference, odom, measured_body_twist) - - assert undamped.linear.x == pytest.approx(0.5) - assert undamped.linear.y == pytest.approx(0.2) - # 0.5 * (ref - measured) damping pulls the command down to (0.2, 0.0). - assert damped.linear.x == pytest.approx(0.2) - assert damped.linear.y == pytest.approx(0.0) - assert math.hypot(damped.linear.x, damped.linear.y) < math.hypot( - undamped.linear.x, undamped.linear.y - ) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py b/dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py deleted file mode 100644 index 5f83d20577..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/test_run_envelope.py +++ /dev/null @@ -1,204 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Run-profile speed application in ``_HolonomicPathFollower``. - -Relocated from ``dannav/test_local_planner_run_envelope.py``. The session -profile is resolved from ``DanHolonomicTCConfig.run_profile`` at construction and -re-resolved live by ``set_run_profile``; the per-goal ``run_profile_name`` -override is gone, so the leak test is recast to session-profile switching. -""" - -from __future__ import annotations - -import math -import time - -import numpy as np -import pytest - -from dimos.core.global_config import GlobalConfig -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.nav_msgs.Path import Path -from dimos.navigation.holonomic_trajectory_controller.module import ( - DanHolonomicTCConfig, - _HolonomicPathFollower, -) -from dimos.navigation.holonomic_trajectory_controller.path_distancer import PathDistancer - - -def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) - - -def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: - return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), - ) - - -def _path_from_points(points: list[tuple[float, float]]) -> Path: - poses: list[PoseStamped] = [] - for index, point in enumerate(points): - if index + 1 < len(points): - next_point = points[index + 1] - yaw = math.atan2(next_point[1] - point[1], next_point[0] - point[0]) - else: - prev_point = points[index - 1] - yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) - poses.append(_pose_stamped(point[0], point[1], yaw)) - return Path(frame_id="map", poses=poses) - - -def _straight_points(length_m: float, spacing_m: float = 0.1) -> list[tuple[float, float]]: - n = round(length_m / spacing_m) - return [(i * spacing_m, 0.0) for i in range(n + 1)] - - -def _make_follower(**overrides: object) -> _HolonomicPathFollower: - return _HolonomicPathFollower(DanHolonomicTCConfig(**overrides)) - - -def _path_speed_at(core: _HolonomicPathFollower, path: Path, pos: tuple[float, float]) -> float: - distancer = PathDistancer(path) - current_pos = np.array(pos, dtype=np.float64) - return core._path_speed_for_index( - distancer, distancer.find_closest_point_index(current_pos), current_pos - ) - - -def _integrate_holonomic_pose( - x_m: float, - y_m: float, - yaw_rad: float, - cmd_body: Twist, - dt_s: float, -) -> tuple[float, float, float]: - """Integrate body-frame cmd_vel in the world frame (test harness only).""" - vx = float(cmd_body.linear.x) - vy = float(cmd_body.linear.y) - wz = float(cmd_body.angular.z) - c = math.cos(yaw_rad) - s = math.sin(yaw_rad) - x_m += (c * vx - s * vy) * dt_s - y_m += (s * vx + c * vy) * dt_s - yaw_rad += wz * dt_s - yaw_rad = math.atan2(math.sin(yaw_rad), math.cos(yaw_rad)) - return x_m, y_m, yaw_rad - - -def test_walk_default_profile_speed() -> None: - """No override: the path speed is the walk profile cruise (0.55 m/s).""" - core = _make_follower() - speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) - assert speed == pytest.approx(0.55) - - -def test_cruise_override_drives_path_speed() -> None: - """``speed_m_s`` overrides the profile cruise (the old planner_robot_speed).""" - core = _make_follower(speed_m_s=1.0) - speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) - assert speed == pytest.approx(1.0) - - -def test_run_conservative_drives_planner_speed_and_command_caps() -> None: - core = _make_follower(run_profile="run_conservative") - path = _path_from_points(_straight_points(6.0)) - - mid_speed = _path_speed_at(core, path, (3.0, 0.0)) - near_goal_speed = _path_speed_at(core, path, (5.7, 0.0)) - - assert mid_speed == pytest.approx(1.5) - assert near_goal_speed == pytest.approx(math.sqrt(2.0 * 1.5 * 0.3)) - - -def test_run_profile_speed_respects_global_nerf() -> None: - core = _make_follower(run_profile="run_conservative", g=GlobalConfig(nerf_speed=0.5)) - speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) - assert speed == pytest.approx(0.75) - - -def test_set_run_profile_switches_session_speed() -> None: - """``set_run_profile`` replaces the dropped per-goal override: switching the - session profile re-resolves the envelope live, and switching back restores - the previous speed (no leak).""" - core = _make_follower() - path = _path_from_points(_straight_points(6.0)) - - assert core.set_run_profile("trot") is True - trot_speed = _path_speed_at(core, path, (3.0, 0.0)) - - assert core.set_run_profile("walk") is True - walk_speed = _path_speed_at(core, path, (3.0, 0.0)) - - assert trot_speed == pytest.approx(1.0) - assert walk_speed == pytest.approx(0.55) - - -def test_set_run_profile_rejects_unknown_profile() -> None: - core = _make_follower() - assert core.set_run_profile("does-not-exist") is False - # The rejected name does not poison the live envelope. - speed = _path_speed_at(core, _path_from_points(_straight_points(6.0)), (3.0, 0.0)) - assert speed == pytest.approx(0.55) - - -def test_run_profile_commands_faster_than_walk_in_closed_loop() -> None: - rate_hz = 60.0 - dt_s = 1.0 / rate_hz - core = _make_follower( - run_profile="run_conservative", control_frequency=rate_hz, goal_tolerance=0.1 - ) - plant_x_m, plant_y_m, plant_yaw_rad = 0.0, 0.0, 0.0 - latest_cmd = Twist() - commanded_speeds: list[float] = [] - stops: list[str] = [] - - def _on_cmd_vel(cmd: Twist) -> None: - nonlocal latest_cmd - latest_cmd = Twist(cmd) - commanded_speeds.append(math.hypot(float(cmd.linear.x), float(cmd.linear.y))) - - cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) - stop_sub = core.stopped_navigating.subscribe(stops.append) - sim_time_s = 1.0 - - try: - core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) - core.start_planning(_path_from_points([(0.1, 0.0), (3.1, 0.0)])) - for _ in range(420): - if "arrived" in stops: - break - time.sleep(dt_s * 1.1) - plant_x_m, plant_y_m, plant_yaw_rad = _integrate_holonomic_pose( - plant_x_m, plant_y_m, plant_yaw_rad, latest_cmd, dt_s - ) - sim_time_s += dt_s - core.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) - ) - finally: - core.close() - cmd_sub.dispose() - stop_sub.dispose() - - assert "arrived" in stops - assert commanded_speeds - assert max(commanded_speeds) > 1.0 - assert max(commanded_speeds) <= 1.5 + 1e-6 diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py deleted file mode 100644 index 5f428f1b17..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_command_limits.py +++ /dev/null @@ -1,209 +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. - -"""Tests for ``trajectory_command_limits``.""" - -import math - -import pytest - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( - HolonomicCommandLimits, - clamp_holonomic_cmd_vel, -) - -_LIMITS = HolonomicCommandLimits( - max_planar_speed_m_s=1.0, - max_yaw_rate_rad_s=0.5, - max_planar_linear_accel_m_s2=2.0, - max_yaw_accel_rad_s2=1.0, -) - - -def test_limits_reject_non_finite_or_negative() -> None: - with pytest.raises(ValueError, match="max_planar_speed_m_s"): - HolonomicCommandLimits( - max_planar_speed_m_s=-1.0, - max_yaw_rate_rad_s=1.0, - max_planar_linear_accel_m_s2=1.0, - max_yaw_accel_rad_s2=1.0, - ) - - -def test_saturate_planar_speed() -> None: - prev = Twist() - raw = Twist(linear=Vector3(2.0, 0.0, 0.0), angular=Vector3(0.0, 0.0, 0.0)) - out = clamp_holonomic_cmd_vel( - prev, - raw, - _LIMITS, - 1.0, - ) - assert out.linear.x == pytest.approx(1.0) - assert out.linear.y == pytest.approx(0.0) - - -def test_saturate_planar_speed_holonomic_direction() -> None: - """Speed cap scales (vx, vy) together, preserving direction.""" - prev = Twist() - raw = Twist( - linear=Vector3(3.0, 4.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - out = clamp_holonomic_cmd_vel( - prev, - raw, - _LIMITS, - 1.0, - ) - v_max = 1.0 - n = math.hypot(3.0, 4.0) - assert out.linear.x == pytest.approx(3.0 / n * v_max) - assert out.linear.y == pytest.approx(4.0 / n * v_max) - - -def test_acceleration_limits_one_axis_from_rest() -> None: - prev = Twist() - raw = Twist(linear=Vector3(1.0, 0.0, 0.0), angular=Vector3(0.0, 0.0, 0.0)) - a_max = 2.0 - dt = 0.1 - limits = HolonomicCommandLimits( - max_planar_speed_m_s=10.0, - max_yaw_rate_rad_s=10.0, - max_planar_linear_accel_m_s2=a_max, - max_yaw_accel_rad_s2=10.0, - ) - out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) - assert out.linear.x == pytest.approx(a_max * dt) - - -def test_acceleration_limit_scales_planar_delta_vector() -> None: - prev = Twist(linear=Vector3(0.5, 0.0, 0.0)) - raw = Twist(linear=Vector3(0.5, 1.0, 0.0)) - a_max = 1.5 - dt = 0.2 - limits = HolonomicCommandLimits( - max_planar_speed_m_s=10.0, - max_yaw_rate_rad_s=10.0, - max_planar_linear_accel_m_s2=a_max, - max_yaw_accel_rad_s2=10.0, - ) - - out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) - - assert out.linear.x == pytest.approx(0.5) - assert out.linear.y == pytest.approx(a_max * dt) - assert math.hypot(out.linear.x - prev.linear.x, out.linear.y - prev.linear.y) == pytest.approx( - a_max * dt - ) - - -def test_acceleration_limit_bounds_planar_reversal() -> None: - prev = Twist(linear=Vector3(0.6, 0.8, 0.0)) - raw = Twist(linear=Vector3(-0.6, -0.8, 0.0)) - a_max = 2.0 - dt = 0.1 - limits = HolonomicCommandLimits( - max_planar_speed_m_s=10.0, - max_yaw_rate_rad_s=10.0, - max_planar_linear_accel_m_s2=a_max, - max_yaw_accel_rad_s2=10.0, - ) - - out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) - - delta_x = out.linear.x - prev.linear.x - delta_y = out.linear.y - prev.linear.y - assert math.hypot(delta_x, delta_y) == pytest.approx(a_max * dt) - assert math.hypot(out.linear.x, out.linear.y) < math.hypot(prev.linear.x, prev.linear.y) - - -def test_slew_uses_previous_command() -> None: - prev = Twist( - linear=Vector3(0.8, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - raw = Twist( - linear=Vector3(1.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - a_max = 1.0 - dt = 0.1 - limits = HolonomicCommandLimits( - max_planar_speed_m_s=2.0, - max_yaw_rate_rad_s=2.0, - max_planar_linear_accel_m_s2=a_max, - max_yaw_accel_rad_s2=10.0, - ) - out = clamp_holonomic_cmd_vel(prev, raw, limits, dt) - assert out.linear.x == pytest.approx(0.8 + a_max * dt) - - -def test_yaw_rate_and_acceleration() -> None: - prev = Twist(angular=Vector3(0.0, 0.0, 0.0)) - raw = Twist(angular=Vector3(0.0, 0.0, 1.0)) - limits = HolonomicCommandLimits( - max_planar_speed_m_s=1.0, - max_yaw_rate_rad_s=0.5, - max_planar_linear_accel_m_s2=1.0, - max_yaw_accel_rad_s2=5.0, - ) - out = clamp_holonomic_cmd_vel(prev, raw, limits, 1.0) - assert out.angular.z == pytest.approx(0.5) - - -def test_yaw_acceleration_step() -> None: - prev = Twist(angular=Vector3(0.0, 0.0, 0.0)) - raw = Twist(angular=Vector3(0.0, 0.0, 1.0)) - limits = HolonomicCommandLimits( - max_planar_speed_m_s=1.0, - max_yaw_rate_rad_s=10.0, - max_planar_linear_accel_m_s2=1.0, - max_yaw_accel_rad_s2=2.0, - ) - out = clamp_holonomic_cmd_vel(prev, raw, limits, 0.1) - assert out.angular.z == pytest.approx(0.2) - - -def test_rejects_non_positive_dt() -> None: - with pytest.raises(ValueError, match="dt_s"): - clamp_holonomic_cmd_vel(Twist(), Twist(), _LIMITS, 0.0) - - -def test_passes_through_unmodeled_vector_components() -> None: - """linear.z, angular x/y come from raw_cmd, not from previous_cmd.""" - prev = Twist( - linear=Vector3(0.0, 0.0, 1.0), - angular=Vector3(0.0, 0.0, 0.0), - ) - raw = Twist( - linear=Vector3(0.0, 0.0, 2.0), - angular=Vector3(0.1, -0.2, 0.0), - ) - out = clamp_holonomic_cmd_vel( - prev, - raw, - HolonomicCommandLimits( - max_planar_speed_m_s=1.0, - max_yaw_rate_rad_s=1.0, - max_planar_linear_accel_m_s2=10.0, - max_yaw_accel_rad_s2=10.0, - ), - 0.05, - ) - assert out.linear.z == pytest.approx(2.0) - assert out.angular.x == pytest.approx(0.1) - assert out.angular.y == pytest.approx(-0.2) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py deleted file mode 100644 index 653bcff372..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_holonomic_tracking_controller.py +++ /dev/null @@ -1,210 +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. - -"""Holonomic tracking controller unit tests.""" - -from __future__ import annotations - -import math - -import pytest - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.navigation.holonomic_trajectory_controller.trajectory_command_limits import ( - HolonomicCommandLimits, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_holonomic_tracking_controller import HolonomicTrackingController -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample -from dimos.utils.trigonometry import angle_diff - - -def _pose_xy_yaw(x: float, y: float, yaw: float) -> Pose: - return Pose( - x, - y, - 0.0, - 0.0, - 0.0, - math.sin(yaw / 2.0), - math.cos(yaw / 2.0), - ) - - -def _ref(time_s: float, pose: Pose, twist: Twist) -> TrajectoryReferenceSample: - return TrajectoryReferenceSample(time_s=time_s, pose_plan=pose, twist_body=twist) - - -def _meas(time_s: float, pose: Pose, twist: Twist) -> TrajectoryMeasuredSample: - return TrajectoryMeasuredSample(time_s=time_s, pose_plan=pose, twist_body=twist) - - -def test_tracking_feedforward_when_aligned() -> None: - ctrl = HolonomicTrackingController(k_position_per_s=2.0, k_yaw_per_s=1.5) - ref_twist = Twist(linear=Vector3(0.4, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.05)) - p = _pose_xy_yaw(1.0, -2.0, math.pi / 6) - out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, Twist())) - assert out.linear.x == pytest.approx(0.4) - assert out.linear.y == pytest.approx(-0.1) - assert out.angular.z == pytest.approx(0.05) - - -def test_default_damping_gains_ignore_measured_velocity() -> None: - ctrl = HolonomicTrackingController(k_position_per_s=2.0, k_yaw_per_s=1.5) - ref_twist = Twist(linear=Vector3(0.4, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.05)) - measured_twist = Twist(linear=Vector3(1.2, -0.8, 0.0), angular=Vector3(0.0, 0.0, 0.7)) - p = _pose_xy_yaw(1.0, -2.0, math.pi / 6) - out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) - assert out.linear.x == pytest.approx(0.4) - assert out.linear.y == pytest.approx(-0.1) - assert out.angular.z == pytest.approx(0.05) - - -def test_velocity_damping_reduces_planar_command_when_measured_velocity_exceeds_reference() -> None: - ctrl = HolonomicTrackingController( - k_position_per_s=0.0, - k_yaw_per_s=0.0, - k_velocity_per_s=0.5, - ) - ref_twist = Twist(linear=Vector3(0.5, 0.2, 0.0), angular=Vector3()) - measured_twist = Twist(linear=Vector3(1.1, 0.6, 0.0), angular=Vector3()) - p = _pose_xy_yaw(0.0, 0.0, 0.0) - - out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) - - assert out.linear.x == pytest.approx(0.2) - assert out.linear.y == pytest.approx(0.0) - assert math.hypot(out.linear.x, out.linear.y) < math.hypot( - ref_twist.linear.x, ref_twist.linear.y - ) - - -def test_yaw_rate_damping_reduces_command_when_measured_rate_exceeds_reference() -> None: - ctrl = HolonomicTrackingController( - k_position_per_s=0.0, - k_yaw_per_s=0.0, - k_yaw_rate_per_s=0.5, - ) - ref_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.4)) - measured_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.9)) - p = _pose_xy_yaw(0.0, 0.0, 0.0) - - out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) - - assert out.angular.z == pytest.approx(0.15) - assert abs(out.angular.z) < abs(ref_twist.angular.z) - - -def test_velocity_damping_does_not_increase_planar_command_when_measured_velocity_is_below_reference() -> ( - None -): - ctrl = HolonomicTrackingController( - k_position_per_s=0.0, - k_yaw_per_s=0.0, - k_velocity_per_s=0.5, - ) - ref_twist = Twist(linear=Vector3(0.5, -0.4, 0.0), angular=Vector3()) - measured_twist = Twist(linear=Vector3(0.2, -0.1, 0.0), angular=Vector3()) - p = _pose_xy_yaw(0.0, 0.0, 0.0) - - out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) - - assert out.linear.x == pytest.approx(0.5) - assert out.linear.y == pytest.approx(-0.4) - - -def test_yaw_rate_damping_does_not_increase_command_when_measured_rate_is_below_reference() -> None: - ctrl = HolonomicTrackingController( - k_position_per_s=0.0, - k_yaw_per_s=0.0, - k_yaw_rate_per_s=0.5, - ) - ref_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.4)) - measured_twist = Twist(linear=Vector3(), angular=Vector3(0.0, 0.0, 0.1)) - p = _pose_xy_yaw(0.0, 0.0, 0.0) - - out = ctrl.control(_ref(0.0, p, ref_twist), _meas(0.0, p, measured_twist)) - - assert out.angular.z == pytest.approx(0.4) - - -@pytest.mark.parametrize("value", [-0.1, math.inf, math.nan]) -def test_velocity_damping_gain_validation_rejects_negative_or_non_finite(value: float) -> None: - with pytest.raises(ValueError, match="k_velocity_per_s"): - HolonomicTrackingController( - k_position_per_s=1.0, - k_yaw_per_s=1.0, - k_velocity_per_s=value, - ) - - -@pytest.mark.parametrize("value", [-0.1, math.inf, math.nan]) -def test_yaw_rate_damping_gain_validation_rejects_negative_or_non_finite(value: float) -> None: - with pytest.raises(ValueError, match="k_yaw_rate_per_s"): - HolonomicTrackingController( - k_position_per_s=1.0, - k_yaw_per_s=1.0, - k_yaw_rate_per_s=value, - ) - - -def test_tracking_rotates_reference_feedforward_into_measured_body_frame() -> None: - ctrl = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=0.0) - ref_p = _pose_xy_yaw(0.0, 1.0, math.pi / 2.0) - meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) - ref_twist = Twist(linear=Vector3(0.5, 0.0, 0.0), angular=Vector3(0.0, 0.0, 0.0)) - out = ctrl.control(_ref(0.0, ref_p, ref_twist), _meas(0.0, meas_p, Twist())) - assert out.linear.x == pytest.approx(0.0, abs=1e-12) - assert out.linear.y == pytest.approx(0.5) - - -def test_tracking_pushes_toward_reference_in_body_frame() -> None: - ctrl = HolonomicTrackingController(k_position_per_s=1.0, k_yaw_per_s=0.0) - ref_p = _pose_xy_yaw(1.0, 0.0, 0.0) - meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) - out = ctrl.control( - _ref(0.0, ref_p, Twist()), - _meas(0.0, meas_p, Twist()), - ) - assert out.linear.x == pytest.approx(1.0) - assert out.linear.y == pytest.approx(0.0) - - -def test_tracking_heading_correction_sign() -> None: - ctrl = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=2.0) - ref_p = _pose_xy_yaw(0.0, 0.0, 0.5) - meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) - out = ctrl.control(_ref(0.0, ref_p, Twist()), _meas(0.0, meas_p, Twist())) - e_psi = angle_diff(0.0, 0.5) - assert out.angular.z == pytest.approx(-2.0 * e_psi) - - -def test_configure_caps_planar_speed_and_yaw_rate() -> None: - ctrl = HolonomicTrackingController(k_position_per_s=100.0, k_yaw_per_s=100.0) - ctrl.configure( - HolonomicCommandLimits( - max_planar_speed_m_s=0.5, - max_yaw_rate_rad_s=0.2, - max_planar_linear_accel_m_s2=10.0, - max_yaw_accel_rad_s2=10.0, - ), - ) - ref_p = _pose_xy_yaw(10.0, 0.0, 0.0) - meas_p = _pose_xy_yaw(0.0, 0.0, 0.0) - out = ctrl.control(_ref(0.0, ref_p, Twist()), _meas(0.0, meas_p, Twist())) - assert math.hypot(out.linear.x, out.linear.y) == pytest.approx(0.5) - ref_p2 = _pose_xy_yaw(0.0, 0.0, 10.0) - out2 = ctrl.control(_ref(0.0, ref_p2, Twist()), _meas(0.0, meas_p, Twist())) - assert abs(out2.angular.z) == pytest.approx(0.2) diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py deleted file mode 100644 index e28acb9e17..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_path_speed_profile.py +++ /dev/null @@ -1,157 +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. - -"""Polyline speed profile behavior tests.""" - -import math - -import numpy as np -import pytest - -from dimos.navigation.holonomic_trajectory_controller.trajectory_path_speed_profile import ( - PathSpeedProfileLimits, - profile_speed_along_polyline, - speed_at_progress_m, -) - - -def _straight_line_path(length_m: float) -> tuple[np.ndarray, np.ndarray]: - path_xy = np.array([[0.0, 0.0], [length_m, 0.0]], dtype=np.float64) - cumulative = np.array([length_m], dtype=np.float64) - return path_xy, cumulative - - -def test_limits_reject_bad_values() -> None: - with pytest.raises(ValueError, match="max_speed_m_s"): - PathSpeedProfileLimits( - max_speed_m_s=-1.0, - max_tangent_accel_m_s2=1.0, - max_normal_accel_m_s2=1.0, - ) - - -def test_line_long_segment_reaches_speed_plateau() -> None: - """Straight 10 m line: enough length to hit ``v_max`` before braking.""" - limits = PathSpeedProfileLimits( - max_speed_m_s=2.0, - max_tangent_accel_m_s2=1.0, - max_normal_accel_m_s2=10.0, - ) - path_xy, cumulative = _straight_line_path(10.0) - s, v = profile_speed_along_polyline( - path_xy, - cumulative, - limits, - goal_decel_m_s2=limits.max_tangent_accel_m_s2, - num_samples=2001, - start_speed_m_s=0.0, - ) - assert s[0] == pytest.approx(0.0) - assert s[-1] == pytest.approx(10.0) - assert v[0] == pytest.approx(0.0) - assert v[-1] == pytest.approx(0.0) - assert max(v) == pytest.approx(2.0, rel=0, abs=0.02) - mid = len(v) // 2 - assert v[mid] == pytest.approx(2.0, rel=0, abs=0.02) - - -def test_line_short_segment_triangular_peak() -> None: - """1 m line, high nominal ``v_max``: peak limited by ``sqrt(a * L)`` triangle.""" - limits = PathSpeedProfileLimits( - max_speed_m_s=10.0, - max_tangent_accel_m_s2=1.0, - max_normal_accel_m_s2=10.0, - ) - length_m = 1.0 - path_xy, cumulative = _straight_line_path(length_m) - _, v = profile_speed_along_polyline( - path_xy, - cumulative, - limits, - goal_decel_m_s2=limits.max_tangent_accel_m_s2, - num_samples=4001, - start_speed_m_s=0.0, - ) - expected_peak = math.sqrt(limits.max_tangent_accel_m_s2 * length_m) - assert max(v) == pytest.approx(expected_peak, rel=0, abs=0.01) - - -def test_tight_corner_centripetal_cap_below_max_speed() -> None: - """90 deg corner: circumradius caps speed below ``max_speed_m_s``.""" - limits = PathSpeedProfileLimits( - max_speed_m_s=3.0, - max_tangent_accel_m_s2=2.0, - max_normal_accel_m_s2=1.0, - ) - leg_m = 0.25 - path_xy = np.array( - [[0.0, 0.0], [leg_m, 0.0], [leg_m, leg_m]], - dtype=np.float64, - ) - segments = path_xy[1:] - path_xy[:-1] - cumulative = np.cumsum(np.linalg.norm(segments, axis=1)) - corner_s = float(cumulative[0]) - s_profile, v_profile = profile_speed_along_polyline( - path_xy, - cumulative, - limits, - goal_decel_m_s2=limits.max_tangent_accel_m_s2, - num_samples=200, - start_speed_m_s=0.0, - ) - at_corner_speed = speed_at_progress_m(corner_s, s_profile, v_profile) - assert at_corner_speed < limits.max_speed_m_s - assert at_corner_speed < 0.6 - - -def test_zero_length_returns_origin() -> None: - limits = PathSpeedProfileLimits(1.0, 1.0, 1.0) - path_xy = np.array([[0.0, 0.0]], dtype=np.float64) - s, v = profile_speed_along_polyline( - path_xy, - np.array([], dtype=np.float64), - limits, - goal_decel_m_s2=1.0, - ) - assert s == [0.0] - assert v == [0.0] - - -def test_polyline_profile_decelerates_before_corner() -> None: - limits = PathSpeedProfileLimits( - max_speed_m_s=2.0, - max_tangent_accel_m_s2=0.5, - max_normal_accel_m_s2=0.1, - ) - path_xy = np.array( - [[0.0, 0.0], [1.0, 0.0], [1.0, 1.0], [2.0, 1.0]], - dtype=np.float64, - ) - segments = path_xy[1:] - path_xy[:-1] - cumulative = np.cumsum(np.linalg.norm(segments, axis=1)) - s_profile, v_profile = profile_speed_along_polyline( - path_xy, - cumulative, - limits, - goal_decel_m_s2=0.5, - num_samples=200, - ) - corner_s = 1.0 - before_corner_speed = speed_at_progress_m(0.85, s_profile, v_profile) - at_corner_speed = speed_at_progress_m(corner_s, s_profile, v_profile) - cruise_speed = speed_at_progress_m(0.4, s_profile, v_profile) - - assert cruise_speed > before_corner_speed - assert before_corner_speed > at_corner_speed - assert at_corner_speed < 0.6 diff --git a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py b/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py deleted file mode 100644 index a8b725802b..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/test_trajectory_run_profiles.py +++ /dev/null @@ -1,124 +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. - -"""Tests for the operator run-profile contract.""" - -import math - -import pytest - -from dimos.navigation.holonomic_trajectory_controller.holonomic_path_controller import command_envelope_overrides_for_profile -from dimos.navigation.holonomic_trajectory_controller.trajectory_run_profiles import ( - GO2_RUN_PROFILES, - RunProfile, - RunProfileError, - RunProfileRegistry, -) - - -def _profile(**overrides: object) -> RunProfile: - base: dict[str, object] = dict( - name="probe", - requested_planner_speed_m_s=1.0, - max_tangent_accel_m_s2=1.0, - max_normal_accel_m_s2=0.6, - goal_decel_m_s2=1.0, - max_planar_cmd_accel_m_s2=5.0, - max_yaw_rate_rad_s=1.0, - max_yaw_accel_rad_s2=5.0, - required_locomotion_mode="default", - ) - base.update(overrides) - return RunProfile(**base) # type: ignore[arg-type] - - -def test_go2_profiles_step_the_speed_envelope_up() -> None: - speeds = [ - GO2_RUN_PROFILES.get(name).requested_planner_speed_m_s - for name in ("walk", "trot", "run_conservative", "run_verified") - ] - assert speeds == sorted(speeds) - assert len(set(speeds)) == len(speeds) - - -@pytest.mark.parametrize( - "field_name", - [ - "requested_planner_speed_m_s", - "max_tangent_accel_m_s2", - "max_normal_accel_m_s2", - "goal_decel_m_s2", - "max_planar_cmd_accel_m_s2", - "max_yaw_rate_rad_s", - "max_yaw_accel_rad_s2", - ], -) -@pytest.mark.parametrize("bad_value", [0.0, -1.0, math.nan, math.inf]) -def test_envelope_fields_reject_non_positive_or_non_finite( - field_name: str, bad_value: float -) -> None: - with pytest.raises(RunProfileError, match=field_name): - _profile(**{field_name: bad_value}) - - -def test_empty_name_rejected() -> None: - with pytest.raises(RunProfileError, match="name"): - _profile(name=" ") - - -def test_empty_locomotion_mode_rejected() -> None: - with pytest.raises(RunProfileError, match="required_locomotion_mode"): - _profile(required_locomotion_mode="") - - -def test_profile_limit_helpers_match_fields() -> None: - profile = GO2_RUN_PROFILES.get("run_conservative") - limits = profile.command_limits() - assert limits.max_planar_speed_m_s == pytest.approx(profile.requested_planner_speed_m_s) - assert limits.max_yaw_rate_rad_s == pytest.approx(profile.max_yaw_rate_rad_s) - - path_limits = profile.path_speed_profile_limits_at(1.25) - assert path_limits.max_speed_m_s == pytest.approx(1.25) - assert path_limits.max_tangent_accel_m_s2 == pytest.approx(profile.max_tangent_accel_m_s2) - - overrides = command_envelope_overrides_for_profile(profile) - assert overrides.max_yaw_rate_rad_s == pytest.approx(profile.max_yaw_rate_rad_s) - assert overrides.max_planar_cmd_accel_m_s2 == pytest.approx(profile.max_planar_cmd_accel_m_s2) - - -def test_registry_rejects_key_name_mismatch() -> None: - walk = GO2_RUN_PROFILES.get("walk") - with pytest.raises(RunProfileError, match="does not match"): - RunProfileRegistry( - profiles={"stroll": walk}, - default_profile_name="stroll", - ) - - -def test_registry_rejects_missing_default() -> None: - walk = GO2_RUN_PROFILES.get("walk") - with pytest.raises(RunProfileError, match="default profile"): - RunProfileRegistry( - profiles={"walk": walk}, - default_profile_name="run_verified", - ) - - -def test_get_unknown_name_raises_with_known_names() -> None: - with pytest.raises(RunProfileError) as excinfo: - GO2_RUN_PROFILES.get("sprint") - message = str(excinfo.value) - assert "sprint" in message - for known in ("walk", "trot", "run_conservative", "run_verified"): - assert known in message diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py deleted file mode 100644 index b1b58ad637..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_export.py +++ /dev/null @@ -1,71 +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. - -"""JSONL export for trajectory control tick logs. - -Each line is one UTF-8 JSON object. Normative field names and units are -documented in ``trajectory_control_tick_jsonl.md`` in this package. -""" - -from __future__ import annotations - -from dataclasses import asdict -import json -from pathlib import Path -from typing import Any - -from dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_log import TrajectoryControlTick - -TRAJECTORY_CONTROL_TICK_JSONL_SCHEMA_VERSION = 1 - - -def trajectory_control_tick_to_jsonl_dict(tick: TrajectoryControlTick) -> dict[str, Any]: - """Map one tick to a JSON-ready dict (stable key order: schema, then dataclass fields).""" - return {"schema_version": TRAJECTORY_CONTROL_TICK_JSONL_SCHEMA_VERSION} | asdict(tick) - - -class JsonlTrajectoryControlTickSink: - """Append control ticks directly to a JSONL file during live runs.""" - - def __init__(self, path: Path | str) -> None: - self.path = Path(path).expanduser() - self.path.parent.mkdir(parents=True, exist_ok=True) - self._file = self.path.open("a", encoding="utf-8") - - def append(self, tick: TrajectoryControlTick) -> None: - self._file.write( - json.dumps( - trajectory_control_tick_to_jsonl_dict(tick), - separators=(",", ":"), - allow_nan=False, - ) - ) - self._file.write("\n") - self._file.flush() - - def close(self) -> None: - self._file.close() - - def __enter__(self) -> JsonlTrajectoryControlTickSink: - return self - - def __exit__(self, *_: object) -> None: - self.close() - - -__all__ = [ - "TRAJECTORY_CONTROL_TICK_JSONL_SCHEMA_VERSION", - "JsonlTrajectoryControlTickSink", - "trajectory_control_tick_to_jsonl_dict", -] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py deleted file mode 100644 index a2f1c66f2a..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_control_tick_log.py +++ /dev/null @@ -1,204 +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. - -"""Per-control-tick telemetry record. - -Each tick captures reference and measured pose and body-frame velocity, pose -tracking error scalars, the commanded ``cmd_vel``, control period ``dt``, and -optional wall or simulation timestamps. Live runs pass ticks to -``JsonlTrajectoryControlTickSink`` when ``GlobalConfig.local_planner_trajectory_tick_log_path`` -is set. - -JSONL export (stable field names) lives in ``dimos.navigation.holonomic_trajectory_controller.trajectory_control_tick_export`` -and ``trajectory_control_tick_jsonl.md``. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from typing import Protocol, runtime_checkable - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.navigation.holonomic_trajectory_controller.trajectory_metrics import ( - commanded_planar_speed, - planar_position_divergence, - pose_errors_vs_reference, -) -from dimos.navigation.holonomic_trajectory_controller.trajectory_types import TrajectoryMeasuredSample, TrajectoryReferenceSample - - -@dataclass(frozen=True) -class TrajectoryControlTick: - """One closed-loop trajectory control iteration (data only).""" - - # Reference: plan-frame pose and body-frame twist, trajectory time basis - ref_time_s: float - ref_x_m: float - ref_y_m: float - ref_yaw_rad: float - ref_twist_linear_x_m_s: float - ref_twist_linear_y_m_s: float - ref_twist_linear_z_m_s: float - ref_twist_angular_x_rad_s: float - ref_twist_angular_y_rad_s: float - ref_twist_angular_z_rad_s: float - - # Measured state in the same frames and time basis as ``trajectory_types`` - meas_time_s: float - meas_x_m: float - meas_y_m: float - meas_yaw_rad: float - meas_twist_linear_x_m_s: float - meas_twist_linear_y_m_s: float - meas_twist_linear_z_m_s: float - meas_twist_angular_x_rad_s: float - meas_twist_angular_y_rad_s: float - meas_twist_angular_z_rad_s: float - - # Error scalars (see ``trajectory_metrics``) - e_along_track_m: float - e_cross_track_m: float - e_heading_rad: float - planar_position_divergence_m: float - - # Command actually published this tick (body frame) - cmd_linear_x_m_s: float - cmd_linear_y_m_s: float - cmd_linear_z_m_s: float - cmd_angular_x_rad_s: float - cmd_angular_y_rad_s: float - cmd_angular_z_rad_s: float - commanded_planar_speed_m_s: float - - dt_s: float - wall_time_s: float | None = None - sim_time_s: float | None = None - - -def _planar_yaw_rad(pose_plan_pose: object) -> float: - return float(pose_plan_pose.orientation.euler.z) - - -def _twist_components(twist: Twist) -> tuple[float, float, float, float, float, float]: - return ( - float(twist.linear.x), - float(twist.linear.y), - float(twist.linear.z), - float(twist.angular.x), - float(twist.angular.y), - float(twist.angular.z), - ) - - -def trajectory_control_tick_from_samples( - reference: TrajectoryReferenceSample, - measurement: TrajectoryMeasuredSample, - command: Twist, - dt_s: float, - *, - wall_time_s: float | None = None, - sim_time_s: float | None = None, -) -> TrajectoryControlTick: - """Build a tick record from trajectory samples and the clamped command.""" - ref_p = reference.pose_plan - meas_p = measurement.pose_plan - x_ref, y_ref = float(ref_p.position.x), float(ref_p.position.y) - yaw_ref = _planar_yaw_rad(ref_p) - x_m, y_m = float(meas_p.position.x), float(meas_p.position.y) - yaw_m = _planar_yaw_rad(meas_p) - - e_at, e_ct, e_psi = pose_errors_vs_reference(x_m, y_m, yaw_m, x_ref, y_ref, yaw_ref) - div = planar_position_divergence(e_at, e_ct) - - rlin = _twist_components(reference.twist_body) - mlin = _twist_components(measurement.twist_body) - clin = _twist_components(command) - v_cmd = commanded_planar_speed(command) - - return TrajectoryControlTick( - ref_time_s=float(reference.time_s), - ref_x_m=x_ref, - ref_y_m=y_ref, - ref_yaw_rad=yaw_ref, - ref_twist_linear_x_m_s=rlin[0], - ref_twist_linear_y_m_s=rlin[1], - ref_twist_linear_z_m_s=rlin[2], - ref_twist_angular_x_rad_s=rlin[3], - ref_twist_angular_y_rad_s=rlin[4], - ref_twist_angular_z_rad_s=rlin[5], - meas_time_s=float(measurement.time_s), - meas_x_m=x_m, - meas_y_m=y_m, - meas_yaw_rad=yaw_m, - meas_twist_linear_x_m_s=mlin[0], - meas_twist_linear_y_m_s=mlin[1], - meas_twist_linear_z_m_s=mlin[2], - meas_twist_angular_x_rad_s=mlin[3], - meas_twist_angular_y_rad_s=mlin[4], - meas_twist_angular_z_rad_s=mlin[5], - e_along_track_m=e_at, - e_cross_track_m=e_ct, - e_heading_rad=e_psi, - planar_position_divergence_m=div, - cmd_linear_x_m_s=clin[0], - cmd_linear_y_m_s=clin[1], - cmd_linear_z_m_s=clin[2], - cmd_angular_x_rad_s=clin[3], - cmd_angular_y_rad_s=clin[4], - cmd_angular_z_rad_s=clin[5], - commanded_planar_speed_m_s=v_cmd, - dt_s=float(dt_s), - wall_time_s=wall_time_s, - sim_time_s=sim_time_s, - ) - - -@runtime_checkable -class TrajectoryControlTickSink(Protocol): - """Consumer of control ticks (in-memory buffer, exporter, metrics, etc.).""" - - def append(self, tick: TrajectoryControlTick) -> None: ... - - -def append_trajectory_control_tick( - sink: TrajectoryControlTickSink | None, - reference: TrajectoryReferenceSample, - measurement: TrajectoryMeasuredSample, - command: Twist, - dt_s: float, - *, - wall_time_s: float | None = None, - sim_time_s: float | None = None, -) -> TrajectoryControlTick | None: - """If ``sink`` is None, skip work and return None; else record and return the tick.""" - if sink is None: - return None - tick = trajectory_control_tick_from_samples( - reference, - measurement, - command, - dt_s, - wall_time_s=wall_time_s, - sim_time_s=sim_time_s, - ) - sink.append(tick) - return tick - - -__all__ = [ - "TrajectoryControlTick", - "TrajectoryControlTickSink", - "append_trajectory_control_tick", - "trajectory_control_tick_from_samples", -] diff --git a/dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py b/dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py deleted file mode 100644 index cd04c23d8e..0000000000 --- a/dimos/navigation/holonomic_trajectory_controller/trajectory_metrics.py +++ /dev/null @@ -1,274 +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 the "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. - -"""Planar trajectory tracking metrics. - -This module is the **normative** definition for telemetry, plots, and tests that -speak about along-track error, cross-track error, divergence, and commanded -planar speed. - -**Frames** - -- Path geometry and horizontal positions use the same **map / plan** horizontal - frame as ``Path`` and ``PathDistancer`` (x forward, y left in that frame). -- ``commanded_planar_speed`` uses **base-link** linear ``Twist`` components - (``linear.x`` forward, ``linear.y`` lateral) as emitted on ``cmd_vel`` for - omnidirectional bases. - -**Along-track and cross-track (reference pose)** - -Given a reference pose ``(x_ref, y_ref, yaw_ref)`` and measured -``(x_meas, y_meas)``, express the position error in a frame aligned with the -reference heading (x = forward along reference, y = left): - -- **Along-track error** ``e_at`` (m): signed component of ``(p_meas - p_ref)`` - on the reference forward axis. Positive means the measured point is **ahead** - of the reference in the reference forward direction. -- **Cross-track error** ``e_ct`` (m): signed component on the left axis. - Positive means the measured point is **to the left** of the reference when - facing along ``yaw_ref``. -- **Heading error** ``e_psi`` (rad): ``angle_diff(yaw_meas, yaw_ref)`` so it - matches ``dimos.utils.trigonometry.angle_diff`` conventions used in - navigation. - -**Divergence from target** - -- **Planar position divergence** (m): ``hypot(e_at, e_ct)``. This is the - default scalar for **speed vs planar position error** style plots when a - full reference pose exists. -- **Planar pose divergence** (mixed units unless scaled): ``sqrt(e_at**2 + - e_ct**2 + (yaw_weight_rad_to_m * e_psi)**2)``. ``yaw_weight_rad_to_m`` maps - heading into an equivalent meter scale for a single RMS; set to ``0`` to - recover position-only divergence. - -**Polyline reference (spatial path only)** - -For a piecewise-linear path, the closest point **on segments** (not only on -vertices) defines a foot point, tangent, and arc length ``s`` from the first -vertex to the foot. **Signed cross-track** uses the left-of-tangent rule -above. **Along-track error vs a time-parameterized reference** needs a scalar -``s_ref`` from the trajectory (arc length along the same polyline): ``e_at = -s_meas - s_ref``. Pure geometry without ``s_ref`` does not define longitudinal -error relative to a moving target; logging and controllers should supply -``s_ref`` once trajectories are timed. - -**Commanded speed for plots** - -At each control tick, sample ``commanded_planar_speed(cmd_twist)`` using the -``Twist`` actually published. This is the magnitude of the horizontal command -in the base frame, **not** a finite difference of odometry. - -**Context-dependent tolerance** - -Obstacle clearance and corridor width are **planner-side** inputs. Tracking -tolerances may be tightened in tight clearance and relaxed in open space. This -module exposes ``TrackingTolerance`` plus ``scale_tolerance_by_clearance`` as a -**deterministic example** of how clearance can modulate tolerances; production -planners may replace that mapping while keeping the same error definitions. -""" - -from __future__ import annotations - -from dataclasses import dataclass -import math - -import numpy as np -from numpy.typing import NDArray - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.utils.trigonometry import angle_diff - - -def pose_errors_vs_reference( - x_meas: float, - y_meas: float, - yaw_meas: float, - x_ref: float, - y_ref: float, - yaw_ref: float, -) -> tuple[float, float, float]: - """Along-track (m), cross-track (m), heading error (rad) vs a reference pose.""" - dx = x_meas - x_ref - dy = y_meas - y_ref - c = math.cos(yaw_ref) - s = math.sin(yaw_ref) - e_at = dx * c + dy * s - e_ct = -dx * s + dy * c - e_psi = angle_diff(yaw_meas, yaw_ref) - return (e_at, e_ct, e_psi) - - -def planar_position_divergence(e_at: float, e_ct: float) -> float: - """Euclidean planar position error (m) from along- and cross-track components.""" - return float(math.hypot(e_at, e_ct)) - - -def planar_pose_divergence( - e_at: float, - e_ct: float, - e_psi: float, - *, - yaw_weight_rad_to_m: float = 0.0, -) -> float: - """RMS of planar position and optional yaw, with yaw scaled to meters.""" - w = yaw_weight_rad_to_m - return float(math.sqrt(e_at * e_at + e_ct * e_ct + (w * e_psi) * (w * e_psi))) - - -def commanded_planar_speed(cmd: Twist) -> float: - """Horizontal speed (m/s) from a commanded ``Twist`` in the base link frame.""" - return float(math.hypot(cmd.linear.x, cmd.linear.y)) - - -@dataclass(frozen=True) -class PolylineProjection: - """Closest point on a polyline and path coordinates at the foot.""" - - foot_xy: tuple[float, float] - segment_start_index: int - tangent_yaw: float - s_along_path_m: float - signed_cross_track_m: float - - -def project_to_polyline(x: float, y: float, polyline_xy: NDArray[np.float64]) -> PolylineProjection: - """Project a point onto a ``(N, 2)`` polyline in the map frame.""" - if polyline_xy.ndim != 2 or polyline_xy.shape[1] != 2: - raise ValueError("polyline_xy must have shape (N, 2)") - n = polyline_xy.shape[0] - if n == 0: - raise ValueError("polyline_xy must be non-empty") - if n == 1: - fx = float(polyline_xy[0, 0]) - fy = float(polyline_xy[0, 1]) - lat = float(math.hypot(x - fx, y - fy)) - return PolylineProjection( - (fx, fy), - 0, - 0.0, - 0.0, - lat, - ) - - p = np.array([x, y], dtype=np.float64) - prefix_s = np.zeros(n, dtype=np.float64) - for i in range(1, n): - prefix_s[i] = prefix_s[i - 1] + float(np.linalg.norm(polyline_xy[i] - polyline_xy[i - 1])) - - best_d2 = math.inf - best_foot = polyline_xy[0].copy() - best_seg = 0 - best_t = 0.0 - best_seg_len = 0.0 - - for i in range(n - 1): - a = polyline_xy[i] - b = polyline_xy[i + 1] - ab = b - a - seg_len2 = float(ab @ ab) - if seg_len2 <= 1e-18: - foot = a - t_param = 0.0 - seg_len = 0.0 - else: - t_param = float(np.clip(((p - a) @ ab) / seg_len2, 0.0, 1.0)) - foot = a + t_param * ab - seg_len = math.sqrt(seg_len2) - d2 = float((p - foot) @ (p - foot)) - if d2 < best_d2: - best_d2 = d2 - best_foot = foot - best_seg = i - best_t = t_param - best_seg_len = seg_len - - s_along = float(prefix_s[best_seg] + best_t * best_seg_len) - - a = polyline_xy[best_seg] - b = polyline_xy[best_seg + 1] - ab = b - a - seg_len = float(np.linalg.norm(ab)) - if seg_len < 1e-9: - yaw = 0.0 - tx, ty = 1.0, 0.0 - else: - tx, ty = float(ab[0] / seg_len), float(ab[1] / seg_len) - yaw = math.atan2(ab[1], ab[0]) - nx, ny = -ty, tx - vx = x - float(best_foot[0]) - vy = y - float(best_foot[1]) - signed_ct = vx * nx + vy * ny - return PolylineProjection( - (float(best_foot[0]), float(best_foot[1])), - best_seg, - yaw, - s_along, - float(signed_ct), - ) - - -def along_track_progress_error(s_meas_m: float, s_ref_m: float) -> float: - """Signed arc-length error (m): measured minus reference along the same path.""" - return float(s_meas_m - s_ref_m) - - -@dataclass(frozen=True) -class TrackingTolerance: - """Absolute envelopes for tracking errors (planner or policy may supply these).""" - - along_track_m: float - cross_track_m: float - heading_rad: float - - def satisfied( - self, - e_at: float, - e_ct: float, - e_psi: float, - ) -> bool: - return ( - abs(e_at) <= self.along_track_m - and abs(e_ct) <= self.cross_track_m - and abs(e_psi) <= self.heading_rad - ) - - -def scale_tolerance_by_clearance( - base: TrackingTolerance, - clearance_m: float, - *, - tight_clearance_m: float = 0.35, - loose_clearance_m: float = 2.0, - tight_scale: float = 0.55, - loose_scale: float = 1.15, -) -> TrackingTolerance: - """Piecewise-linear scale of tolerances vs a scalar clearance (example policy). - - When ``clearance_m`` is small, tolerances shrink (tighter tracking in narrow - space). When clearance is large, tolerances grow modestly. Values are - illustrative; planners may substitute their own mapping. - """ - c = clearance_m - if c <= tight_clearance_m: - k = tight_scale - elif c >= loose_clearance_m: - k = loose_scale - else: - u = (c - tight_clearance_m) / (loose_clearance_m - tight_clearance_m) - k = tight_scale + u * (loose_scale - tight_scale) - return TrackingTolerance( - along_track_m=base.along_track_m * k, - cross_track_m=base.cross_track_m * k, - heading_rad=base.heading_rad * k, - ) diff --git a/dimos/navigation/nav_3d/mls_planner/goal_relay.py b/dimos/navigation/nav_3d/mls_planner/goal_relay.py index ef2c312694..905c4f8973 100644 --- a/dimos/navigation/nav_3d/mls_planner/goal_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/goal_relay.py @@ -28,34 +28,6 @@ class GoalRelayConfig(ModuleConfig): pass -class PoseOdomRelayConfig(ModuleConfig): - child_frame_id: str = "base_link" - - -class PoseOdomRelay(Module): - """Convert GO2Connection's PoseStamped odom to nav_msgs.Odometry.""" - - config: PoseOdomRelayConfig - - odom: In[PoseStamped] - odometry: Out[Odometry] - - @rpc - def start(self) -> None: - super().start() - self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) - - def _on_odom(self, msg: PoseStamped) -> None: - self.odometry.publish( - Odometry( - ts=msg.ts, - frame_id=msg.frame_id, - child_frame_id=self.config.child_frame_id, - pose=msg, - ) - ) - - class GoalRelay(Module): """Adapt odometry and goal points to the planner's PoseStamped inputs.""" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 6f88d412b8..20d001df11 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -153,7 +153,7 @@ "collection-recorder": "dimos.learning.collection.recorder.CollectionRecorder", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", - "dan-holonomic-tc": "dimos.navigation.holonomic_trajectory_controller.module.DanHolonomicTC", + "dan-holonomic-tc": "dimos.navigation.dannav.holonomic_tc.module.DanHolonomicTC", "dan-local-planner": "dimos.navigation.dannav.local_planner.module.DanLocalPlanner", "demo-calculator-skill": "dimos.agents.skills.demo_calculator_skill.DemoCalculatorSkill", "demo-monitoring": "dimos.agents.demos.demo_capabilities.DemoMonitoring", @@ -233,7 +233,6 @@ "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", "pointlio-pose-recorder": "dimos.hardware.sensors.lidar.pointlio.pose_recorder.PointlioPoseRecorder", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", - "pose-odom-relay": "dimos.navigation.nav_3d.mls_planner.goal_relay.PoseOdomRelay", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py index 0f5c119066..a4bae2df38 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -16,12 +16,6 @@ """3D navigation on Go2 with voxel-grid mapping, MLS planning, and holonomic trajectory control over WebRTC. -Decouples planning from control: ``MLSPlannerNative`` owns route safety and -emits the ``Path`` (empty when nothing ahead is traversable), while -``DanHolonomicTC`` follows that path with the holonomic tracking law. Compared -to ``unitree_go2_nav_3d`` this swaps ``PointLio`` + ``RayTracingVoxelMap`` for -``VoxelGridMapper``, adds ``PoseOdomRelay``, and replaces ``BasicPathFollower`` -with ``DanHolonomicTC``. """ from typing import Any @@ -30,9 +24,9 @@ from dimos.core.global_config import global_config from dimos.mapping.voxels import VoxelGridMapper from dimos.navigation.dannav.local_planner.module import DanLocalPlanner -from dimos.navigation.holonomic_trajectory_controller.module import DanHolonomicTC +from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay, PoseOdomRelay +from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config from dimos.robot.unitree.go2.connection import GO2Connection @@ -40,7 +34,9 @@ voxel_size = 0.05 # Height of the head-mounted lidar above the ground while standing. -go2_lidar_height = 1.0 +# While in case of Go2 it's ~ .3m, but in this blueprint +# MLSPlanner works better on Go2 lidar when the value is 0.5 +go2_lidar_height = 0.5 def _render_global_map(msg: Any) -> Any: @@ -65,8 +61,6 @@ def _render_path(msg: Any) -> Any: "visual_override": { **rerun_config["visual_override"], "world/global_map": _render_global_map, - # MLS path is remapped to planner_path for DanLocalPlanner; suppress the - # hot re-rooted stream so rerun only shows the gated path on world/path. "world/planner_path": None, "world/path": _render_path, "world/surface_map": None, @@ -77,9 +71,7 @@ def _render_path(msg: Any) -> Any: unitree_go2_mls_htc = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), - # "mcf" for stair traversal GO2Connection.blueprint(motion_mode="mcf"), - PoseOdomRelay.blueprint(), VoxelGridMapper.blueprint( voxel_size=voxel_size, frame_id="world", @@ -95,11 +87,16 @@ def _render_path(msg: Any) -> Any: step_threshold_m=0.16, step_penalty_weight=1.0, viz_publish_hz=0.0, - ).remappings([(MLSPlannerNative, "path", "planner_path")]), + ).remappings( + [ + (MLSPlannerNative, "path", "planner_path"), + # The planner's start pose is the robot's odom pose + (MLSPlannerNative, "start_pose", "odom"), + ] + ), GoalRelay.blueprint(), - DanLocalPlanner.blueprint(lock_replan=3.0), - DanHolonomicTC.blueprint(), + # Seeting resample_spacing_m to > 0.0 will smooth out jagged paths retunned my MLSP + DanLocalPlanner.blueprint(resample_spacing_m=0.1), + DanHolonomicTC.blueprint(run_profile='walk'), MovementManager.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) - -__all__ = ["unitree_go2_mls_htc"] diff --git a/docs/usage/configuration.md b/docs/usage/configuration.md index fec5ad3ead..9adaf1435c 100644 --- a/docs/usage/configuration.md +++ b/docs/usage/configuration.md @@ -108,7 +108,6 @@ Config( robot_width=0.3, robot_rotation_diameter=0.6, nerf_speed=1.0, - planner_robot_speed=None, mcp_port=9990, build_native=False, dtop=False, From 9bbc6d580ccb4089b4cf4d3de131a038b7d638fb Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Thu, 2 Jul 2026 13:26:13 +0000 Subject: [PATCH 6/9] [autofix.ci] apply automated fixes --- .../dannav/geometry/path_speed_profile.py | 12 +++------- .../holonomic_tracking_controller.py | 5 +++- .../navigation/dannav/holonomic_tc/module.py | 23 ++++--------------- .../holonomic_tc/test_dan_holonomic_tc.py | 3 +-- .../test_holonomic_path_follower.py | 20 ++++++++-------- .../test_holonomic_tracking_controller.py | 4 +++- .../dannav/holonomic_tc/test_run_envelope.py | 4 +--- .../navigation/dannav/local_planner/module.py | 6 ++--- .../local_planner/test_dan_local_planner.py | 4 +++- .../navigation/unitree_go2_mls_htc.py | 6 ++--- 10 files changed, 36 insertions(+), 51 deletions(-) diff --git a/dimos/navigation/dannav/geometry/path_speed_profile.py b/dimos/navigation/dannav/geometry/path_speed_profile.py index 683fa5bd3e..c3cd75715f 100644 --- a/dimos/navigation/dannav/geometry/path_speed_profile.py +++ b/dimos/navigation/dannav/geometry/path_speed_profile.py @@ -105,9 +105,7 @@ def _polyline_geometry_speed_caps_m_s( vertex_s = np.asarray(vertex_s_m, dtype=np.float64) interior_s = vertex_s[1:-1] - local_scale = np.maximum( - np.maximum(interior_s - vertex_s[:-2], vertex_s[2:] - interior_s), 1.0 - ) + local_scale = np.maximum(np.maximum(interior_s - vertex_s[:-2], vertex_s[2:] - interior_s), 1.0) tol = np.maximum(1e-9, local_scale * 1e-9) # Each interior vertex arc length is present as a sample, so match every @@ -177,9 +175,7 @@ def _speed_profile_from_geometry_caps( v_prev_sq = v_backward[i] * v_backward[i] + 2.0 * goal_decel_m_s2 * ds v_backward[i - 1] = min(float(geometry_cap_m_s[i - 1]), math.sqrt(max(0.0, v_prev_sq))) - return [ - min(float(geometry_cap_m_s[i]), v_forward[i], v_backward[i]) for i in range(len(s_m)) - ] + return [min(float(geometry_cap_m_s[i]), v_forward[i], v_backward[i]) for i in range(len(s_m))] def profile_speed_along_polyline( @@ -212,9 +208,7 @@ def profile_speed_along_polyline( return s_profile, v_profile -def speed_at_progress_m( - progress_m: float, s_m: Sequence[float], v_m_s: Sequence[float] -) -> float: +def speed_at_progress_m(progress_m: float, s_m: Sequence[float], v_m_s: Sequence[float]) -> float: """Linearly interpolate profile speed at arc length ``progress_m``.""" if not s_m: return 0.0 diff --git a/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py index 320a8ef679..c410b7a39b 100644 --- a/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py @@ -30,7 +30,10 @@ from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.navigation.dannav.holonomic_tc.command_limits import HolonomicCommandLimits -from dimos.navigation.dannav.holonomic_tc.types import TrajectoryMeasuredSample, TrajectoryReferenceSample +from dimos.navigation.dannav.holonomic_tc.types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) from dimos.utils.trigonometry import angle_diff diff --git a/dimos/navigation/dannav/holonomic_tc/module.py b/dimos/navigation/dannav/holonomic_tc/module.py index e3f81d0856..0452c46b93 100644 --- a/dimos/navigation/dannav/holonomic_tc/module.py +++ b/dimos/navigation/dannav/holonomic_tc/module.py @@ -146,9 +146,7 @@ def __init__(self, config: DanHolonomicTCConfig) -> None: override = config.speed_m_s if override is not None and (not math.isfinite(override) or override <= 0.0): - raise ValueError( - f"speed_m_s must be a positive finite float, got {override!r}" - ) + raise ValueError(f"speed_m_s must be a positive finite float, got {override!r}") self._cruise_speed_override = override envelope = self._resolve_run_envelope() @@ -320,11 +318,7 @@ def _initial_state(self, path: Path, current_odom: PoseStamped | None) -> Planne first only when ``align_heading_before_move`` is set and the start is misaligned with the first path tangent. """ - if ( - not self._align_heading_before_move - or current_odom is None - or len(path.poses) == 0 - ): + if not self._align_heading_before_move or current_odom is None or len(path.poses) == 0: return "path_following" first_yaw = path.poses[0].orientation.euler[2] @@ -510,10 +504,7 @@ def _profiled_path_speed_m_s(self, progress_m: float) -> float: def _ensure_path_speed_profile(self, path_distancer: PathDistancer) -> None: path_id = id(path_distancer._path) - if ( - self._path_speed_profile_s is None - or self._path_speed_profile_path_id != path_id - ): + if self._path_speed_profile_s is None or self._path_speed_profile_path_id != path_id: self._rebuild_path_speed_profile(path_distancer) self._path_speed_profile_path_id = path_id @@ -628,12 +619,8 @@ def __init__(self, **kwargs: Any) -> None: @rpc def start(self) -> None: super().start() - self.register_disposable( - self._core.cmd_vel.subscribe(self.nav_cmd_vel.publish) - ) - self.register_disposable( - self._core.stopped_navigating.subscribe(self._on_core_stopped) - ) + self.register_disposable(self._core.cmd_vel.subscribe(self.nav_cmd_vel.publish)) + self.register_disposable(self._core.stopped_navigating.subscribe(self._on_core_stopped)) self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) self.register_disposable(Disposable(self.path.subscribe(self._on_path))) if self.stop_movement.transport is not None: diff --git a/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py index a279062a96..9a7a81fa87 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py +++ b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py @@ -23,9 +23,8 @@ import time from typing import Any, Literal -import pytest - from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] +import pytest from dimos.core.stream import Stream, Transport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped diff --git a/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py index 42d9108486..52b5b8093b 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py @@ -20,22 +20,22 @@ from __future__ import annotations +from dataclasses import dataclass import math import time -from dataclasses import dataclass 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.nav_msgs.Path import Path +from dimos.navigation.dannav.geometry.path_speed_profile import ( + PathSpeedProfileLimits, +) from dimos.navigation.dannav.holonomic_tc.module import ( ActiveRunEnvelope, DanHolonomicTCConfig, _HolonomicPathFollower, ) -from dimos.navigation.dannav.geometry.path_speed_profile import ( - PathSpeedProfileLimits, -) from dimos.navigation.dannav.holonomic_tc.run_profiles import RunProfile @@ -153,9 +153,7 @@ def _on_cmd_vel(cmd: Twist) -> None: plant_yaw_rad += wz * dt_s plant_yaw_rad = math.atan2(math.sin(plant_yaw_rad), math.cos(plant_yaw_rad)) sim_time_s += dt_s - core.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) - ) + core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) finally: core.close() cmd_sub.dispose() @@ -169,10 +167,14 @@ def test_closed_loop_straight_line_arrives_with_goal_decel() -> None: goal_tolerance_m = 0.08 goal_x_m = 1.0 result = _run_follower( - _make_follower(speed_m_s=cruise_speed_m_s, control_frequency=60.0, goal_tolerance=goal_tolerance_m), + _make_follower( + speed_m_s=cruise_speed_m_s, control_frequency=60.0, goal_tolerance=goal_tolerance_m + ), points=[(0.1, 0.0), (goal_x_m, 0.0)], ) - speeds = [_planar_speed_m_s(cmd) for cmd in result.command_history if _planar_speed_m_s(cmd) > 0.05] + speeds = [ + _planar_speed_m_s(cmd) for cmd in result.command_history if _planar_speed_m_s(cmd) > 0.05 + ] assert "arrived" in result.stop_messages assert speeds diff --git a/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py index cda275cc94..605b91b050 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py @@ -31,7 +31,9 @@ from dimos.navigation.dannav.holonomic_tc.command_limits import ( HolonomicCommandLimits, ) -from dimos.navigation.dannav.holonomic_tc.holonomic_tracking_controller import HolonomicTrackingController +from dimos.navigation.dannav.holonomic_tc.holonomic_tracking_controller import ( + HolonomicTrackingController, +) from dimos.navigation.dannav.holonomic_tc.types import ( TrajectoryMeasuredSample, TrajectoryReferenceSample, diff --git a/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py index c4c1ca3bca..05482f82b8 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py +++ b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py @@ -105,9 +105,7 @@ def _on_cmd_vel(cmd: Twist) -> None: plant_yaw_rad += wz * dt_s plant_yaw_rad = math.atan2(math.sin(plant_yaw_rad), math.cos(plant_yaw_rad)) sim_time_s += dt_s - core.handle_odom( - _pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s) - ) + core.handle_odom(_pose_stamped(plant_x_m, plant_y_m, plant_yaw_rad, ts=sim_time_s)) finally: core.close() cmd_sub.dispose() diff --git a/dimos/navigation/dannav/local_planner/module.py b/dimos/navigation/dannav/local_planner/module.py index 64f4aaf4c0..9f21003afa 100644 --- a/dimos/navigation/dannav/local_planner/module.py +++ b/dimos/navigation/dannav/local_planner/module.py @@ -15,7 +15,7 @@ """Path-stream middleware between ``MLSPlannerNative`` and ``DanHolonomicTC``. ``MLSPlannerNative`` re-roots and re-emits a path on every lidar frame: the -stream is sparse, unsmoothed, and unthrottled. +stream is sparse, unsmoothed, and unthrottled. - ``lock_replan``: commit-window throttle (forward a path only at commit moments, suppress in between so the follower keeps a stable lookahead). @@ -231,9 +231,7 @@ def start(self) -> None: # pose and armed goal when a planner path arrives. self.register_disposable(Disposable(self.odom.subscribe(self._on_odom))) self.register_disposable(Disposable(self.goal.subscribe(self._on_goal))) - self.register_disposable( - Disposable(self.planner_path.subscribe(self._on_planner_path)) - ) + self.register_disposable(Disposable(self.planner_path.subscribe(self._on_planner_path))) def _on_odom(self, msg: PoseStamped) -> None: self._gate.on_odom(msg) diff --git a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py index a048ef390a..c0a5d3dad4 100644 --- a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py +++ b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py @@ -30,7 +30,9 @@ def _path_from_points(points: list[tuple[float, float]]) -> Path: poses = [ - PoseStamped(ts=1.0, frame_id="world", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) + PoseStamped( + ts=1.0, frame_id="world", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0] + ) for x, y in points ] return Path(frame_id="world", poses=poses) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py index a4bae2df38..9ca75871ee 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -23,8 +23,8 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.mapping.voxels import VoxelGridMapper -from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC +from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative @@ -34,7 +34,7 @@ voxel_size = 0.05 # Height of the head-mounted lidar above the ground while standing. -# While in case of Go2 it's ~ .3m, but in this blueprint +# While in case of Go2 it's ~ .3m, but in this blueprint # MLSPlanner works better on Go2 lidar when the value is 0.5 go2_lidar_height = 0.5 @@ -97,6 +97,6 @@ def _render_path(msg: Any) -> Any: GoalRelay.blueprint(), # Seeting resample_spacing_m to > 0.0 will smooth out jagged paths retunned my MLSP DanLocalPlanner.blueprint(resample_spacing_m=0.1), - DanHolonomicTC.blueprint(run_profile='walk'), + DanHolonomicTC.blueprint(run_profile="walk"), MovementManager.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) From 4b2a91ccb227e5c629c80451381189621d0ddd5e Mon Sep 17 00:00:00 2001 From: danvi Date: Thu, 2 Jul 2026 22:18:16 +0800 Subject: [PATCH 7/9] fix mypy; typo --- dimos/navigation/dannav/geometry/path_speed_profile.py | 2 +- .../dannav/holonomic_tc/holonomic_tracking_controller.py | 3 ++- .../unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/dannav/geometry/path_speed_profile.py b/dimos/navigation/dannav/geometry/path_speed_profile.py index c3cd75715f..d728ceb234 100644 --- a/dimos/navigation/dannav/geometry/path_speed_profile.py +++ b/dimos/navigation/dannav/geometry/path_speed_profile.py @@ -128,7 +128,7 @@ def _polyline_geometry_speed_caps_m_s( def _profile_sample_distances_m( total_length_m: float, - vertex_s_m: Sequence[float], + vertex_s_m: NDArray[np.float64], num_samples: int, ) -> list[float]: """Uniform arc-length samples plus every polyline vertex.""" diff --git a/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py index c410b7a39b..cc205b73bb 100644 --- a/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py @@ -27,6 +27,7 @@ import math +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.navigation.dannav.holonomic_tc.command_limits import HolonomicCommandLimits @@ -37,7 +38,7 @@ from dimos.utils.trigonometry import angle_diff -def _planar_yaw_rad(pose_plan: object) -> float: +def _planar_yaw_rad(pose_plan: Pose) -> float: return float(pose_plan.orientation.euler.z) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py index 9ca75871ee..c36b317a16 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -95,7 +95,7 @@ def _render_path(msg: Any) -> Any: ] ), GoalRelay.blueprint(), - # Seeting resample_spacing_m to > 0.0 will smooth out jagged paths retunned my MLSP + # Setting resample_spacing_m to > 0.0 will smooth out jagged paths retunned my MLSP DanLocalPlanner.blueprint(resample_spacing_m=0.1), DanHolonomicTC.blueprint(run_profile="walk"), MovementManager.blueprint(), From f71ee99e6f87262b6dbc20986a22362428064669 Mon Sep 17 00:00:00 2001 From: danvi Date: Thu, 2 Jul 2026 22:36:42 +0800 Subject: [PATCH 8/9] fix race --- .../navigation/dannav/holonomic_tc/module.py | 53 +++++++++++-------- 1 file changed, 30 insertions(+), 23 deletions(-) diff --git a/dimos/navigation/dannav/holonomic_tc/module.py b/dimos/navigation/dannav/holonomic_tc/module.py index 0452c46b93..060fa81ae0 100644 --- a/dimos/navigation/dannav/holonomic_tc/module.py +++ b/dimos/navigation/dannav/holonomic_tc/module.py @@ -26,7 +26,7 @@ from dataclasses import dataclass import math -from threading import Event, RLock, Thread +from threading import Event, RLock, Thread, current_thread import time import traceback from typing import Any, Literal, TypeAlias @@ -169,11 +169,7 @@ def __init__(self, config: DanHolonomicTCConfig) -> None: # lifecycle def close(self) -> None: - with self._lock: - thread = self._thread self.stop_planning() - if thread is not None: - thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) def handle_odom(self, msg: PoseStamped) -> None: with self._lock: @@ -182,14 +178,17 @@ def handle_odom(self, msg: PoseStamped) -> None: def start_planning(self, path: Path) -> None: self.stop_planning() - self._stop_planning_event = Event() - with self._lock: + self._stop_planning_event = Event() self._path = path self._path_distancer = PathDistancer(path) self._previous_odom_for_velocity = None self._rebuild_path_speed_profile(self._path_distancer) - self._thread = Thread(target=self._thread_entrypoint, daemon=True) + self._thread = Thread( + target=self._thread_entrypoint, + args=(self._stop_planning_event,), + daemon=True, + ) self._thread.start() def update_path(self, path: Path) -> bool: @@ -208,12 +207,17 @@ def update_path(self, path: Path) -> bool: return True def stop_planning(self) -> None: - self.cmd_vel.on_next(Twist()) - self._stop_planning_event.set() - with self._lock: + thread = self._thread self._thread = None + self._stop_planning_event.set() + if thread is not None and thread is not current_thread(): + thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + if thread.is_alive(): + logger.warning("Holonomic control thread did not exit within join timeout.") + + self.cmd_vel.on_next(Twist()) self._reset_state() # run-profile envelope @@ -273,9 +277,8 @@ def _apply_run_envelope(self, envelope: ActiveRunEnvelope) -> None: self._controller.set_profile(envelope.profile) self._controller.set_speed(envelope.speed_m_s) with self._lock: - path_distancer = self._path_distancer - if path_distancer is not None: - self._rebuild_path_speed_profile(path_distancer) + if self._path_distancer is not None: + self._rebuild_path_speed_profile(self._path_distancer) # introspection @@ -293,16 +296,21 @@ def get_state(self) -> NavigationState: # control loop - def _thread_entrypoint(self) -> None: + def _thread_entrypoint(self, stop_event: Event) -> None: try: - self._loop() + self._loop(stop_event) except Exception as e: traceback.print_exc() logger.exception("Error in holonomic trajectory control", exc_info=e) self.stopped_navigating.on_next("error") finally: - self._reset_state() - self.cmd_vel.on_next(Twist()) + with self._lock: + owns_state = self._thread is current_thread() + if owns_state: + self._thread = None + self._reset_state() + if owns_state: + self.cmd_vel.on_next(Twist()) def _change_state(self, new_state: PlannerState) -> None: if new_state == self._state: @@ -328,9 +336,7 @@ def _initial_state(self, path: Path, current_odom: PoseStamped | None) -> Planne return "path_following" return "initial_rotation" - def _loop(self) -> None: - stop_event = self._stop_planning_event - + def _loop(self, stop_event: Event) -> None: with self._lock: path = self._path current_odom = self._current_odom @@ -482,10 +488,11 @@ def _path_speed_at_position( path_distancer: PathDistancer, current_pos: np.ndarray, ) -> float: - self._ensure_path_speed_profile(path_distancer) envelope = self._active_envelope progress_m = float(path_distancer.project(current_pos).s_along_path_m) - profile_speed = self._profiled_path_speed_m_s(progress_m) + with self._lock: + self._ensure_path_speed_profile(path_distancer) + profile_speed = self._profiled_path_speed_m_s(progress_m) distance_cap = math.sqrt( max( 0.0, From 37a6eaa536602166e1cf7a5079a90034cdab3fc4 Mon Sep 17 00:00:00 2001 From: danvi Date: Thu, 2 Jul 2026 22:55:54 +0800 Subject: [PATCH 9/9] fix lint --- dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md b/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md index e91be7f02e..bc7e7bae6f 100644 --- a/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md +++ b/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md @@ -108,4 +108,3 @@ Example CLI override: ```bash dimos run unitree-go2-mls-htc -o danholonomictc.run_profile=trot ``` -