Add MuJoCo simulation harness for elevation mapping validation

[deepanaishtaweera]

Builds procedural terrain in MuJoCo, feeds ray-cast depth frames into ElevationMap, and scores the resulting map cell-for-cell against a top-down ray-cast ground-truth height map.

scene (MJCF)  ──►  depth frames (mj_multiRay)  ──►  ElevationMap  ──┐
      │                                                             ├─►  metrics
      └────────►  ground truth (mj_ray, straight down)  ────────────┘

Nothing renders — both the sensor and the ground truth are ray casts, so the whole harness runs headless on CPU apart from the mapping itself.

Running it

pixi run test-all              # existing unit tests + simulation suite
pixi run sim --all --out sim/report

What's here

Module	Responsibility
sim/emsim/scenes.py	8 procedural terrains as MJCF, each with a closed-form surface
sim/emsim/heightmap.py	Top-down ray-cast ground truth; map-grid alignment
sim/emsim/sensor.py	Pinhole depth camera via mj_multiRay, optional range noise / dropout
sim/emsim/runner.py	Drives ElevationMap over a trajectory, collects timings
sim/emsim/metrics.py	RMSE / MAE / bias / p95 / coverage
sim/emsim/cli.py	Accuracy table + ground-truth/estimate/error triptychs

Only runner.py needs CuPy, so the scene, sensor and ground-truth tests run on any machine.

Tests

168 new tests on top of the 87 existing ones. All 255 pass, nothing skipped.

File	GPU	Covers
test_scenes.py	no	MJCF compiles; closed-form surfaces are what they claim
test_heightmap.py	no	Ray-cast truth vs analytic; grid alignment; body exclusion
test_sensor.py	no	Camera intrinsics, pose conventions, range/dropout noise
test_lidar.py	mixed	LiDAR scan patterns, frames, body exclusion, per-pattern accuracy
test_elevation_accuracy.py	yes	Per-scene RMSE/p95/coverage, gradient and step-height recovery, convergence, occlusion, noise
test_trajectories.py	mixed	Paths, body motion, mapping under full 6-DoF pose
test_map_shifting_sim.py	yes	Centre tracking; world-fixed features survive shifting
test_map_layers.py	yes	is_valid, variance, time, upper_bound, normals, traversability, plugins, clear
test_performance.py	yes	Ingestion throughput; no per-frame stalls

The ray-cast sampler is validated against each scene's independently derived analytic surface before any accuracy test relies on it. Accuracy thresholds sit well above what the pipeline currently achieves — they catch regressions rather than pinning down today's numbers.

Current results

Stationary 360° sweep, 24 frames, 8 m map at 0.04 m, scored within r ≤ 2.5 m, on Jetson Orin:

scene	cover	rmse	mae	bias	p95
flat	97.5%	0.0027	0.0019	+0.0019	0.0052
slope	92.6%	0.0027	0.0020	+0.0019	0.0051
rough	97.5%	0.0027	0.0019	+0.0019	0.0050
steps	90.2%	0.0097	0.0034	+0.0008	0.0069
boxes	90.8%	0.0150	0.0034	+0.0029	0.0067
gap	70.2%	0.0298	0.0052	+0.0051	0.0080
wall	91.1%	0.0896	0.0097	-0.0057	0.0054
mixed	96.2%	0.0072	0.0031	+0.0016	0.0072

High RMSE with low p95 is the signature of cells straddling a vertical face (wall, gap, boxes) — genuinely ambiguous, so both are reported. Throughput on mixed with the traversability filter running: ~100 Hz point-cloud ingestion at 19k points/frame, 3 ms layer export.

CUDA setup

Self-contained: no sudo, no writing into /usr/local/cuda. The only external prerequisite is JetPack itself (developed against JP6 / R36.4.7, CUDA 12.6).

• CuPy from stock PyPI (cupy-cuda12x); its aarch64 wheels work against the JetPack runtime.
• PyTorch — needed by the traversability filter, which calls .cuda() — has no CUDA-capable aarch64 wheel on stock PyPI, so it comes from NVIDIA's Jetson index https://pypi.jetson-ai-lab.io/jp6/cu126, scoped to linux-aarch64 so the manifest still resolves on x86. That wheel links against cuDSS, which JetPack does not ship, so nvidia-cudss-cu12 is pulled from PyPI and its lib directory added to LD_LIBRARY_PATH.

One latent defect found

