From a distance measurement to a wheel command
- InputMeasure the surroundings
ranges: float[180] + hit positions: float[180,3]An ideal Physics Raycast Sensor measures colliders. The planner receives no object names or transforms.
- ProcessBuild a route from observations
5 cm occupancy grid → 0.43 m inflation → A*A known flat test boundary and simulator ego pose are assumed. The route is recalculated at 10 Hz.
- OutputDrive and measure again
path → v, ω → wheel drives → next scanMissing scans stop the robot. A changed box layout changes the generated route.
The previous robot followed a route we supplied. Here the only destination is [1.3, 0]. An Isaac Sim 6.2 Physics Raycast Sensor measures the surroundings. Our small Python planner turns those measurements into an occupancy grid, inflates obstacles to allow for the robot's width, and finds a route with A*. Wheel drives and PhysX produce the actual motion.
1. What does the sensor return?
A scanner sits 0.25 m above the chassis frame, approximately 0.48 m above the test floor. It casts 180 horizontal rays across 360°, 2° apart, with a 2.5 m maximum range. The sensor runs with the physics steps; the controller reads a complete frame every six steps, or 10 Hz at 60 physics steps/s. This is an ideal, instantaneous physics-range model, not a rotating RTX LiDAR or a camera model.
The native isaacsim.sensors.experimental.physics.RaycastSensor produces per-ray depths and world-space hit positions. Our program does not supply obstacle locations to the planner. NVIDIA Physics Raycast Sensor
angles: float[180] radians in the robot's horizontal frame
ranges: float[180] distance in metres; 2.5 means no nearer hit
hit_positions: float[180,3] XYZ endpoints in the stage's world frame
pose: [x, y, yaw] current chassis position and heading
For a robot near [-1.3, 0], the central ray points along +X. The recorded first scan in the central-box test contained approximately:
{
"ray_index": 90,
"angle_degrees": 0,
"range_m": 1.075,
"hit_position_world": [-0.225, 0.0, 0.48]
}
This says that something intersects the beam 1.075 m away. It does not identify a box by name. The front face of the test box happens to lie at X=-0.225 m. Other rays see different parts of that face; a miss returns the configured maximum range and must not become an occupied cell.
Inspect the recorded ray sample.

