diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 072e661d9a..e8fc9954f9 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -64,7 +64,6 @@ class GlobalConfig(BaseSettings): robot_width: float = 0.3 robot_rotation_diameter: float = 0.6 nerf_speed: float = 1.0 - planner_robot_speed: float | None = None mcp_port: int = 9990 build_native: bool = DEFAULT_BUILD_NATIVE dtop: bool = False @@ -78,12 +77,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/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/dannav/geometry/path_speed_profile.py b/dimos/navigation/dannav/geometry/path_speed_profile.py new file mode 100644 index 0000000000..d728ceb234 --- /dev/null +++ b/dimos/navigation/dannav/geometry/path_speed_profile.py @@ -0,0 +1,215 @@ +# 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 + +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 _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_caps_m_s( + path_xy: NDArray[np.float64], + vertex_s_m: NDArray[np.float64], + s_profile: Sequence[float], + limits: PathSpeedProfileLimits, +) -> 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( + total_length_m: float, + vertex_s_m: NDArray[np.float64], + 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_caps_m_s(path_xy, vertex_s_m, s_profile, limits) + 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 + 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/dannav/holonomic_tc/command_limits.py b/dimos/navigation/dannav/holonomic_tc/command_limits.py new file mode 100644 index 0000000000..337c74cad3 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/command_limits.py @@ -0,0 +1,117 @@ +# 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 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 +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, + ), + ) 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..bc7e7bae6f --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/docs/run_profiles.md @@ -0,0 +1,110 @@ +# 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/dannav/holonomic_tc/holonomic_path_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py new file mode 100644 index 0000000000..8796fe78f8 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py @@ -0,0 +1,164 @@ +# 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 numpy as np + +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.dannav.holonomic_tc.command_limits import ( + HolonomicCommandLimits, + clamp_holonomic_cmd_vel, +) +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: + 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) + + +class HolonomicPathController: + """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 + position reference. Not a car-style or Pure Pursuit path law. + """ + + def __init__( + self, + global_config: GlobalConfig, + profile: RunProfile, + 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._profile = profile + 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._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_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: + profile = self._profile + return HolonomicCommandLimits( + max_planar_speed_m_s=self._speed, + 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_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(normalize_angle(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 _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/dannav/holonomic_tc/holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py new file mode 100644 index 0000000000..cc205b73bb --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py @@ -0,0 +1,167 @@ +# 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 used elsewhere in navigation. + +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.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.types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) +from dimos.utils.trigonometry import angle_diff + + +def _planar_yaw_rad(pose_plan: Pose) -> 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) diff --git a/dimos/navigation/dannav/holonomic_tc/module.py b/dimos/navigation/dannav/holonomic_tc/module.py new file mode 100644 index 0000000000..060fa81ae0 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/module.py @@ -0,0 +1,672 @@ +# 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, current_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.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.base import NavigationState +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.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.dannav.holonomic_tc.types import ( + 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: + """Resolved movement envelope for the current session profile.""" + + profile: RunProfile + speed_m_s: float + path_limits: PathSpeedProfileLimits + goal_decel_m_s2: float + + +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``. + """ + + 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 + + _lock: RLock + _stop_planning_event: Event + _state: PlannerState + _global_config: GlobalConfig + _goal_tolerance: float + _controller: HolonomicPathController + _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 + + def __init__(self, config: DanHolonomicTCConfig) -> None: + self.cmd_vel = Subject() + self.stopped_navigating = Subject() + + self._config = config + self._global_config = config.g + self._lock = RLock() + self._stop_planning_event = Event() + self._state = "idle" + 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._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.profile, + 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: + self.stop_planning() + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._current_odom = msg + + def start_planning(self, path: Path) -> None: + self.stop_planning() + + 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, + args=(self._stop_planning_event,), + 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) + self._rebuild_path_speed_profile(self._path_distancer) + + return True + + def stop_planning(self) -> None: + 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 + + 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 + ) + return ActiveRunEnvelope( + profile=profile, + speed_m_s=speed, + path_limits=profile.path_speed_profile_limits_at(speed), + goal_decel_m_s2=profile.goal_decel_m_s2, + ) + + 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_profile(envelope.profile) + self._controller.set_speed(envelope.speed_m_s) + with self._lock: + if self._path_distancer is not None: + self._rebuild_path_speed_profile(self._path_distancer) + + # 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}") + + # control loop + + def _thread_entrypoint(self, stop_event: Event) -> None: + try: + 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: + 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: + return + self._state = new_state + 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) + if abs(initial_yaw_error) < self._orientation_tolerance: + return "path_following" + return "initial_rotation" + + def _loop(self, stop_event: Event) -> None: + 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": + # 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) + return self._controller.rotate(yaw_error, current_odom, measured_body_twist) + + 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() + + 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, + current_odom, + current_pos, + path_speed, + ) + measured_body_twist = self._estimate_measured_body_twist(current_odom) + return self._controller.advance_reference( + reference_sample, + current_odom, + measured_body_twist, + ) + + 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_at_position( + self, + path_distancer: PathDistancer, + current_pos: np.ndarray, + ) -> float: + envelope = self._active_envelope + progress_m = float(path_distancer.project(current_pos).s_along_path_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, + 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) + 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._previous_odom_for_velocity = None + self._controller.set_speed(self._active_envelope.speed_m_s) + self._controller.reset_errors() + + 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), + ) + + +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.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 + + +class DanHolonomicTC(Module): + """Follow a planned ``Path`` with the holonomic tracking law. + + 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``. + ``stop_movement`` cancels the current path. + """ + + config: DanHolonomicTCConfig + + path: In[Path] + odom: In[PoseStamped] + 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.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))) + + @rpc + def stop(self) -> None: + self._core.close() + self.nav_cmd_vel.publish(Twist()) + super().stop() + + 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, + # 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 get_state(self) -> NavigationState: + return self._core.get_state() diff --git a/dimos/navigation/dannav/holonomic_tc/run_profiles.py b/dimos/navigation/dannav/holonomic_tc/run_profiles.py new file mode 100644 index 0000000000..d202e82b15 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/run_profiles.py @@ -0,0 +1,134 @@ +# 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: ``DanHolonomicTCConfig.run_profile`` and +``_HolonomicPathFollower._resolve_run_envelope``. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math + +from dimos.navigation.dannav.geometry.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 + + 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}" + ) + + 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.""" + + profiles: Mapping[str, RunProfile] + + 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}" + ) + 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 + + +GO2_RUN_PROFILES = RunProfileRegistry( + 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, + ), + "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, + ), + "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, + ), + }, +) 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/dannav/holonomic_tc/test_dan_holonomic_tc.py b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py new file mode 100644 index 0000000000..9a7a81fa87 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py @@ -0,0 +1,212 @@ +# 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`` module integration: cancel inputs, path hot-swap, arrival.""" + +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, Literal + +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 +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.base import NavigationState +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.""" + + 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 _odom(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( + 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 + + def feed_odom(self, x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> None: + 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) + + 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.odom.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 _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 + + +@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)])) + + if cancel_via == "empty_path": + h.feed_empty_path() + else: + h.feed_stop(True) + + _assert_route_cleared(h) + + +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(1.0, 0.0, 0.0) + h.feed_path(_path_from_points([(0.0, 0.0), (2.0, 0.0)])) + assert not _wait_until(lambda: bool(h.captured.goal_reached), timeout=0.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 h.captured.goal_reached[-1].data is True + + +def test_arrival_publishes_goal_reached_without_final_spin() -> None: + # 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 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..52b5b8093b --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py @@ -0,0 +1,226 @@ +# 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 + +from dataclasses import dataclass +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.geometry.path_speed_profile import ( + PathSpeedProfileLimits, +) +from dimos.navigation.dannav.holonomic_tc.module import ( + ActiveRunEnvelope, + DanHolonomicTCConfig, + _HolonomicPathFollower, +) +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..605b91b050 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py @@ -0,0 +1,150 @@ +# 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..05482f82b8 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py @@ -0,0 +1,128 @@ +# 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/dannav/holonomic_tc/types.py b/dimos/navigation/dannav/holonomic_tc/types.py new file mode 100644 index 0000000000..20f0389764 --- /dev/null +++ b/dimos/navigation/dannav/holonomic_tc/types.py @@ -0,0 +1,63 @@ +# 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. + +**Plan horizontal frame (``pose_plan``)** + +- Positions and yaw use the same plan horizontal convention as ``Path`` and + ``PathDistancer``. + +**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 TrajectorySample: + """Timed pose + body twist sample (shared shape).""" + + 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 TrajectoryReferenceSample(TrajectorySample): + """One timed point on the reference trajectory (target).""" + + +@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 new file mode 100644 index 0000000000..9f21003afa --- /dev/null +++ b/dimos/navigation/dannav/local_planner/module.py @@ -0,0 +1,245 @@ +# 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. + +- ``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``). + +: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.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.dannav.geometry.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.0 # resample spacing in m; 0 disables smoothing + smoothing_window: int = 100 # moving-average window + + +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; 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. + 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: PoseStamped) -> 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 is + 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 + unchanged to ``DanHolonomicTC``. + """ + + config: DanLocalPlannerConfig + + planner_path: In[Path] # from MLSPlannerNative.path (remapped) + odom: In[PoseStamped] # from GO2Connection.odom + 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.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_odom(self, msg: PoseStamped) -> 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..c0a5d3dad4 --- /dev/null +++ b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py @@ -0,0 +1,147 @@ +# 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 ``_ReplanGate`` commit-window gating.""" + +from __future__ import annotations + +import math +from typing import Any + +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.dannav.local_planner.module import ( + DanLocalPlannerConfig, + _ReplanGate, +) + + +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 _odom(x: float, y: float, *, ts: float = 1.0) -> PoseStamped: + return PoseStamped( + ts=ts, + frame_id="world", + 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") + + +def _gate(**config: Any) -> _ReplanGate: + config.setdefault("resample_spacing_m", 0.0) + return _ReplanGate(DanLocalPlannerConfig(**config)) + + +def test_replan_after_advancing_lock_is_published() -> None: + gate = _gate(lock_replan=0.5) + 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(_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(_odom(0.6, 0.0)) + committed = _path_from_points([(0.6, 0.0), (5.0, 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(_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(_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 not None + 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(_odom(0.0, 0.0)) + gate.on_planner_path(_path_from_points([(0.0, 0.0), (5.0, 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 + 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(_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 + # 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(_odom(0.1, 0.0)) + restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + 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(_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(_odom(0.1, 0.0)) + restart = _path_from_points([(0.1, 0.0), (5.0, 0.0)]) + 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(_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 not None diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 61220a0fae..b14fffaab6 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -120,6 +120,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", @@ -152,6 +153,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.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", "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 new file mode 100644 index 0000000000..c36b317a16 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -0,0 +1,102 @@ +#!/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. + +""" + +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.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 +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.05 +# Height of the head-mounted lidar above the ground while standing. +# 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: + 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": 0, + }, + "memory_limit": "8192MB", + "visual_override": { + **rerun_config["visual_override"], + "world/global_map": _render_global_map, + "world/planner_path": None, + "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), + GO2Connection.blueprint(motion_mode="mcf"), + VoxelGridMapper.blueprint( + voxel_size=voxel_size, + frame_id="world", + 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, + ).remappings( + [ + (MLSPlannerNative, "path", "planner_path"), + # The planner's start pose is the robot's odom pose + (MLSPlannerNative, "start_pose", "odom"), + ] + ), + GoalRelay.blueprint(), + # 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(), +).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) diff --git a/docs/usage/configuration.md b/docs/usage/configuration.md index 1642a31be5..6e6bc7b04c 100644 --- a/docs/usage/configuration.md +++ b/docs/usage/configuration.md @@ -109,7 +109,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,