The normal layers have an undeclared dependency on the traversability filter. update_map_with_kernel ends with self.update_normal(self.traversability_input), but traversability_input is only ever populated inside the if self.traversability_filter is not None: branch immediately above.

With the filter loaded — the configuration this environment sets up — the normals are correct: the harness recovers 15.18° on a 15° ramp and normal_z = 0.9999 on flat ground. But traversability_filter is set to None whenever weights.dat fails to load, and in that state the buffer stays all zeros, so normal_x/normal_y/normal_z silently become (0, 0, 1) everywhere regardless of terrain — with no warning beyond the one about the filter itself.

Not fixed here: the dilation step doesn't depend on the learned filter, so the likely fix is to run it unconditionally and keep only the self.traversability_filter(...) call behind the guard — but update_normal's parameter is named dilated_map, so that is a call about the upstream algorithm and deserves its own review. tests/test_map_layers.py skips its normals tests in that configuration rather than asserting against known-bad output.

Notes on the ground-truth sampler

• Grid alignment. Map cell centres always land on the (k + 0.5) * resolution lattice, so the sampler builds its grid there too and window comparison is integer indexing — no interpolation between estimate and truth. Verified end-to-end against a real map export.
• Height-field triangle edges. MuJoCo splits each height-field quad into two triangles; a ray landing exactly on the shared diagonal can miss both and fall through. A grid symmetric about the origin puts every x == y sample on such a diagonal (2 cells in 40,000 on rough). The sampler casts a second ray at a 10 µm asymmetric offset and keeps the higher hit.
• Near-horizontal rays. MuJoCo walks height fields cell by cell, so rays skimming across one dominate cost. Keeping the top image row 15° below the horizon is worth ~13× on height-field scenes at no cost in usable coverage.

Sensors

Two backends behind one interface (pose_for then capture -> DepthCapture), so the run loop does not care which it holds. Select with RunConfig(sensor=...) or --sensor.

camera (default) — a pinhole depth camera: dense, short-range, every one of its 19 200 rays returning within about 3 m.

lidar — real scan patterns via MuJoCo-LiDAR (MIT): vlp32, hdl64, os128, airy96, Livox mid360/avia/mid70/horizon, and a plain grid. Sparse, ring-structured, long-range — a VLP-32 returns ~53 000 of 120 000 rays spread over tens of metres, and the Livox patterns are non-repetitive so successive scans sample different points. That is the input the package actually receives on a robot, and it loads the map very differently from a dense camera patch.

A level-mounted spinning unit puts most of its rings above the horizon and covers only ~12% of a 2.5 m disc; lidar_tilt_down_deg fixes that, and the 20° default takes it to ~85%:

pattern	tilt	coverage r≤2.5 m	rmse	returns/scan
vlp32	0°	11.7%	0.0230	26 250
vlp32	20°	85.4%	0.0102	52 709
os128	10°	75.2%	0.0075	106 922
mid360	30°	74.4%	0.0124	5 117
avia	25°	96.5%	0.0094	20 642

Integration notes: mujoco-lidar reads the sensor pose from a site in the model, so scenes carry a geom-free lidar_mount mocap body (no geoms, so it cannot occlude anything or perturb the camera and ground-truth paths). get_hit_points returns points in the sensor frame — verified against a yawed sensor, where only R @ p + t lands on the terrain. bodyexclude has to go through args={} or the robot shell swallows every ray from the inside.

Ray-cast backends: Warp

lidar_backend defaults to "auto" — Warp when warp-lang reports a CUDA device, cpu otherwise. The cpu path is mj_multiRay underneath, the same call the depth camera uses, so it buys scan patterns rather than speed. Per-scan cost of a VLP-32 (120 000 rays) on an Orin:

scene	cpu	warp
flat	27.9 ms	11.0 ms	2.5x
boxes	36.4 ms	12.8 ms	2.9x
wall	33.2 ms	11.3 ms	2.9x
gap	38.3 ms	10.4 ms	3.7x
steps	40.3 ms	10.6 ms	3.8x
mixed	133.3 ms	14.9 ms	8.9x
rough	204.0 ms	15.6 ms	13.1x
slope	11 016 ms	12.5 ms	884x

slope is why this is not merely an optimisation. MuJoCo's CPU height-field ray cast walks the grid cell by cell, so a ray skimming along a flat height field crosses thousands of cells before it exits — and a 360° LiDAR aims an entire ring exactly that way. A 24-frame slope run is four and a half minutes on CPU and under a second on Warp. Warp builds a BVH and barely notices.

