navigation-primitives

# Navigation Primitives — Shared Substrate ## Purpose Shared mobile-robot substrate: occupancy maps, A* planning, robot footprints, differential/holonomic kinematics, and chase-camera math. ## Prerequisites - Built Isaac Sim with `isaacsim.core.experimental`, `wheeled_robots` controllers, and `SimulationManager`. - A loaded robot articulation for footprint derivation; `numpy`, `scipy`, `cv2` for planning/collision helpers. - A `map.yaml`/`map.png` pair from `occupancy-map` when using pre-baked grids (optional for runtime projection). ## Limitations - This is the shared substrate only — it does not drive robots, record SDG, or publish ROS topics; jump to the specialization skills for those. - Footprints/kinematics assume authored collider geometry and correct scene units; garbage colliders yield garbage footprints. - Oriented-footprint PhysX checks require `SimulationManager.initialize()`; the grid-based check works offline but is coarser. ## Troubleshooting | Error / symptom | Cause | Solution | |---|---|---| | Robot falls through floor / feet float | Spawned without `z_offset` | Spawn at `z = ground + compute_robot_footprint(...)["z_offset"]` | | Path clips walls in render but not omap | Skipped oriented-footprint validation | Run `footprint_clips_grid` per waypoint (Validation Pipeline steps 6-7) | | Grid full of shell/signage obstacles | Visual-bbox projection without filtering | Prefer collider-driven filtering; apply the geometric filter list | Foundation layer for mobile robot navigation. Consumed by: - `isaac-sim-robot-navigation`: runtime navigation in custom scripts (RL policy, physics-vs-baked, GPU OOM). - `mobility-gen`: two-phase MobilityGen SDG (record -> replay+render). - `occupancy-map`: produces the `map.yaml` consumed here. Read this first for any mobile-robot work, then jump to the specialization. ## Available Scripts | Script | Purpose | Arguments | |---|---|---| | `scripts/occupancy_map_from_usd.py` | Rasterize a 2D occupancy grid directly from USD geometry (no PhysX required) | see script --help | | `scripts/robot_footprint.py` | Compute robot footprint dimensions and Z-offset from USD collision geometry | see script --help | ## Running scripts From agent runtimes that expose skill execution helpers, invoke helpers with `run_script()`: ```python run_script("scripts/occupancy_map_from_usd.py", args=["--help"]) ``` From a built Isaac Sim tree, run the same file with `./python.sh` (Linux) or `python.bat` (Windows) from `_build/*/release`, or execute shell helpers directly when they do not require the simulator. ## Shared NVIDIA APIs | Capability | Module | |---|---| | Occupancy maps | `isaacsim.replicator.mobility_gen.impl.occupancy_map.OccupancyMap` | | A* path planner | `isaacsim.replicator.mobility_gen.impl.path_planner.generate_paths` | | Runtime omap from stage | `isaacsim.asset.gen.omap.bindings._omap.Generator` | | Robot articulation | `isaacsim.core.experimental.prims.Articulation` | | Differential controller | `isaacsim.robot.experimental.wheeled_robots.controllers.DifferentialController` | | Holonomic controller | `isaacsim.robot.wheeled_robots.controllers.holonomic_controller.HolonomicController` | | Physics lifecycle | `isaacsim.core.simulation_manager.SimulationManager` | `OccupancyMap.from_ros_yaml(path)` loads a YAML+PNG pair (produced by `occupancy-map`). Both isaac-sim-robot-navigation and mobility-gen consume this same format. ## Robot Footprints & Z-Offsets — Derived at Runtime Do not hardcode footprints. Walk the articulation's collider prims and union their world-space AABBs. This handles every robot (Spot, Carter, VSVXL, Jetbot, Kaya, H1, custom) and stays correct when assets change. `compute_robot_footprint(stage, robot_root)` — walk CollisionAPI prims, union their AABBs, return size, z_offset, inscribed_radius, circumscribed_radius. See [`scripts/robot_footprint.py`](scripts/robot_footprint.py). Use `inscribed_radius` when the robot can rotate freely in place (over-conservative, zero clip). Use `circumscribed_radius` only when you require zero false negatives. For non-circular robots (Spot, VSVXL), prefer the oriented-footprint check below over a single radius. **Always spawn the robot at `z = ground + z_offset`**. Missing the Z-offset is the #1 cause of "robot falls through the floor" or "feet pop above ground" bugs. ### Reference Values (sanity check only) If your `compute_robot_footprint` output is far from these, your collider authoring or scene units are wrong: | Robot | Expected size (m) | Expected z_offset | Inscribed r | |---|---|---|---| | Spot | ~1.08 × 0.44 × 0.55 | ~0.69 | ~0.22 | | Spot + arm | ~1.10 × 0.40 × 1.20 | ~0.69 | ~0.20 | | Nova Carter | track_w=0.499, wheel_r=0.14 | ~0.0 | ~0.25 | | VSVXL | ~2.52 × 1.72, 6-wheel diff | ~0.0 | ~0.86 | | Jetbot | wheel_base=0.1125, wheel_r=0.03 | ~0.02 | ~0.06 | | Kaya (holonomic) | wheel_base=0.10, wheel_r=0.04 | ~0.02 | ~0.10 | | H1 (humanoid) | — | ~1.05 | ~0.20 | ## Occupancy Map from USD (Direct Projection) Use when you need a runtime omap and don't already have a `map.yaml`. For the canonical `map.yaml` workflow consumed by MobilityGen, use `occupancy-map` instead. `occupancy_map_from_usd(stage, x_range, y_range, resolution, z_cutoff, colliders_only)` — rasterize USD geometry into a 2D uint8 grid (0=free, 255=occupied). See [`scripts/occupancy_map_from_usd.py`](scripts/occupancy_map_from_usd.py). ### Obstacle Filtering (CRITICAL — learned 2026-03-13) Two strategies, in order of preference: **A. Collider-driven (preferred when assets have authored colliders).** Iterate only prims with `UsdPhysics.CollisionAPI`. This already excludes visual-only geometry (signage, decals, light cones, debug arrows) without a filter list. ```python from pxr import UsdPhysics for prim in Usd.PrimRange(stage.GetPrimAtPath("/World")): if not prim.HasAPI(UsdPhysics.CollisionAPI): continue enabled = prim.GetAttribute("physics:collisionEnabled") if enabled and enabled.Get() is False: continue # rasterize this prim's AABB into the grid ``` **B. Visual-bbox + filter list (fallback for scenes without colliders).** Naive bbox projection fills the grid with shell, zones, signage. Filter aggressively: ```python SKIP_SCOPES = {"GroundPlane", "Looks", "Lighting", "Render", "PushGraph", "DomeLight", "DemoCamera", "Spot_01", "Spot_02", "Obstacles_Anim"} SKIP_PREFIXES = ("Floor", "FL_", "FR_", "Exit_", "Hum", "SM_Deluxe") SKIP_CHILDREN = ("sm_warehouse_mega", "Zones", "signage") ``` Geometric filters (apply after either strategy): - Skip area > 3000 m² (shell, zone assemblies) - Skip height < 0.1 m (floor markings, safety tape) - Skip Z_min > 3.6 m (ceiling-only objects, ≈ 3× robot height) - Skip Z_max < `fp["z_offset"]` + 0.05 m (anything the robot can drive over) **Result on V4 KION**: 244 real obstacles (vs 5000+ before filtering), 70% free space. ### Buffer Sizing — Derived from Footprint The buffer is `circumscribed_radius + safety_margin`, not a magic constant. Empirical `safety_margin` defaults (validated 2026-03-15): | Context | Safety margin | |---|---| | Open corridor, smooth control | 0.10 m | | Aisle navigation, cluttered | 0.30 m | | Cluttered + non-zero yaw error | 0.50 m | ```python fp = compute_robot_footprint(stage, "/World/Robot") buffer_m = fp["circumscribed_radius"] + 0.30 # aisle default buffer_cells = int(round(buffer_m / RESOLUTION)) ``` For Spot (`circumscribed_radius ≈ 0.58 m`) this yields ~0.88–1.08 m, not the legacy 1.5 m blanket value. The legacy value was conservatively tuned against a circular proxy; with the oriented-footprint check (below) you recover the extra ~0.5 m of navigable space. ## A* Path Planning Erode by `inscribed_radius` (fast, conservative). Then validate the smoothed path with an oriented-footprint collision check, which recovers the navigable space the inscribed-radius erosion threw away. ```python from scipy.ndimage import binary_erosion import numpy as np fp = compute_robot_footprint(stage, "/World/Robot") kernel_r = int(fp["inscribed_radius"] / RESOLUTION) kernel = np.zeros((2*kernel_r+1, 2*kernel_r+1), dtype=bool) for dy in range(-kernel_r, kernel_r+1): for dx in range(-kernel_r, kernel_r+1): if dx*dx + dy*dy <= kernel_r*kernel_r: kernel[dy+kernel_r, dx+kernel_r] = True navigable = (grid == 0) eroded = binary_erosion(navigable, structure=kernel) # Standard A* over `eroded` (heapq-based) ``` ### Oriented-Footprint Collision Check (recommended for non-circular robots) Drives the same PhysX query the simulator uses. Works in two modes: **1. Against PhysX scene (after `SimulationManager.initialize()`):** use `get_physx_scene_query_interface().overlap_box`. Returns hit count; >0 means clip. ```python import carb from omni.physx import get_physx_scene_query_interface from pxr import Gf def footprint_clips(x: float, y: float, yaw: float, fp: dict, z_query: float = 0.2) -> bool: # half-extents of the robot footprint half = carb.Float3(fp["size"][0] / 2, fp["size"][1] / 2, fp["size"][2] / 2) origin = carb.Float3(x, y, z_query + fp["size"][2] / 2) # quaternion (x, y, z, w) for yaw about Z rot = Gf.Rotation(Gf.Vec3d(0, 0, 1), np.degrees(yaw)).GetQuat() quat = carb.Float4(*rot.GetImaginary(), rot.GetReal()) hits = get_physx_scene_query_interface().overlap_box( half, origin, quat, lambda h: True, anyHit=True, # early-exit on first hit ) return hits > 0 ``` **2. Against the rasterized omap (no PhysX needed):** stamp the rotated rectangle onto the obstacle grid and AND with the occupied mask. ```python import cv2 def footprint_clips_grid(px: int, py: int, yaw: float, fp: dict, grid: np.ndarray) -> bool: w_cells = fp["size"][0] / RESOLUTION h_cells = fp["size"][1] / RESOLUTION rect = ((px, py), (w_cells, h_cells), np.degrees(yaw)) pts = cv2.boxPoints(rect).astype(np.int32) mask = np.zeros_like(grid, dtype=np.uint8) cv2.fillPoly(mask, [pts], 1) return bool(np.any((grid > 0) & (mask > 0))) ``` ### Validation Pipeline (MANDATORY) 1. `compute_robot_footprint(stage, robot_root)` — get size, z_offset, radii. 2. Rasterize obstacles onto grid (0.25 m resolution typical). 3. Binary erode with circular kernel of radius = `inscribed_radius / resolution`. 4. A* pathfind on eroded grid. 5. Catmull-Rom smooth the raw path; assign yaw = `atan2(dy, dx)` along the curve. 6. **For every smoothed waypoint, run `footprint_clips_grid(px, py, yaw, fp, grid)`** (or `footprint_clips(...)` against PhysX). Reject the path on any hit. 7. If a single waypoint fails, snap to the nearest navigable cell and re-validate. If multiple fail, the inscribed-radius A* path is fundamentally bad — re-plan with a larger erosion kernel (`circumscribed_radius`). Skipping steps 6–7 produces paths that look fine on the omap but clip walls in render — especially on rectangular robots (Spot, VSVXL) cornering through aisles. ## Kinematics Helpers [`scripts/kinematics.py`](scripts/kinematics.py) — standalone differential-drive and holonomic kinematics with pure-pursuit path following. No Isaac Sim dependency; works offline for planning or inside a simulation loop. Key exports: - `differential_forward(v, ω, params)` → `(vL, vR)` wheel rad/s - `differential_inverse(vL, vR, params)` → `(v, ω)` body twist - `holonomic_forward(vx, vy, ω, params)` → 3 wheel velocities (120°-spaced) - `holonomic_inverse(wheel_vels, params)` → `(vx, vy, ω)` - `pure_pursuit_step(pos, yaw, path, config)` — PD-steered differential path follower - `holonomic_path_step(pos, yaw, path, config)` — strafing holonomic path follower ## Differential Drive Kinematics For `motion_generation` controller composition, see `motion-generation`; this section owns the geometry, wheel kinematics, and navigation tuning. ```python # Wheel velocities from body twist vL = (vx - omega * track_w / 2) / wheel_r vR = (vx + omega * track_w / 2) / wheel_r ``` PD steering (validated on Nova Carter / VSVXL): - KP=2.5, KD=1.2, MAX_W=1.5 rad/s - Waypoint tolerance: 4.0m - Speed reduction near waypoints and during large heading errors - Out-of-bounds: |Z| > 50 or |X|/|Y| > 500 → mark dead VSVXL validated parameters (LLM Advisor Grok-4, 2026-03-15): - ω = v / r (0.15m wheels → 10 rad/s for 1.5 m/s) - Physics: dt=1/120s, substeps=4 (effective 480Hz) - PID heading: Kp=2.0, Ki=0.1, Kd=0.5, max angular 1.0 rad/s - Aisle speed: 0.8 m/s; corridor speed: 1.5 m/s - Corridor buffer: 0.5m; aisle buffer: 0.2m + reduced speed ## Holonomic / Mecanum (Kaya, AMR) Use `HolonomicController` with `HolonomicRobotUsdSetup` to extract wheel positions, orientations, mecanum angles from the robot USD. Apply via `WheeledRobot.apply_wheel_actions`. 2D MobilityGen action `[lin, ang]` → 3D holonomic command `[forward, lateral=0, yaw]`. See `mobility-gen` for the `WheeledMobilityGenRobot.build()` override pattern. ## DifferentialController + Articulation (Kit 110) ```python from isaacsim.robot.experimental.wheeled_robots.controllers import DifferentialController from isaacsim.core.experimental.prims import Articulation import numpy as np robot = Articulation("/World/Robot") robot.initialize_cpp_data_view()