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..7aae98545d 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.config.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..118cd69e2d 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: NDArray[np.uint8] | 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/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py
index 451630414c..683999c768 100644
--- a/dimos/navigation/replanning_a_star/local_planner.py
+++ b/dimos/navigation/replanning_a_star/local_planner.py
@@ -64,7 +64,7 @@ class LocalPlanner(Resource):
     _goal_tolerance: float
     _controller: Controller
 
-    _speed: float = 0.55
+    _speed: float = 0.85
     _control_frequency: float = 10
     _orientation_tolerance: float = 0.35
     _navigation_costmap_interval: float = 1.0
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..6db69f574b
--- /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
+
+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:
+    import rerun as rr
+
+    # 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..18eb3ad292 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.6  # 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/g1/g1.urdf b/dimos/robot/unitree/g1/g1.urdf
index 948969df26..695f53eaf9 100644
--- a/dimos/robot/unitree/g1/g1.urdf
+++ b/dimos/robot/unitree/g1/g1.urdf
@@ -570,7 +570,7 @@
   <!-- mid360 -->
   <link name="mid360_link"></link>
   <joint name="mid360_joint" type="fixed">
-    <origin xyz="0.0002835 0.00003 0.41618" rpy="0 0.04014257279586953 0"/>
+    <origin xyz="0.0002835 0.00003 0.41618" rpy="0 -0.04014257279586953 0"/>
     <parent link="torso_link"/>
     <child link="mid360_link"/>
   </joint>
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
diff --git a/dimos/web/websocket_vis/websocket_vis_module.py b/dimos/web/websocket_vis/websocket_vis_module.py
index ed319e52ca..f8ff82495b 100644
--- a/dimos/web/websocket_vis/websocket_vis_module.py
+++ b/dimos/web/websocket_vis/websocket_vis_module.py
@@ -47,7 +47,6 @@
 
 from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT
 from dimos.core.core import rpc
-from dimos.core.global_config import global_config
 from dimos.core.module import Module, ModuleConfig
 from dimos.core.stream import In, Out
 from dimos.mapping.models import LatLon
@@ -355,7 +354,7 @@ async def move_command(sid: str, data: dict[str, Any]) -> None:
     def _run_uvicorn_server(self) -> None:
         config = uvicorn.Config(
             self.app,  # type: ignore[arg-type]
-            host=global_config.listen_host,
+            host=self.config.g.listen_host,
             port=self.config.port,
             log_level="error",  # Reduce verbosity
         )