Maps come out identical between backends to float32 precision (1.5e-5 on ranges and points), and there are tests asserting both that parity and the speedup.

warp-lang ships manylinux_2_34 wheels, so the platform entries declare glibc 2.34 — uv otherwise assumes 2.28 and rejects them.

Since everything defaults to auto, a silent fall back to CPU would pass quietly while running up to 884x slower. RunResult.sensor_backend records what actually ran, the end-to-end LiDAR tests assert the run used the resolved default, and a test fails (rather than skips) if CUDA is present but auto did not pick Warp. The backend in use is printed once per test run.

All scenes on the LiDAR

VLP-32 at 20° tilt, 24-frame sweep, 10 m map at 0.04 m, scored within r ≤ 2.5 m:

scene	coverage	rmse	mae	bias	p95	pts/frame
flat	88.9%	0.0047	0.0033	+0.0033	0.0101	44 505
slope	89.4%	0.0041	0.0029	+0.0026	0.0089	51 970
rough	88.6%	0.0049	0.0034	+0.0033	0.0106	43 628
mixed	88.3%	0.0088	0.0045	+0.0029	0.0150	44 817
steps	87.3%	0.0098	0.0044	+0.0024	0.0136	46 779
gap	83.6%	0.0339	0.0053	+0.0052	0.0051	45 036
wall	82.5%	0.0120	0.0040	+0.0029	0.0110	55 787
boxes	81.4%	0.0285	0.0061	+0.0055	0.0123	44 883

Accuracy holds up under sparse ring sampling. Note gap covers 83.6% here against 70.2% for the depth camera: the LiDAR sees into the gap from a distance and at a shallower angle than a short-range downward-tilted camera can.

Trajectories and body motion

RunConfig.trajectory sets the nominal path — static, spin (rotate in place), line, circle (translate and rotate together), figure8 (a lemniscate whose yaw rate reverses sign twice per lap).

RunConfig.body_motion then adds what a legged base actually does on top of that path: vertical bob, lateral sway in the body frame so it follows the heading, and roll/pitch. Amplitudes default to zero, so nothing changes unless asked for:

RunConfig(trajectory="circle", body_motion=BodyMotion.walking())

BodyMotion.walking() uses trotting-quadruped amplitudes — 4 cm bob, 3 cm sway, 4° roll, 3° pitch, six cycles over the run — with the four terms quarter-cycle out of phase so the attitude traces a loop rather than heaving in lockstep.

This closed a real hole. Before it, nine of twelve trajectory uses in the suite were spin, two were static, one was line, and circle was implemented but never used: nothing combined translation with rotation, and nothing moved the base in z, roll or pitch at all — so a pose-handling error could have hidden behind a constant offset in every accuracy number here.

Sensors now take a full body rotation rather than a yaw angle (as_rotation_matrix accepts a yaw, an (r, p, y) triple or a 3×3 matrix), so base roll and pitch carry through to the camera and LiDAR on top of their own mount tilt, and move_to receives the real body attitude instead of just the heading.

One caveat worth knowing when reading amplitudes back: a sinusoid sampled at n_steps points does not generally land on its peaks — six cycles over 24 steps samples every 90°, so a term offset by 45° only ever reaches 0.707 of its amplitude. The tests bound the observed span rather than asserting the nominal value.

Visualisation

Figures are written as PNGs, headless. From the CLI with --out DIR --plots {comparison,layers,surface,convergence,filmstrip,all}, or from the tests with --viz-dir DIR, where every test that has something worth seeing emits a figure named after itself:

pixi run pytest sim/tests --viz-dir sim/report
pixi run sim --all --preview --out sim/report     # the scenes themselves

--preview draws each scene as a 3D surface and a top-down height map with one depth frame and one LiDAR scan overlaid — the clearest side-by-side of how differently the two sensors sample the same ground.

Rendered views

pixi run render --all --views all --out sim/report

Renders through MuJoCo's own rasteriser — oblique, front, top, side and graze presets, about a second per frame. The cameras sit deliberately low, and side/graze exist because slope and rough (~0.12 m peak-to-peak over 8 m) do not read from an elevated camera at all. Getting that working headless on a Jetson took some doing, which is why it has its own module and pixi task:

