Huge worlds¶
Status: draft for review. Full implementation is v0.7, but four things are built into v0.2 so
that v0.7 is an extension and not a rewrite: double-precision positions, a floating origin,
region identifiers on every body, and geodetic conversion in core/world/coords/.
1. Coordinate frames¶
Frame |
Type |
Use |
|---|---|---|
ECEF (WGS84) |
|
Anchor of a georeferenced world; never used for dynamics |
World ENU |
|
East-north-up tangent plane at the profile’s |
Region-local |
|
World ENU minus the active region’s origin. Physics, signals of type |
Render-local |
|
Region-local minus the camera (or sensor) position, computed on the CPU in |
Invariant 6 in practice: no struct in core/, modules/, proto/ or MCAP stores a position in
world or region frames as float. A clang-tidy check plus a naming rule (*World, *Local
suffixes; float allowed only for *Render) enforces it.
1.1 Why a floating origin when positions are already double¶
double has about 1 nm resolution at 10 000 km, so storage is not the problem. The problem is
that physics engines and GPUs compute in float internally (PhysX, and GPU vertex transforms),
where resolution at 10 km from the origin is about 1 mm and contact solving degrades earlier.
MuJoCo uses double (mjtNum) by default. Rebasing keeps coordinates handed to any backend
below a few kilometres.
1.2 Rebase¶
// core/world/coords/Frames.hpp
namespace lagrange::coords {
struct Geodetic { double latitudeRad, longitudeRad, heightM; }; // WGS84
struct Ecef { double x, y, z; }; // m
struct Enu { double e, n, u; }; // m, relative to a GeoOrigin
class GeoOrigin {
public:
explicit GeoOrigin(Geodetic origin);
Ecef toEcef(Enu) const;
Enu fromEcef(Ecef) const;
Geodetic toGeodetic(Enu) const;
Enu fromGeodetic(Geodetic) const;
std::array<double, 9> enuFromEcefRotation() const;
};
// WGS84 constants (NIMA TR8350.2): a = 6378137.0 m, 1/f = 298.257223563. Named in code with the source.
struct RegionOrigin {
std::array<double, 3> worldEnu; // origin of the region-local frame in world ENU
std::uint64_t rebaseCount; // increments on every rebase
};
} // namespace lagrange::coords
Trigger: when the focus entity (the ego robot, or the centroid of robots in the region) is more than
rebase_distance(default 1000 m) from the region origin.Action, between ticks: subtract the offset from every region-local position in
core/world, call each rigid-body backend’s origin-shift function (PhysX has one; MuJoCo and Jolt state is shifted by rewriting body positions), bumprebaseCount, and publish the new origin.Signals of type
Poseare region-local, so their values jump at a rebase. Every consumer that keeps history (recording, scopes, ROS TF) receives the origin with the data: MCAP stores the origin on a channel,SceneDelta.rebased_frametells the viewer, and the ROS bridge publishes amap → regiontransform so thatmap-frame poses stay continuous.Rebase offsets are whole metres, so the subtraction is exact in
doubleand does not disturb determinism.Earth curvature: a single ENU plane deviates from the ellipsoid by about 8 cm at 1 km and about 20 m at 16 km from its origin. Tiles are authored in world ENU with their true heights (the importer converts geodetic → ECEF → ENU), so geometry is correct; gravity direction is taken as the local ellipsoid normal at the region origin and updated at rebase. For flights spanning more than about 100 km the region frame should be re-anchored to a new tangent plane; that rotation is planned as an extension of rebase and is not in v0.7 scope.
1.3 Status in v0.2¶
The server sets the region origin once, at load (server::recentre): when the robot starts more
than 1000 m from the origin, the whole-metre part of its position becomes World::region, and
every position held in the world frame loses it (unbounded planes only along their normal).
--spawn X,Y[,Z[,YAW]] places the robot. Acceptance criterion 8 is
FloatingOrigin.AWorld500KmOutSimulatesLikeOneAtTheOrigin in server/test/test_robot.cpp: the
warehouse authored 500 km out gives bit-identical poses and lidar ranges within 1.2e-10 m (the
rounding of the authored coordinates) after 10 s; without the floating origin it is off by
5e-7 m in pose and 8e-5 m in range.
1.4 Rebase during a run (v0.7)¶
Every 100 ms of simulated time the server compares the robot’s first body with the region
origin. Beyond 1000 m (kRebaseDistanceM) it moves the origin to the body’s whole-metre position:
it saves the run’s state (
Scheduler::saveState);shiftRegionmoves the world tables and the scene by the offset, asrecentredoes at load;it rebuilds the run on them, so static geometry (MuJoCo’s model, Jolt’s scenery, the lidar casters, fields loaded with
Field::load(path, region)) is built in the new frame;it restores the state and calls
Scheduler::shiftOrigin, which moves the region-localPosevalues on the bus (outputs, held inputs, window sums) and calls every module’sIModule::shiftOrigin: MuJoCo’s free joints and mocap bodies, Jolt’s bodies, the multicopter’s home and setpoint, ArduPilot’s start point. Traffic, scenarios and the vehicle model keep lane coordinates and need nothing.
Consumers:
the viewer gets a new snapshot whose
LocalFramehas the newregion_enuandrebase_count, and moves its camera by the difference, so the view does not jump;the ROS bridge keeps
/odom, its TF,/ground_truth/odomand entity states in the region of the first build (it adds the accumulated offset), so Nav2 and TF see no jump;a recording gets a
LocalFrameon/lagrange/scene/frameat each rebase. A replay does not rebase: it shows the recorded poses as they are.
FloatingOrigin.ARebaseDuringTheRunKeepsTheTrajectoryOn{Mujoco,Jolt} (server/test/ test_robot.cpp) rebase the warehouse demo by (37, -12, 0) m at 5 s: at 10 s the pose matches the
run that was never rebased within 1e-9 m on MuJoCo (double) and 1e-4 m on Jolt (float).
Not yet done:
Third-party plugins (the C ABI) are not told about a rebase; a plugin that keeps world positions must read them from the bus.
rebase_distancefrom the profile (world.streaming.rebase_distance) is not read; the distance is the constant.A rebase rebuilds the run, which takes about as long as loading it (tens of milliseconds for the samples); with huge worlds that is once per kilometre.
2. Tile streaming¶
Worlds are USD stages whose terrain, buildings and props are payloads, so they can be loaded and unloaded without reopening the stage.
worlds/coastal_survey_root.usda # root: geo origin metadata, tile index, robots
└── tiles/
├── L0/tile_0_0.usdc # 4096 m tiles, coarse LOD (always loadable quickly)
├── L1/tile_3_5.usdc # 1024 m tiles
└── L2/tile_14_22.usdc # 256 m tiles, full detail + collision
Tile prim (in the root layer or an index layer):
def Xform "tile_L2_14_22" (
prepend apiSchemas = ["WorldTileAPI_1"]
prepend payload = @tiles/L2/tile_14_22.usdc@
)
{
int world:tile:level = 2
int2 world:tile:index = (14, 22)
double3 world:tile:boundsMin = (3584, 5632, -20) # m, world ENU
double3 world:tile:boundsMax = (3840, 5888, 180)
double world:tile:geometricError = 0.5 # m, for LOD selection
bool world:tile:hasCollision = true
double3 xformOp:translate = (3584, 5632, 0)
uniform token[] xformOpOrder = ["xformOp:translate"]
}
WorldTileAPI_1 is a new schema in schemas/world/ (added to RFC 0002’s list). Tile contents are
authored relative to the tile’s own origin, so vertex data is small-magnitude float and the
tile’s double translate places it.
Streaming manager (core/world/streaming/):
Aspect |
Design |
|---|---|
Interest sources |
Each robot, each sensor with range, each viewer camera (sent as a hint over the protocol) |
Selection |
For each interest source, tiles within |
Hysteresis |
Unload only beyond |
Loading |
Worker threads load the payload into a private USD stage mask, convert to SoA and GPU buffers ( |
Collision |
Level-2 tiles carry convex or triangle-mesh static colliders and heightfields. A tile becomes “physics-ready” only when its colliders are in the backend. A robot is not allowed to enter a tile that is not physics-ready: the scheduler slows the real-time factor (or, in deterministic mode, blocks) until it is. This makes results independent of disk speed |
Memory budgets |
Profile keys for GPU and CPU megabytes; the least-recently-needed tiles are dropped first |
Determinism |
In deterministic mode the tile set is a pure function of robot positions (no camera hints, no time-based hysteresis; distance hysteresis only) |
Protocol |
The viewer receives tile prims as |
2.1 Status in v0.7¶
Built:
WorldTileAPI_1(schemas/world).stage::loadStageopens a stage with its tiles’ payloads unloaded (every other payload loaded) and lists the tiles inModel::tiles. WithReadOptions::tileit reads one tile: the stage masked to that prim, its payload alone loaded, so USD composes it as in the whole stage. A tile holds static scenery only.streaming::TileSelector(core/world/streaming) chooses the tiles: refinement by geometric error per metre from the coarsest level down, the collision tiles a robot can reach, coarse tiles kept until their finer replacements are loaded, distance hysteresis plus a 5 s delay (none in deterministic mode), a memory budget that never drops a robot’s ground, andphysicsReady().The server streams for any loaded stage with tiles. Every 100 ms of simulated time (with the rebase check) it runs the selection on the robot’s position and loads tiles on up to four worker threads. When the loaded set changes, the world tables are put back to what they were before any tile (
truncateModel), the loaded tiles appended in order (appendTile, moved into the region frame, a coarse tile’s prims left out where a loaded finer tile is), and the run rebuilt keeping its state. Jolt saves only its moving bodies’ state for this, so a state fits a world whose scenery changed.Physics-ready gating: a run with ROS takes loads as they finish and holds (no steps) while a collision tile within 5 m plus half a second of travel of the robot is missing. A run without ROS (headless, Python,
lagrange determinism) waits for its loads in the step, so its tile set depends only on the robot’s path:Streaming.TheTilesAroundTheRobotAreLoadedAnd TheRunIsDeterministicruns the sample twice and compares state hashes.Profile keys
world.streaming.load_radiusandunload_radius(defaults 600 m and 800 m).The World workspace shows the loaded tiles on the finest tiles’ grid (
WorldState: loaded collision tiles level of detail 0, others 2), the robot’s active radius, the floating origin in world ENU with its rebase count and last rebase, the georeference, host memory and the load queue.Sample:
assets/worlds/tiled_town/(written bymake_tiled_town.py), a 2048 m town in 4 coarse tiles of 1024 m and 64 collision tiles of 256 m.
Not yet:
Interest from sensors’ ranges. Viewer cameras are hints (
SetCameraInterest, sent when the camera moved 10 m, at most twice a second) in runs with ROS; runs without ROS are deterministic and follow the robot only.Memory budgets from the profile; the selector supports them, the server passes none.
A tile change rebuilds the run (tens of milliseconds for the sample, more for big tiles); an in-place add and remove of static geometry in the backends would avoid it.
Lanes, EMI sources and other schema instances in tiles are not taken (static scenery only).
3. Active regions and simplified distant agents¶
A region is a bubble of full simulation around one or more robots. Outside all regions the world is static geometry plus simplified agents.
// core/world/regions/Region.hpp
namespace lagrange::regions {
struct RegionId { std::uint32_t value; };
struct Region {
RegionId id;
coords::RegionOrigin origin;
double radiusM; // profile: world.streaming.active_region_radius
std::vector<std::uint32_t> robots; // robots that define the region
std::uint32_t owner; // process/node that simulates it; 0 in single-process mode
};
enum class AgentLod : std::uint8_t {
Full, // rigid bodies, all enabled domains
Kinematic, // pose follows a path; collision shape present; no electrical/thermal
Statistical // position and velocity only; no body; updated at the traffic rate
};
} // namespace lagrange::regions
Rules:
Every body and every signal instance has a
RegionIdcolumn from v0.2 on (always region 0 until v0.7). This is the “region boundaries designed in from day one” requirement.Two robots whose bubbles overlap share one region (regions merge and split with hysteresis). Robots far apart get separate regions with separate origins, and their backends are separate scenes.
Agents controlled by the traffic module move between
Statistical,KinematicandFullas they approach a region. Promotion happens at a distance greater than any sensor range in the region, so sensors never see a pop-in.Signals from a robot outside every viewer’s interest are still computed in full if the robot is a robot under test. Level-of-detail reduction applies to traffic and props, never silently to user robots.
Later distribution (post-1.0, not designed in detail here): a region is the unit that could move
to another process or machine. The pieces that make this possible later are already
constraints now: modules talk only through the signal bus (invariant 1), signals carry a region
id, cross-region interaction happens only through the traffic/agent hand-off and environment
fields, and the hand-off record (AgentRecord) is a serialisable value type.
3.1 Status in v0.7¶
Built:
regions::RegionPlanner(core/world/regions): robots within two region radii of each other (directly or through others) share a region; robots that shared one stay together until farther apart than 2.5 radii (splitFactor1.25). Deterministic.regions::AgentRecordis the hand-off value (base pose and twist in world ENU, joint positions and velocities).More robots in one run:
lagrange_server --add-robot NAME=FILE@X,Y[,Z[,YAW]](repeatable). An added robot’s bodies, joints and sensors are prefixedNAME_(prefixModel), and its joints are passive: nothing drives it yet.server::RegionHost: oneServerrun per region, each with its own floating origin, physics scene and streamed tiles, stepped in lockstep 100 ms at a time; headless (no ROS, no viewers).Regions.RobotsTenKilometresApartRunInTheirOwnRegions(server/test/ test_regions.cpp): two AMRs 10 km apart get two regions, and the one 10 km out drives exactly as a lone AMR at the origin (1e-9 m).
Not yet:
Moving a robot between regions while running (merge and split with an
AgentRecordhand-off):RegionHost::stepUntilstops withUnsupportedwhen robots meet or part.Agent levels of detail (
AgentLod): traffic agents are kinematic in every region.Serving a multi-region run to viewers and ROS; the
lagrangecommand has no regions mode.
4. Geospatial pipeline¶
Offline tools in io/geospatial/, run by the user or by tools/build_world. Output is the tiled
USD layout of section 2.
flowchart LR dem["GeoTIFF / DEM"] --> terr["terrain mesher: quadtree tiles, skirts, LODs"] osm["OSM extract"] --> feat["feature builder: roads, buildings (extruded), land use"] xodr["OpenDRIVE"] --> road["road mesher + lane tables"] tiles3d["OGC 3D Tiles (glTF content)"] --> conv["tile converter: glTF -> USD, re-tile"] terr --> merge["tile assembler"] feat --> merge road --> merge conv --> merge merge --> coll["collision bake (heightfields, convex)"] coll --> usd["tiled USD + index + attribution file"]
Input |
Library |
Notes |
|---|---|---|
GeoTIFF / DEM |
GDAL 3.12.2, PROJ 9.7.1 (apt) |
Reprojected to the world ENU frame. Vertical datum handled explicitly: orthometric heights are converted to ellipsoidal with a geoid grid if the user supplies one, otherwise the tool refuses unless |
OSM |
a PBF reader (candidate: libosmium; license and packaging TODO(verify)) |
Buildings extruded from |
OpenDRIVE |
own reader ( |
Reference line, lanes, elevation → mesh + |
OGC 3D Tiles |
own reader for |
Photogrammetry tiles are visual only; they get no collision unless the user requests a simplified collision bake |
WGS84 ↔ ENU |
GeographicLib 2.7 (apt) for geodesy; our |
Conversion code is tested against GeographicLib |
Every generated world has an ATTRIBUTION.md listing sources and licenses, written by the tool.
4.1 Status in v0.7¶
Built (lagrange_geospatial, io/geospatial/):
lagrange_build_world OUT_DIR --origin LAT,LON,HEIGHT|ground --size M [--dem FILE]... [--geoid FILE | --assume-ellipsoidal] [--osm FILE.osm]... [--tiles3d TILESET.json]...writes the layout of section 2: two levels (coarse visual tiles, fine tiles with colliders),.usdctiles, a root.usdaandATTRIBUTION.md.Elevation through GDAL (any raster, any CRS PROJ knows; bilinear between pixel centres). The vertical datum is never guessed (section 4’s rule): a geoid undulation raster (
egm96_15.gtxfrom proj-data) or--assume-ellipsoidal. Each vertex’s height is the geodetic point under its east/north position (fixed-point iteration, so curvature is right).Terrain collision: one height field per fine tile on the collision grid (
ShapeType::HeightField,World::heightfields): MuJoCohfield, JoltHeightFieldShape, the CPU lidar and radar casters (exact: the ray clipped to the grid, cells walked in order, two triangles each), the viewer’s collision shading, and USD (a grid mesh withlagrange:heightField:columnsandrows). The GPU lidar does not see height fields yet:--lidar gpuis refused on a streamed world. Radar treats terrain as ground clutter. Collision tiles load only withinworld.streaming.collision_radius(300 m) of a robot; coarse tiles cover the rest. A world with terrain says so in its layer (lagrangeGroundPlane = 0), and the infinite ground plane is taken away. (Terrain as convex prisms took a minute to load on MuJoCo: its spec API names meshes in quadratic time.)OpenStreetMap XML: buildings (closed ways; height from
height,building:levelsx 3 m or 9 m) as walls and roofs, with colliders over the ear-clipped footprint’s triangles; roads as ribbons draped on the terrain (visual). Multipolygon relations are not read.3D Tiles: leaf content (glb, glTF, b3dm with RTC_CENTER) through Assimp, transforms accumulated into ECEF, then ENU; visual only.
tools/ci/build_sample_world.shbuilds the 40 km sample (roadmap §8): Copernicus DEM GLO-30 around the PX4 home in Zurich, OpenStreetMap for the central 4 km, then drives the AMR on it headless. Nothing it writes is committed (licences:dependencies.md).
Not yet: height fields in the GPU lidar, OSM PBF (libosmium), re-tiling 3D Tiles, land use.
5. Sensors and rendering at scale¶
GPU scenes use camera-relative (or sensor-relative) transforms: the model matrix sent to the GPU is
float(tileOriginRegionLocal − eyeRegionLocal)computed indouble.Acceleration structures (Embree scenes, compute BVHs, Vulkan TLAS) are built per tile (BLAS) with a top level over loaded tiles, so loading a tile does not rebuild everything.
Depth precision: reversed-Z with a floating-point depth buffer for the viewport and cameras, so near 0.05 m and far 50 km fit in one frustum.
Long-range lidar and radar use the tile LOD that matches their range resolution.
6. What is tested when¶
Release |
Test |
|---|---|
v0.2 |
Unit tests for |
v0.4 |
A drone flight crossing two rebases; MAVLink GNSS output is continuous |
v0.7 |
40 km sample world streams within memory budgets; a robot never enters a tile that is not physics-ready; deterministic tile selection; viewer frame time stays within budget while tiles load |