diff --git a/.gitignore b/.gitignore index 9064ce16ff..120aa41759 100644 --- a/.gitignore +++ b/.gitignore @@ -79,7 +79,6 @@ yolo11n.pt /results results/ **/cpp/result -**/rust/result CLAUDE.MD /assets/teleop_certs/ diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 7c7ff59ff8..2c8ab22dfc 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -171,8 +171,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.frame_id, - child_frame_id=self.config.child_frame_id, + frame_id=FRAME_ODOM, + child_frame_id=FRAME_BODY, translation=Vector3( msg.pose.position.x, msg.pose.position.y, diff --git a/dimos/mapping/costmapper.py b/dimos/mapping/costmapper.py index 127c903733..2c374bdf91 100644 --- a/dimos/mapping/costmapper.py +++ b/dimos/mapping/costmapper.py @@ -14,9 +14,7 @@ from dataclasses import asdict import time -from typing import Any -import numpy as np from pydantic import Field from reactivex import combine_latest, operators as ops @@ -34,33 +32,10 @@ logger = setup_logger() -_COLOR_UNKNOWN = (0, 0, 0, 0) -_COLOR_FREE = (72, 73, 129, 255) -_COLOR_OCCUPIED = (255, 140, 0, 255) -_COLOR_LETHAL = (220, 30, 30, 255) - -# Indexed by grid value + 1: 0 = unknown, 1 = free, 2..101 = cost 1..100. -_COSTMAP_COLOR_LOOKUP_TABLE = np.empty((102, 4), dtype=np.uint8) -_COSTMAP_COLOR_LOOKUP_TABLE[0] = _COLOR_UNKNOWN -_COSTMAP_COLOR_LOOKUP_TABLE[1] = _COLOR_FREE -_COSTMAP_COLOR_LOOKUP_TABLE[2:101] = _COLOR_OCCUPIED -_COSTMAP_COLOR_LOOKUP_TABLE[101] = _COLOR_LETHAL - -_COSTMAP_Z_OFFSET = 0.02 - - -def costmap_to_rerun(grid: OccupancyGrid) -> Any: - return grid.to_rerun( - color_lookup_table=_COSTMAP_COLOR_LOOKUP_TABLE, - z_offset=_COSTMAP_Z_OFFSET, - ) - class Config(ModuleConfig): algo: str = "height_cost" config: OccupancyConfig = Field(default_factory=HeightCostConfig) - # for robots that cant see directly below themself - initial_safe_radius_meters: float = 0.0 class CostMapper(Module): @@ -107,27 +82,5 @@ def stop(self) -> None: # @timed() # TODO: fix thread leak in timed decorator def _calculate_costmap(self, msg: PointCloud2) -> OccupancyGrid: - occupancy_function = OCCUPANCY_ALGOS[self.config.algo] - grid = occupancy_function(msg, **asdict(self.config.config)) - self._apply_initial_safe_radius(grid) - return grid - - def _apply_initial_safe_radius(self, grid: OccupancyGrid) -> None: - radius_meters = self.config.initial_safe_radius_meters - if radius_meters <= 0 or grid.grid.size == 0: - return - - resolution = grid.resolution - origin_x = grid.origin.position.x - origin_y = grid.origin.position.y - - rows, columns = np.ogrid[: grid.grid.shape[0], : grid.grid.shape[1]] - cell_world_x = columns * resolution + origin_x - cell_world_y = rows * resolution + origin_y - distance_squared_meters = cell_world_x**2 + cell_world_y**2 - - # Half-cell tolerance: a cell counts as inside if any part of it overlaps - # the disc. Avoids floating-point boundary flakiness from radius/resolution. - effective_radius_meters = radius_meters + resolution * 0.5 - safe_mask = distance_squared_meters <= effective_radius_meters**2 - grid.grid[safe_mask] = 0 + fn = OCCUPANCY_ALGOS[self.config.algo] + return fn(msg, **asdict(self.config.config)) diff --git a/dimos/mapping/ray_tracing/module.py b/dimos/mapping/ray_tracing/module.py index 5b215f9ff6..8d055b42c5 100644 --- a/dimos/mapping/ray_tracing/module.py +++ b/dimos/mapping/ray_tracing/module.py @@ -26,8 +26,8 @@ class RayTracingVoxelMapConfig(NativeModuleConfig): cwd: str | None = "rust" - executable: str = "result/bin/voxel_ray_tracing" - build_command: str | None = "nix build path:." + executable: str = "target/release/voxel_ray_tracing" + build_command: str | None = "cargo build --release" stdin_config: bool = True voxel_size: float = 0.1 diff --git a/dimos/mapping/ray_tracing/rust/flake.lock b/dimos/mapping/ray_tracing/rust/flake.lock deleted file mode 100644 index a548660557..0000000000 --- a/dimos/mapping/ray_tracing/rust/flake.lock +++ /dev/null @@ -1,78 +0,0 @@ -{ - "nodes": { - "dimos-repo": { - "flake": false, - "locked": { - "lastModified": 1779865691, - "narHash": "sha256-2CVWcov7DiC1qX/B/zFKDJiSYsnbrZ3FNT/viprFWTQ=", - "ref": "refs/heads/jeff/feat/g1_raycast", - "rev": "51666bcd298c1d08bdee179f176f45c0a7dd417d", - "revCount": 744, - "type": "git", - "url": "file:../../../.." - }, - "original": { - "type": "git", - "url": "file:../../../.." - } - }, - "flake-utils": { - "inputs": { - "systems": "systems" - }, - "locked": { - "lastModified": 1731533236, - "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", - "owner": "numtide", - "repo": "flake-utils", - "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", - "type": "github" - }, - "original": { - "owner": "numtide", - "repo": "flake-utils", - "type": "github" - } - }, - "nixpkgs": { - "locked": { - "lastModified": 1779560665, - "narHash": "sha256-tpyBcxPpcQb8ukyNF7DoCwfSY3VPsxHoYwj00Cayv5o=", - "owner": "NixOS", - "repo": "nixpkgs", - "rev": "64c08a7ca051951c8eae34e3e3cb1e202fe36786", - "type": "github" - }, - "original": { - "owner": "NixOS", - "ref": "nixos-unstable", - "repo": "nixpkgs", - "type": "github" - } - }, - "root": { - "inputs": { - "dimos-repo": "dimos-repo", - "flake-utils": "flake-utils", - "nixpkgs": "nixpkgs" - } - }, - "systems": { - "locked": { - "lastModified": 1681028828, - "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", - "owner": "nix-systems", - "repo": "default", - "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", - "type": "github" - }, - "original": { - "owner": "nix-systems", - "repo": "default", - "type": "github" - } - } - }, - "root": "root", - "version": 7 -} diff --git a/dimos/mapping/ray_tracing/rust/flake.nix b/dimos/mapping/ray_tracing/rust/flake.nix deleted file mode 100644 index bb2fbfd52d..0000000000 --- a/dimos/mapping/ray_tracing/rust/flake.nix +++ /dev/null @@ -1,42 +0,0 @@ -{ - description = "Voxel ray tracing native module for DimOS"; - - inputs = { - nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; - flake-utils.url = "github:numtide/flake-utils"; - # Relative git+file: will be deprecated (nix#12281) but there's no - # viable alternative for reaching local path deps outside the flake dir currently - # presumably an alternative will be added before this is removed - dimos-repo = { url = "git+file:../../../.."; flake = false; }; - }; - - outputs = { self, nixpkgs, flake-utils, dimos-repo }: - flake-utils.lib.eachDefaultSystem (system: - let - pkgs = import nixpkgs { inherit system; }; - - src = pkgs.runCommand "voxel-ray-tracing-src" {} '' - mkdir -p $out/dimos/mapping/ray_tracing/rust - cp -r ${./src} $out/dimos/mapping/ray_tracing/rust/src - cp ${./Cargo.toml} $out/dimos/mapping/ray_tracing/rust/Cargo.toml - cp ${./Cargo.lock} $out/dimos/mapping/ray_tracing/rust/Cargo.lock - - mkdir -p $out/native/rust - cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module - cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros - ''; - in { - packages.default = pkgs.rustPlatform.buildRustPackage { - pname = "voxel-ray-tracing"; - version = "0.1.0"; - - inherit src; - cargoRoot = "dimos/mapping/ray_tracing/rust"; - buildAndTestSubdir = "dimos/mapping/ray_tracing/rust"; - - cargoHash = "sha256-g30NaoLdtWT5YBsEnE4Xv+EMnI5HHFtZAUtdEL/VbKQ="; - - meta.mainProgram = "voxel_ray_tracing"; - }; - }); -} diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py index bad3fc9ab2..b468bbcac0 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -485,7 +485,6 @@ def to_rerun( opacity: float = 1.0, cost_range: tuple[int, int] | None = None, background: str | None = None, - color_lookup_table: np.ndarray | None = None, ) -> Archetype: """Convert to 3D textured mesh overlay on floor plane. @@ -505,9 +504,8 @@ def to_rerun( step_w = max(1, grid.shape[1] // max_tex) grid = grid[::step_h, ::step_w] - if color_lookup_table is None: - color_lookup_table = _build_occupancy_lut(colormap, opacity, background) - rgba = np.ascontiguousarray(color_lookup_table[np.clip(grid + 1, 0, 101)]) + lut = _build_occupancy_lut(colormap, opacity, background) + rgba = np.ascontiguousarray(lut[np.clip(grid + 1, 0, 101)]) # Apply cost_range filter on downsampled grid if cost_range is not None: diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index cdcc490551..a958f8dba0 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -24,7 +24,6 @@ import numpy as np from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance @@ -132,19 +131,6 @@ def pitch(self) -> float: def yaw(self) -> float: return self.pose.yaw - def to_pose_stamped(self) -> PoseStamped: - return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=[self.position.x, self.position.y, self.position.z], - orientation=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ], - ) - # -- Serialization -- def lcm_encode(self) -> bytes: diff --git a/dimos/navigation/replanning_a_star/min_cost_astar.py b/dimos/navigation/replanning_a_star/min_cost_astar.py index 03212e87b8..025045c2c9 100644 --- a/dimos/navigation/replanning_a_star/min_cost_astar.py +++ b/dimos/navigation/replanning_a_star/min_cost_astar.py @@ -68,12 +68,11 @@ def _reconstruct_path( start_tuple: tuple[int, int], goal_tuple: tuple[int, int], ) -> Path: - frame_id = costmap.frame_id waypoints: list[PoseStamped] = [] while current in parents: world_point = costmap.grid_to_world(current) pose = PoseStamped( - frame_id=frame_id, + frame_id="world", position=[world_point.x, world_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), # Identity quaternion ) @@ -82,7 +81,7 @@ def _reconstruct_path( start_world_point = costmap.grid_to_world(start_tuple) start_pose = PoseStamped( - frame_id=frame_id, + frame_id="world", position=[start_world_point.x, start_world_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), ) @@ -98,32 +97,31 @@ def _reconstruct_path( or (waypoints[-1].x - goal_point.x) ** 2 + (waypoints[-1].y - goal_point.y) ** 2 > 1e-10 ): goal_pose = PoseStamped( - frame_id=frame_id, + frame_id="world", position=[goal_point.x, goal_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), ) waypoints.append(goal_pose) - return Path(frame_id=frame_id, poses=waypoints) + return Path(frame_id="world", poses=waypoints) def _reconstruct_path_from_coords( path_coords: list[tuple[int, int]], costmap: OccupancyGrid, ) -> Path: - frame_id = costmap.frame_id waypoints: list[PoseStamped] = [] for gx, gy in path_coords: world_point = costmap.grid_to_world((gx, gy)) pose = PoseStamped( - frame_id=frame_id, + frame_id="world", position=[world_point.x, world_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), ) waypoints.append(pose) - return Path(frame_id=frame_id, poses=waypoints) + return Path(frame_id="world", poses=waypoints) def min_cost_astar( diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 9ea8607bb2..cf3e0d472b 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -19,13 +19,12 @@ from reactivex.disposable import Disposable from dimos.core.core import rpc -from dimos.core.module import Module, ModuleConfig +from dimos.core.module import Module from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.base import NavigationInterface, NavigationState from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner @@ -34,16 +33,8 @@ logger = setup_logger() -class ReplanningAStarPlannerConfig(ModuleConfig): - robot_width: float | None = None - robot_rotation_diameter: float | None = None - - class ReplanningAStarPlanner(Module, NavigationInterface): - config: ReplanningAStarPlannerConfig - odom: In[PoseStamped] # TODO: Use TF. - odometry: In[Odometry] global_costmap: In[OccupancyGrid] goal_request: In[PoseStamped] clicked_point: In[PointStamped] @@ -60,31 +51,13 @@ class ReplanningAStarPlanner(Module, NavigationInterface): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - overrides = { - name: value - for name, value in ( - ("robot_width", self.config.robot_width), - ("robot_rotation_diameter", self.config.robot_rotation_diameter), - ) - if value is not None - } - effective_global_config = ( - self.config.g.model_copy(update=overrides) if overrides else self.config.g - ) - self._planner = GlobalPlanner(effective_global_config) + self._planner = GlobalPlanner(self.config.g) @rpc def start(self) -> None: super().start() self.register_disposable(Disposable(self.odom.subscribe(self._planner.handle_odom))) - self.register_disposable( - Disposable( - self.odometry.subscribe( - lambda msg: self._planner.handle_odom(msg.to_pose_stamped()) - ) - ) - ) self.register_disposable( Disposable(self.global_costmap.subscribe(self._planner.handle_global_costmap)) ) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 24d90dd7fe..e2bfa2d1ea 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -84,6 +84,7 @@ "teleop-quest-xarm6": "dimos.teleop.quest.blueprints:teleop_quest_xarm6", "teleop-quest-xarm7": "dimos.teleop.quest.blueprints:teleop_quest_xarm7", "teleop-quest-xarm7-video": "dimos.teleop.quest.blueprints:teleop_quest_xarm7_video", + "uintree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav:uintree_g1_primitive_no_nav", "unitree-g1": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_agentic:unitree_g1_agentic", "unitree-g1-agentic-sim": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_agentic_sim:unitree_g1_agentic_sim", @@ -96,8 +97,6 @@ "unitree-g1-joystick": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_joystick:unitree_g1_joystick", "unitree-g1-nav-onboard": "dimos.robot.unitree.g1.blueprints.navigation.unitree_g1_nav_onboard:unitree_g1_nav_onboard", "unitree-g1-nav-sim": "dimos.robot.unitree.g1.blueprints.navigation.unitree_g1_nav_sim:unitree_g1_nav_sim", - "unitree-g1-nav-simple": "dimos.robot.unitree.g1.blueprints.navigation.unitree_g1_nav_simple:unitree_g1_nav_simple", - "unitree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_primitive_no_nav:unitree_g1_primitive_no_nav", "unitree-g1-shm": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_shm:unitree_g1_shm", "unitree-g1-sim": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_sim:unitree_g1_sim", "unitree-go2": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2", diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py index 7a40ad71a4..98248916ea 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py @@ -16,13 +16,13 @@ """Basic G1 stack: base sensors plus real robot connection and ROS nav.""" from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_primitive_no_nav import ( - unitree_g1_primitive_no_nav, +from dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav import ( + uintree_g1_primitive_no_nav, ) from dimos.robot.unitree.g1.connection import G1Connection unitree_g1_basic = autoconnect( - unitree_g1_primitive_no_nav, + uintree_g1_primitive_no_nav, G1Connection.blueprint(), ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py index 628e8a8867..32d2d52b8b 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py @@ -17,13 +17,13 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner -from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_primitive_no_nav import ( - unitree_g1_primitive_no_nav, +from dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav import ( + uintree_g1_primitive_no_nav, ) from dimos.robot.unitree.g1.mujoco_sim import G1SimConnection unitree_g1_basic_sim = autoconnect( - unitree_g1_primitive_no_nav, + uintree_g1_primitive_no_nav, G1SimConnection.blueprint(), ReplanningAStarPlanner.blueprint(), ) diff --git a/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py b/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py index 1fe25fb40d..d64b2c8aa0 100644 --- a/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py @@ -15,13 +15,15 @@ from __future__ import annotations +import os + from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config -from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_onboard import _unitree_g1_onboard from dimos.robot.unitree.g1.config import G1, G1_LOCAL_PLANNER_PRECOMPUTED_PATHS +from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk from dimos.robot.unitree.g1.g1_rerun import ( g1_odometry_tf_override, g1_static_robot, @@ -30,7 +32,13 @@ unitree_g1_nav_onboard = ( autoconnect( - _unitree_g1_onboard, + FastLio2.blueprint( + host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), + lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), + mount=G1.internal_odom_offsets["mid360_link"], + map_freq=1.0, + config="default.yaml", + ), create_nav_stack( planner="simple", vehicle_height=G1.height_clearance, @@ -57,6 +65,7 @@ }, ), MovementManager.blueprint(), + G1HighLevelDdsSdk.blueprint(), vis_module( viewer_backend=global_config.viewer, rerun_config=nav_stack_rerun_config( diff --git a/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_simple.py b/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_simple.py deleted file mode 100644 index e1c3bd7ba4..0000000000 --- a/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_simple.py +++ /dev/null @@ -1,56 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Simple G1 nav stack: onboard sensors, raytracing costmap, and A* replanning.""" - -from dimos.core.coordination.blueprints import autoconnect -from dimos.mapping.costmapper import CostMapper -from dimos.mapping.pointclouds.occupancy import HeightCostConfig -from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner -from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_onboard import _unitree_g1_onboard -from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_vis import unitree_g1_vis -from dimos.robot.unitree.g1.config import G1 - -assert G1.height_clearance is not None and G1.width_clearance is not None - -g1_overhead_safety_margin = 0.2 -g1_overhead_clearance = G1.height_clearance + g1_overhead_safety_margin -g1_max_step_height = 0.10 -g1_rotation_diameter = 0.8 -voxel_resolution = 0.05 -g1_safe_radius_margin = 0.6 - -unitree_g1_nav_simple = autoconnect( - _unitree_g1_onboard, - RayTracingVoxelMap.blueprint(voxel_size=voxel_resolution), - CostMapper.blueprint( - config=HeightCostConfig( - resolution=voxel_resolution, - can_pass_under=g1_overhead_clearance, - can_climb=g1_max_step_height, - ), - initial_safe_radius_meters=G1.width_clearance + g1_safe_radius_margin, - ), - ReplanningAStarPlanner.blueprint( - robot_width=G1.width_clearance, - robot_rotation_diameter=g1_rotation_diameter, - ), - MovementManager.blueprint(), - unitree_g1_vis, -).global_config(n_workers=10, robot_model="unitree_g1") - -__all__ = ["unitree_g1_nav_simple"] diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py similarity index 93% rename from dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py rename to dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py index 3a115d4607..6ae1510fa9 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py @@ -25,7 +25,7 @@ from dimos.hardware.sensors.camera.module import CameraModule from dimos.hardware.sensors.camera.webcam import Webcam from dimos.hardware.sensors.camera.zed import compat as zed -from dimos.mapping.costmapper import CostMapper, costmap_to_rerun +from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels import VoxelGridMapper from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -50,6 +50,15 @@ def _convert_camera_info(camera_info: Any) -> Any: ) +def _convert_navigation_costmap(grid: Any) -> Any: + return grid.to_rerun( + colormap="Accent", + z_offset=0.015, + opacity=0.2, + background="#484981", + ) + + def _static_base_link(rr: Any) -> list[Any]: return [ rr.Boxes3D( @@ -86,7 +95,7 @@ def _g1_rerun_blueprint() -> Any: "blueprint": _g1_rerun_blueprint, "visual_override": { "world/camera_info": _convert_camera_info, - "world/navigation_costmap": costmap_to_rerun, + "world/navigation_costmap": _convert_navigation_costmap, }, "static": { "world/tf/base_link": _static_base_link, @@ -121,7 +130,7 @@ def _create_webcam() -> Webcam: else autoconnect() ) -unitree_g1_primitive_no_nav = ( +uintree_g1_primitive_no_nav = ( autoconnect( _with_vis, _camera, @@ -154,4 +163,4 @@ def _create_webcam() -> Webcam: ) ) -__all__ = ["unitree_g1_primitive_no_nav"] +__all__ = ["uintree_g1_primitive_no_nav"] diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py deleted file mode 100644 index 96f97146b1..0000000000 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import os - -from dimos.core.coordination.blueprints import autoconnect -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_vis import unitree_g1_vis -from dimos.robot.unitree.g1.config import G1 -from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk - -# Underscore-prefixed: a shared sub-blueprint, not a runnable blueprint of its own. -_unitree_g1_onboard = autoconnect( - FastLio2.blueprint( - host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), - lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), - mount=G1.internal_odom_offsets["mid360_link"], - map_freq=1.0, - config="default.yaml", - ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), - G1HighLevelDdsSdk.blueprint(), - unitree_g1_vis, -).global_config(n_workers=12, robot_model="unitree_g1") diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py deleted file mode 100644 index 43dfe90991..0000000000 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from typing import Any - -import rerun as rr - -from dimos.core.global_config import global_config -from dimos.mapping.costmapper import costmap_to_rerun -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.nav_stack.main import nav_stack_rerun_config -from dimos.robot.unitree.g1.g1_rerun import g1_odometry_tf_override, g1_static_robot -from dimos.visualization.vis_module import vis_module - -_PATH_Z_LIFT = 0.3 -_PATH_COLOR_RGBA = (0, 255, 128, 255) -_PATH_RADIUS_METERS = 0.05 - - -def _g1_path_colors(path: Path) -> Any: - # Empty geometry instead of None so the stale path actually clears. - if not path.poses: - return rr.LineStrips3D([]) - - points = [[pose.x, pose.y, pose.z + _PATH_Z_LIFT] for pose in path.poses] - return rr.LineStrips3D([points], colors=[_PATH_COLOR_RGBA], radii=_PATH_RADIUS_METERS) - - -unitree_g1_vis = vis_module( - viewer_backend=global_config.viewer, - rerun_config=nav_stack_rerun_config( - { - "visual_override": { - "world/odometry": g1_odometry_tf_override, - "world/lidar": None, - "world/local_map": None, - "world/global_map_fastlio": None, - "world/global_costmap": costmap_to_rerun, - "world/path": _g1_path_colors, - }, - "static": {"world/tf/robot": g1_static_robot}, - "memory_limit": "1GB", - }, - vis_throttle=0.5, - ), -) - -__all__ = ["unitree_g1_vis"] diff --git a/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py b/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py index 462b48d959..8ad34c95bf 100644 --- a/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py +++ b/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py @@ -79,17 +79,6 @@ class G1HighLevelDdsSdkConfig(ModuleConfig): motion_switcher_timeout: float = 5.0 loco_client_timeout: float = 10.0 cmd_vel_timeout: float = 0.2 - # deadzone compensation - min_effective_linear_velocity: float = 0.05 # m/s - min_effective_angular_velocity: float = 0.2 # radians/s - - -def _boost_above_deadzone(value: float, min_effective_magnitude: float) -> float: - if value == 0.0 or min_effective_magnitude <= 0.0: - return value - if abs(value) >= min_effective_magnitude: - return value - return min_effective_magnitude if value > 0 else -min_effective_magnitude class G1HighLevelDdsSdk(Module, HighLevelG1Spec): @@ -161,13 +150,9 @@ def stop(self) -> None: @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: assert self.loco_client is not None - raw_vx = twist.linear.x - raw_vy = twist.linear.y - raw_vyaw = twist.angular.z - - vx = _boost_above_deadzone(raw_vx, self.config.min_effective_linear_velocity) - vy = _boost_above_deadzone(raw_vy, self.config.min_effective_linear_velocity) - vyaw = _boost_above_deadzone(raw_vyaw, self.config.min_effective_angular_velocity) + vx = twist.linear.x + vy = twist.linear.y + vyaw = twist.angular.z if self._stop_timer: self._stop_timer.cancel() diff --git a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_security.py b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_security.py index 632429483d..1d64b7920e 100644 --- a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_security.py +++ b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_security.py @@ -17,7 +17,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config -from dimos.mapping.costmapper import costmap_to_rerun from dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic import unitree_go2_agentic from dimos.visualization.vis_module import vis_module @@ -29,6 +28,15 @@ def _convert_camera_info(camera_info: Any) -> Any: ) +def _convert_navigation_costmap(grid: Any) -> Any: + return grid.to_rerun( + colormap="Accent", + z_offset=0.015, + opacity=0.2, + background="#484981", + ) + + def _static_base_link(rr: Any) -> list[Any]: return [ rr.Boxes3D( @@ -67,7 +75,7 @@ def _go2_rerun_blueprint() -> Any: "blueprint": _go2_rerun_blueprint, "visual_override": { "world/camera_info": _convert_camera_info, - "world/navigation_costmap": costmap_to_rerun, + "world/navigation_costmap": _convert_navigation_costmap, }, "static": { "world/tf/base_link": _static_base_link, diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index 928b2dc23a..96a291163d 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -21,7 +21,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import pSHMTransport -from dimos.mapping.costmapper import costmap_to_rerun from dimos.msgs.sensor_msgs.Image import Image from dimos.robot.unitree.go2.connection import GO2Connection from dimos.visualization.vis_module import vis_module @@ -51,6 +50,15 @@ def _convert_global_map(grid: Any) -> Any: return grid.to_rerun(bottom_cutoff=0) +def _convert_navigation_costmap(grid: Any) -> Any: + return grid.to_rerun( + colormap="Accent", + z_offset=0.015, + opacity=0.2, + background="#484981", + ) + + def _static_base_link(rr: Any) -> list[Any]: return [ rr.Boxes3D( @@ -98,7 +106,7 @@ def _go2_rerun_blueprint() -> Any: "world/camera_info": _convert_camera_info, "world/global_map": _convert_global_map, "world/merged_map": _convert_global_map, - "world/navigation_costmap": costmap_to_rerun, + "world/navigation_costmap": _convert_navigation_costmap, }, "max_hz": { "world/global_map": 0, # publishes at ~7.8 Hz