• Tegra's EGL exposes no usable EGL_PLATFORM_DEVICE_EXT display and is GLES-only, while MuJoCo's context asks for desktop EGL_OPENGL_BIT. Rendering therefore goes through Mesa's software rasteriser (mesalib) with GALLIUM_DRIVER=llvmpipe; otherwise Mesa tries the Tegra KMS nodes and reports kmsro: driver missing.
• Three EGL devices are enumerated and only one yields a working context. MuJoCo caches its EGL display on the first context it builds, so a failed device cannot be retried in-process — emsim.render.pick_egl_device runs the whole handshake itself first and sets MUJOCO_EGL_DEVICE_ID.
• LD_LIBRARY_PATH must point at Mesa before the process starts, since the dynamic loader reads it once. That is what the render task is for.

Scenes gained a skybox, checker materials and directional lights so the terrain is legible, and height-field scenes drop the ground plane 1 cm so the plane and the flat parts of the field stop z-fighting (applied to the analytic surface too, so ground truth stays consistent). Materials and lights play no part in ray casting, so the sensor and ground-truth paths are untouched. Rendered PNGs are gitignored.

Rendering is only ever used for looking at scenes — the sensors and the ground truth are ray casts and never touch OpenGL.

Known inefficiency

The depth camera ray casts on the CPU via mj_multiRay, and MuJoCo's CPU height-field cast walks the grid cell by cell. Measured cost of one 160×120 frame:

scene	ms/frame	height field?
flat	2.6	no
wall	3.4	no
boxes	3.8	no
gap	4.4	no
steps	5.8	no
mixed	23.6	yes
rough	30.9	yes
slope	371.9	yes

slope is 100× the box scenes and accounts for the two slowest tests in the suite (~6.4 s each, 16 frames apiece). The LiDAR path already avoids this through Warp; the camera does not, because it predates that backend and is deliberately dependency-free.

Routing the camera's rays through the same Warp backend would fix it — a pinhole bundle is rays from a single origin, and MjLidarWrapper.trace_rays accepts arbitrary (theta, phi), so the pinhole directions convert exactly. Not done here: it would couple the default sensor to the optional mujoco-lidar dependency, which is a design call worth making deliberately rather than folding into this PR.

[deepanaishtaweera]

Pushed 432e9b6, which drops the _require_working_normals skip guard from sim/tests/test_map_layers.py and rewrites the sim/README.md section that documented the normals defect as live.

Context: the harness caught a real regression — update_map_with_kernel feeds update_normal the traversability_input buffer, but the dilation that fills it had been moved inside the if self.traversability_filter is not None: guard, so with the filter unavailable every normal was (0, 0, 1) regardless of terrain. Fixed in #4.

With that fix the normals are correct in both configurations, so the skip guard had nothing left to protect against and the two normals tests become the regression test for #4. test_traversability_layer keeps its skip — it genuinely needs the learned weights.

Merge order: #4 should go in first. It is independent of this branch (it applies cleanly to dev, and sim/ is not on dev yet), but if this lands first on a runner without a CUDA-capable torch, the filter will be disabled and those two normals tests will fail until #4 is in.

sim/tests/test_map_layers.py is 26 passed on this branch as pushed.

[Copilot]

Copilot encountered an error and was unable to review this pull request. You can try again by re-requesting a review.

[Copilot]

Pull request overview

Copilot reviewed 23 out of 25 changed files in this pull request and generated no new comments.

Suppressed comments (2)

sim/emsim/sensor.py:238

• Noise is applied after the initial min/max-range filter, but valid does not re-check the noisy ranges against [min_range, max_range]. This can yield returns whose stored cap.ranges exceed max_range (or fall below min_range) once noise is enabled, which breaks the sensor contract and makes downstream code/tests sensitive to out-of-range values.

        hit = (self._geomid >= 0) & (ranges >= self.min_range) & (ranges <= self.max_range)
        ranges, keep = self.noise.apply(ranges)
        valid = hit & keep & (ranges > 0)

        points = self._dirs_cam[valid] * ranges[valid, None]

sim/emsim/lidar.py:210

• Like the depth camera, the LiDAR applies noise after range-gating but does not re-check the noisy ranges against [min_range, max_range]. With noise enabled, this can emit points whose DepthCapture.ranges exceed max_range (or fall below min_range), even though the sensor is documented as dropping those returns.

        hit = (ranges >= self.min_range) & (ranges <= self.max_range)
        ranges_noisy, keep = self.noise.apply(ranges)
        valid = hit & keep & (ranges_noisy > 0)