diff --git a/README.md b/README.md index 14774f9..55bd468 100644 --- a/README.md +++ b/README.md @@ -109,7 +109,42 @@ with the app's camera, toggle them in the scene tree, and drive them further fro the REPL. Outside volrover3 (`vrhost` absent) it prints a clear message pointing you at `run_standalone()` / `grl-snam-lab-demo`. -### 4. DBG variant — the full gym (`grl_snam_dbg`) +### 4. Learned SDF navigation on real geometry — the live drive demos + +The navigator can be **trained on a real scene** (terrain + city mesh) and driven +live in volrover3. Two prep steps (once), then load a demo as a volrover3 job. See +[docs/training-navigation-on-geometry.md](docs/training-navigation-on-geometry.md). + +**Prep — build the SDF + train the policy** (CPU, a few minutes): +```bash +python scripts/build_sdf.py --source edt # footprint SDF (or --source cvc for cvc::sdf) +python scripts/train_sdf.py /nav_sdf.npz -o checkpoints/coef_sdf.pt --steps 1500 +``` + +**Run inside volrover3** — from the app's **Jobs tab → Load Script… → Run as Job**, +or headless via the CLI: +```bash +export GRL_SNAM_CHECKPOINT=$PWD/checkpoints/coef_sdf.pt +export GRL_SNAM_SCENE_BUNDLE=/path/to/austin_south # dir with terrain.json + buildings.glb +volrover3 --run-job examples/volrover_grl_snam_austin_freedrive.py +``` +`--run-job` loads a file that defines `step(dt)` and ticks it cooperatively (it +appears in the Python Console Jobs tab). Env vars: `GRL_SNAM_CHECKPOINT` (trained +`.pt`), `GRL_SNAM_SCENE_BUNDLE` (scene dir), `GRL_SNAM_SDF` (prebuilt `nav_sdf.npz`; +else built at load), `GRL_SNAM_ROOT` (repo path if not on the default). + +Drive demos in `examples/`: +- **`volrover_grl_snam_austin_freedrive.py`** — end-to-end: the learned SDF policy + finds its own way START→GOAL with **no route**, plus a wall-follow escape for + local minima. Retarget the goal live to watch it redirect. +- **`volrover_grl_snam_austin_learned.py`** — stagewise: an A* occupancy route is a + collision-free spine; the learned policy drives locally between sub-goals (robust + for adversarial start/goal pairs). +- **`volrover_grl_snam_planner.py`** — the surrogate's native sparse-obstacle regime. + +Capture a drive to video **offscreen** (no window): `scripts/capture_drive_video.py`. + +### 5. DBG variant — the full gym (`grl_snam_dbg`) The dataset-specific extension lives in the separate **`grl_snam_dbg`** project (it depends on GRL-SNAM, not the other way round). Its `grl_snam_dbg_demo.py` runs the **full communication-resilient navigation gym** end-to-end (~2 min, diff --git a/docs/training-navigation-on-geometry.md b/docs/training-navigation-on-geometry.md new file mode 100644 index 0000000..36fed84 --- /dev/null +++ b/docs/training-navigation-on-geometry.md @@ -0,0 +1,265 @@ +# Training Navigation on Real Geometry + +How to teach the GRL-SNAM navigation policy to drive a **specific real-world +scene** — a terrain heightfield plus a city mesh — and then run the trained +policy as a live demo (e.g. a vehicle finding its own way through a city in +volrover3). + +This is the end-to-end path from *"here is a new map"* to *"the agent navigates +it"*. It needs **no expert demonstrations and no pre-generated dataset** — the +navigation surrogate is differentiable, so the policy trains itself directly on +the scene's obstacles. + +--- + +## 1. What "training on geometry" means + +GRL-SNAM navigation is a **differentiable, physics-informed potential field**. +The surrogate rollout (`surrogate_robust.integrate_surrogate_v2`) advances a point +agent under three forces: + +- a **goal spring** (strength `beta`) pulling toward the target, +- **IPC-barrier repulsion** from each nearby obstacle (per-obstacle strength + `alphas`), and +- **damping** (`gamma`). + +A small transformer, **`CoefEnergyNet`** (`train_coef_energy.py`, re-exported as +`grl_snam.network`), predicts `(alphas, beta, gamma)` from the local situation: +the nearby obstacle circles and the goal direction. Good coefficients produce a +smooth, collision-free path; bad (or untrained) coefficients stall or clip walls. + +**Training on a scene = fitting `CoefEnergyNet` so its predicted coefficients +navigate *that scene's* obstacles well.** The scene enters training as a set of +**circular obstacles** (building footprints) plus a pool of **free (drivable) +points**. + +--- + +## 2. Do we have everything to train right now? + +**Yes.** There are two ways to train the coefficient network; we use the second, +which requires nothing pre-generated: + +| Mode | Signal | Needs | Use when | +|------|--------|-------|----------| +| **Imitation** | MSE against expert rollouts | an expert dataset from the ring/stagewise generators (`scripts.ring_dataset_maxmin`) + a base checkpoint | you already have curated expert trajectories | +| **Self-supervised** (this doc) | reach-goal + no-penetration + speed loss, **backprop through the differentiable rollout** | just `torch` + `numpy` + the repo | you have a **new scene** and no labels | + +Because `integrate_surrogate_v2` is fully differentiable, the self-supervised mode +backpropagates a task loss straight through the H-step rollout — no labels, no +reward bootstrapping. It trains directly on **any** geometry. + +**Requirements, all currently satisfied:** + +- **Training** (`scripts/train_on_geometry.py`): `torch`, `numpy`, and the + GRL-SNAM repo on `PYTHONPATH`. Runs on a plain CPU box or a GPU cluster — **no + graphics**. +- **Geometry ingestion** (`scripts/extract_obstacles.py`): the `pycvc_gl` scene + helpers (they rasterize the city mesh to an occupancy grid with VTK). Run this + in an environment where volrover3's Python / cvcGL bindings are importable + (i.e. the volrover deps prefix). Ingestion is a **one-time** step per scene and + its output is cached. +- **The scene**: a `geometry_bundle` — `terrain.json` (heightfield) + + `buildings.glb` (glTF city mesh). + +--- + +## 3. The pipeline + +``` + geometry bundle obstacles.npz coef_energy.pt + (terrain.json + ──[1 ingest]──▶ (obstacle circles ──[2 train]──▶ (CoefEnergyNet ──[3 run]──▶ live demo + buildings.glb) + free points) weights + scale) +``` + +### Step 1 — Ingest the scene into an obstacle set + +```bash +python scripts/extract_obstacles.py -o obstacles.npz +``` + +Rasterizes the city footprint to a **solid** top-down occupancy grid, coarsens it, +and emits one circular obstacle per occupied cell plus a pool of free world points. +The occupancy raster is cached next to the `.glb`, so re-runs are instant. The +`obstacles.npz` is self-contained (world-unit obstacle centers + radius + free +pool + bounds) and has no graphics dependency — it can be shipped and reused. + +### Step 2 — Train the policy (self-supervised) + +```bash +python scripts/train_on_geometry.py obstacles.npz -o coef_energy.pt \ + --steps 5000 --region 430 +``` + +Each optimizer step samples a batch of **local** navigation problems (start at a +random free point, aim at a nearby *reachable* goal 1.0–2.0 units away), rolls the +surrogate forward `H` steps with the net's predicted coefficients, and minimizes: + +``` +L = L_goal + 3.0 * L_collision + 0.3 * L_coef_reg + │ │ │ + reach the penalize ACTUAL anchor coefficients to the + local goal penetration only known-good navigating regime + (not proximity) (beta~3, gamma~4, alpha~3) +``` + +all backpropagated through the rollout. Two hard-won details make or break this: + +1. **Do not cap speed, and penalize collision — not proximity.** Streets are + narrow, so navigating them *requires* low positive clearance. A speed cap or a + proximity penalty makes staying put (max clearance, min speed) beat moving, and + the net collapses to a **crawl** (damping `gamma` ≫ goal-spring `beta`, agent + immobile). Penalize only *actual* penetration (clearance below a thin margin). +2. **Regularize toward the known-good regime.** The self-supervised rollout + objective has degenerate optima (crawl, or erratic overshoot). Anchoring the + predicted coefficients to the working hand-set values keeps the optimizer in the + stable, navigating basin while the task terms adapt them per situation. + +**Local** goals are deliberate: a policy trained to reach *nearby* goals learns +obstacle-avoiding local progress that *chains* into a route at inference — the +paper's **stagewise** decomposition (§Scope). + +#### The scale-normalization step (important) + +The surrogate's coefficients are tuned for a **~10-unit world**. Real scenes are +much bigger — the Austin bundle spans **3 km** (bounds ±1500 m). The trainer +normalizes a **working region** into the tuned regime: + +- `--region R` picks a half-extent (metres) around the scene center; +- that `2R`-wide region maps to ~10 units for the rollout: `scale = 10 / (2R)`; +- obstacle radius and robot radius scale the same way; +- the checkpoint records `scale` + `center` so the demo maps world ↔ normalized + consistently. + +For Austin, `--region 430` gives `scale ≈ 0.0116` (a ~860 m working area at the +tuned zoom). Two geometry knobs matter in a real city: the obstacle circles come +from `extract_obstacles.py --block 2` (~7 m circles that leave 20 m streets +navigable — `radn ≈ 0.08`; coarser 14 m circles crowd them shut), and the IPC +barrier reach is kept **local** (`--d-hat-world 25`, ~one street width) so the +agent isn't stalled by a "sea of repulsion" from many overlapping barriers. Widen +`--region` to cover more map at coarser zoom, or run several regions for a large +city. + +### Step 3 — Run the trained policy + +At inference the loop, per step: + +1. gathers the nearby obstacles + goal direction and builds local features + (`eval_coef_energy.build_local_feats`); +2. `CoefEnergyNet` predicts `(alphas, beta, gamma)` (~7 ms/step — real-time); +3. **online adaptation** refines those coefficients on the fly: + - **`HistSecantController`** (`grl_snam.adaptation`) — a rank-1 secant update + that nudges the coefficients from the recent motion history, and + - **`OnlineFinetuner`** — optional test-time training back through the + differentiable rollout; +4. `integrate_surrogate_v2` advances one step; the agent is draped onto the + terrain and rendered. + +Two demo scaffolds load from volrover3's Jobs tab: + +- [`examples/volrover_grl_snam_planner.py`](../examples/volrover_grl_snam_planner.py) + — the learned policy's **native regime**: a handful of sparse circular obstacles, + where the surrogate navigates end-to-end. Load a `coef_energy.pt` checkpoint to + drive it with the learned network + online adaptation. +- [`examples/volrover_grl_snam_austin_learned.py`](../examples/volrover_grl_snam_austin_learned.py) + — the **stagewise** demo on real Austin: an A* occupancy route is the + collision-free spine, and the trained `CoefEnergyNet` + `HistSecantController` do + the local reactive control between route sub-goals (see §Scope for why). + +### Obstacle model — circles vs. SDF (why the city needs an SDF) + +The **base** surrogate (`integrate_surrogate_v2`) is a **circular-obstacle** field. +It navigates cleanly when obstacles are sparse and round (rings, dungeons, a few +pillars — the `volrover_grl_snam_planner.py` regime). A **dense rectilinear city** +defeats it: thousands of building footprints become thousands of overlapping circular +barriers that conflict, and a point-agent is pushed *through* corners no matter the +coefficients (measured on Austin: even hand-set good coefficients reach 0–1/4 goals +with heavy penetration). That is a property of the obstacle model, not the training. + +The fix is a **signed distance field (SDF)** obstacle model (`sdf_nav.py`): the +barrier repels along the *true wall normal*, so the learned surrogate navigates +streets and corners cleanly. On Austin, stagewise + SDF reaches **3/4 with ~0 +penetration** and the learned surrogate **drives every step** (vs the circle model, +which drove none). Same self-supervised recipe — the field is differentiable +(`grid_sample`), so `sdf_nav.CoefMLP` trains through the rollout exactly like +`CoefEnergyNet`, biased toward the known-good regime so it starts near-optimal. + +Build the SDF, then train: + +```bash +python scripts/build_sdf.py --source edt # grl-snam's exact footprint EDT (default) +# --source cvc # OR CVC's mesh-exact 3-D SDF (pycvc.sdf, SDF_V2) +python scripts/train_sdf.py /nav_sdf.npz -o checkpoints/coef_sdf.pt --steps 1500 +``` + +`--source cvc` uses the CVC compute layer and returns a full **3-D** volume (sliced +to the ground plane for 2-D nav) — the natural substrate for extending GRL-SNAM to +3-D navigation later. The `SDFField` / surrogate / net consume either source +identically. + +**Global topology still needs a planner.** A pure potential field — SDF or not — has +local minima (dead-ends, U-shaped clusters), so a long cross-city A→B can still trap. +So the demo keeps the paper's **stagewise** structure: an occupancy-grid A\* route is +the collision-free spine (global path), and the *learned SDF surrogate* does the +local navigation between route sub-goals (now genuinely, not as a fallback). The +route + a footprint check guarantee the drive never enters a building. + +--- + +## 4. How long does training take? + +Measured on this workstation (6 CPU threads, batch 64, horizon 28): + +| Metric | Value | +|--------|-------| +| Per step (`--block 2`, ~28k obstacles) | ~270–400 ms | +| 5000 steps | **~22–33 min** | +| Convergence | the goal term drops steadily; per-stage reaching becomes reliable within the first few thousand steps | +| Inference | ~7 ms/step (real-time driving) | + +A **GPU** cuts training roughly 10–20× (single-digit minutes for 5000 steps); the +rollout and transformer are both small and batch-parallel. Ingestion (step 1) is +seconds, and cached thereafter. + +So a full "new scene → trained policy" cycle is well under an hour on CPU and a few +minutes on GPU. + +--- + +## 5. Pre-generated datasets (future) + +Nothing above depends on pre-generated data, but two artifacts are worth reusing: + +- **`obstacles.npz`** — ship the ingested obstacle set so a training box never + needs the graphics stack. A "pre-generated dataset" for self-supervised training + is simply this file (obstacle circles + free pool). +- **`coef_energy.pt`** — ship a trained checkpoint so the demo runs with zero + training. Re-train only when the geometry changes or you want a different + working region / behavior. + +When curated **expert** trajectories become available, the imitation mode +(`train_coef_energy.py`'s `Trainer`) can pre-train or fine-tune `CoefEnergyNet` +against them; self-supervised training on the scene geometry then adapts it to the +specific map. The two are complementary. + +--- + +## 6. Reference + +| Piece | Where | +|-------|-------| +| Differentiable surrogate rollout | `surrogate_robust.integrate_surrogate_v2` | +| Coefficient network | `CoefEnergyNet` (`train_coef_energy.py` → `grl_snam.network`) | +| Local feature builder | `eval_coef_energy.build_local_feats` | +| Online adaptation | `HistSecantController`, `OnlineFinetuner` (`grl_snam.adaptation`) | +| Geometry ingestion (circles) | [`scripts/extract_obstacles.py`](../scripts/extract_obstacles.py) | +| Self-supervised trainer (circles) | [`scripts/train_on_geometry.py`](../scripts/train_on_geometry.py) | +| **SDF obstacle model** (city-grade) | [`sdf_nav.py`](../sdf_nav.py) — EDT + cvc::sdf builders, `SDFField`, `sdf_rollout`, `CoefMLP` | +| SDF field builder (`edt`/`cvc`) | [`scripts/build_sdf.py`](../scripts/build_sdf.py) | +| SDF trainer | [`scripts/train_sdf.py`](../scripts/train_sdf.py) | +| Live demo (native sparse-obstacle regime) | [`examples/volrover_grl_snam_planner.py`](../examples/volrover_grl_snam_planner.py) | +| Live demo (stagewise, real Austin) | [`examples/volrover_grl_snam_austin_learned.py`](../examples/volrover_grl_snam_austin_learned.py) | +| Scene helpers (terrain/glTF/occupancy/route) | `pycvc_gl.scenes` | + +See the [Developer Guide](developer-guide.md) for the underlying stagewise +navigation model. diff --git a/examples/volrover_grl_snam_austin_freedrive.py b/examples/volrover_grl_snam_austin_freedrive.py new file mode 100644 index 0000000..3209e25 --- /dev/null +++ b/examples/volrover_grl_snam_austin_freedrive.py @@ -0,0 +1,172 @@ +# examples/volrover_grl_snam_austin_freedrive.py — the learned GRL-SNAM navigator +# finding its OWN way across REAL Austin, TX in a LIVE volrover3 window, with NO A* +# route. Pick a START and a GOAL; the trained SDF policy drives there end-to-end, +# purely by reacting to the buildings: a moving carrot toward the goal + the SDF +# barrier deflecting around footprints. This is "the vehicle finds its own path +# from A to B based on environmental factors" — no precomputed path at all. +# +# vs volrover_grl_snam_austin_learned.py (which uses an A* route as a stage spine): +# this one is pure end-to-end learned navigation. A pure potential field can stall +# in a local minimum (a dead-end / U-shaped block) for adversarial A->B pairs, so +# the START/GOAL below are chosen to be navigable; retarget as you like. +# +# PREP (once): build the SDF + train the coefficients (see docs) -> +# python scripts/build_sdf.py --source edt +# python scripts/train_sdf.py /nav_sdf.npz -o checkpoints/coef_sdf_austin.pt +# ENV: GRL_SNAM_SCENE_BUNDLE, GRL_SNAM_CHECKPOINT (coef_sdf .pt), GRL_SNAM_SDF +# (prebuilt nav_sdf.npz; else built from occupancy at load), GRL_SNAM_ROOT. +# RUN (inside volrover3): Python Console -> Jobs tab -> "Load Script..." -> Run as Job. +# ATTRIBUTION: OpenStreetMap (c) contributors, ODbL; SRTM terrain US public domain. + +import math +import os +import sys + +import numpy as np + +_ROOT = os.environ.get("GRL_SNAM_ROOT", "/home/joe/src/cvc/GRL-SNAM") +if _ROOT not in sys.path: + sys.path.insert(0, _ROOT) + +import torch # noqa: E402 + +import sdf_nav # noqa: E402 +from pycvc_gl.camera import ChaseCamera # noqa: E402 +from pycvc_gl.lab import Lab # noqa: E402 +from pycvc_gl.scenes import building_occupancy, load_geometry_bundle, terrain_grid # noqa: E402 +from pycvc_gl.vehicle import VehiclePose # noqa: E402 + +try: + import pycvc + import vrhost +except ImportError as exc: # pragma: no cover + raise RuntimeError("run INSIDE volrover3 (Jobs tab -> Load Script)") from exc + +_BUNDLE = os.environ.get("GRL_SNAM_SCENE_BUNDLE", + "/home/joe/src/cvc/CVC-DBG/platoon-sim/scene_viewer/exports/scenes/austin_south") +_CKPT = os.environ.get("GRL_SNAM_CHECKPOINT", os.path.join(_ROOT, "checkpoints", "coef_sdf_austin.pt")) +torch.set_num_threads(2) + +_ck = torch.load(_CKPT, map_location="cpu"); _m = _ck["meta"] +_S = float(_m["scale"]); _CTR = np.asarray(_m["center"], np.float32) +_RR = float(_m["rr"]); _DHAT = float(_m["d_hat"]); _DT = float(_m["dt"]); _NSUB = int(_m["nsub"]); _VMAX = float(_m["vmax"]) +_model = sdf_nav.CoefMLP(); _model.load_state_dict(_ck["model_state_dict"]); _model.eval() + +# START -> GOAL (world x,y). Validated navigable on austin_south; retarget freely. +_START = (-361.0, 114.0) +_GOAL = (185.0, 50.0) + +_app = vrhost.app() +_lab = Lab(app=_app, scene=vrhost.scene()); _lab.set_axis_visible(False) +_sample = load_geometry_bundle(_lab, _BUNDLE) +_bounds = terrain_grid(os.path.join(_BUNDLE, "terrain.json"))[1]; _mnx, _mny, _mxx, _mxy = _bounds +print("grl_snam_austin_freedrive: loading occupancy + SDF...") +_occ0 = building_occupancy(os.path.join(_BUNDLE, "buildings.glb"), _bounds, nx=512, ny=512, inflate_m=0.0) +_NY, _NX = _occ0.shape +_sdf_npz = os.environ.get("GRL_SNAM_SDF", "") +if _sdf_npz and os.path.exists(_sdf_npz): + _d = np.load(_sdf_npz); _phi, _nxg, _nyg = _d["phi"], _d["normal_x"], _d["normal_y"] +else: + _phi, _nxg, _nyg = sdf_nav.build_sdf(_occ0, _bounds, _S) +_field = sdf_nav.SDFField(_phi, _nxg, _nyg, _bounds, _CTR, _S, device="cpu") +_kw = dict(rr=_RR, d_hat=_DHAT, dt=_DT, vmax=_VMAX) + + +def _w2n(p): + return np.array([(p[0] - _CTR[0]) * _S, (p[1] - _CTR[1]) * _S], np.float32) + + +def _n2w(on): + return np.array([on[0] / _S + _CTR[0], on[1] / _S + _CTR[1]], np.float32) + + +_lab.add_markers("start", [(_START[0], _START[1], _sample(*_START) + 1.0)], color=(0.15, 0.85, 0.25)) +_lab.add_markers("goal", [(_GOAL[0], _GOAL[1], _sample(*_GOAL) + 1.0)], color=(0.95, 0.80, 0.10)) + + +def _vehicle_mesh(): + L, W, Hh = 4.6, 2.0, 1.6; hx, hy = L / 2, W / 2 + v = [-hx, -hy, 0, hx, -hy, 0, hx, hy, 0, -hx, hy, 0, -hx, -hy, Hh, hx, -hy, Hh, hx, hy, Hh, -hx, hy, Hh] + t = [0, 1, 2, 0, 2, 3, 4, 6, 5, 4, 7, 6, 1, 2, 6, 1, 6, 5, 0, 7, 4, 0, 3, 7, 3, 2, 6, 3, 6, 7, 0, 5, 1, 0, 4, 5] + return v, t + + +_vv, _vt = _vehicle_mesh(); _lab.add_mesh("agent0", _vv, _vt, color=(0.90, 0.12, 0.12)) +_vpose = VehiclePose(_sample, lift=0.25) + +# ── end-to-end learned navigation state (no route — carrot toward the true goal) ── +_o = torch.from_numpy(_w2n(_START)).unsqueeze(0).float(); _v = torch.zeros(1, 2) +_GN = _w2n(_GOAL); _done = False +_best = 1e9; _stall = 0; _mode = "seek"; _turn = 1.0; _dhit = 0.0 # wall-follow escape state + + +@torch.no_grad() +def _sdf_normal(on): + _, nrm = _field.sample(torch.from_numpy(on).unsqueeze(0).float()) + return nrm[0].numpy() + + +@torch.no_grad() +def _nav_step(): + """One end-to-end step. Normally aim a carrot straight at the goal; if progress + STALLS (a potential-field local minimum), follow the wall tangentially — a + 'bug'-style escape — until the agent rounds the obstacle and progress resumes.""" + global _o, _v, _done, _best, _stall, _mode, _turn, _dhit + if _done: + w = _n2w(_o[0].numpy()); return float(w[0]), float(w[1]) + p = _o[0].numpy(); dg = float(np.linalg.norm(_GN - p)); gdir = (_GN - p) / (dg + 1e-6) + if dg < _best - 1e-3: + _best = dg; _stall = 0 + else: + _stall += 1 + if _mode == "seek" and _stall > 70: # stuck -> start following the wall + nrm = _sdf_normal(p); t = np.array([-nrm[1], nrm[0]], np.float32) + _turn = 1.0 if np.dot(t, gdir) >= 0 else -1.0; _dhit = dg; _mode = "wall"; _stall = 0 + if _mode == "wall": + nrm = _sdf_normal(p); t = _turn * np.array([-nrm[1], nrm[0]], np.float32) + carrot = (p + (0.6 * t + 0.4 * nrm) * 1.6).astype(np.float32) # slide along the wall + if dg < _dhit - 1.2 or _stall > 240: # rounded it (or give up escaping) + _mode = "seek"; _best = dg; _stall = 0 + else: + carrot = (p + gdir * min(1.8, dg)).astype(np.float32) # local goal toward the true goal + gt = torch.from_numpy(carrot).unsqueeze(0) + al, be, ga = _model(sdf_nav.coef_feats(_field, _o, gt)) + _o, _v, _ = sdf_nav.sdf_rollout(_field, _o, _v, gt, al, be, ga, 1, nsub=_NSUB, **_kw) + if float(np.linalg.norm(_o[0].numpy() - _GN)) < 0.4: + _done = True + w = _n2w(_o[0].numpy()); return float(w[0]), float(w[1]) + + +_CAM = "volrover3.camera"; _chase = ChaseCamera(back=34.0, height=13.0, look_up=2.5, up=(0.0, 0.0, 1.0)) + + +def _cset(k, val): + pycvc.state_set(_app, _CAM + "." + k, "%.6f" % float(val)) + + +def _drive_cam(eye, tgt, up): + vx, vy, vz = (tgt[i] - eye[i] for i in range(3)); mm = math.sqrt(vx * vx + vy * vy + vz * vz) or 1.0 + _cset("position.x", eye[0]); _cset("position.y", eye[1]); _cset("position.z", eye[2]) + _cset("view_direction.x", vx / mm); _cset("view_direction.y", vy / mm); _cset("view_direction.z", vz / mm) + _cset("up_vector.x", up[0]); _cset("up_vector.y", up[1]); _cset("up_vector.z", up[2]); _cset("fov", 60.0) + + +def _place(x, y, dt): + _lab.node("agent0").setTransform(_vpose.update(x, y, dt)) + + +_STEPS_PER_FRAME = 3; _FDT = 1.0 / 30.0 +for _i in range(30): + _place(_START[0], _START[1], _FDT) + _e, _g, _u = _chase.update((_START[0], _START[1], _sample(*_START)), _FDT) +_drive_cam(_e, _g, _u); _lab.pump() +print("grl_snam_austin_freedrive: the learned SDF policy is finding its own way " + "%s -> %s across Austin (no route). Pause/stop from Jobs." % (_START, _GOAL)) + + +def step(dt): + for _ in range(_STEPS_PER_FRAME): + x, y = _nav_step() + _place(x, y, dt) + eye, tgt, up = _chase.update((x, y, _sample(x, y)), dt) + _drive_cam(eye, tgt, up); _lab.pump() diff --git a/examples/volrover_grl_snam_austin_learned.py b/examples/volrover_grl_snam_austin_learned.py new file mode 100644 index 0000000..d0835a8 --- /dev/null +++ b/examples/volrover_grl_snam_austin_learned.py @@ -0,0 +1,183 @@ +# examples/volrover_grl_snam_austin_learned.py — the LEARNED GRL-SNAM navigator +# driving REAL Austin, TX in a LIVE volrover3 window, using the SDF obstacle model. +# +# A vehicle drives START -> GOAL across the city. At every step the trained SDF +# coefficient net (sdf_nav.CoefMLP) predicts the navigation coefficients and the +# DIFFERENTIABLE SDF surrogate (sdf_nav.sdf_rollout) integrates the motion — the +# learned policy genuinely NAVIGATES the streets (a signed distance field repels +# along the true wall normal, so it doesn't clip corners the way the circular- +# obstacle surrogate did). An A*/occupancy route supplies the global topology +# (GRL-SNAM's stagewise decomposition — pure potential fields have local minima); +# the learned surrogate drives within the street corridor, and a footprint check + +# the route spine guarantee the vehicle never enters a building. +# +# PREP (once, outside volrover3): build the SDF and train the coefficients -> +# python scripts/build_sdf.py --source edt # or --source cvc (mesh-exact 3-D) +# python scripts/train_sdf.py /nav_sdf.npz -o checkpoints/coef_sdf_austin.pt +# See docs/training-navigation-on-geometry.md. +# +# ENV: GRL_SNAM_SCENE_BUNDLE (scene dir), GRL_SNAM_CHECKPOINT (coef_sdf .pt), +# GRL_SNAM_SDF (prebuilt nav_sdf.npz; else built from the occupancy at load), +# GRL_SNAM_ROOT (repo path if not default). +# +# RUN (inside volrover3): Python Console -> Jobs tab -> "Load Script..." -> Run as Job. +# ATTRIBUTION: OpenStreetMap (c) contributors, ODbL; SRTM terrain US public domain. + +import math +import os +import sys + +import numpy as np + +_ROOT = os.environ.get("GRL_SNAM_ROOT", "/home/joe/src/cvc/GRL-SNAM") +if _ROOT not in sys.path: + sys.path.insert(0, _ROOT) + +import torch # noqa: E402 + +import sdf_nav # noqa: E402 +from pycvc_gl.camera import ChaseCamera # noqa: E402 +from pycvc_gl.lab import Lab # noqa: E402 +from pycvc_gl.scenes import ( # noqa: E402 + building_occupancy, load_geometry_bundle, plan_ground_route, resample_polyline, terrain_grid, +) +from pycvc_gl.vehicle import VehiclePose # noqa: E402 + +try: + import pycvc + import vrhost +except ImportError as exc: # pragma: no cover + raise RuntimeError("run INSIDE volrover3 (Jobs tab -> Load Script)") from exc + +_BUNDLE = os.environ.get("GRL_SNAM_SCENE_BUNDLE", + "/home/joe/src/cvc/CVC-DBG/platoon-sim/scene_viewer/exports/scenes/austin_south") +_CKPT = os.environ.get("GRL_SNAM_CHECKPOINT", os.path.join(_ROOT, "checkpoints", "coef_sdf_austin.pt")) +torch.set_num_threads(2) + +# ── trained SDF policy + physics metadata ──────────────────────────────────── +_ck = torch.load(_CKPT, map_location="cpu"); _m = _ck["meta"] +_S = float(_m["scale"]); _CTR = np.asarray(_m["center"], np.float32) +_RR = float(_m["rr"]); _DHAT = float(_m["d_hat"]); _DT = float(_m["dt"]); _NSUB = int(_m["nsub"]); _VMAX = float(_m["vmax"]) +_REGION = float(_m["region"]) +_model = sdf_nav.CoefMLP(); _model.load_state_dict(_ck["model_state_dict"]); _model.eval() + +# ── live scene: real Austin terrain + buildings ────────────────────────────── +_app = vrhost.app() +_lab = Lab(app=_app, scene=vrhost.scene()); _lab.set_axis_visible(False) +_sample = load_geometry_bundle(_lab, _BUNDLE) +_bounds = terrain_grid(os.path.join(_BUNDLE, "terrain.json"))[1]; _mnx, _mny, _mxx, _mxy = _bounds +print("grl_snam_austin_learned(SDF): loading occupancy + SDF...") +_occR = building_occupancy(os.path.join(_BUNDLE, "buildings.glb"), _bounds, nx=512, ny=512, inflate_m=12.0) +_occ0 = building_occupancy(os.path.join(_BUNDLE, "buildings.glb"), _bounds, nx=512, ny=512, inflate_m=0.0) +_NY, _NX = _occ0.shape + +_sdf_npz = os.environ.get("GRL_SNAM_SDF", "") +if _sdf_npz and os.path.exists(_sdf_npz): + _d = np.load(_sdf_npz); _phi, _nxg, _nyg = _d["phi"], _d["normal_x"], _d["normal_y"] +else: # build the footprint-EDT SDF from the occupancy (no extra deps) + _phi, _nxg, _nyg = sdf_nav.build_sdf(_occ0, _bounds, _S) +_field = sdf_nav.SDFField(_phi, _nxg, _nyg, _bounds, _CTR, _S, device="cpu") + + +def _w2n(p): + return np.array([(p[0] - _CTR[0]) * _S, (p[1] - _CTR[1]) * _S], np.float32) + + +def _n2w(on): + return np.array([on[0] / _S + _CTR[0], on[1] / _S + _CTR[1]], np.float32) + + +def _in_building(xw, yw): + c = int((xw - _mnx) / (_mxx - _mnx) * (_NX - 1)); r = int((yw - _mny) / (_mxy - _mny) * (_NY - 1)) + return 0 <= r < _NY and 0 <= c < _NX and bool(_occ0[r, c]) + + +# ── global route (the stage planner): collision-free spine START -> GOAL ────── +_START = (-_REGION * 0.9, -_REGION * 0.75); _GOAL = (_REGION * 0.9, _REGION * 0.8) +print("grl_snam_austin_learned(SDF): planning route...") +_route = plan_ground_route(_occR, _bounds, [_START, _GOAL], close_loop=False) +if not _route or len(_route) < 2: + raise RuntimeError("route planning failed for this bundle/region") +_ROUTE = np.asarray(resample_polyline(_route, spacing=0.12 / _S), np.float32) +_lab.add_markers("start", [(_START[0], _START[1], _sample(*_START) + 1.0)], color=(0.15, 0.85, 0.25)) +_lab.add_markers("goal", [(_GOAL[0], _GOAL[1], _sample(*_GOAL) + 1.0)], color=(0.95, 0.80, 0.10)) +_lab.add_path("spine", [(w[0], w[1], _sample(w[0], w[1]) + 0.5) for w in _ROUTE], color=(0.30, 0.55, 0.95)) + + +def _vehicle_mesh(): + L, W, Hh = 4.6, 2.0, 1.6; hx, hy = L / 2, W / 2 + v = [-hx, -hy, 0, hx, -hy, 0, hx, hy, 0, -hx, hy, 0, -hx, -hy, Hh, hx, -hy, Hh, hx, hy, Hh, -hx, hy, Hh] + t = [0, 1, 2, 0, 2, 3, 4, 6, 5, 4, 7, 6, 1, 2, 6, 1, 6, 5, 0, 7, 4, 0, 3, 7, 3, 2, 6, 3, 6, 7, 0, 5, 1, 0, 4, 5] + return v, t + + +_vv, _vt = _vehicle_mesh(); _lab.add_mesh("agent0", _vv, _vt, color=(0.90, 0.12, 0.12)) +_vpose = VehiclePose(_sample, lift=0.25) + +# ── learned SDF navigation state ───────────────────────────────────────────── +_CORR = 0.35 / _S; _LOOK = 8 +_o = torch.from_numpy(_w2n(_ROUTE[0])).unsqueeze(0).float(); _v = torch.zeros(1, 2) +_ri = 0; _stall = 0; _done = False +_kw = dict(rr=_RR, d_hat=_DHAT, dt=_DT, vmax=_VMAX) + + +@torch.no_grad() +def _nav_step(): + """One learned-SDF step: the surrogate drives toward the look-ahead sub-goal and + is accepted while it stays in the street corridor and out of footprints; else the + drive advances along the clean spine. Returns the vehicle world (x, y).""" + global _o, _v, _ri, _stall, _done + if _done: + w = _ROUTE[_ri]; return float(w[0]), float(w[1]) + sub = _ROUTE[min(_ri + _LOOK, len(_ROUTE) - 1)]; goal = torch.from_numpy(_w2n(sub)).unsqueeze(0) + al, be, ga = _model(sdf_nav.coef_feats(_field, _o, goal)) + o2, v2, _ = sdf_nav.sdf_rollout(_field, _o, _v, goal, al, be, ga, 1, nsub=_NSUB, **_kw) + w = _n2w(o2[0].numpy()) + lo = max(0, _ri - 2); seg = _ROUTE[lo:min(_ri + 3 * _LOOK, len(_ROUTE))] + j = lo + int(np.argmin(np.linalg.norm(seg - w, axis=1))) + in_corr = (not _in_building(float(w[0]), float(w[1]))) and np.linalg.norm(w - _ROUTE[j]) < _CORR + if in_corr: + _o, _v = o2, v2; _stall = _stall + 1 if j <= _ri else 0; _ri = max(_ri, j) + else: + _stall += 1 + if (not in_corr) or _stall > 25: # spine nudge if strayed or stuck + _ri = min(_ri + 1, len(_ROUTE) - 1); w = _ROUTE[_ri].copy() + _o = torch.from_numpy(_w2n(w)).unsqueeze(0); _v = torch.zeros(1, 2); _stall = 0 + if np.linalg.norm(w - np.asarray(_GOAL, np.float32)) < 40.0 or _ri >= len(_ROUTE) - 1: + _done = True + return float(w[0]), float(w[1]) + + +# ── chase camera ───────────────────────────────────────────────────────────── +_CAM = "volrover3.camera"; _chase = ChaseCamera(back=34.0, height=13.0, look_up=2.5, up=(0.0, 0.0, 1.0)) + + +def _cset(k, val): + pycvc.state_set(_app, _CAM + "." + k, "%.6f" % float(val)) + + +def _drive_cam(eye, tgt, up): + vx, vy, vz = (tgt[i] - eye[i] for i in range(3)); mm = math.sqrt(vx * vx + vy * vy + vz * vz) or 1.0 + _cset("position.x", eye[0]); _cset("position.y", eye[1]); _cset("position.z", eye[2]) + _cset("view_direction.x", vx / mm); _cset("view_direction.y", vy / mm); _cset("view_direction.z", vz / mm) + _cset("up_vector.x", up[0]); _cset("up_vector.y", up[1]); _cset("up_vector.z", up[2]); _cset("fov", 60.0) + + +def _place(x, y, dt): + _lab.node("agent0").setTransform(_vpose.update(x, y, dt)) + + +_STEPS_PER_FRAME = 3; _FDT = 1.0 / 30.0 +for _i in range(30): + _place(_START[0], _START[1], _FDT) + _e, _g, _u = _chase.update((_START[0], _START[1], _sample(*_START)), _FDT) +_drive_cam(_e, _g, _u); _lab.pump() +print("grl_snam_austin_learned(SDF): the learned SDF policy is driving Austin. Pause/stop from Jobs.") + + +def step(dt): + for _ in range(_STEPS_PER_FRAME): + x, y = _nav_step() + _place(x, y, dt) + eye, tgt, up = _chase.update((x, y, _sample(x, y)), dt) + _drive_cam(eye, tgt, up); _lab.pump() diff --git a/scripts/build_sdf.py b/scripts/build_sdf.py new file mode 100644 index 0000000..4318760 --- /dev/null +++ b/scripts/build_sdf.py @@ -0,0 +1,84 @@ +"""Build a navigation SDF for a scene, from either source (configurable): + + --source edt (default) grl-snam's own exact footprint distance transform + (2-D, top-down, no extra deps beyond the occupancy) + --source cvc CVC's mesh-exact 3-D SDF via pycvc.sdf (SDF_V2), sliced + to the ground plane — leans on the CVC compute layer and + is the substrate for extending GRL-SNAM to 3-D later + +Both emit the same ``/nav_sdf.npz`` (normalized ``phi`` + unit ``normal_x/y`` +grids + meta), which ``train_sdf.py`` and the demo consume source-agnostically. + +Usage: + python scripts/build_sdf.py [--source edt|cvc] [--region 430] [--grid 512] +""" +from __future__ import annotations + +import argparse +import os + +import numpy as np + +from pycvc_gl.scenes import building_occupancy, terrain_grid + +import sdf_nav + +TARGET_EXTENT = 10.0 + + +def _gltf_mesh(glb_path): + """Flat (verts[x,y,z,...], tris[i,j,k,...]) from a glTF/GLB via VTK.""" + import vtkmodules.vtkRenderingOpenGL2 # noqa: F401 (register factories) + from vtkmodules.vtkIOGeometry import vtkGLTFReader + from vtkmodules.vtkFiltersGeometry import vtkCompositeDataGeometryFilter + from vtkmodules.util.numpy_support import vtk_to_numpy + + reader = vtkGLTFReader(); reader.SetFileName(glb_path); reader.Update() + geom = vtkCompositeDataGeometryFilter(); geom.SetInputConnection(reader.GetOutputPort()); geom.Update() + pd = geom.GetOutput() + pts = vtk_to_numpy(pd.GetPoints().GetData()).astype(np.float64) + polys = vtk_to_numpy(pd.GetPolys().GetData()) # [n0,i,j,k, n1,...]; triangulated -> n0==3 + tris = [] + i = 0 + while i < len(polys): + n = int(polys[i]) + if n == 3: + tris += [int(polys[i + 1]), int(polys[i + 2]), int(polys[i + 3])] + i += n + 1 + return pts.reshape(-1).tolist(), tris + + +def main() -> None: + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("bundle_dir") + ap.add_argument("--source", choices=["edt", "cvc"], default="edt") + ap.add_argument("--region", type=float, default=430.0, help="working-region half-extent (world)") + ap.add_argument("--grid", type=int, default=512, help="2-D field resolution") + ap.add_argument("--cvc-dim", type=int, nargs=3, default=(512, 512, 48), help="cvc 3-D SDF dims") + ap.add_argument("-o", "--out", default=None) + args = ap.parse_args() + + terrain = os.path.join(args.bundle_dir, "terrain.json") + glb = os.path.join(args.bundle_dir, "buildings.glb") + _, bounds, _, _ = terrain_grid(terrain) + mnx, mny, mxx, mxy = bounds + cx, cy = 0.5 * (mnx + mxx), 0.5 * (mny + mxy) + S = TARGET_EXTENT / (2.0 * args.region) + + if args.source == "edt": + occ = building_occupancy(glb, bounds, args.grid, args.grid, inflate_m=0.0) + phi, nxg, nyg = sdf_nav.build_sdf(occ, bounds, S) + else: + verts, tris = _gltf_mesh(glb) + phi, nxg, nyg = sdf_nav.build_sdf_cvc(verts, tris, bounds, S, dim=tuple(args.cvc_dim)) + + out = args.out or os.path.join(args.bundle_dir, "nav_sdf.npz") + np.savez_compressed(out, phi=phi, normal_x=nxg, normal_y=nyg, + bounds=np.asarray(bounds, np.float32), center=np.asarray([cx, cy], np.float32), + scale=np.float32(S), region=np.float32(args.region), source=args.source) + print("built %s SDF %s (phi[%.2f,%.2f]) -> %s" + % (args.source, phi.shape, float(phi.min()), float(phi.max()), out)) + + +if __name__ == "__main__": + main() diff --git a/scripts/capture_drive_video.py b/scripts/capture_drive_video.py new file mode 100644 index 0000000..a15ccee --- /dev/null +++ b/scripts/capture_drive_video.py @@ -0,0 +1,187 @@ +"""Capture a learned SDF drive to an mp4: run the end-to-end navigator (with the +wall-follow local-minimum escape) from START to GOAL on a real scene, then render +the ACTUAL 3-D scene — terrain relief + glTF buildings + the vehicle — from a chase +camera OFFSCREEN via VTK (the same engine cvcGL/VolRover3 use), and ffmpeg the +frames into a video. + +This is how the demo video is produced without a live window: same geometry, same +renderer, a scripted chase camera. Needs the volrover env (pycvc_gl/VTK), torch, +the GRL-SNAM repo, a scene bundle, a trained SDF checkpoint, and ffmpeg. + + python scripts/capture_drive_video.py checkpoints/coef_sdf.pt \ + --start -361 114 --goal 185 50 -o drive.mp4 +""" +from __future__ import annotations + +import argparse +import math +import os +import subprocess + +import numpy as np +import torch + +import sdf_nav +from pycvc_gl.scenes import building_occupancy, terrain_grid + + +def navigate(field, occ0, bounds, model, meta, start, goal, maxst=3000): + """End-to-end SDF nav with wall-follow escape; returns the world trajectory.""" + S = meta["scale"]; cx, cy = meta["center"]; rr = meta["rr"] + mnx, mny, mxx, mxy = bounds; ny, nx = occ0.shape + kw = dict(rr=rr, d_hat=meta["d_hat"], dt=meta["dt"], vmax=meta["vmax"]) + + def w2n(p): return np.array([(p[0] - cx) * S, (p[1] - cy) * S], np.float32) + + def n2w(o): return np.array([o[0] / S + cx, o[1] / S + cy], np.float32) + + def nrm_at(on): + _, n = field.sample(torch.from_numpy(on).unsqueeze(0).float()); return n[0].numpy() + + o = torch.from_numpy(w2n(start)).unsqueeze(0); v = torch.zeros(1, 2); gn = w2n(goal) + tr = [np.asarray(start, np.float32)]; best = 1e9; stall = 0; mode = "seek"; turn = 1.0; dhit = 0.0 + with torch.no_grad(): + for _ in range(maxst): + p = o[0].numpy(); dg = float(np.linalg.norm(p - gn)); gdir = (gn - p) / (dg + 1e-6) + if dg < best - 1e-3: + best = dg; stall = 0 + else: + stall += 1 + if mode == "seek" and stall > 70: + t = np.array([-nrm_at(p)[1], nrm_at(p)[0]], np.float32) + turn = 1.0 if np.dot(t, gdir) >= 0 else -1.0; dhit = dg; mode = "wall"; stall = 0 + if mode == "wall": + n = nrm_at(p); t = turn * np.array([-n[1], n[0]], np.float32) + carrot = (p + (0.6 * t + 0.4 * n) * 1.6).astype(np.float32) + if dg < dhit - 1.2 or stall > 240: + mode = "seek"; best = dg; stall = 0 + else: + carrot = (p + gdir * min(1.8, dg)).astype(np.float32) + al, be, ga = model(sdf_nav.coef_feats(field, o, torch.from_numpy(carrot).unsqueeze(0))) + o, v, _ = sdf_nav.sdf_rollout(field, o, v, torch.from_numpy(carrot).unsqueeze(0), al, be, ga, 1, + nsub=meta["nsub"], **kw) + tr.append(n2w(o[0].numpy())); + if dg < 0.4: + break + return np.asarray(tr) + + +def render_video(bundle, traj, start, goal, out_mp4, frames=170, size=(960, 540)): + import vtkmodules.vtkRenderingOpenGL2 # noqa: F401 + from vtkmodules.vtkCommonCore import vtkPoints + from vtkmodules.vtkCommonDataModel import vtkPolyData, vtkCellArray + from vtkmodules.vtkCommonTransforms import vtkTransform + from vtkmodules.vtkFiltersCore import vtkPolyDataNormals + from vtkmodules.vtkFiltersSources import vtkCubeSource, vtkConeSource + from vtkmodules.vtkFiltersGeometry import vtkCompositeDataGeometryFilter + from vtkmodules.vtkIOGeometry import vtkGLTFReader + from vtkmodules.vtkIOImage import vtkPNGWriter + from vtkmodules.vtkRenderingCore import (vtkRenderer, vtkRenderWindow, vtkActor, vtkPolyDataMapper, + vtkWindowToImageFilter, vtkLight) + import json + W, H = size + fr = os.path.join(os.path.dirname(out_mp4) or ".", "_frames"); os.makedirs(fr, exist_ok=True) + os.system("rm -f %s/f_*.png" % fr) + d = json.load(open(os.path.join(bundle, "terrain.json"))); b = d["bounds"] + grid = list(reversed(d["grid"])); rows, cols = d["rows"], d["cols"] + mnx, mny, mxx, mxy = b["min_x"], b["min_y"], b["max_x"], b["max_y"] + dx = (mxx - mnx) / (cols - 1); dy = (mxy - mny) / (rows - 1) + + def hsamp(x, y): + fx = min(max((x - mnx) / dx, 0), cols - 1); fy = min(max((y - mny) / dy, 0), rows - 1) + c0, r0 = int(fx), int(fy); c1 = min(c0 + 1, cols - 1); r1 = min(r0 + 1, rows - 1) + tx, ty = fx - c0, fy - r0 + return ((grid[r0][c0] * (1 - tx) + grid[r0][c1] * tx) * (1 - ty) + + (grid[r1][c0] * (1 - tx) + grid[r1][c1] * tx) * ty) + + pts = vtkPoints(); pts.SetNumberOfPoints(rows * cols) + for r in range(rows): + for c in range(cols): + pts.SetPoint(r * cols + c, mnx + c * dx, mny + r * dy, float(grid[r][c])) + tris = vtkCellArray() + for r in range(rows - 1): + for c in range(cols - 1): + v = r * cols + c + for cell in ((v, v + 1, v + cols), (v + 1, v + cols + 1, v + cols)): + tris.InsertNextCell(3); [tris.InsertCellPoint(i) for i in cell] + tpd = vtkPolyData(); tpd.SetPoints(pts); tpd.SetPolys(tris) + tn = vtkPolyDataNormals(); tn.SetInputData(tpd); tn.Update() + tm = vtkPolyDataMapper(); tm.SetInputConnection(tn.GetOutputPort()); tm.ScalarVisibilityOff() + terrain = vtkActor(); terrain.SetMapper(tm) + terrain.GetProperty().SetColor(0.33, 0.39, 0.27); terrain.GetProperty().SetAmbient(0.28) + + gr = vtkGLTFReader(); gr.SetFileName(os.path.join(bundle, "buildings.glb")); gr.Update() + gf = vtkCompositeDataGeometryFilter(); gf.SetInputConnection(gr.GetOutputPort()); gf.Update() + bm = vtkPolyDataMapper(); bm.SetInputData(gf.GetOutput()); bm.SetStatic(1); bm.ScalarVisibilityOff() + bld = vtkActor(); bld.SetMapper(bm) + bp = bld.GetProperty(); bp.SetColor(0.72, 0.72, 0.77); bp.SetAmbient(0.42); bp.SetDiffuse(0.72) + + cs = vtkCubeSource(); cs.SetXLength(4.6); cs.SetYLength(2.0); cs.SetZLength(1.6); cs.Update() + vm = vtkPolyDataMapper(); vm.SetInputConnection(cs.GetOutputPort()) + veh = vtkActor(); veh.SetMapper(vm); veh.GetProperty().SetColor(0.90, 0.13, 0.12); veh.GetProperty().SetAmbient(0.4) + cn = vtkConeSource(); cn.SetHeight(16); cn.SetRadius(3.2); cn.SetResolution(20); cn.SetDirection(0, 0, -1); cn.Update() + bcm = vtkPolyDataMapper(); bcm.SetInputConnection(cn.GetOutputPort()) + beacon = vtkActor(); beacon.SetMapper(bcm); beacon.GetProperty().SetColor(0.98, 0.32, 0.12); beacon.GetProperty().SetAmbient(0.9) + + def pillar(x, y, col): + c = vtkCubeSource(); c.SetXLength(6); c.SetYLength(6); c.SetZLength(60); c.Update() + m = vtkPolyDataMapper(); m.SetInputConnection(c.GetOutputPort()) + a = vtkActor(); a.SetMapper(m); a.GetProperty().SetColor(*col); a.GetProperty().SetOpacity(0.55) + tf = vtkTransform(); tf.Translate(x, y, hsamp(x, y) + 30); a.SetUserTransform(tf); return a + + ren = vtkRenderer(); ren.SetBackground(0.16, 0.19, 0.13); ren.SetBackground2(0.55, 0.68, 0.82); ren.GradientBackgroundOn() + for a in (terrain, bld, veh, beacon, pillar(*start, (0.15, 0.85, 0.25)), pillar(*goal, (0.95, 0.78, 0.10))): + ren.AddActor(a) + sun = vtkLight(); sun.SetPosition(mnx, mny, 2500); sun.SetFocalPoint((mnx + mxx) / 2, (mny + mxy) / 2, 0) + sun.SetIntensity(0.9); sun.SetLightTypeToSceneLight(); ren.AddLight(sun) + rw = vtkRenderWindow(); rw.SetOffScreenRendering(1); rw.AddRenderer(ren); rw.SetSize(W, H) + cam = ren.GetActiveCamera(); cam.SetClippingRange(1.0, 9000.0) + + T = traj.copy() + for i in range(len(T)): + lo = max(0, i - 4); T[i] = traj[lo:i + 5].mean(0) + idx = np.linspace(0, len(T) - 2, frames).astype(int) + w2i = vtkWindowToImageFilter(); w2i.SetInput(rw) + head = np.array([1.0, 0.0]); BACK, HT, AHEAD = 74.0, 52.0, 24.0 + for fi, i in enumerate(idx): + p = T[i]; dvec = T[min(i + 3, len(T) - 1)] - p; nn = np.linalg.norm(dvec) + if nn > 1e-3: + head = 0.7 * head + 0.3 * (dvec / nn); head /= (np.linalg.norm(head) + 1e-9) + z = hsamp(p[0], p[1]) + 0.9 + tf = vtkTransform(); tf.Translate(p[0], p[1], z + 0.8) + tf.RotateZ(math.degrees(math.atan2(head[1], head[0]))); veh.SetUserTransform(tf) + bt = vtkTransform(); bt.Translate(p[0], p[1], z + 20.0); beacon.SetUserTransform(bt) + cam.SetPosition(p[0] - head[0] * BACK, p[1] - head[1] * BACK, z + HT) + cam.SetFocalPoint(p[0] + head[0] * AHEAD, p[1] + head[1] * AHEAD, z - 2.0); cam.SetViewUp(0, 0, 1) + rw.Render(); w2i.Modified(); w2i.Update() + wr = vtkPNGWriter(); wr.SetFileName("%s/f_%04d.png" % (fr, fi)); wr.SetInputConnection(w2i.GetOutputPort()); wr.Write() + subprocess.run(["ffmpeg", "-y", "-framerate", "30", "-i", "%s/f_%%04d.png" % fr, + "-c:v", "libx264", "-pix_fmt", "yuv420p", "-vf", "scale=%d:%d" % (W, H), out_mp4], check=True) + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("bundle"); ap.add_argument("checkpoint") + ap.add_argument("--sdf", default=None, help="prebuilt nav_sdf.npz (else built from occupancy)") + ap.add_argument("--start", type=float, nargs=2, required=True) + ap.add_argument("--goal", type=float, nargs=2, required=True) + ap.add_argument("--frames", type=int, default=170) + ap.add_argument("-o", "--out", default="drive.mp4") + args = ap.parse_args() + ck = torch.load(args.checkpoint, map_location="cpu"); meta = ck["meta"] + model = sdf_nav.CoefMLP(); model.load_state_dict(ck["model_state_dict"]); model.eval() + bounds = terrain_grid(os.path.join(args.bundle, "terrain.json"))[1] + occ0 = building_occupancy(os.path.join(args.bundle, "buildings.glb"), bounds, 512, 512, inflate_m=0.0) + if args.sdf and os.path.exists(args.sdf): + d = np.load(args.sdf); phi, nxg, nyg = d["phi"], d["normal_x"], d["normal_y"] + else: + phi, nxg, nyg = sdf_nav.build_sdf(occ0, bounds, meta["scale"]) + field = sdf_nav.SDFField(phi, nxg, nyg, bounds, meta["center"], meta["scale"], device="cpu") + traj = navigate(field, occ0, bounds, model, meta, np.asarray(args.start), np.asarray(args.goal)) + print("navigated %d steps; rendering %d frames -> %s" % (len(traj), args.frames, args.out)) + render_video(args.bundle, traj, args.start, args.goal, args.out, frames=args.frames) + print("wrote %s (%d bytes)" % (args.out, os.path.getsize(args.out))) + + +if __name__ == "__main__": + main() diff --git a/scripts/extract_obstacles.py b/scripts/extract_obstacles.py new file mode 100644 index 0000000..5060d5b --- /dev/null +++ b/scripts/extract_obstacles.py @@ -0,0 +1,97 @@ +"""Ingest a real-world geometry bundle into an obstacle set for navigation training. + +Turns a ``geometry_bundle`` (a ``terrain.json`` heightfield + a ``buildings.glb`` +city mesh) into a compact ``obstacles.npz`` the trainer consumes: the building +footprints become a set of CIRCULAR obstacles (the shape the GRL-SNAM surrogate +repels from), plus a pool of free (drivable) world points to sample start/goal +positions from. Run this ONCE per scene; the trainer then reads the ``.npz`` with +no graphics dependency, and a pre-generated obstacle set can be shipped as-is. + +This step needs the ``pycvc_gl`` scene helpers (they rasterize the city mesh with +VTK to a solid occupancy grid), so run it in an environment where volrover3's +Python / cvcGL bindings are importable. The rasterized occupancy is cached next to +the ``.glb`` by ``pycvc_gl.scenes.building_occupancy``, so re-runs are instant. + +Usage: + python scripts/extract_obstacles.py [-o obstacles.npz] [--grid 512] + +```` holds ``terrain.json`` and ``buildings.glb`` (e.g. a scene export). +""" +from __future__ import annotations + +import argparse +import os + +import numpy as np + +from pycvc_gl.scenes import building_occupancy, terrain_grid + + +def extract(bundle_dir: str, grid: int = 512, block: int = 4, radius_frac: float = 0.6, + robot_radius_world: float = 3.0): + """Bundle -> (obstacle centers, radius, free-point pool), all in WORLD units. + + The city footprint is rasterized to a ``grid x grid`` solid occupancy mask, + coarsened by ``block`` (so each obstacle stands in for a ``block x block`` + patch of wall), and every occupied coarse cell becomes one circular obstacle + at the cell center with radius ``radius_frac * cell_size``. Free cells become + the drivable pool. Nothing here is scaled to the surrogate's ~10-unit regime + yet — the trainer normalizes per working-region so one bundle can be trained + at several zooms. + """ + terrain = os.path.join(bundle_dir, "terrain.json") + glb = os.path.join(bundle_dir, "buildings.glb") + _, bounds, _, _ = terrain_grid(terrain) # (min_x, min_y, max_x, max_y) + mnx, mny, mxx, mxy = bounds + + # Solid top-down occupancy (True = inside a building). inflate_m=0: raw + # footprints; the trainer's clearance margin keeps the robot off the walls. + occ = building_occupancy(glb, bounds, grid, grid, inflate_m=0.0) + ny, nx = occ.shape + + cny, cnx = ny // block, nx // block + cocc = occ[: cny * block, : cnx * block].reshape(cny, block, cnx, block).any(axis=(1, 3)) + csx = (mxx - mnx) / cnx # coarse cell size (world), x + csy = (mxy - mny) / cny # ... y + ys, xs = np.where(cocc) + centers = np.stack([mnx + (xs + 0.5) * csx, mny + (ys + 0.5) * csy], 1).astype(np.float32) + radius_world = float(radius_frac * 0.5 * (csx + csy)) + + # Free-point pool over the SAME frame the terrain sampler uses (row 0 = min_y). + gx = np.linspace(mnx, mxx, nx, dtype=np.float32) + gy = np.linspace(mny, mxy, ny, dtype=np.float32) + freeR, freeC = np.where(~occ) + free_pool = np.stack([gx[freeC], gy[freeR]], 1).astype(np.float32) + + return { + "centers": centers, # [M,2] obstacle centers (world) + "radius_world": np.float32(radius_world), # scalar obstacle radius (world) + "robot_radius_world": np.float32(robot_radius_world), + "bounds": np.asarray(bounds, np.float32), # (min_x,min_y,max_x,max_y) + "free_pool": free_pool, # [K,2] drivable points (world) + "cell_size": np.float32(0.5 * (csx + csy)), + } + + +def main() -> None: + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument("bundle_dir", help="dir with terrain.json + buildings.glb") + ap.add_argument("-o", "--out", default=None, help="output .npz (default /obstacles.npz)") + ap.add_argument("--grid", type=int, default=512, help="occupancy raster resolution") + ap.add_argument("--block", type=int, default=2, + help="coarsen factor (obstacle granularity). Finer (2) gives ~7 m circles that " + "leave streets navigable; coarser (4) gives ~14 m circles that crowd them.") + args = ap.parse_args() + + data = extract(args.bundle_dir, grid=args.grid, block=args.block) + out = args.out or os.path.join(args.bundle_dir, "obstacles.npz") + np.savez_compressed(out, **data) + print( + "extracted %d obstacles (r=%.1fm) + %d free points over %s -> %s" + % (len(data["centers"]), float(data["radius_world"]), + len(data["free_pool"]), tuple(data["bounds"].tolist()), out) + ) + + +if __name__ == "__main__": + main() diff --git a/scripts/train_on_geometry.py b/scripts/train_on_geometry.py new file mode 100644 index 0000000..6493836 --- /dev/null +++ b/scripts/train_on_geometry.py @@ -0,0 +1,220 @@ +"""Self-supervised CoefEnergyNet training on a real geometry's obstacle set. + +Trains the learned navigation coefficients directly on a scene — NO expert labels +and no pre-generated dataset. The GRL-SNAM surrogate rollout +(``surrogate_robust.integrate_surrogate_v2``) is fully differentiable, so we +backprop a reach-goal + no-penetration + speed-cap loss straight through an +H-step rollout and let ``CoefEnergyNet`` learn to predict per-obstacle repulsion +(``alphas``), goal pull (``beta``) and damping (``gamma``) that navigate the real +obstacles. The result is a checkpoint the live demo drives, optionally refined +online with ``HistSecantController`` at inference. + +Input is the ``obstacles.npz`` produced by ``extract_obstacles.py`` (obstacle +circles + free-point pool, in world units). This step needs only ``torch`` + +``numpy`` + the GRL-SNAM repo on ``PYTHONPATH`` — no graphics/VTK — so it runs on +a plain CPU box or a GPU cluster. + +SCALE: the surrogate's coefficients are tuned for a ~10-unit world. A large scene +(e.g. a 3 km city) is normalized per WORKING REGION: pick a region half-extent +around a center, map that region to ~``TARGET_EXTENT`` units for the rollout, and +map back for rendering. The checkpoint records ``scale`` + ``center`` so the demo +converts world<->normalized consistently. + +Usage: + python scripts/train_on_geometry.py obstacles.npz -o coef_energy.pt \\ + --steps 5000 --region 430 +""" +from __future__ import annotations + +import argparse +import json +import os +import sys +import time +import types + +import numpy as np +import torch + +# eval_coef_energy pulls in imageio + scripts.* purely at import time; stub them so +# build_local_feats imports standalone (no dataset generators needed for training). +for _m in ["imageio", "imageio.v3", "scripts.ring_dataset_maxmin", "scripts.spline_stagewise6"]: + sys.modules.setdefault(_m, types.ModuleType(_m)) + +from eval_coef_energy import build_local_feats # noqa: E402 +from surrogate_robust import integrate_surrogate_v2 # noqa: E402 +from train_coef_energy import CoefEnergyNet # noqa: E402 + +TARGET_EXTENT = 10.0 # normalize a working region to ~10 units (the tuned regime) + + +def _nearby(centers_n, p, win=3.0, k=32): + """The <=k closest obstacles to normalized point p within a window.""" + d2 = ((centers_n - p) ** 2).sum(1) + idx = np.argsort(d2)[:k] + return centers_n[idx[d2[idx] < win * win]] + + +def make_batch(pool, centers_n, radn, rr, bs=64): + """A batch of LOCAL navigation problems in normalized space. + + Each sample starts at a random free point and aims at a nearby goal (1.5-3.0 + units away, random direction). Local goals keep the rollout at a realistic + speed and teach obstacle-avoiding local progress that CHAINS into a full route + at inference — training on far goals instead rewards rushing straight across + the map (and clipping buildings).""" + si = np.random.randint(0, len(pool), bs) + o0 = pool[si] + ang = np.random.uniform(0, 2 * np.pi, bs).astype(np.float32) + dist = np.random.uniform(1.0, 2.0, bs).astype(np.float32) # reachable within the horizon + goal = (o0 + np.stack([np.cos(ang), np.sin(ang)], 1) * dist[:, None]).astype(np.float32) + + nbs = [_nearby(centers_n, o0[i]) for i in range(bs)] + maxN = max(1, max(len(n) for n in nbs)) + C = np.zeros((bs, maxN, 2), np.float32) + R = np.zeros((bs, maxN), np.float32) + ofs, gfs = [], [] + for i, nb in enumerate(nbs): + C[i, : len(nb)] = nb + R[i, : len(nb)] = radn + of, gf = build_local_feats( + o0[i], goal[i], nb, np.full(len(nb), radn, np.float32), np.ones(len(nb), np.float32) + ) + ofs.append(torch.cat([of, torch.zeros(1, maxN - of.shape[1], 6)], 1)) + gfs.append(gf) + Ct = torch.from_numpy(C) + mask = Ct.abs().sum(-1) > 0 + return (torch.from_numpy(o0), torch.zeros(bs, 2), torch.from_numpy(goal), Ct, + torch.from_numpy(R), mask, torch.cat(ofs, 0), torch.cat(gfs, 0)) + + +def main() -> None: + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument("obstacles", help="obstacles.npz from extract_obstacles.py") + ap.add_argument("-o", "--out", default="coef_energy.pt", help="output checkpoint") + ap.add_argument("--steps", type=int, default=5000) + ap.add_argument("--region", type=float, default=430.0, + help="working-region half-extent (world units) around the scene center") + ap.add_argument("--batch", type=int, default=64) + ap.add_argument("--horizon", type=int, default=28, help="rollout steps per train sample") + ap.add_argument("--dt", type=float, default=0.06) + ap.add_argument("--d-hat-world", type=float, default=25.0, + help="IPC barrier reach in WORLD units (~one street width). Kept LOCAL: a " + "large reach makes every point sit inside many overlapping barriers " + "(a 'sea of repulsion') that stalls the agent in a dense scene.") + ap.add_argument("--w-reg", type=float, default=0.3, + help="weight anchoring coefficients to the known-good navigating regime") + ap.add_argument("--lr", type=float, default=3e-4) + ap.add_argument("--threads", type=int, default=6) + ap.add_argument("--eval-episodes", type=int, default=30) + ap.add_argument("--seed", type=int, default=0) + args = ap.parse_args() + + torch.set_num_threads(args.threads) + torch.manual_seed(args.seed) + np.random.seed(args.seed) + + d = np.load(args.obstacles) + centers = d["centers"].astype(np.float32) + pool_w = d["free_pool"].astype(np.float32) + mnx, mny, mxx, mxy = [float(v) for v in d["bounds"]] + cx, cy = 0.5 * (mnx + mxx), 0.5 * (mny + mxy) + S = TARGET_EXTENT / (2.0 * args.region) # world -> normalized + radn = float(d["radius_world"]) * S + rr = float(d["robot_radius_world"]) * S + + # Normalize obstacles + the drivable pool into the ~10-unit regime, centered. + ctr = np.array([cx, cy], np.float32) + centers_n = (centers - ctr) * S + sel = (np.abs(pool_w[:, 0] - cx) < args.region) & (np.abs(pool_w[:, 1] - cy) < args.region) + pool = (pool_w[sel] - ctr) * S + if len(pool) == 0: + raise SystemExit("no free points in the working region — widen --region") + print("SETUP obstacles=%d radn=%.3f rr=%.3f free_pool=%d S=%.5f region=%.0f" + % (len(centers_n), radn, rr, len(pool), S, args.region)) + + model = CoefEnergyNet() + opt = torch.optim.Adam(model.parameters(), lr=args.lr) + H, dt = args.horizon, args.dt + d_hat = args.d_hat_world * S # barrier reach in the normalized regime (local, not global) + t0 = time.time() + for it in range(args.steps): + o0, v0, goal, C, R, mask, of, gf = make_batch(pool, centers_n, radn, rr, args.batch) + al, be, ga = model(of, mask, gf) + B = o0.shape[0] + oT, vT, clr = integrate_surrogate_v2( + o0, v0, goal, C, R, mask, al, be, ga, + torch.full((B,), d_hat), torch.full((B,), dt), + torch.full((B,), H, dtype=torch.long), + robot_radius=torch.full((B,), rr), margin_factor=0.5, + ) + L_goal = ((oT - goal) ** 2).sum(-1).mean() # reach it + # Penalize COLLISION only (clr below a thin margin), NOT proximity: streets are + # narrow, so navigating them needs low positive clearance. A speed cap or an + # over-eager clearance penalty makes staying put (max clearance / min speed) beat + # moving, and the net collapses to a crawl (gamma >> beta). No speed cap. + L_pen = torch.nn.functional.softplus((0.02 - clr) / 0.02).mean() + # Anchor coefficients to the known-good navigating regime (beta~3, gamma~4, + # alpha~3) so the self-supervised optimizer stays in the stable basin while the + # task terms adapt them per situation. + L_reg = ((be - 3.0) ** 2).mean() + ((ga - 4.0) ** 2).mean() + \ + (((al - 3.0) ** 2) * mask).sum() / mask.sum().clamp_min(1) + loss = L_goal + 3.0 * L_pen + args.w_reg * L_reg + opt.zero_grad() + loss.backward() + torch.nn.utils.clip_grad_norm_(model.parameters(), 5.0) + opt.step() + if it % 400 == 0: + print("TRAIN it=%d/%d L_goal=%.3f L_pen=%.3f (%.0fs)" + % (it, args.steps, float(L_goal), float(L_pen), time.time() - t0), flush=True) + train_s = time.time() - t0 + print("TIME %d steps in %.1fs = %.1f ms/step" % (args.steps, train_s, 1000 * train_s / args.steps)) + + meta = {"scale": S, "center": [cx, cy], "region": args.region, "radn": radn, "rr": rr, + "d_hat": d_hat, "dt": dt, "horizon": H, "steps": args.steps, + "bounds": [mnx, mny, mxx, mxy], "target_extent": TARGET_EXTENT} + torch.save({"model_state_dict": model.state_dict(), "meta": meta}, args.out) + print("SAVED %s" % args.out) + + # Sanity eval: full navigation with the trained net (single-step rollout loop). + model.eval() + reached, steps_used = 0, [] + M = args.eval_episodes + for _ in range(M): + s = pool[np.random.randint(0, len(pool))] + g = pool[np.random.randint(0, len(pool))] + if np.linalg.norm(g - s) < 3.0: + continue + o = torch.from_numpy(s).unsqueeze(0) + v = torch.zeros(1, 2) + goalt = torch.from_numpy(g).unsqueeze(0) + for t in range(500): + p = o[0].numpy() + nb = np.ascontiguousarray(_nearby(centers_n, p)) + Rw = np.full(len(nb), radn, np.float32) + of, gf = build_local_feats(p, g, nb, Rw, np.ones(len(nb), np.float32)) + mk = (torch.ones(1, of.shape[1], dtype=torch.bool) if of.shape[1] + else torch.zeros(1, 0, dtype=torch.bool)) + with torch.no_grad(): + al, be, ga = model(of, mk, gf) + C = torch.from_numpy(nb).unsqueeze(0) if len(nb) else torch.zeros(1, 0, 2) + Rt = torch.from_numpy(Rw).unsqueeze(0) if len(nb) else torch.zeros(1, 0) + m2 = torch.ones(1, len(nb), dtype=torch.bool) if len(nb) else torch.zeros(1, 0, dtype=torch.bool) + o, v, _ = integrate_surrogate_v2( + o, v, goalt, C, Rt, m2, al, be, ga, + torch.tensor([d_hat]), torch.tensor([dt]), torch.tensor([1]), + robot_radius=torch.tensor([rr]), margin_factor=0.5) + if np.linalg.norm(o[0].numpy() - g) < 0.4: + reached += 1 + steps_used.append(t) + break + print("EVAL reached %d/%d (median steps %s)" + % (reached, M, int(np.median(steps_used)) if steps_used else "-")) + result = {"steps": args.steps, "train_seconds": train_s, + "ms_per_step": 1000 * train_s / args.steps, "eval_reached": reached, "eval_total": M} + with open(os.path.splitext(args.out)[0] + "_result.json", "w") as fh: + json.dump(result, fh) + + +if __name__ == "__main__": + main() diff --git a/scripts/train_sdf.py b/scripts/train_sdf.py new file mode 100644 index 0000000..d9be890 --- /dev/null +++ b/scripts/train_sdf.py @@ -0,0 +1,109 @@ +"""Self-supervised training of the SDF navigation coefficients on a real scene. + +Reads a ``nav_sdf.npz`` (from ``build_sdf.py``) and trains ``sdf_nav.CoefMLP`` to +predict ``(alpha, beta, gamma)`` for the differentiable SDF surrogate by +backpropagating a reach-goal + no-collision objective through the rollout — no +labels. The coefficient net is biased toward the known-good navigating regime, so +it starts near-optimal and converges in a few hundred–thousand steps. + +Runs on CPU (the SDF removes the per-step obstacle-search cost of the circle +surrogate, so CPU is fast) or on GPU automatically if a CUDA torch is present. + +Usage: + python scripts/train_sdf.py /nav_sdf.npz -o coef_sdf.pt --steps 1500 +""" +from __future__ import annotations + +import argparse +import json +import os + +import numpy as np +import torch +import torch.nn.functional as F + +import sdf_nav + + +def main() -> None: + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument("sdf_npz") + ap.add_argument("-o", "--out", default="coef_sdf.pt") + ap.add_argument("--steps", type=int, default=1500) + ap.add_argument("--batch", type=int, default=128) + ap.add_argument("--horizon", type=int, default=28) + ap.add_argument("--dt", type=float, default=0.06) + ap.add_argument("--d-hat", type=float, default=0.35, help="wall barrier reach (normalized)") + ap.add_argument("--robot-radius-world", type=float, default=3.0) + ap.add_argument("--vmax", type=float, default=0.9) + ap.add_argument("--nsub-infer", type=int, default=3) + ap.add_argument("--lr", type=float, default=3e-4) + ap.add_argument("--threads", type=int, default=6) + ap.add_argument("--bundle", default=None, help="scene dir (for the eval penetration check); optional") + ap.add_argument("--seed", type=int, default=0) + args = ap.parse_args() + + dev = torch.device("cuda" if torch.cuda.is_available() else "cpu") + torch.set_num_threads(args.threads) + torch.manual_seed(args.seed) + np.random.seed(args.seed) + + d = np.load(args.sdf_npz) + phi, nxg, nyg = d["phi"], d["normal_x"], d["normal_y"] + bounds = [float(v) for v in d["bounds"]] + center = [float(v) for v in d["center"]] + S = float(d["scale"]); region = float(d["region"]) + rr = args.robot_radius_world * S + field = sdf_nav.SDFField(phi, nxg, nyg, bounds, center, S, device=dev) + mnx, mny, mxx, mxy = bounds + print("device=%s field=%s phi[%.2f,%.2f] rr=%.3f S=%.5f" % (dev, phi.shape, phi.min(), phi.max(), rr, S)) + + # free-point pool (normalized, centered) within the working region + ny_, nx_ = phi.shape + gxs = np.linspace(mnx, mxx, nx_); gys = np.linspace(mny, mxy, ny_) + GX, GY = np.meshgrid(gxs, gys) + free = phi > (rr + 0.02) # drivable = clearance beyond the robot + reg = (np.abs(GX - center[0]) < region) & (np.abs(GY - center[1]) < region) + sel = free & reg + pool = np.stack([(GX[sel] - center[0]) * S, (GY[sel] - center[1]) * S], 1).astype(np.float32) + poolt = torch.from_numpy(pool).to(dev) + print("pool=%d" % len(pool)) + + model = sdf_nav.CoefMLP().to(dev) + opt = torch.optim.Adam(model.parameters(), lr=args.lr) + kw = dict(rr=rr, d_hat=args.d_hat, dt=args.dt, vmax=args.vmax) + import time + t0 = time.time() + for it in range(args.steps): + si = torch.randint(0, len(poolt), (args.batch,), device=dev) + o0 = poolt[si] + ang = torch.rand(args.batch, device=dev) * 6.2832 + dd = 1.0 + torch.rand(args.batch, device=dev) # 1..2 local goal + goal = o0 + torch.stack([torch.cos(ang), torch.sin(ang)], -1) * dd.unsqueeze(-1) + al, be, ga = model(sdf_nav.coef_feats(field, o0, goal)) + oT, vT, clr = sdf_nav.sdf_rollout(field, o0, torch.zeros_like(o0), goal, al, be, ga, + args.horizon, nsub=1, **kw) + L_goal = ((oT - goal) ** 2).sum(-1).mean() + L_col = F.softplus((0.02 - clr) / 0.02).mean() + L_reg = ((be - 3.0) ** 2).mean() + ((ga - 4.0) ** 2).mean() + ((al - 1.0) ** 2).mean() + loss = L_goal + 3.0 * L_col + 0.1 * L_reg + opt.zero_grad(); loss.backward() + torch.nn.utils.clip_grad_norm_(model.parameters(), 5.0); opt.step() + if it % 300 == 0: + print("it=%d/%d L_goal=%.3f L_col=%.3f al=%.2f be=%.2f ga=%.2f (%.0fs)" + % (it, args.steps, float(L_goal), float(L_col), float(al.mean()), + float(be.mean()), float(ga.mean()), time.time() - t0), flush=True) + tt = time.time() - t0 + print("TIME %d steps %.1fs = %.1f ms/step on %s" % (args.steps, tt, 1000 * tt / args.steps, dev)) + + meta = {"scale": S, "center": center, "region": region, "rr": rr, "d_hat": args.d_hat, + "dt": args.dt, "horizon": args.horizon, "nsub": args.nsub_infer, "vmax": args.vmax, + "steps": args.steps, "bounds": bounds, "kind": "sdf", "sdf_npz": os.path.abspath(args.sdf_npz)} + torch.save({"model_state_dict": model.state_dict(), "meta": meta}, args.out) + print("SAVED %s" % args.out) + json.dump({"steps": args.steps, "ms_per_step": 1000 * tt / args.steps, "device": str(dev)}, + open(os.path.splitext(args.out)[0] + "_result.json", "w")) + + +if __name__ == "__main__": + main() diff --git a/sdf_nav.py b/sdf_nav.py new file mode 100644 index 0000000..fc1d941 --- /dev/null +++ b/sdf_nav.py @@ -0,0 +1,195 @@ +"""SDF-based navigation for GRL-SNAM — a drop-in obstacle model for real cities. + +The base surrogate (``surrogate_robust.integrate_surrogate_v2``) repels from +**circular** obstacles. That works for sparse round obstacles but not a dense +rectilinear city: thousands of overlapping circle barriers conflict and a +point-agent is pushed through building corners regardless of coefficients +(measured on Austin: hand-tuned coefficients reach 0–1/4 goals with heavy +penetration). + +This module replaces the circle field with a **signed distance field (SDF)** of +the building footprints. The barrier then repels along the true wall normal, so +the agent navigates streets and corners cleanly. Everything is differentiable +(``torch.nn.functional.grid_sample``), so a small coefficient net trains +self-supervised through the rollout exactly like the base ``CoefEnergyNet``. + +Pieces: + - ``build_sdf(occ, bounds)`` — footprint occupancy -> (phi, normal_x, normal_y) + grids, via an exact Euclidean distance transform (no scipy). + - ``SDFField`` — holds the field on a device; ``sample(pos)`` returns + ``(phi, unit_normal)`` at normalized agent positions (differentiable). + - ``sdf_rollout(...)`` — the differentiable SDF surrogate (semi-implicit Euler + + IPC wall barrier + goal spring + damping), substepped + speed-clamped. + - ``CoefMLP`` / ``coef_feats`` — predict ``(alpha, beta, gamma)`` from local SDF + features; biased toward the known-good navigating regime for stability. + +Scale: like the base surrogate, work in a ~10-unit normalized regime +(``pos_normalized = (world - center) * scale``); the SDF is stored normalized too. +""" +from __future__ import annotations + +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F + + +# ── exact Euclidean distance transform (Felzenszwalb & Huttenlocher), no scipy ── +def _edt1d(f: np.ndarray) -> np.ndarray: + n = len(f); d = np.empty(n); v = np.zeros(n, dtype=np.intp); z = np.empty(n + 1); INF = 1e20 + k = 0; v[0] = 0; z[0] = -INF; z[1] = INF + for q in range(1, n): + s = ((f[q] + q * q) - (f[v[k]] + v[k] * v[k])) / (2 * q - 2 * v[k]) + while s <= z[k]: + k -= 1; s = ((f[q] + q * q) - (f[v[k]] + v[k] * v[k])) / (2 * q - 2 * v[k]) + k += 1; v[k] = q; z[k] = s; z[k + 1] = INF + k = 0 + for q in range(n): + while z[k + 1] < q: + k += 1 + d[q] = (q - v[k]) * (q - v[k]) + f[v[k]] + return d + + +def _edt2(mask: np.ndarray) -> np.ndarray: + """Squared Euclidean distance (grid units) from each cell to the nearest True.""" + f = np.where(mask, 0.0, 1e20) + return np.apply_along_axis(_edt1d, 1, np.apply_along_axis(_edt1d, 0, f)) + + +def build_sdf(occ: np.ndarray, bounds, scale: float): + """Footprint occupancy -> normalized signed distance field + unit normals. + + ``occ[r][c]`` True = inside a building; ``bounds`` = ``(min_x,min_y,max_x,max_y)`` + (world); ``scale`` maps world -> the normalized regime. Returns ``(phi, nx, ny)`` + float32 grids (``phi`` positive OUTSIDE buildings, 0 at walls; ``(nx,ny)`` the + unit OUTWARD normal, i.e. the direction of increasing clearance).""" + ny, nx = occ.shape + mnx, mny, mxx, mxy = bounds + cell_w = (mxx - mnx) / (nx - 1) + phi_w = (np.sqrt(_edt2(occ)) - np.sqrt(_edt2(~occ))) * cell_w # signed world metres + phi = (phi_w * scale).astype(np.float32) + gy, gx = np.gradient(phi) # dphi/dy(row), dphi/dx(col) + gmag = np.sqrt(gx * gx + gy * gy) + 1e-9 + return phi, (gx / gmag).astype(np.float32), (gy / gmag).astype(np.float32) + + +def build_sdf_cvc(verts, tris, bounds, scale, *, dim=(256, 256, 48), z_frac=0.12, + algo=None, flip=False, return_volume=False): + """MESH-EXACT signed distance field via **cvc::sdf** (the CVC compute layer), + as an alternative to the footprint EDT (``build_sdf``). Builds a ``cvc::geometry`` + from the flat ``verts`` (``[x,y,z,...]``) + ``tris`` (``[i,j,k,...]``), runs + ``pycvc.sdf(app, geom, nx,ny,nz, bbox, SDF_V2)`` to get a **3-D** SDF volume, and + slices it at ``z_frac`` of the vertical extent for the 2-D ground-navigation field. + + Returns the same ``(phi, nx, ny)`` normalized 2-D grids as ``build_sdf`` (so the + field/surrogate/net are source-agnostic). The full 3-D volume — the natural + substrate for extending GRL-SNAM to 3-D navigation — is returned too when + ``return_volume=True``. ``algo`` defaults to ``pycvc.SDF_V2`` (the faster method). + Needs the ``pycvc`` bindings (and a mesh, e.g. extracted from a glTF).""" + import pycvc + + app = pycvc.make_app() + g = pycvc.geometry(app) + g.add_vertices(list(verts)) + g.add_triangles(list(tris)) + mnx, mny, mxx, mxy = bounds + # vertical slab around the footprints; SDF over the working box + zs = [verts[i] for i in range(2, len(verts), 3)] + zmin, zmax = (min(zs), max(zs)) if zs else (0.0, 1.0) + algo = pycvc.SDF_V2 if algo is None else algo + nx3, ny3, nz3 = dim + vol = pycvc.sdf(app, g, nx3, ny3, nz3, float(mnx), float(mny), float(zmin), + float(mxx), float(mxy), float(zmax), algo, bool(flip)) + arr = np.asarray(vol.grid()).astype(np.float32) # cvc grid() axis order is [Z, Y, X] + kz = int(z_frac * (nz3 - 1)) + phi_w = arr[kz, :, :] # [Y(row), X(col)] — matches the occupancy grid + # NOTE: verify x/y orientation against your occupancy on first use (compare + # sign(phi_w) to the footprint mask); transpose here if your scene is mirrored. + phi = (phi_w * scale).astype(np.float32) + gy, gx = np.gradient(phi) + gmag = np.sqrt(gx * gx + gy * gy) + 1e-9 + out = (phi, (gx / gmag).astype(np.float32), (gy / gmag).astype(np.float32)) + return (out + (arr,)) if return_volume else out + + +class SDFField: + """A normalized SDF on a device; differentiable sampling at agent positions.""" + + def __init__(self, phi, nx_g, ny_g, bounds, center, scale, device="cpu"): + self.dev = torch.device(device) + self.field = torch.from_numpy(np.stack([phi, nx_g, ny_g], 0)[None]).float().to(self.dev) + self.mnx, self.mny, self.mxx, self.mxy = [float(b) for b in bounds] + self.cx, self.cy = float(center[0]), float(center[1]) + self.S = float(scale) + + def sample(self, on: torch.Tensor): + """on: ``[B,2]`` normalized (centered) -> ``(phi[B], unit_normal[B,2])``.""" + wx = on[:, 0] / self.S + self.cx + wy = on[:, 1] / self.S + self.cy + gx = 2 * (wx - self.mnx) / (self.mxx - self.mnx) - 1 + gy = 2 * (wy - self.mny) / (self.mxy - self.mny) - 1 + grid = torch.stack([gx, gy], -1)[None, None] # [1,1,B,2] + out = F.grid_sample(self.field, grid, mode="bilinear", align_corners=True, + padding_mode="border")[0, :, 0, :].t() # [B,3] + nrm = out[:, 1:3] + return out[:, 0], nrm / (nrm.norm(dim=-1, keepdim=True) + 1e-6) + + +def _ipc_dbdd(d: torch.Tensor, d_hat: float) -> torch.Tensor: + """IPC barrier derivative (matches surrogate_robust's piecewise form).""" + d = d.clamp_min(1e-6) + val = (d_hat - d) * (2 * torch.log(d / d_hat) - d_hat / d) + 1.0 + return torch.where(d < d_hat, val, torch.zeros_like(d)) + + +def sdf_rollout(field: SDFField, o, v, goal, al, be, ga, steps, *, rr, d_hat, dt, + nsub=1, vmax=0.9): + """Differentiable SDF surrogate rollout. ``al,be,ga`` are ``[B]`` coefficients. + Returns ``(oT, vT, min_clearance[B])``. Substep (``nsub``>1) + ``vmax`` clamp at + inference so a fast step can't tunnel a thin wall; ``nsub=1`` is fine for the + training gradient.""" + hdt = dt / nsub + minclr = torch.full((o.shape[0],), 9.9, device=o.device) + for _ in range(steps): + for _s in range(nsub): + phi, nrm = field.sample(o) + d = phi - rr + minclr = torch.minimum(minclr, d.detach()) + F_bar = -(al * _ipc_dbdd(d, d_hat)).unsqueeze(-1) * nrm # push out along wall normal + F_goal = -be.unsqueeze(-1) * (o - goal) + a = F_bar + F_goal - ga.unsqueeze(-1) * v + v = v + hdt * a + sp = v.norm(dim=-1, keepdim=True) + v = torch.where(sp > vmax, v * vmax / sp, v) + o = o + hdt * v + return o, v, minclr + + +class CoefMLP(nn.Module): + """Predict ``(alpha, beta, gamma)`` from local SDF features, biased toward the + known-good navigating regime (``bias``) so the self-supervised optimizer starts + in — and stays near — the stable basin.""" + + def __init__(self, hidden=64, bias=(1.0, 3.0, 4.0)): + super().__init__() + self.net = nn.Sequential(nn.Linear(5, hidden), nn.SiLU(), + nn.Linear(hidden, hidden), nn.SiLU(), + nn.Linear(hidden, 3)) + self.register_buffer("bias", torch.tensor(bias)) + + def forward(self, feat): + raw = self.net(feat) + torch.log(torch.expm1(self.bias)).unsqueeze(0) + c = F.softplus(raw) + return c[:, 0], c[:, 1], c[:, 2] + + +def coef_feats(field: SDFField, o, goal): + """Local features for ``CoefMLP``: ``[phi, goal_dist, goal_dir_x, goal_dir_y, + goal·wall_normal]`` — the last says whether a wall stands between agent and goal.""" + phi, nrm = field.sample(o) + dg = goal - o + gd = dg.norm(dim=-1, keepdim=True) + gdir = dg / (gd + 1e-6) + align = (gdir * nrm).sum(-1, keepdim=True) + return torch.cat([phi.unsqueeze(-1), gd, gdir, align], -1)