2. Why inflate an obstacle?
A* searches a 5 cm grid. Measured endpoints mark occupied cells. The planner expands them by 0.43 m, covering the robot's approximately 0.36 m circumscribed footprint plus tracking margin. A path for the robot's center must stay outside this expanded region. A line that a point can pass through may still be too narrow for the physical chassis and wheels.
We start without an obstacle map. Unknown cells are treated as traversable inside the known 5×4 m flat test area. Observations accumulate, and the planner recomputes a route at 10 Hz. This example uses static obstacles and does not erase old obstacle observations. It is not a dynamic-object tracker.
A* explores neighboring grid cells, comparing the cost already traveled with an estimate of the distance remaining to the goal. Our implementation checks eight neighbors and rejects diagonal moves that cut through blocked corners. When it reaches the goal cell, it traces the preceding cells back to the start.
S . . # # # . . G
. . . # # # . . .
. . . # # # . . .
. . . . . . . . .
This schematic uses S for the robot, G for the goal, # for inflated obstacles and . for traversable cells. A route must go around the blocked cells. The output is a variable-length float[N,2] list of cell-center XY coordinates. The first recorded central-box plan had 53 points, beginning [[-1.30, 0.00], [-1.25, -0.05], …]. These coordinates are calculated after reading a scan; they are not the route supplied to the old controller.
3. How does the route become motion?
The controller selects a short look-ahead point from the newly calculated path, measures its heading relative to the robot, and sends a forward velocity and yaw rate to the differential wheel drives. The cyan line in the viewport is a live plan, not a prerecorded trajectory. Blue lines show measured hits. Neither line moves the robot body directly.
180 measured ranges + known ego pose
→ occupied endpoints → footprint inflation → A* route
→ nearby path point → forward speed v / turning rate ω
→ left and right wheel speeds → PhysX motion
→ a new sensor frame → a revised plan
The maximum forward speed remains 0.32 m/s. An additional forward-range guard suppresses forward motion below 0.46 m. A missing/stale scan or an unavailable route makes the robot stop. The guard is an educational fallback, not a certified safety function.
The ego pose comes directly from the simulator. There is no SLAM or real wheel odometry. The generated WorldGen room still supplies appearance; the floor and boxes are explicit colliders. Low objects below the horizontal sensing plane, stairs and arbitrary generated furniture are outside this exercise's coverage.
First time? Get the lab files
Open a terminal on the Isaac Sim machine for GPU labs, or on your own computer for CPU-only steps. Press Enter after each line. You can copy a multi-line command block as a whole.
cd "$HOME"
git clone https://github.com/james-joobs/awesome-nvidia-physical-ai.git
cd awesome-nvidia-physical-ai
pwd
This example clones into your home folder. If you already cloned the repo, skip git clone and enter your project folder. In each new terminal, check that pwd ends in awesome-nvidia-physical-ai so commands can find the labs/ files.
Run the sensor and fixed-route comparison
1. Prepare the same generated room
Use the NVIDIA Linux setup guide and complete the WorldGen import lab first. Run commands from the repository root on a compatible NVIDIA Linux workstation:
nvidia-smi
mkdir -p runs/worldgen-navigation
cp -R labs/worldgen-isaac/assets runs/worldgen-navigation/assets
export WORLDGEN_DATA_ROOT="$PWD/runs/worldgen-navigation"
The prepared assets need no FLUX weights or new generation. Accept the NVIDIA Omniverse agreement before declaring ACCEPT_EULA=Y yourself:
export ACCEPT_EULA=Y
bash labs/worldgen-navigation/run.sh sensor --layout center
The real Isaac Sim GUI opens, shows the measured beams and computed route, and closes after saving the run. Open runs/worldgen-navigation/sensor/result.json and scans.json afterwards. The saved scene.usdc is an initial scene; its waypoint-free controller runs through the Python script, not the USD Play button alone.
2. Obstruct the old route
The lower layout adds a second box on the old negative-Y detour. The old waypoint list remains unchanged. Both controllers receive the same start, goal, robot, colliders and speed limit. The sensor planner never receives the layout name or the boxes' transforms.
bash labs/worldgen-navigation/run.sh scripted --layout lower \
--output /work/scripted-lower --max-seconds 20
bash labs/worldgen-navigation/run.sh sensor --layout lower \
--output /work/sensor-lower
bash labs/worldgen-navigation/run.sh sensor --layout upper \
--output /work/sensor-upper
/work maps to WORLDGEN_DATA_ROOT in the container. The upper layout mirrors the extra box, so a successful planner should choose the other side again. These are three controlled layouts, not a general navigation benchmark. Compare actual contact reports, measured poses and generated plans in the saved files.
3. Remove sensor data deliberately
bash labs/worldgen-navigation/run.sh sensor --sensor-off \
--output /work/sensor-off --max-seconds 3
The robot should remain still. This checks that it cannot silently keep driving using a hidden, preloaded route when range frames are unavailable.
4. Record the native GUI
On an X11 desktop with xwininfo and ffmpeg's x11grab support, use a fresh recording directory:
mkdir -p runs/worldgen-navigation-film
cp -R labs/worldgen-isaac/assets runs/worldgen-navigation-film/assets
python3 labs/worldgen-navigation/record_gui.py \
--work "$PWD/runs/worldgen-navigation-film" --scenario sensor \
--layout lower --max-seconds 30 --ffmpeg /path/to/ffmpeg
The helper waits for a real viewport frame before starting control. Raw native-window footage, logs, range arrays and trajectory evidence stay together. package.py creates the viewport crops and comparison; its edit metadata records any speed adjustment. Wall-clock recording duration and simulation duration are reported separately.
Inspect the evidence without an NVIDIA GPU
The plots use actual saved poses and scan endpoints. Green is the sensor-controlled trajectory; the dashed red path is the fixed controller that stalled. These are post-run data plots, separate from the native GUI footage.
| Run | Goal reached | Time to goal (simulation seconds) | Obstacle contact reports |
|---|---|---|---|
| Fixed route, lower passage blocked | No | — (20 s run) | 638 |
| Sensor, central box | Yes | 12.08 | 0 |
| Sensor, lower passage blocked | Yes | 12.08 | 0 |
| Sensor, upper passage blocked | Yes | 12.08 | 0 |
| Sensor data disabled | No; remains still | — (3 s check) | 0 |
638 counts reports during a persistent contact, not distinct impacts. Successful recordings include a further 3 s goal hold. Mirrored layouts produced almost identical travel times; these are individual runs, not averaged benchmark results.
Download the lower-layout initial USD scene. Keep assets/ beside sensor/ when opening sensor/scene.usdc. The Python controller is provided separately in the source.
python3 labs/worldgen-navigation/test_navigation.py
python3 labs/worldgen-navigation/check.py
The first command tests planning behavior from observations and the stop conditions. The second checks the actual Recorded experiments, range shapes and timestamps, route choices, contacts, endpoints and media hashes. To check your own complete run:
python3 labs/worldgen-navigation/check.py --run runs/worldgen-navigation/sensor-lower
Return to scale and collider checks · WorldGen generation · Media provenance
Example run and recording
Lower passage blocked: the robot chooses the upper side
The controller receives only ranges, ego pose and a goal. The cyan line is recomputed online; it is not a supplied waypoint list.
Upper passage blocked: the choice changes
Move the added box to the opposite side without changing the planner. The measured route changes with the scan.
One box, no supplied detour
The original central obstacle is detected from range hits. The planner builds its first route from an empty obstacle map.
The fixed-route baseline in the changed layout
The previous waypoint list is unchanged and ignores range data. Live contacts stop the robot at the added box; the goal is not reached.
Source code
navigation.py
View files on GitHub"""Small, inspectable scan-to-grid planner; no simulator or scene access.
Inputs are robot pose, a goal, and measured range endpoints. Unknown cells are
optimistically traversable inside a known, flat test boundary. No SLAM is done.
"""
import heapq
import math
class Navigator:
resolution = .05
radius = .43 # Circumscribed robot footprint plus tracking margin.
bounds = (-2.5, -2., 2.5, 2.)
def __init__(self, goal=(1.3, 0.)):
self.goal = tuple(goal)
self.hits = set()
self.path = []
self.replans = 0
self.blocked = set()
self.last_time = None
def cell(self, point):
return tuple(int(round((point[i] - self.bounds[i]) / self.resolution)) for i in (0, 1))
def point(self, cell):
return tuple(self.bounds[i] + cell[i] * self.resolution for i in (0, 1))
def allowed(self, cell):
x, y = self.point(cell)
return (self.bounds[0] + self.radius <= x <= self.bounds[2] - self.radius
and self.bounds[1] + self.radius <= y <= self.bounds[3] - self.radius
and cell not in self.blocked)
def line_free(self, start, end):
n = max(1, math.ceil(math.dist(start, end) / (self.resolution / 2)))
return all(self.allowed(self.cell([start[j] + (end[j] - start[j]) * i / n for j in (0, 1)]))
for i in range(n + 1))
def update(self, pose, depths, endpoints, sim_time):
"""Only measured endpoints within range become occupied cells."""
if not depths or len(depths) != len(endpoints):
self.path = []
return False
if self.last_time is not None and sim_time <= self.last_time:
self.path = []
return False
self.last_time = sim_time
for distance, point in zip(depths, endpoints):
if math.isfinite(distance) and .05 <= distance < 2.499 and all(math.isfinite(q) for q in point[:2]):
self.hits.add(self.cell(point))
n = math.ceil(self.radius / self.resolution)
offsets = [(i, j) for i in range(-n, n+1) for j in range(-n, n+1)
if math.hypot(i, j) * self.resolution <= self.radius + self.resolution / 2]
self.blocked = {(x+i, y+j) for x, y in self.hits for i, j in offsets}
self.path = self.plan(pose[:2])
self.replans += 1
return bool(self.path)
def plan(self, start):
source, goal = self.cell(start), self.cell(self.goal)
if not self.allowed(source) or not self.allowed(goal):
return []
queue = [(math.dist(source, goal), 0., source)]
costs, parents = {source: 0.}, {}
while queue:
_, cost, node = heapq.heappop(queue)
if cost > costs[node]:
continue
if node == goal:
route = [node]
while route[-1] != source:
route.append(parents[route[-1]])
return [self.point(c) for c in reversed(route)]
for dx, dy in ((1, -1), (1, 0), (1, 1), (0, -1), (0, 1), (-1, -1), (-1, 0), (-1, 1)):
neighbor = (node[0]+dx, node[1]+dy)
if not self.allowed(neighbor):
continue
if dx and dy and (not self.allowed((node[0]+dx, node[1])) or not self.allowed((node[0], node[1]+dy))):
continue
candidate = cost + math.hypot(dx, dy)
if candidate < costs.get(neighbor, float('inf')):
costs[neighbor] = candidate
parents[neighbor] = node
heapq.heappush(queue, (candidate + math.dist(neighbor, goal), candidate, neighbor))
return []
def command(self, pose, front_min, sensor_age):
x, y, yaw = pose
distance = math.dist((x, y), self.goal)
if distance < .12:
return 0., 0., 'goal'
if sensor_age > .25 or not self.path:
return 0., 0., 'sensor-stop' if sensor_age > .25 else 'no-path'
target = self.path[0]
for point in self.path:
if math.dist((x, y), point) <= .40 and self.line_free((x, y), point):
target = point
error = math.atan2(math.sin(math.atan2(target[1]-y, target[0]-x)-yaw),
math.cos(math.atan2(target[1]-y, target[0]-x)-yaw))
omega = max(-1.1, min(1.1, 2.8*error))
speed = min(.32, .9*distance) * max(0., math.cos(error)) if abs(error) < .65 else 0.
if front_min < .46:
speed = 0.
return speed, omega, 'follow-scan-path'
run.sh
View files on GitHub#!/usr/bin/env bash
# Start an isolated Isaac Sim 6.2 container. ACCEPT_EULA must be supplied by the user.
set -euo pipefail
: "${ACCEPT_EULA:?Set ACCEPT_EULA=Y only after accepting the NVIDIA Omniverse agreement.}"
[[ "$ACCEPT_EULA" == Y ]] || { echo "ACCEPT_EULA must be Y after your own acceptance." >&2; exit 1; }
worldgen_lab_root="$(cd "$(dirname "$0")" && pwd)"
worldgen_data_root="${WORLDGEN_DATA_ROOT:-$worldgen_lab_root}"
mkdir -p "$worldgen_data_root/cache"
worldgen_scenario="${1:-sensor}"
shift || true
worldgen_xauth="${XAUTHORITY:-/run/user/$(id -u)/gdm/Xauthority}"
worldgen_display="${DISPLAY:-:1}"
worldgen_desktop_args=()
worldgen_headless=false
for worldgen_arg in "$@"; do
[[ "$worldgen_arg" == --headless ]] && worldgen_headless=true
done
if ! "$worldgen_headless"; then
[[ -f "$worldgen_xauth" ]] || { echo "Set XAUTHORITY to your X11 desktop authority file for GUI use." >&2; exit 1; }
worldgen_desktop_args=(-e "DISPLAY=$worldgen_display" -e XAUTHORITY=/tmp/isaac-xauthority
-v "$worldgen_xauth:/tmp/isaac-xauthority:ro" -v /tmp/.X11-unix:/tmp/.X11-unix:rw)
fi
exec docker run --rm --user 0:0 --gpus all --shm-size=2g \
-e ACCEPT_EULA="$ACCEPT_EULA" -e PRIVACY_CONSENT=N -e NVIDIA_DRIVER_CAPABILITIES=all \
"${worldgen_desktop_args[@]}" \
-v "$worldgen_data_root/cache:/root/.cache:rw" \
-v "$worldgen_lab_root:/lab:ro" -v "$worldgen_data_root:/work:rw" \
--entrypoint /isaac-sim/python.sh nvcr.io/nvidia/isaac-sim:6.2.0 \
/lab/run.py --assets /work/assets --output "/work/$worldgen_scenario" --scenario "$worldgen_scenario" \
--allow-root --/omni/client/omniverse/use_omnihub=false --/renderer/multiGpu/enabled=false --/rtx/hydra/supportMultiTickRate=false "$@"
run.py
View files on GitHub"""Sensor navigation in the WorldGen scene.
Scene setup derives from the frozen worldgen-isaac lab (commit f035e0e).
The Navigator receives scan endpoints and ego pose, never obstacle transforms.
Run inside Isaac Sim 6.2, outside the WorldGen Python environment.
The visual shell is not a collider. The test floor and crate are explicit proxies.
"""
import argparse,json,math,os,time,traceback
from pathlib import Path
from importlib.metadata import version, PackageNotFoundError
p=argparse.ArgumentParser()
p.add_argument('--assets',type=Path,required=True);p.add_argument('--output',type=Path,required=True)
p.add_argument('--scenario',choices=['sensor','scripted'],default='sensor')
p.add_argument('--room-emission',type=float,default=400.);p.add_argument('--headless',action='store_true');p.add_argument('--collision-view',action='store_true')
p.add_argument('--raw-collider',action='store_true');p.add_argument('--no-floor',action='store_true');p.add_argument('--record-handshake',action='store_true')
p.add_argument('--layout',choices=['center','lower','upper'],default='center')
p.add_argument('--sensor-off',action='store_true')
p.add_argument('--max-seconds',type=float,default=30.)
p.add_argument('--hold-seconds',type=float,default=3.)
a,kit_args=p.parse_known_args();a.assets=a.assets.resolve();a.output=a.output.resolve();a.output.mkdir(parents=True,exist_ok=True)
(a.output/'FAILED').unlink(missing_ok=True)
from isaacsim import SimulationApp
app=SimulationApp({'headless':a.headless,'width':1280,'height':720,'window_width':1440,'window_height':900,
'renderer':'RaytracedLighting','anti_aliasing':2,'fast_shutdown':True})
failed=True
try:
import numpy as np
import carb,omni.usd,omni.physx
from pxr import Gf,Sdf,UsdGeom,UsdShade,UsdLux,UsdPhysics,PhysxSchema,PhysicsSchemaTools
from isaacsim.core.simulation_manager import SimulationManager
import isaacsim.core.experimental.utils.stage as stage_utils
from isaacsim.core.rendering_manager import RenderingManager, ViewportManager
from omni.kit.viewport.utility import get_active_viewport
settings=carb.settings.get_settings()
settings.set_bool('/physics/updateToUsd',True)
settings.set_bool('/rtx/hydra/readTransformsFromFabricInRenderDelegate',False)
settings.set_bool('/persistent/app/viewport/Viewport/Viewport0/scene/cameras/visible',False)
stage_utils.create_new_stage()
class SimulationLoop:
@property
def current_time(self):return SimulationManager.get_simulation_time()
def initialize(self):
SimulationManager.setup_simulation(dt=1/60)
SimulationManager.initialize_physics()
RenderingManager.set_dt(1/60)
def step(self,render=True):
SimulationManager.step()
if render:RenderingManager.render()
simulation=SimulationLoop()
stage=omni.usd.get_context().get_stage();UsdGeom.SetStageUpAxis(stage,'Z');UsdGeom.SetStageMetersPerUnit(stage,1.)
root=UsdGeom.Xform.Define(stage,'/World');stage.SetDefaultPrim(root.GetPrim())
manifest=json.loads((a.assets/'calibration.json').read_text())
config=manifest['physics_proxy'];robot_cfg=manifest['robot']
def xform(path,pos):
obj=UsdGeom.Xform.Define(stage,path);obj.AddTranslateOp().Set(Gf.Vec3d(*pos));return obj
def material(name,rgb,emission=0):
mat=UsdShade.Material.Define(stage,'/World/Looks/'+name);shader=UsdShade.Shader.Define(stage,mat.GetPath().AppendChild('Surface'))
shader.CreateIdAttr('UsdPreviewSurface');shader.CreateInput('diffuseColor',Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*rgb))
shader.CreateInput('roughness',Sdf.ValueTypeNames.Float).Set(.65)
shader.CreateInput('emissiveColor',Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*(np.array(rgb)*emission)))
mat.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(),'surface');return mat
def bind(obj,mat):UsdShade.MaterialBindingAPI.Apply(obj.GetPrim()).Bind(mat)
green=material('RobotGreen',(.30,.65,.025),.15);dark=material('Rubber',(.045,.05,.055),.05)
steel=material('Steel',(.35,.39,.42));orange=material('TestCrate',(.96,.34,.025),.1);gray=material('ProxyFloor',(.33,.40,.42),.1)
blue=material('Probe',(.03,.35,.95),.2);white=material('Ruler',(.94,.94,.85),.3)
def physics_material(name,friction):
mat=UsdShade.Material.Define(stage,'/World/Looks/'+name);api=UsdPhysics.MaterialAPI.Apply(mat.GetPrim())
api.CreateStaticFrictionAttr(friction);api.CreateDynamicFrictionAttr(friction);api.CreateRestitutionAttr(0.)
return mat
traction=physics_material('Traction',.9);caster_mat=physics_material('Caster',.015)
def box(path,pos,size,mat,collision=False):
o=UsdGeom.Cube.Define(stage,path);o.CreateSizeAttr(1.);o.AddTranslateOp().Set(Gf.Vec3d(*pos));o.AddScaleOp().Set(Gf.Vec3d(*size));bind(o,mat)
if collision:UsdPhysics.CollisionAPI.Apply(o.GetPrim())
return o
def rigid(obj,mass):
UsdPhysics.RigidBodyAPI.Apply(obj.GetPrim());UsdPhysics.MassAPI.Apply(obj.GetPrim()).CreateMassAttr(mass)
# The render mesh is a decimation of the generated RGB-distance shell.
mesh_data=np.load(a.assets/'room-mesh.npz');mesh=UsdGeom.Mesh.Define(stage,'/World/GeneratedRoom')
mesh.CreatePointsAttr(mesh_data['points']);mesh.CreateFaceVertexCountsAttr([3]*len(mesh_data['indices']));mesh.CreateFaceVertexIndicesAttr(mesh_data['indices'].reshape(-1))
mesh.CreateSubdivisionSchemeAttr('none');mesh.CreateDoubleSidedAttr(True)
UsdGeom.PrimvarsAPI(mesh).CreatePrimvar('st',Sdf.ValueTypeNames.TexCoord2fArray,UsdGeom.Tokens.vertex).Set(mesh_data['uv'])
mesh.CreateExtentAttr([Gf.Vec3f(*mesh_data['points'].min(0).tolist()),Gf.Vec3f(*mesh_data['points'].max(0).tolist())])
# Native OmniPBR avoids the PreviewSurface texture-graph translation on RTX.
room_mat=UsdShade.Material.Define(stage,'/World/Looks/Room')
room_shader=UsdShade.Shader.Define(stage,'/World/Looks/Room/Shader')
room_shader.CreateIdAttr('OmniPBR')
room_shader.GetImplementationSourceAttr().Set(UsdShade.Tokens.sourceAsset)
room_shader.SetSourceAsset(Sdf.AssetPath('OmniPBR.mdl'),'mdl')
room_shader.SetSourceAssetSubIdentifier('OmniPBR','mdl')
out=room_shader.CreateOutput('out',Sdf.ValueTypeNames.Token)
room_mat.CreateSurfaceOutput('mdl').ConnectToSource(out)
for name in ['diffuse_texture','emissive_color_texture']:
room_shader.CreateInput(name,Sdf.ValueTypeNames.Asset).Set(Sdf.AssetPath(str(a.assets/'panorama.jpg')))
room_shader.CreateInput('project_uvw',Sdf.ValueTypeNames.Bool).Set(False)
room_shader.CreateInput('texture_scale',Sdf.ValueTypeNames.Float2).Set(Gf.Vec2f(1,1))
room_shader.CreateInput('diffuse_color_constant',Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(1))
room_shader.CreateInput('reflection_roughness_constant',Sdf.ValueTypeNames.Float).Set(1.)
room_shader.CreateInput('enable_emission',Sdf.ValueTypeNames.Bool).Set(True)
room_shader.CreateInput('emissive_color',Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(1))
room_shader.CreateInput('emissive_intensity',Sdf.ValueTypeNames.Float).Set(a.room_emission)
bind(mesh,room_mat)
if a.raw_collider:
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
UsdPhysics.MeshCollisionAPI.Apply(mesh.GetPrim()).CreateApproximationAttr('none')
if a.collision_view:mesh.CreateVisibilityAttr('invisible')
if not a.no_floor:
floor=box('/World/ProxyFloor',config['floor_center'],config['floor_size'],gray,True)
UsdShade.MaterialBindingAPI.Apply(floor.GetPrim()).Bind(traction,materialPurpose='physics')
if not a.collision_view:floor.CreateVisibilityAttr('invisible')
obstacle_paths=['/World/TestCrate']
if a.layout!='center':
extra=box('/World/ExtraCrate',(0,-.95 if a.layout=='lower' else .95,.3),(.45,.60,.6),orange,True)
obstacle_paths.append('/World/ExtraCrate')
crate=box('/World/TestCrate',config['obstacle_center'],config['obstacle_size'],orange,True)
# One metre, in the chosen simulation scale, rather than a claim about the generated simulation.
box('/World/OneMetreBar',(0,.95,.06),(1,.015,.01),white)
for x in np.linspace(-.5,.5,11):box(f'/World/Tick{int(round((x+.5)*10))}',(float(x),.95,.065),(.01,.06 if abs(x)>.49 else .035,.01),white)
paths=[];drives=[]
if a.scenario=='probes':
for i,(x,y) in enumerate([(-1,-.7),(-1,.7),(1,-.7),(1,.7)]):
obj=xform(f'/World/Probe{i}',(x,y,.75));rigid(obj,.1)
if not a.headless:PhysxSchema.PhysxRigidBodyAPI.Apply(obj.GetPrim()).CreateDisableGravityAttr(True)
box(str(obj.GetPath())+'/Geometry',(0,0,0),(.1,.1,.1),blue,True);paths.append(str(obj.GetPath()))
else:
robot=xform('/World/Robot',(0,0,0));UsdPhysics.ArticulationRootAPI.Apply(robot.GetPrim())
PhysxSchema.PhysxArticulationAPI.Apply(robot.GetPrim()).CreateEnabledSelfCollisionsAttr(False)
start=np.array(manifest['route']['start']);chassis=xform('/World/Robot/Chassis',start);rigid(chassis,8.)
PhysxSchema.PhysxContactReportAPI.Apply(chassis.GetPrim()).CreateThresholdAttr(0)
box('/World/Robot/Chassis/Body',(0,0,0),robot_cfg['body_size'],green,True)
box('/World/Robot/Chassis/Top',(0,0,.13),(.38,.30,.08),steel)
box('/World/Robot/Chassis/Front',(.28,0,.035),(.012,.22,.04),white)
caster=UsdGeom.Sphere.Define(stage,'/World/Robot/Chassis/Caster');caster.CreateRadiusAttr(.05);caster.AddTranslateOp().Set(Gf.Vec3d(-.21,0,-.18));bind(caster,dark)
UsdPhysics.CollisionAPI.Apply(caster.GetPrim());UsdShade.MaterialBindingAPI.Apply(caster.GetPrim()).Bind(caster_mat,materialPurpose='physics')
for i,y in enumerate([.26,-.26]):
path=f'/World/Robot/Wheel{i}';local=np.array([.10,y,-.12]);wheel=xform(path,start+local);rigid(wheel,.3);PhysxSchema.PhysxContactReportAPI.Apply(wheel.GetPrim()).CreateThresholdAttr(0)
cylinder=UsdGeom.Cylinder.Define(stage,path+'/Tyre');cylinder.CreateAxisAttr('Y');cylinder.CreateRadiusAttr(.11);cylinder.CreateHeightAttr(.06);bind(cylinder,dark)
UsdPhysics.CollisionAPI.Apply(cylinder.GetPrim());UsdShade.MaterialBindingAPI.Apply(cylinder.GetPrim()).Bind(traction,materialPurpose='physics')
for j in range(3):
spoke=box(path+f'/Spoke{j}',(0,(-.031 if y<0 else .031),0),(.16,.006,.015),white)
spoke.AddRotateYOp().Set(j*60.)
joint=UsdPhysics.RevoluteJoint.Define(stage,f'/World/Robot/Joint{i}');joint.CreateAxisAttr('Y')
joint.CreateBody0Rel().SetTargets([chassis.GetPath()]);joint.CreateBody1Rel().SetTargets([wheel.GetPath()])
joint.CreateLocalPos0Attr(Gf.Vec3f(*local));joint.CreateLocalPos1Attr(Gf.Vec3f(0))
drive=UsdPhysics.DriveAPI.Apply(joint.GetPrim(),'angular');drive.CreateTypeAttr('force');drive.CreateStiffnessAttr(0);drive.CreateDampingAttr(20);drive.CreateMaxForceAttr(6);drive.CreateTargetVelocityAttr(0)
drives.append(drive)
paths=['/World/Robot/Chassis']
from isaacsim.sensors.experimental.physics import Raycast,RaycastSensor
from navigation import Navigator
angles=np.linspace(-math.pi,math.pi,180,endpoint=False)
scanner=RaycastSensor(Raycast.create('/World/Robot/Chassis/RangeSensor',
ray_origins=[[0.,0.,0.]]*len(angles),ray_directions=[[math.cos(t),math.sin(t),0.] for t in angles],
min_range=.05,max_range=2.5,output_frame='WORLD',translations=[[0.,0.,.25]]))
cap=UsdGeom.Cylinder.Define(stage,'/World/Robot/Chassis/SensorCap');cap.CreateAxisAttr('Z');cap.CreateRadiusAttr(.06);cap.CreateHeightAttr(.04);cap.AddTranslateOp().Set(Gf.Vec3d(0,0,.24));bind(cap,blue)
cyan=material('LiveSensor',(.015,.8,.85),.7)
def curve(path,mat,width):
c=UsdGeom.BasisCurves.Define(stage,path);c.CreateTypeAttr('linear');c.CreateWidthsAttr([width]);c.SetWidthsInterpolation('constant');bind(c,mat);return c
scan_lines=curve('/World/LiveScan',blue,.006);path_line=curve('/World/PlannedPath',cyan,.012)
goal_marker=UsdGeom.Cylinder.Define(stage,'/World/Goal');goal_marker.CreateRadiusAttr(.12);goal_marker.CreateHeightAttr(.015);goal_marker.AddTranslateOp().Set(Gf.Vec3d(1.3,0,.055));bind(goal_marker,green)
if a.scenario=='scripted':
scan_lines.CreateVisibilityAttr('invisible');path_line.CreateVisibilityAttr('invisible')
timing={'sensor_s':0.,'planner_s':0.,'draw_s':0.,'step_s':0.}
navigator=Navigator();scan_log=[];last_scan_time=-1e9;front_min=2.5
# Save the source scene before stepping. The exported state can be inspected in the GUI.
def aim(camera,eye,target):
matrix=Gf.Matrix4d(1).SetLookAt(Gf.Vec3d(*eye),Gf.Vec3d(*target),Gf.Vec3d(0,0,1)).GetInverse()
UsdGeom.Xformable(camera).AddTransformOp().Set(matrix)
camera=UsdGeom.Camera.Define(stage,'/World/FilmCamera');camera.GetPrim().ApplyAPI('OmniSensorAPI');aim(camera,(0,-1.8,1.35),(0,.6,.45));camera.CreateFocalLengthAttr(11);camera.CreateHorizontalApertureAttr(36);camera.CreateClippingRangeAttr(Gf.Vec2f(.03,120))
dome=UsdLux.DomeLight.Define(stage,'/World/Dome');dome.CreateIntensityAttr(450)
key=UsdLux.SphereLight.Define(stage,'/World/Light');key.AddTranslateOp().Set(Gf.Vec3d(-.3,-.5,2.1));key.CreateIntensityAttr(1800);key.CreateRadiusAttr(.7)
stage.Export(str(a.output/'scene.usdc'))
portable=Sdf.Layer.FindOrOpen(str(a.output/'scene.usdc'))
for name in ['diffuse_texture','emissive_color_texture']:
portable.GetAttributeAtPath('/World/Looks/Room/Shader.inputs:'+name).default=Sdf.AssetPath(os.path.relpath(a.assets/'panorama.jpg',a.output))
portable.Save()
simulation.initialize()
if not a.headless:
viewport=get_active_viewport();ViewportManager.set_camera(camera)
import asyncio
from omni.kit.viewport.utility import capture_viewport_to_file
from PIL import Image
for _ in range(8):
simulation.step()
capture=capture_viewport_to_file(viewport,str(a.output/'initial-rgb.png'))
async def wait_for_frame():
return await asyncio.wait_for(capture.wait_for_result(),timeout=120)
app.run_coroutine(wait_for_frame())
image_deadline=time.monotonic()+10
while True:
try:
with Image.open(a.output/'initial-rgb.png') as frame:
pixels=np.array(frame)
break
except (FileNotFoundError,OSError):
if time.monotonic()>image_deadline:raise
RenderingManager.render();time.sleep(.02)
(a.output/'initial-rgb.png').chmod(0o644)
assert pixels.ndim==3 and pixels[:,:,:3].mean()>5,'Native viewport remained black'
print('RENDER_CHECK',pixels.shape,float(pixels[:,:,:3].mean()),flush=True)
if a.scenario=='probes':
for _ in range(40):RenderingManager.render()
else:
for _ in range(40):simulation.step(render=True)
inspector=omni.physx.get_physx_interface()
def pose(path):
v=inspector.get_rigidbody_transformation(path);assert v['ret_val'],path
return list(v['position']),list(v['rotation'])
raycasts={}
for name,xy in [('floor_left',[-1,-1]),('floor_right',[1,-1]),('crate',[0,0])]:
hit=omni.physx.get_physx_scene_query_interface().raycast_closest((xy[0],xy[1],2.),(0.,0.,-1.),5.)
raycasts[name]={'hit':bool(hit['hit']),'collider':str(hit.get('collision','')),'position':list(hit.get('position',[]))}
contacts=[]
def on_contact(headers,data):
for header in headers:
actor0=str(PhysicsSchemaTools.intToSdfPath(header.actor0));actor1=str(PhysicsSchemaTools.intToSdfPath(header.actor1))
if any(path in actor0+actor1 for path in obstacle_paths) and 'Robot' in actor0+actor1:
contacts.append({'actor0':actor0,'actor1':actor1,'type':int(header.type),'time':simulation.current_time})
subscription=omni.physx.get_physx_simulation_interface().subscribe_contact_report_events(on_contact)
if a.record_handshake:
(a.output/'ready.json').write_text(json.dumps({'camera':str(camera.GetPath()),'application':'Isaac Sim 6.2 native window'}))
deadline=time.monotonic()+60
while not (a.output/'record-go').exists():
if time.monotonic()>deadline:raise TimeoutError('Native recorder did not start')
RenderingManager.render()
if a.scenario!='probes':
for _ in range(45):simulation.step(render=True)
print('NATIVE_GUI_RECORDING_STARTED',flush=True)
if a.scenario=='probes' and not a.headless:
for path in paths:PhysxSchema.PhysxRigidBodyAPI.Get(stage,path).GetDisableGravityAttr().Set(False)
samples=[];waypoints=manifest['route']['detour_waypoints'];waypoint=0
steps=int(a.max_seconds*60)
start_wall=time.monotonic();start_sim=simulation.current_time;reached=False;goal_time=None;phase='starting'
for step in range(steps):
position,rotation=pose(paths[0]);x,y,z=position;qx,qy,qz,qw=rotation
yaw=math.atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz))
elapsed=simulation.current_time-start_sim
if step%6==0 and not a.sensor_off:
tick=time.perf_counter()
frame=scanner.get_data()
depths=np.asarray(frame['depths']).reshape(-1).astype(float).tolist()
endpoints=np.asarray(frame['hit_positions']).reshape(-1,3).astype(float).tolist()
reading_time=float(frame['time'])
assert len(depths)==180 and all(math.isfinite(q) for q in depths),'Invalid range scan'
last_scan_time=simulation.current_time
# hit prim paths and scene objects are deliberately not passed to the planner.
timing['sensor_s']+=time.perf_counter()-tick;tick=time.perf_counter()
navigator.update((x,y,yaw),depths,endpoints,reading_time)
timing['planner_s']+=time.perf_counter()-tick;tick=time.perf_counter()
forward=[d for t,d in zip(angles,depths) if abs(t)<math.radians(25)]
front_min=min(forward)
scan_log.append({'sim_time_s':elapsed,'sensor_time_s':reading_time,'pose_xyyaw':[x,y,yaw],
'depths_m':depths,'hit_positions_world':endpoints,'front_min_m':front_min,
'planned_path_xy':navigator.path,'occupied_cells':len(navigator.hits)})
sensor_origin=[x,y,z+.25]
line_points=[]
for d,pt in zip(depths,endpoints):
if .05<=d<2.499:line_points.extend([sensor_origin,pt])
scan_lines.CreatePointsAttr([Gf.Vec3f(*pt) for pt in line_points]);scan_lines.CreateCurveVertexCountsAttr([2]*(len(line_points)//2))
path_points=[Gf.Vec3f(q[0],q[1],.075) for q in navigator.path]
path_line.CreatePointsAttr(path_points);path_line.CreateCurveVertexCountsAttr([len(path_points)] if len(path_points)>1 else [])
timing['draw_s']+=time.perf_counter()-tick
v=omega=0.
if a.scenario=='sensor':
v,omega,phase=navigator.command((x,y,yaw),front_min,simulation.current_time-last_scan_time)
reached=phase=='goal'
else:
distance=math.dist(waypoints[waypoint],(x,y))
if distance<.12:
if waypoint<len(waypoints)-1:waypoint+=1
else:reached=True
if not reached:
target=waypoints[waypoint];error=math.atan2(math.sin(math.atan2(target[1]-y,target[0]-x)-yaw),math.cos(math.atan2(target[1]-y,target[0]-x)-yaw))
omega=max(-1.1,min(1.1,2.8*error));v=min(.32,.9*distance)*max(0.,math.cos(error)) if abs(error)<.65 else 0.
phase='fixed-waypoints'
for drive,speed in zip(drives,[(v-.26*omega)/.11,(v+.26*omega)/.11]):
drive.GetTargetVelocityAttr().Set(math.degrees(speed))
tick=time.perf_counter();simulation.step(render=True);timing['step_s']+=time.perf_counter()-tick
if step%60==0:print('SIM_STEP',step,phase,front_min,position,flush=True)
if step%6==0:
samples.append({'step':step,'sim_time_s':simulation.current_time-start_sim,'wall_s':time.monotonic()-start_wall,
'poses':{path:pose(path) for path in paths},'command':[v,omega],'waypoint':waypoint if a.scenario=='scripted' else None,
'phase':phase,'front_min_m':front_min,'sensor_age_s':simulation.current_time-last_scan_time if last_scan_time>0 else None})
if reached:
if goal_time is None:goal_time=elapsed
if elapsed-goal_time>=a.hold_seconds:break
if not a.headless:time.sleep(max(0,(step+1)/60-(time.monotonic()-start_wall)))
for drive in drives:drive.GetTargetVelocityAttr().Set(0)
try: sim_version=version('isaacsim')
except PackageNotFoundError:
from isaacsim.core.version import get_version
sim_version=get_version()[0]
result={'status':'complete','scenario':a.scenario,'layout':a.layout,'sensor_off':a.sensor_off,'collision_view':a.collision_view,'no_floor':a.no_floor,'raw_collider':a.raw_collider,
'isaacsim_version':sim_version,'physics':'live PhysX; joint velocity drives; not a USD trajectory replay',
'control':'measured physics raycast scan → endpoint occupancy → inflation → A* → pose-feedback wheel control' if a.scenario=='sensor' else 'frozen detour waypoints',
'timing':timing,'sensor':{'class':'isaacsim.sensors.experimental.physics.RaycastSensor','rays':180,'max_range_m':2.5,'height_above_chassis_m':.25,'controller_read_hz':10,'scan_frames':len(scan_log),'replans':navigator.replans},
'navigation':{'obstacle_transforms_as_input':False,'known_floor_bounds':navigator.bounds,'inflation_radius_m':navigator.radius,'grid_resolution_m':navigator.resolution,'goal_xy':navigator.goal,'pose_source':'simulator ground truth; not SLAM or wheel odometry'},
'goal_reached_s':goal_time,'start_sim_s':start_sim,'duration_sim_s':simulation.current_time-start_sim,'duration_wall_s':time.monotonic()-start_wall,'reached_goal':reached,
'stage':{'up_axis':str(UsdGeom.GetStageUpAxis(stage)),'meters_per_unit':UsdGeom.GetStageMetersPerUnit(stage),'render_mesh_collision':mesh.GetPrim().HasAPI(UsdPhysics.CollisionAPI),'multitick_rendering':settings.get_as_bool('/rtx/hydra/supportMultiTickRate'),'room_emission_display_setting':a.room_emission,'proxy_floor':not a.no_floor},'raycasts':raycasts,'crate_contact_events':contacts,'samples':samples,'final_poses':{path:pose(path) for path in paths}}
(a.output/'scans.json').write_text(json.dumps(scan_log,separators=(',',':'))+'\n')
(a.output/'result.json').write_text(json.dumps(result,indent=2)+'\n')
print('RUN_RESULT',json.dumps({k:v for k,v in result.items() if k not in ['samples','crate_contact_events']}),flush=True)
failed=False
except BaseException:
traceback.print_exc()
finally:
if failed:(a.output/'FAILED').touch()
app.close()
if failed:raise SystemExit(1)
record_gui.py
View files on GitHub"""Record the real native Isaac Sim window while run.py performs live physics."""
import argparse,json,os,signal,subprocess,time
from pathlib import Path
p=argparse.ArgumentParser();p.add_argument('--work',type=Path,required=True)
p.add_argument('--scenario',choices=['sensor','scripted'],default='sensor')
p.add_argument('--ffmpeg',type=Path,required=True);p.add_argument('--collision-view',action='store_true')
p.add_argument('--layout',choices=['center','lower','upper'],default='center');p.add_argument('--max-seconds',default='40')
a=p.parse_args();root=Path(__file__).resolve().parent;a.work=a.work.resolve()
output=a.work/a.scenario;output.mkdir(parents=True,exist_ok=True)
assert not (output/'ready.json').exists(),'Use a new output folder for a fresh recording'
env=dict(os.environ,WORLDGEN_DATA_ROOT=str(a.work),DISPLAY=os.environ.get('DISPLAY',':1'),XAUTHORITY=os.environ.get('XAUTHORITY',f'/run/user/{os.getuid()}/gdm/Xauthority'))
command=['bash',str(root/'run.sh'),a.scenario,'--record-handshake','--layout',a.layout,'--max-seconds',a.max_seconds]+(['--collision-view'] if a.collision_view else [])
recorder=None
with (output/'simulator.log').open('w') as log:
process=subprocess.Popen(command,env=env,stdout=log,stderr=subprocess.STDOUT)
try:
deadline=time.monotonic()+240
while not (output/'ready.json').exists():
if process.poll() is not None:raise RuntimeError('Isaac Sim stopped before the viewport was ready; see simulator.log')
if time.monotonic()>deadline:raise TimeoutError('Isaac Sim startup timed out')
time.sleep(.3)
listing=subprocess.check_output(['xwininfo','-root','-tree'],env=env,text=True)
window=next(row.split()[0] for row in listing.splitlines() if ('"Isaac Sim' in row or '"Isaac-Sim' in row) and 'mutter-x11-frames' not in row)
metadata=subprocess.check_output(['xwininfo','-id',window],env=env,text=True)
(output/'window.txt').write_text(metadata)
with (output/'recorder.log').open('w') as recordlog:
recorder=subprocess.Popen([str(a.ffmpeg),'-nostdin','-hide_banner','-loglevel','warning','-y','-f','x11grab','-draw_mouse','0','-framerate','30','-window_id',str(int(window,16)),'-i',env['DISPLAY']+'.0','-an','-vf','scale=trunc(iw/2)*2:trunc(ih/2)*2','-c:v','libx264','-preset','ultrafast','-crf','18','-pix_fmt','yuv420p','-movflags','+faststart',str(output/'native-gui.mp4')],env=env,stdout=recordlog,stderr=subprocess.STDOUT)
time.sleep(.5);assert recorder.poll() is None,'X11 recorder could not start'
(output/'record-go').touch()
process.wait(timeout=300)
if (output/'FAILED').exists() or not (output/'result.json').exists():raise RuntimeError('Simulation failed; keep the logs for diagnosis')
finally:
if recorder and recorder.poll() is None:
recorder.send_signal(signal.SIGINT);recorder.wait(timeout=15)
if process.poll() is None:process.terminate()
print('Native GUI recording:',output/'native-gui.mp4')
package.py
View files on GitHub"""Package native GUI crops and an honest sequential controller comparison.
Requires ffmpeg and Pillow on the editing machine. All footage stays at 1×.
"""
import argparse,hashlib,json,subprocess,tempfile
from pathlib import Path
from PIL import Image,ImageDraw,ImageFont
p=argparse.ArgumentParser();p.add_argument('--source',type=Path,required=True);p.add_argument('--output',type=Path,required=True)
p.add_argument('--ffmpeg',default='ffmpeg');p.add_argument('--font',required=True);p.add_argument('--font-ko',required=True)
a=p.parse_args();a.output.mkdir(parents=True,exist_ok=True)
def ff(*args):subprocess.run([a.ffmpeg,'-hide_banner','-loglevel','error','-y',*map(str,args)],check=True)
def digest(path):return hashlib.sha256(path.read_bytes()).hexdigest()
records={}
for label,scenario,stem in [('lower','sensor','worldgen-navigation-lower'),('baseline','scripted','worldgen-navigation-fixed'),('upper','sensor','worldgen-navigation-upper'),('center','sensor','worldgen-navigation-center')]:
folder=a.source/label/scenario
assert 'Width: 1440' in (folder/'window.txt').read_text() and 'Height: 900' in (folder/'window.txt').read_text()
r=json.loads((folder/'result.json').read_text());duration=r['duration_wall_s']+.95
output=a.output/(stem+'.mp4')
ff('-ss','0.4','-i',folder/'native-gui.mp4','-t',duration,'-an','-vf','crop=952:536:50:64,scale=960:540',
'-c:v','libx264','-preset','slow','-crf','23','-pix_fmt','yuv420p','-movflags','+faststart',output)
ff('-ss','5','-i',output,'-frames:v','1',a.output/(stem+'.jpg'))
records[stem]={'source_native_sha256':digest(folder/'native-gui.mp4'),'crop_xywh':[50,64,952,536],
'trim_start_s':.4,'speed':1.,'duration_limit_s':duration,'simulation_s':r['duration_sim_s'],
'simulation_wall_s':r['duration_wall_s'],'goal_reached_s':r['goal_reached_s'],'published_sha256':digest(output)}
# The complete sensor take is shown after a 10 s excerpt of the fixed controller.
# A hard cut and persistent labels distinguish separate real simulator runs.
with tempfile.TemporaryDirectory() as directory:
temp=Path(directory)
for lang in ['en','ko']:
font=ImageFont.truetype(a.font if lang=='en' else a.font_ko,25)
labels=['Fixed waypoints | New box blocks the route','Range sensor + A* | Replans from measurements'] if lang=='en' else ['기존 경유점 제어 | 추가 상자에 막힘','거리 센서 + A* | 측정값으로 경로 재계산']
parts=[]
for i,(name,text) in enumerate(zip(['worldgen-navigation-fixed','worldgen-navigation-lower'],labels)):
badge=Image.new('RGBA',(960,48),(247,249,250,235));draw=ImageDraw.Draw(badge)
draw.rectangle((0,0,5,48),fill='#76b900');draw.text((17,9),text,font=font,fill='#1b252b')
png=temp/f'label-{lang}-{i}.png';badge.save(png)
part=temp/f'part-{lang}-{i}.mp4';parts.append(part)
options=['-i',a.output/(name+'.mp4'),'-i',png,'-filter_complex','[0:v][1:v]overlay=0:0:format=auto,format=yuv420p']
if i==0:options+=['-t','10']
ff(*options,'-an','-c:v','libx264','-preset','slow','-crf','23',part)
listing=temp/f'concat-{lang}.txt';listing.write_text(''.join(f"file '{part}'\n" for part in parts))
stem='worldgen-navigation' if lang=='en' else 'worldgen-navigation-ko'
output=a.output/(stem+'.mp4')
ff('-f','concat','-safe','0','-i',listing,'-c','copy','-movflags','+faststart',output)
ff('-ss','13','-i',output,'-frames:v','1',a.output/(stem+'.jpg'))
records[stem]={'edit':'10 s fixed-route excerpt, then complete sensor-lower viewport take; labeled hard cut',
'sources':['worldgen-navigation-fixed','worldgen-navigation-lower'],'speed':1.,'published_sha256':digest(output)}
(a.output/'navigation-edit.json').write_text(json.dumps(records,indent=2)+'\n')
print('Native crops and bilingual sequential comparison packaged at 1× speed.')
plot.py
View files on GitHub"""Visualize actual poses and measured hits after the run; no rendered footage."""
import argparse,json
from pathlib import Path
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from matplotlib.font_manager import FontProperties
p=argparse.ArgumentParser();p.add_argument('--evidence',type=Path,required=True);p.add_argument('--output',type=Path,required=True);p.add_argument('--font');a=p.parse_args();a.output.mkdir(parents=True,exist_ok=True)
if a.font:plt.rcParams['font.family']=FontProperties(fname=a.font).get_name()
plt.rcParams.update({'svg.fonttype':'path','axes.unicode_minus':False,'font.size':10})
for lang in ['en','ko']:
fig,axes=plt.subplots(1,3,figsize=(12,4.4),layout='constrained');fig.patch.set_facecolor('#f5f7f8')
for ax,case,label in zip(axes,['center','lower','upper'],['Central box','Lower route blocked','Upper route blocked'] if lang=='en' else ['중앙 상자','아래쪽 통로 막힘','위쪽 통로 막힘']):
folder=a.evidence/case;r=json.loads((folder/'result.json').read_text());scans=json.loads((folder/'scans.json').read_text())
ax.add_patch(Rectangle((-.225,-.4),.45,.8,color='#eba146',alpha=.8))
if case!='center':ax.add_patch(Rectangle((-.225,-1.25 if case=='lower' else .65),.45,.6,color='#eba146',alpha=.8))
hits=[pt for scan in scans[::10] for d,pt in zip(scan['depths_m'],scan['hit_positions_world']) if d<2.499]
ax.scatter([p[0] for p in hits],[p[1] for p in hits],s=7,c='#5295b7',label='Sensor hits' if lang=='en' else '센서 측정')
points=[s['poses']['/World/Robot/Chassis'][0] for s in r['samples']]
ax.plot([p[0] for p in points],[p[1] for p in points],c='#659f00',lw=2.6,label='Measured robot path' if lang=='en' else '실제 이동 경로')
if case=='lower':
old=json.loads((a.evidence/'baseline/result.json').read_text());points=[s['poses']['/World/Robot/Chassis'][0] for s in old['samples']]
ax.plot([p[0] for p in points],[p[1] for p in points],'--',c='#bc4f38',lw=2,label='Fixed route' if lang=='en' else '기존 제어')
ax.scatter([-1.3,1.3],[0,0],c=['#3b6d8e','#659f00'],s=30)
ax.set(xlim=(-1.75,1.75),ylim=(-1.6,1.6),title=label,xlabel='X (m)');ax.set_aspect('equal');ax.grid(alpha=.17)
for spine in ax.spines.values():spine.set_edgecolor('#d0d5da')
axes[0].set_ylabel('Y (m)');axes[1].legend(loc='lower center',fontsize=8)
fig.suptitle('Recorded sensor hits and physical chassis motion' if lang=='en' else '실제 센서 측정점과 몸체 이동 기록',fontsize=14)
path=a.output/f'paths.{lang}.svg';fig.savefig(path);plt.close(fig)
path.write_text('\n'.join(line.rstrip() for line in path.read_text().splitlines())+'\n')
test_navigation.py
View files on GitHub"""Behavior checks for planning from observations, independent of Isaac Sim."""
import math
import unittest
from navigation import Navigator
def observations(lower=False, upper=False):
# A dense synthetic scan of facing surfaces; scene geometry is never an API input.
points = [[-.225, y/100, .48] for y in range(-40, 41, 2)]
if lower:
points += [[-.225, y/100, .48] for y in range(-125, -64, 2)]
if upper:
points += [[-.225, y/100, .48] for y in range(65, 126, 2)]
return [math.dist((-1.3, 0), p[:2]) for p in points], points
class ScanPlanning(unittest.TestCase):
def test_new_observation_changes_route_without_waypoints(self):
nav = Navigator()
nav.update((-1.3, 0, 0), [2.5], [[1.2, 0, .48]], 1.)
self.assertTrue(all(abs(p[1]) < .01 for p in nav.path))
nav.update((-1.3, 0, 0), *observations(), 2.)
self.assertTrue(max(abs(p[1]) for p in nav.path) > .8)
self.assertTrue(all(nav.allowed(nav.cell(p)) for p in nav.path))
def test_side_is_determined_by_measured_obstruction(self):
lower, upper = Navigator(), Navigator()
lower.update((-1.3, 0, 0), *observations(lower=True), 1.)
upper.update((-1.3, 0, 0), *observations(upper=True), 1.)
self.assertGreater(max(p[1] for p in lower.path), .8)
self.assertLess(min(p[1] for p in upper.path), -.8)
def test_missing_or_stale_scan_stops(self):
nav = Navigator()
self.assertEqual(nav.command((-1.3, 0, 0), 2.5, 1.)[:2], (0., 0.))
nav.update((-1.3, 0, 0), [2.5], [[1.2, 0, .48]], 1.)
self.assertEqual(nav.command((-1.3, 0, 0), 2.5, .26)[:2], (0., 0.))
def test_complete_barrier_does_not_produce_a_path(self):
nav = Navigator()
points = [[-.225, y/100, .48] for y in range(-200, 201, 2)]
nav.update((-1.3, 0, 0), [1.]*len(points), points, 1.)
self.assertFalse(nav.path)
self.assertEqual(nav.command((-1.3, 0, 0), 1., 0.)[:2], (0., 0.))
if __name__ == '__main__':
unittest.main()
check.py
View files on GitHub"""Validate recorded sensor navigation without NumPy, CUDA or Isaac Sim."""
import argparse
import hashlib
import json
import math
from pathlib import Path
def check_run(folder):
r = json.loads((folder / 'result.json').read_text())
scans = json.loads((folder / 'scans.json').read_text())
assert r['status'] == 'complete' and r['isaacsim_version'].startswith('6.2')
assert r['stage']['meters_per_unit'] == 1 and r['stage']['up_axis'] == 'Z'
assert not r['stage']['render_mesh_collision'] and r['stage']['proxy_floor']
assert r['navigation']['obstacle_transforms_as_input'] is False
samples = r['samples']
path = '/World/Robot/Chassis'
positions = [s['poses'][path][0] for s in samples]
assert all(all(math.isfinite(v) for v in p) and .20 < p[2] < .28 for p in positions)
assert all(abs(p[0]) < 2.1 and abs(p[1]) < 1.6 for p in positions)
assert r['sensor']['scan_frames'] == len(scans)
for previous, current in zip(scans, scans[1:]):
assert current['sensor_time_s'] > previous['sensor_time_s']
assert abs(current['sim_time_s'] - previous['sim_time_s'] - .1) < 1e-5
for frame in scans:
assert len(frame['depths_m']) == len(frame['hit_positions_world']) == 180
assert all(math.isfinite(v) and .05 <= v <= 2.50001 for v in frame['depths_m'])
assert all(len(p) == 3 and all(math.isfinite(v) for v in p) for p in frame['hit_positions_world'])
final = r['final_poses'][path][0]
if r['sensor_off']:
assert not scans and not r['reached_goal']
assert max(math.dist(p[:2], positions[0][:2]) for p in positions) < .01
assert all(s['command'] == [0., 0.] for s in samples)
elif r['scenario'] == 'sensor':
assert len(scans) > 30 and r['reached_goal'] and not r['crate_contact_events']
assert math.dist(final[:2], (1.3, 0)) < .13
assert min(scans[0]['depths_m']) < 1.1
if r['layout'] == 'lower':
assert max(p[1] for p in positions) > .8 and min(p[1] for p in positions) > -.2
else:
assert min(p[1] for p in positions) < -.8
else:
assert r['layout'] == 'lower' and not r['reached_goal'] and r['crate_contact_events']
assert -.6 < final[0] < -.4 and final[1] < -.6
print(folder.name, r['scenario'], r['layout'], 'verified')
return r
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--run', type=Path)
args = parser.parse_args()
if args.run:
check_run(args.run)
return
folder = Path(__file__).resolve().parent
record = json.loads((folder / 'recorded.json').read_text())
for case in record['runs']:
check_run(folder / 'evidence' / case)
root = folder.parents[1]
for name, expected in record['files'].items():
data = (root / name).read_bytes()
assert len(data) == expected['bytes'] and hashlib.sha256(data).hexdigest() == expected['sha256'], name
print('Actual scans, route adaptation, contacts, sensor-off stop and media verified.')
if __name__ == '__main__':
main()




