diff --git a/.gitignore b/.gitignore index 42bdddfa45..36f5106575 100644 --- a/.gitignore +++ b/.gitignore @@ -76,6 +76,7 @@ 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 2c8ab22dfc..7c7ff59ff8 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=FRAME_ODOM, - child_frame_id=FRAME_BODY, + frame_id=self.frame_id, + child_frame_id=self.config.child_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, diff --git a/dimos/mapping/costmapper.py b/dimos/mapping/costmapper.py index 2c374bdf91..127c903733 100644 --- a/dimos/mapping/costmapper.py +++ b/dimos/mapping/costmapper.py @@ -14,7 +14,9 @@ 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 @@ -32,10 +34,33 @@ 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): @@ -82,5 +107,27 @@ def stop(self) -> None: # @timed() # TODO: fix thread leak in timed decorator def _calculate_costmap(self, msg: PointCloud2) -> OccupancyGrid: - fn = OCCUPANCY_ALGOS[self.config.algo] - return fn(msg, **asdict(self.config.config)) + 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 diff --git a/dimos/mapping/ray_tracing/module.py b/dimos/mapping/ray_tracing/module.py index 28af45e28a..f506b85155 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 = "target/release/voxel_ray_tracing" - build_command: str | None = "cargo build --release" + executable: str = "result/bin/voxel_ray_tracing" + build_command: str | None = "nix build path:." 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 new file mode 100644 index 0000000000..a548660557 --- /dev/null +++ b/dimos/mapping/ray_tracing/rust/flake.lock @@ -0,0 +1,78 @@ +{ + "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 new file mode 100644 index 0000000000..bb2fbfd52d --- /dev/null +++ b/dimos/mapping/ray_tracing/rust/flake.nix @@ -0,0 +1,42 @@ +{ + 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 b468bbcac0..bad3fc9ab2 100644 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/OccupancyGrid.py @@ -485,6 +485,7 @@ 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. @@ -504,8 +505,9 @@ def to_rerun( step_w = max(1, grid.shape[1] // max_tex) grid = grid[::step_h, ::step_w] - lut = _build_occupancy_lut(colormap, opacity, background) - rgba = np.ascontiguousarray(lut[np.clip(grid + 1, 0, 101)]) + 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)]) # 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 a958f8dba0..cdcc490551 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -24,6 +24,7 @@ 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 @@ -131,6 +132,19 @@ 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 025045c2c9..03212e87b8 100644 --- a/dimos/navigation/replanning_a_star/min_cost_astar.py +++ b/dimos/navigation/replanning_a_star/min_cost_astar.py @@ -68,11 +68,12 @@ 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="world", + frame_id=frame_id, position=[world_point.x, world_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), # Identity quaternion ) @@ -81,7 +82,7 @@ def _reconstruct_path( start_world_point = costmap.grid_to_world(start_tuple) start_pose = PoseStamped( - frame_id="world", + frame_id=frame_id, position=[start_world_point.x, start_world_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), ) @@ -97,31 +98,32 @@ 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="world", + frame_id=frame_id, position=[goal_point.x, goal_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), ) waypoints.append(goal_pose) - return Path(frame_id="world", poses=waypoints) + return Path(frame_id=frame_id, 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="world", + frame_id=frame_id, position=[world_point.x, world_point.y, 0.0], orientation=Quaternion(0, 0, 0, 1), ) waypoints.append(pose) - return Path(frame_id="world", poses=waypoints) + return Path(frame_id=frame_id, 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 cf3e0d472b..9ea8607bb2 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -19,12 +19,13 @@ from reactivex.disposable import Disposable from dimos.core.core import rpc -from dimos.core.module import Module +from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.base import NavigationInterface, NavigationState from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner @@ -33,8 +34,16 @@ 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] @@ -51,13 +60,31 @@ class ReplanningAStarPlanner(Module, NavigationInterface): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self._planner = GlobalPlanner(self.config.g) + 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) @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 efd0f563ba..cbc9eba176 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -82,7 +82,6 @@ "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", @@ -94,6 +93,8 @@ "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 98248916ea..7a40ad71a4 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.uintree_g1_primitive_no_nav import ( - uintree_g1_primitive_no_nav, +from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_primitive_no_nav import ( + unitree_g1_primitive_no_nav, ) from dimos.robot.unitree.g1.connection import G1Connection unitree_g1_basic = autoconnect( - uintree_g1_primitive_no_nav, + unitree_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 32d2d52b8b..628e8a8867 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.uintree_g1_primitive_no_nav import ( - uintree_g1_primitive_no_nav, +from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_primitive_no_nav import ( + unitree_g1_primitive_no_nav, ) from dimos.robot.unitree.g1.mujoco_sim import G1SimConnection unitree_g1_basic_sim = autoconnect( - uintree_g1_primitive_no_nav, + unitree_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 d64b2c8aa0..1fe25fb40d 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,15 +15,13 @@ 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, @@ -32,13 +30,7 @@ unitree_g1_nav_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", - ), + _unitree_g1_onboard, create_nav_stack( planner="simple", vehicle_height=G1.height_clearance, @@ -65,7 +57,6 @@ }, ), 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 new file mode 100644 index 0000000000..e1c3bd7ba4 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_simple.py @@ -0,0 +1,56 @@ +#!/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_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py new file mode 100644 index 0000000000..96f97146b1 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -0,0 +1,37 @@ +#!/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/uintree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py similarity index 93% rename from dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py rename to dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py index 6ae1510fa9..3a115d4607 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_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 +from dimos.mapping.costmapper import CostMapper, costmap_to_rerun from dimos.mapping.voxels import VoxelGridMapper from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -50,15 +50,6 @@ 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( @@ -95,7 +86,7 @@ def _g1_rerun_blueprint() -> Any: "blueprint": _g1_rerun_blueprint, "visual_override": { "world/camera_info": _convert_camera_info, - "world/navigation_costmap": _convert_navigation_costmap, + "world/navigation_costmap": costmap_to_rerun, }, "static": { "world/tf/base_link": _static_base_link, @@ -130,7 +121,7 @@ def _create_webcam() -> Webcam: else autoconnect() ) -uintree_g1_primitive_no_nav = ( +unitree_g1_primitive_no_nav = ( autoconnect( _with_vis, _camera, @@ -163,4 +154,4 @@ def _create_webcam() -> Webcam: ) ) -__all__ = ["uintree_g1_primitive_no_nav"] +__all__ = ["unitree_g1_primitive_no_nav"] diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py new file mode 100644 index 0000000000..43dfe90991 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +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 8ad34c95bf..462b48d959 100644 --- a/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py +++ b/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py @@ -79,6 +79,17 @@ 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): @@ -150,9 +161,13 @@ def stop(self) -> None: @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: assert self.loco_client is not None - vx = twist.linear.x - vy = twist.linear.y - vyaw = twist.angular.z + 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) 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 1d64b7920e..632429483d 100644 --- a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_security.py +++ b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_security.py @@ -17,6 +17,7 @@ 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 @@ -28,15 +29,6 @@ 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( @@ -75,7 +67,7 @@ def _go2_rerun_blueprint() -> Any: "blueprint": _go2_rerun_blueprint, "visual_override": { "world/camera_info": _convert_camera_info, - "world/navigation_costmap": _convert_navigation_costmap, + "world/navigation_costmap": costmap_to_rerun, }, "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 96a291163d..928b2dc23a 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -21,6 +21,7 @@ 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 @@ -50,15 +51,6 @@ 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( @@ -106,7 +98,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": _convert_navigation_costmap, + "world/navigation_costmap": costmap_to_rerun, }, "max_hz": { "world/global_map": 0, # publishes at ~7.8 Hz