움직이는 장애물을 보고 멈추고 다시 우회하기

주행 중 차단벽이 닫혔다 열립니다. 이전 지도에는 관측이 남아 막히지만, 새 제어는 거리 관측으로 지도를 갱신하고 다시 출발합니다. 실제 Isaac Sim GUI를 1배속으로 녹화했습니다.

  • WorldGen
  • Isaac Sim 6.2
  • Physics Raycast Sensor
  • A*
움직이는 장애물에 멈추고 다시 출발하기
실행 영상실행 영상과 기록

센서가 있어도 지도가 오래되면 길을 못 찾습니다

데이터는 어떻게 흐를까요?
  1. 입력거리와 측정 시각 함께 읽기거리[180] + 측정 지점[180,3] + 측정 시각

    로봇은 측정값과 자기 위치를 받습니다. 장애물의 이동 일정은 계획기에 넘기지 않습니다.

  2. 처리지도를 갱신하고 움직임 판단빈 곳 확인 → 오래된 관측 제거 → A* → 정지 판단

    통로가 다시 열릴 수 있습니다. 센서 데이터가 오래됐거나 경로가 없으면 바퀴를 멈춥니다.

  3. 출력경로를 다시 정해 실제로 이동새 경로 → 바퀴 드라이브 → 다음 위치 측정

    정지 유지와 우회 방향 변경, 충돌체와 표시 위치의 일치를 실제 기록으로 검사합니다.

앞선 제어기는 센서가 감지한 지점을 지도에 계속 추가했지만, 한번 넣은 지점을 지우지는 않았습니다. 장애물이 움직이고 나면 그 자리에 관측이 남습니다. 실제 통로는 열려 있어도 A*에는 벽이 남아 있는 셈입니다.

이번에는 Isaac Sim 6.2의 실제 Physics Raycast Sensor와 바퀴로 움직이는 로봇을 유지하면서, 새 관측으로 지도를 갱신합니다. 필요하면 멈춰 기다리고, 통로가 달라지면 경로를 다시 계산합니다. 로봇 제어기에 장애물의 좌표나 이동 일정은 전달하지 않습니다.

새 스캔에서 지도 갱신·정지 판단·바퀴 제어까지

1. 지금 보이는 표면과 예전에 본 지점을 구분합니다

입력은 여전히 수평 광선 180개의 거리입니다. 최대 2.5m까지 측정한 결과를 초당 10번 읽습니다. 지도에는 지점의 위치와 마지막으로 본 시각을 함께 저장합니다.

텍스트
ranges:        float[180]       이번 프레임의 거리, 단위 m
hit_positions: float[180,3]     광선이 닿은 지점의 월드 XYZ
sensor_time:   float            측정한 시뮬레이션 시각
observed[(i,j)] = (x, y, 마지막으로_본_시각)

예전에 본 지점이 지금 센서에서 어느 방향에 있는지 구하고, 가장 가까운 방향의 현재 광선과 비교합니다. 광선이 예전 지점보다 10cm 이상 더 멀리까지 뻗었다면, 그 지점은 더 이상 광선을 막지 않습니다. 해당 관측을 지도에서 지웁니다. 같은 프레임에서 실제로 감지한 지점은 우선해서 유지합니다.

아무것도 만나지 않은 광선은 거리 2.5를 돌려주지만, SDK의 해당 hit_position은 [0,0,0]입니다. 이 영벡터를 월드 원점에 있는 장애물로 취급하면 안 됩니다. 예전 지점의 방향과 현재 거리를 맞춰 비교해야 합니다.

2초 동안 다시 관측하지 못한 지점도 다음 유효 프레임을 처리할 때 지도에서 제거합니다. 작은 시험 환경에서 관측을 무한히 쌓지 않기 위한 설정입니다. 가려진 공간이 비었다는 증거는 아닙니다. 시험 바닥 안의 미관측 칸은 통행 가능으로 취급하며, 센서 데이터 자체가 오래되면 로봇을 멈춥니다.

2. 경로 계획과 정지 판단을 나눕니다

새 지도에 로봇 크기만큼 여유를 두고 A*를 실행합니다. 작은 격자 변화만으로 좌우 우회로가 계속 뒤집히지 않도록 직전 스캔에서 계산한 경로에서 멀어지는 후보에는 작은 비용을 더합니다. 실행 전에 경로를 입력하는 방식은 아닙니다. 기존 통로가 실제로 막히면 다른 경로를 찾아야 합니다.

그 다음에는 다음 조건으로 움직임을 제한합니다.

조건 바퀴에 내리는 명령
마지막 측정이 0.25초보다 오래됨 좌우 바퀴 정지
가장 가까운 측정 표면이 0.39m 미만 좌우 바퀴 정지
정면 장애물이 0.75m 안으로 들어옴 0.6초간 제동
통과 가능한 경로가 없음 전진·회전 명령을 모두 0으로 두고 대기
경로는 있지만 정면 거리가 0.58m 미만 전진 없이 회전
새 측정과 통과할 경로가 있음 계산한 경로 추종

정면은 ±25° 범위입니다. 정면 거리가 다시 0.95m보다 커지면 다음 제동을 감지할 수 있도록 조건을 초기화합니다. 이 로봇과 통제된 배치에서 정한 수치이며, 실제 로봇의 안전 인증이나 정지거리 보장값은 아닙니다.

3. 로봇이 실제로 회전하는 중심을 기준으로 제어합니다

구동 바퀴의 차축은 몸체 중심보다 0.10m 앞에 있습니다. 제자리 회전도 이 차축을 중심으로 일어나므로 몸체 중심은 조금 움직입니다. 새 제어기는 차축 중앙을 추종하고, 그 지점에서 본 로봇의 외접 반경 약 0.425m와 추종 여유를 합쳐 0.52m만큼 장애물을 넓힙니다. 이전 제어기는 몸체 중심과 기존의 0.43m 설정을 그대로 사용합니다.

물리적인 목적지는 여전히 몸체 XY [1.3,0]입니다. 새 제어기는 차축 XY [1.4,0]으로 접근한 뒤 +X 방향을 바라보게 맞춥니다. 차축 위치 허용 오차는 8cm, 최종 방향 허용 오차는 0.08rad입니다. 격자 경계에서 빠져나올 수 있도록 현재 시작 칸 하나는 확장 영역에서 해제할 수 있지만, 실제로 감지한 장애물 칸은 유지합니다.

텍스트
거리 프레임 + 현재 로봇 위치 + 측정 시각
  → 관측 지점 갱신·삭제·만료
  → 로봇 크기 고려 → 경로가 불필요하게 바뀌지 않도록 A* 계산
  → 정지 / 대기 / 회전 / 주행 / 도착 방향 맞추기
  → 바퀴 드라이브 → PhysX가 계산한 위치 → 다음 스캔

4. 장애물은 어떻게 움직일까요?

자주색 물체는 키네마틱 PhysX 강체입니다. actors.py가 물리 스텝마다 set_kinematic_targets()로 연속적인 목표 위치를 전달합니다. 센서는 그렇게 움직인 실제 충돌체를 읽고, 저장한 강체 위치로 목표를 따라 움직였는지 검사합니다. 로봇 몸체의 위치를 직접 바꾸지는 않으며, 로봇은 바퀴 드라이브로 움직입니다.

6.2 호환 렌더링 경로에서는 별도의 표시용 메시를 매 스텝 실제 PhysX 위치에 맞춰 갱신합니다. 이 표시용 메시에는 충돌체를 붙이지 않습니다. 요청한 목표 좌표나 녹화된 궤적을 재생하는 방식이 아닙니다. 검사기는 render_position_xyz와 실제 충돌체 위치가 같은지 확인합니다. 경로가 없을 때는 선을 숨겨, 대기 중인데 예전 청록색 경로가 남아 있지 않게 했습니다.

Isaac Sim GUI의 실제 차단벽·측정 광선·로봇

장면을 구동하는 코드는 이동 일정을 알지만, navigation.py는 그 코드를 가져오거나 장애물 좌표를 입력받지 않습니다. 주황색 점은 지도에 저장한 관측, 청록색 선은 현재 계산한 차축 경로입니다. NVIDIA 물리 텐서 API · Physics Raycast Sensor

처음이면 실습 파일부터 받기

GPU 실습은 Isaac Sim을 실행할 머신에서 터미널을 엽니다. CPU 실습은 지금 쓰는 컴퓨터에서 해도 됩니다. 아래 명령은 한 줄씩 입력하고 Enter를 누르세요. 여러 줄짜리 명령 블록은 통째로 복사해도 됩니다.

Shell
cd "$HOME"
git clone https://github.com/james-joobs/awesome-nvidia-physical-ai.git
cd awesome-nvidia-physical-ai
pwd

홈 폴더에 받는 예입니다. 이미 내려받았다면 git clone은 건너뛰고 내 실습 폴더로 이동합니다. 새 터미널을 열 때마다 pwd의 출력 끝이 awesome-nvidia-physical-ai인지 확인하세요. 그래야 labs/로 시작하는 파일을 찾을 수 있습니다.

닫히는 차단벽과 통로에 들어오는 상자를 실행합니다

1. 같은 로봇 실험실 준비하기

Docker에서 NVIDIA GPU를 사용할 수 있는 호환 Linux 머신을 준비합니다. 처음이라면 Linux 환경 설정과 WorldGen 공간 가져오기부터 확인하세요. 저장소 루트에서 입력합니다.

Shell
nvidia-smi
mkdir -p runs/worldgen-dynamic
cp -R labs/worldgen-isaac/assets runs/worldgen-dynamic/assets
export WORLDGEN_DATA_ROOT="$PWD/runs/worldgen-dynamic"

준비된 공간을 쓰므로 FLUX 가중치를 새로 받지 않습니다. NVIDIA Omniverse 약관에 동의한 뒤 본인의 동의를 나타내는 환경 변수를 설정합니다.

Shell
export ACCEPT_EULA=Y
bash labs/worldgen-dynamic-navigation/run.sh gate --controller dynamic

차단벽이 2~3.8초에 통로로 들어와 막고 있다가 8~9.8초에 빠져나옵니다. 프로그램에 지정한 최대 이동 속도는 1.5m/s입니다. 로봇이 멈추고 기다리다가, 센서 관측으로 지도가 갱신되면 다시 움직이는지 확인하세요. 결과는 runs/worldgen-dynamic/gate/에 저장됩니다.

2. 이전의 누적 지도 방식으로 반복하기

Shell
bash labs/worldgen-dynamic-navigation/run.sh gate --controller static \
  --output /work/gate-static --max-seconds 18

static_navigation.py는 앞선 제어기를 보존한 파일입니다. 시작점·물리적인 목적지·센서·로봇·움직이는 장애물·이동 일정과 최대 속도 0.32m/s가 같습니다. 이전 방식은 관측을 계속 쌓으며, 제어 기준점과 여유 반경도 기존 설정을 유지합니다. 차단벽이 물러난 뒤 지도 점과 로봇의 상태를 비교하세요.

컨테이너의 /work는 앞서 지정한 WORLDGEN_DATA_ROOT입니다. 실행마다 다른 출력 폴더를 쓰면 기록을 구분하기 쉽습니다.

3. 이미 주행하던 통로를 막아 보기

Shell
bash labs/worldgen-dynamic-navigation/run.sh cart --controller dynamic \
  --output /work/cart-dynamic --max-seconds 38

먼저 중앙의 고정 상자를 피해 우회합니다. 이어 크기 0.6m인 두 번째 상자가 2~4초에 Y=-2.7에서 Y=-0.95로 이동해 아래쪽 통로에 멈춥니다. 속도는 0.875m/s입니다. 첫 계획은 아래쪽을 향했고, 이 통로가 막히는 관측을 받은 뒤 다른 경로로 바뀌었는지 확인하세요. 우회 경유점 좌표는 로봇에 전달하지 않습니다.

4. 주행 중 거리 프레임을 끊어 보기

Shell
bash labs/worldgen-dynamic-navigation/run.sh gate --controller dynamic \
  --dropout-start 1 --dropout-seconds 1 \
  --output /work/sensor-gap

시험 코드가 시뮬레이션 시간 1초 동안 거리 프레임을 제어기에 전달하지 않습니다. 제어기는 마지막 측정의 시각만 알 수 있습니다. 정지한 뒤 새 프레임을 받으면 다시 움직이는지 보세요. 지도에서 오래된 칸을 지웠다는 이유로 센서 없이 주행하지는 않습니다.

5. 실제 프로그램 창 녹화하기

X11 데스크톱에 xwininfo와 x11grab을 지원하는 ffmpeg를 준비합니다.

Shell
mkdir -p runs/worldgen-dynamic-film
cp -R labs/worldgen-isaac/assets runs/worldgen-dynamic-film/assets
python3 labs/worldgen-dynamic-navigation/record_gui.py \
  --work "$PWD/runs/worldgen-dynamic-film" --scenario gate \
  --controller dynamic --max-seconds 32 --ffmpeg /path/to/ffmpeg

녹화할 때마다 새 폴더를 사용하세요. 실제 뷰포트가 준비되면 제어와 녹화를 시작합니다. 원본 창 영상, scans.json, result.json, 로그, 시작 장면 scene.usdc를 함께 남깁니다. USD는 장면 구성을 살펴보는 파일이며, 로봇 제어와 장애물 구동은 Python 스크립트가 담당합니다.

실제 정지·경로 변경·영상 기록을 검사합니다

차단벽 이동과 로봇 진행의 실제 기록

새 제어는 4.0~7.5초 검사 구간에서 실제 정지 상태를 유지했습니다. 아래쪽에 통과할 공간이 생긴 8.6초에 새 경로를 계산했습니다. 이전 제어는 차단벽이 완전히 빠져나간 9.8초 뒤에도 경로를 찾지 못했습니다. 마지막 스캔만 같은 계획기의 빈 지도에 넣으면 경로가 나옵니다. 물리적인 벽보다 지도에 쌓인 관측이 문제였음을 확인하는 검사입니다.

실행 도착 도착까지의 시뮬레이션 시간 장애물 접촉 보고
이전 누적 지도·차단벽 실패 — (18초 실행) 0
새 지도·차단벽 성공 20.07 0
새 지도·통로에 들어오는 상자 성공 25.32 0
새 지도·센서 프레임 1초 중단 성공 20.65 0

각각 한 번씩 기록한 실행이며 평균 성능이 아닙니다. 도착 시간에는 이후 약 2초의 정지 유지 시간을 포함하지 않았습니다. 실제 충돌체와 화면 표시 좌표의 차이도 검사했고, 기록에서는 0이었습니다.

첫 우회로가 막힌 뒤 실제로 이동한 경로

점선은 처음 센서로 계산한 차축 경로, 실선은 실제 몸체 이동입니다. 위쪽으로 돌아가는 첫 계획은 상자가 움직이기 시작한 2초 뒤인 3.1초에 나왔습니다. 4.3초에는 제동했고, 이후 다른 쪽으로 돌아 도착했습니다.

아래 기록에서는 지도 칸 하나를 지웠지만 아직 경로가 없어 기다립니다. 다음 기록에서는 경로 66개 지점을 찾아 전진 0.2811m/s와 회전 -1.1rad/s를 함께 명령합니다. 지도에서 칸을 지웠다고 바로 출발하지는 않습니다.

JSON
[
  {
    "scan_time_s": 8.1,
    "cleared_cells": 1,
    "path_points": 0,
    "phase": "wait-for-path",
    "command_v_omega": [
      0.0,
      0.0
    ]
  },
  {
    "scan_time_s": 8.6,
    "cleared_cells": 2,
    "path_points": 66,
    "phase": "follow-scan-path",
    "command_v_omega": [
      0.2811,
      -1.1
    ]
  }
]

실제 관측 예시 파일 · 두 배치의 시작 USD 장면

Shell
python3 labs/worldgen-dynamic-navigation/test_navigation.py
python3 labs/worldgen-dynamic-navigation/check.py

GPU 없이 실제 장애물의 이동, 측정 시각, 로봇의 정지 유지, 지도에서 지운 관측, 경로 변경, 도착 위치, 접촉 기록과 영상 해시를 확인합니다. 직접 실행한 결과도 검사할 수 있습니다.

Shell
python3 labs/worldgen-dynamic-navigation/check.py --run runs/worldgen-dynamic/cart-dynamic

비교 영상은 Isaac Sim GUI를 1배속으로 녹화했습니다. 이전 방식의 일부 구간과 새 방식의 실행을 구분해 표시했고, 개별 녹화도 아래에서 볼 수 있습니다.

현재 로봇 위치와 5×4m 시험 바닥 경계는 알고 있다고 가정합니다. 이상적인 수평 거리 센서를 사용하며, 생성된 가구 전체의 충돌까지 검증하지는 않았습니다. 장애물 속도나 사람의 다음 움직임을 예측하는 제어기, SLAM, 실물 로봇의 안전 검증으로 볼 수는 없습니다. 더 확장할 때는 Nav2의 장애물 표시·삭제 설정도 참고할 수 있습니다. 이번 실행에 Nav2를 사용한 것은 아닙니다.

이전 센서 실습 · 공간 크기·충돌체 · 영상·입력 출처

실행 영상과 기록

닫히는 차단벽 앞에서 기다렸다 다시 출발

움직이는 실제 충돌체가 통로를 막습니다. 바퀴 명령을 0으로 두고 대기하다가 새 관측으로 통과할 경로를 찾으면 다시 움직입니다.

주행하던 우회로에 상자가 들어오면

처음 센서로 계산한 경로는 고정 상자 아래쪽을 향합니다. 그 통로에 다른 상자가 들어오자 제동하고, 이후 계산한 경로로 반대편을 돌아 도착합니다.

이전 방식: 벽이 물러나도 지도에는 관측이 남습니다

같은 거리 센서와 움직이는 차단벽에 이전 제어기를 그대로 사용했습니다. 차단벽이 빠져나간 뒤에도 통과할 경로를 찾지 못해 정지해 있습니다.

실습 소스 코드

navigation.pyGitHub에서 파일 보기
Python
"""Refresh an observed obstacle map and supervise motion from range scans.

No obstacle poses, actor schedule, or simulator handles enter this module.
This is a small planar educational controller, not a safety-certified stack.
"""
import heapq
import math
from static_navigation import Navigator as StaticNavigator


class Navigator(StaticNavigator):
    memory_seconds = 2.0
    clear_margin = .10
    radius = .52  # 0.425 m axle-centered footprint plus tracking margin.
    axle_offset = .10

    def __init__(self, goal=(1.3, 0.)):
        self.body_goal = tuple(goal)
        # Desired final chassis heading is +X. Plan the actual rotation center.
        super().__init__((goal[0]+self.axle_offset, goal[1]))
        self.observed = {}
        self.depths = None
        self.front_min = self.nearest = 2.5
        self.brake_until = -1.
        self.brake_latched = False
        self.stats = {'cleared': 0, 'expired': 0, 'occupied': 0}

    def control_pose(self, pose):
        x, y, yaw = pose
        return (x+self.axle_offset*math.cos(yaw), y+self.axle_offset*math.sin(yaw), yaw)

    def update(self, pose, depths, endpoints, sim_time):
        if (not depths or len(depths) != len(endpoints)
                or not all(math.isfinite(v) and .05 <= v <= 2.5001 for v in depths)
                or self.last_time is not None and sim_time <= self.last_time):
            self.path = []
            return False
        new_hits = {self.cell(point): (point[0], point[1], sim_time)
                    for distance, point in zip(depths, endpoints)
                    if distance < 2.499 and all(math.isfinite(q) for q in point[:2])}
        x, y, yaw = pose
        cleared = expired = 0
        # Compare each old surface with the nearest current beam. A longer beam
        # observes free space through that old location. Miss positions are zero
        # in the SDK, so they must never be treated as a real XYZ measurement.
        for cell, (px, py, stamp) in list(self.observed.items()):
            if cell in new_hits:
                continue
            distance = math.hypot(px-x, py-y)
            angle = (math.atan2(py-y, px-x)-yaw+math.pi) % (2*math.pi)
            index = int(round(angle/(2*math.pi)*len(depths))) % len(depths)
            if distance + self.clear_margin < depths[index]:
                del self.observed[cell]
                cleared += 1
            elif sim_time-stamp > self.memory_seconds:
                del self.observed[cell]
                expired += 1
        self.observed.update(new_hits)
        self.hits = set(self.observed)
        # Reuse the baseline's inflation and A*, but replace its permanent map
        # with the updated observations before every plan.
        result = super().update(self.control_pose(pose), depths, endpoints, sim_time)
        self.depths = list(depths)
        n = len(depths)
        self.front_min = min(d for i, d in enumerate(depths)
                             if abs(-math.pi+2*math.pi*i/n) < math.radians(25))
        self.nearest = min(depths)
        if self.front_min < .75 and not self.brake_latched:
            self.brake_until = sim_time + .6
            self.brake_latched = True
        if self.front_min > .95:
            self.brake_latched = False
        self.stats = {'cleared': cleared, 'expired': expired, 'occupied': len(self.observed)}
        return result

    def plan(self, start):
        # Quantization/turning can put the robot's center one cell inside the
        # inflated margin. Permit leaving this known self cell, while retaining
        # actual measured occupied cells and every neighboring blocked cell.
        source = self.cell(start)
        if source not in self.hits:
            self.blocked.discard(source)
        goal = self.cell(self.goal)
        if not self.allowed(source) or not self.allowed(goal):
            return []
        # Prefer continuity with the last sensor-generated plan. This prevents
        # tiny scan/grid changes from flipping between equally short detours.
        history = [self.cell(point) for point in self.path]
        penalties = {}
        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(cell) for cell 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
                if neighbor not in penalties:
                    penalties[neighbor] = .08*min(10, min((abs(neighbor[0]-p[0])+abs(neighbor[1]-p[1]) for p in history), default=0))
                candidate = cost+math.hypot(dx,dy)+penalties[neighbor]
                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):
        if self.last_time is None or sensor_age > .25:
            return 0., 0., 'sensor-stop'
        now = self.last_time + sensor_age
        if self.nearest < .39:
            return 0., 0., 'close-obstacle-stop'
        if now < self.brake_until:
            return 0., 0., 'obstacle-brake'
        control = self.control_pose(pose)
        v, omega, phase = super().command(control, self.front_min, sensor_age)
        distance = math.dist(control[:2], self.goal)
        if phase == 'goal' and distance > .08:
            error = math.atan2(math.sin(math.atan2(self.goal[1]-control[1], self.goal[0]-control[0])-pose[2]),
                               math.cos(math.atan2(self.goal[1]-control[1], self.goal[0]-control[0])-pose[2]))
            v = min(.12, .9*distance)*max(0., math.cos(error)) if abs(error) < .65 else 0.
            omega, phase = max(-.8, min(.8, 2.8*error)), 'final-approach'
        elif phase == 'goal' and abs(pose[2]) > .08:
            v, omega, phase = 0., max(-.8, min(.8, -2*pose[2])), 'align-at-goal'
        if phase == 'no-path':
            return 0., 0., 'wait-for-path'
        if self.front_min < .58:
            return 0., omega, 'turn-clear' if abs(omega) > .05 else 'obstacle-stop'
        return v, omega, phase
static_navigation.pyGitHub에서 파일 보기
Python
"""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'
actors.pyGitHub에서 파일 보기
Python
"""Scenario actuator, intentionally independent of the sensor controller.

Continuous, finite-speed kinematic targets drive the obstacle in PhysX.
"""

def ramp(t, start, duration):
    return max(0., min(1., (t-start)/duration))


def position(scenario, time_s):
    if scenario == 'gate':
        y = 2.7 * (1-ramp(time_s, 2., 1.8)) + 2.7*ramp(time_s, 8., 1.8)
        return [.35, y, .3]
    return [.4, -2.7 + 1.75*ramp(time_s, 2., 2.), .3]


def size(scenario):
    return [.22, 3.4, .6] if scenario == 'gate' else [.6, .6, .6]
run.shGitHub에서 파일 보기
Shell
#!/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:-gate}"
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.pyGitHub에서 파일 보기
Python
"""Dynamic-obstacle navigation in the WorldGen scene.

Scene setup derives from the worldgen-navigation lab (commit 44d2fba).
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=['gate','cart'],default='gate')
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('--controller',choices=['static','dynamic'],default='dynamic')
p.add_argument('--dropout-start',type=float,default=-1.)
p.add_argument('--dropout-seconds',type=float,default=0.)
p.add_argument('--sensor-off',action='store_true')
p.add_argument('--max-seconds',type=float,default=32.)
p.add_argument('--hold-seconds',type=float,default=2.)
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 and not a.headless: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')
    import actors
    obstacle_paths=['/World/MovingObstacle']
    purple=material('MovingObstacle',(.65,.025,.38),.22)
    moving=xform('/World/MovingObstacle',actors.position(a.scenario,0.));rigid(moving,50.)
    UsdPhysics.RigidBodyAPI(moving.GetPrim()).CreateKinematicEnabledAttr(True)
    actor_collider=box('/World/MovingObstacle/Geometry',(0,0,0),actors.size(a.scenario),purple,True)
    actor_collider.CreateVisibilityAttr('invisible')
    # The compatibility renderer needs an explicit visual mirror for targets
    # driven through the physics tensor API. It follows measured solver state.
    display=xform('/World/MovingDisplay',actors.position(a.scenario,0.))
    display_translate=UsdGeom.Xformable(display).GetOrderedXformOps()[0]
    box('/World/MovingDisplay/Geometry',(0,0,0),actors.size(a.scenario),purple)
    if a.scenario=='cart':
        crate=box('/World/TestCrate',config['obstacle_center'],config['obstacle_size'],orange,True)
        obstacle_paths.append('/World/TestCrate')
    # 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
    if a.controller=='dynamic':
        from navigation import Navigator
    else:
        from static_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);c.CreateVisibilityAttr('invisible');return c
    scan_lines=curve('/World/LiveScan',blue,.006);path_line=curve('/World/PlannedPath',cyan,.012)
    map_points=UsdGeom.Points.Define(stage,'/World/ObservedMap');map_points.CreateWidthsAttr([.035]);map_points.SetWidthsInterpolation('constant');bind(map_points,orange);map_points.CreateVisibilityAttr('invisible')
    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)
    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()
    import omni.physics.tensors as tensors
    tensor_sim=tensors.create_simulation_view('numpy',stage_id=stage_utils.get_stage_id(stage))
    actor_view=tensor_sim.create_rigid_body_view('/World/MovingObstacle')
    assert actor_view.count==1
    actor_indices=np.array([0],dtype=np.uint32)
    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)
    paths.append('/World/MovingObstacle')
    samples=[];phase_events=[];cleared_total=expired_total=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
        actor_target=np.array([actors.position(a.scenario,elapsed)+[0.,0.,0.,1.]],dtype=np.float32)
        actor_view.set_kinematic_targets(actor_target,actor_indices)
        dropout=a.dropout_start<=elapsed<a.dropout_start+a.dropout_seconds
        if step%6==0 and not a.sensor_off and not dropout:
            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=reading_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)
            stats=getattr(navigator,'stats',{'cleared':0,'expired':0,'occupied':len(navigator.hits)})
            cleared_total+=stats['cleared'];expired_total+=stats['expired']
            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),'map_update':stats})
            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])
            if line_points:
                scan_lines.CreatePointsAttr([Gf.Vec3f(*pt) for pt in line_points]);scan_lines.CreateCurveVertexCountsAttr([2]*(len(line_points)//2));scan_lines.CreateVisibilityAttr('inherited')
            else:scan_lines.CreateVisibilityAttr('invisible')
            path_points=[Gf.Vec3f(q[0],q[1],.075) for q in navigator.path]
            if len(path_points)>1:
                path_line.CreatePointsAttr(path_points);path_line.CreateCurveVertexCountsAttr([len(path_points)]);path_line.CreateVisibilityAttr('inherited')
            else:path_line.CreateVisibilityAttr('invisible')
            cells=[Gf.Vec3f(*navigator.point(cell),.48) for cell in navigator.hits]
            if cells:
                map_points.CreatePointsAttr(cells);map_points.CreateExtentAttr([Gf.Vec3f(*[min(p[i] for p in cells)-.02 for i in range(3)]),Gf.Vec3f(*[max(p[i] for p in cells)+.02 for i in range(3)])]);map_points.CreateVisibilityAttr('inherited')
            else:map_points.CreateVisibilityAttr('invisible')
            timing['draw_s']+=time.perf_counter()-tick
        v=omega=0.
        sensor_age=simulation.current_time-last_scan_time
        v,omega,phase=navigator.command((x,y,yaw),front_min,sensor_age)
        reached=phase=='goal'
        if not phase_events or phase_events[-1]['phase']!=phase:
            phase_events.append({'time_s':elapsed,'phase':phase,'position_xyz':position,'front_min_m':front_min})
        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=False)
        actual_actor_position=pose('/World/MovingObstacle')[0]
        display_translate.Set(Gf.Vec3d(*actual_actor_position))
        if not a.headless:RenderingManager.render()
        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':None,'dropout_injected':dropout,'obstacle_target_xyz':actor_target[0,:3].tolist(),'render_position_xyz':list(display_translate.Get()),
                '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,'controller':a.controller,'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':'range sensor → refreshed occupancy → A* → stop supervisor → physical wheel drives' if a.controller=='dynamic' else 'previous static accumulating-map sensor controller',
        'moving_actor':{'type':'PhysX kinematic rigid body driven by set_kinematic_targets','size':actors.size(a.scenario),'schedule_passed_to_controller':False,'display':'non-colliding visual prim synchronized from actual PhysX pose every physics step','motion':'gate enters 2–3.8 s, withdraws 8–9.8 s' if a.scenario=='gate' else 'box enters the lower passage 2–4 s, then remains'},
        'dropout':{'start_s':a.dropout_start,'duration_s':a.dropout_seconds},'map_cleared_total':cleared_total,'map_expired_total':expired_total,
        '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,'body_goal_xy':[1.3,0.],
            'control_reference':'axle midpoint 0.10 m ahead of chassis' if a.controller=='dynamic' else 'chassis center',
            'desired_final_heading_rad':0. if a.controller=='dynamic' else None,'pose_source':'simulator ground truth; not SLAM or wheel odometry'},
        'phase_events':phase_events,'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.pyGitHub에서 파일 보기
Python
"""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=['gate','cart'],default='gate')
p.add_argument('--ffmpeg',type=Path,required=True);p.add_argument('--collision-view',action='store_true')
p.add_argument('--controller',choices=['static','dynamic'],default='dynamic');p.add_argument('--max-seconds',default='32')
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','--controller',a.controller,'--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.pyGitHub에서 파일 보기
Python
"""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 [('gate-dynamic','gate','worldgen-dynamic-navigation-gate'),('gate-static','gate','worldgen-dynamic-navigation-static'),('cart-dynamic','cart','worldgen-dynamic-navigation-cart')]:
    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 13 s excerpt of the previous 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=['Previous | Sensor + permanent obstacle map','Updated | Clear old hits, stop and replan'] if lang=='en' else ['이전 방식 | 센서 관측을 계속 쌓는 지도','새 방식 | 지도 갱신·정지·경로 재계획']
        parts=[]
        for i,(name,text) in enumerate(zip(['worldgen-dynamic-navigation-static','worldgen-dynamic-navigation-gate'],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','13']
            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-dynamic-navigation' if lang=='en' else 'worldgen-dynamic-navigation-ko'
        output=a.output/(stem+'.mp4')
        ff('-f','concat','-safe','0','-i',listing,'-c','copy','-movflags','+faststart',output)
        ff('-ss','19','-i',output,'-frames:v','1',a.output/(stem+'.jpg'))
        records[stem]={'edit':'13 s previous permanent-map excerpt, then complete refreshed-map gate take; labeled hard cut',
            'sources':['worldgen-dynamic-navigation-static','worldgen-dynamic-navigation-gate'],'speed':1.,'published_sha256':digest(output)}
(a.output/'dynamic-edit.json').write_text(json.dumps(records,indent=2)+'\n')
print('Native crops and bilingual sequential comparison packaged at 1× speed.')
plot.pyGitHub에서 파일 보기
Python
"""Plot recorded physical trajectories; these figures are not simulator footage."""
import argparse,json,math
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':11})
runs={name:json.loads((a.evidence/name/'result.json').read_text()) for name in ['gate-static','gate-dynamic','cart-dynamic']}

def save(fig,name):
    path=a.output/name;fig.savefig(path);fig.savefig(path.with_suffix(".png"),dpi=140);plt.close(fig)
    path.write_text('\n'.join(line.rstrip() for line in path.read_text().splitlines())+'\n')

for lang in ['en','ko']:
    fig,axes=plt.subplots(2,1,figsize=(8,6),layout='constrained',sharex=True);fig.patch.set_facecolor('#f5f7f8')
    new=runs['gate-dynamic'];samples=new['samples'];times=[s['sim_time_s'] for s in samples]
    axes[0].plot(times,[s['poses']['/World/MovingObstacle'][0][1] for s in samples],c='#a13286',lw=2)
    axes[0].set_ylabel('Gate Y (m)' if lang=='en' else '차단벽 Y (m)')
    axes[0].set_title('Actual gate position and robot progress' if lang=='en' else '실제 차단벽 위치와 로봇의 진행')
    for case,color,label in [('gate-static','#c27037','Previous map' if lang=='en' else '이전 지도'),('gate-dynamic','#659f00','Updated map' if lang=='en' else '새 지도')]:
        s=runs[case]['samples'];axes[1].plot([v['sim_time_s'] for v in s],[v['poses']['/World/Robot/Chassis'][0][0] for v in s],color=color,lw=2.2,label=label)
    axes[1].set(ylabel='Robot X (m)' if lang=='en' else '로봇 X (m)',xlabel='Simulation time (s)' if lang=='en' else '시뮬레이션 시간 (초)')
    axes[1].legend(loc='lower right')
    for ax in axes:
        ax.axvspan(3.8,8.,color='#a13286',alpha=.07);ax.axvline(9.8,color='#9dabb4',ls='--',lw=1);ax.grid(alpha=.16)
        for spine in ax.spines.values():spine.set_edgecolor('#ced6db')
    fig.suptitle('The route opens, but permanent map memory still blocks the old planner' if lang=='en' else '길은 열렸지만 이전 지도에는 장애물이 남습니다',fontsize=13)
    save(fig,f'gate.{lang}.svg')
    r=runs['cart-dynamic'];samples=r['samples'];scans=json.loads((a.evidence/'cart-dynamic/scans.json').read_text())
    fig,ax=plt.subplots(figsize=(7,6),layout='constrained');fig.patch.set_facecolor('#f5f7f8')
    ax.add_patch(Rectangle((-2.5,-2),5,4,facecolor='#fafbfc',edgecolor='#b7c4cb'))
    ax.add_patch(Rectangle((-.225,-.4),.45,.8,color='#e5a14d',alpha=.9))
    ax.add_patch(Rectangle((.1,-1.25),.6,.6,color='#a13286',alpha=.8))
    ax.add_patch(Rectangle((.1,-3),.6,.6,fill=False,edgecolor='#a13286',ls='--'))
    ax.annotate('',xy=(.4,-1.3),xytext=(.4,-2.35),arrowprops={'arrowstyle':'->','color':'#a13286','lw':2})
    initial=scans[0]['planned_path_xy'];ax.plot([p[0] for p in initial],[p[1] for p in initial],'--',c='#3b9cae',lw=1.4,label='First computed axle path' if lang=='en' else '처음 계산한 차축 경로')
    points=[s['poses']['/World/Robot/Chassis'][0] for s in samples]
    ax.plot([p[0] for p in points],[p[1] for p in points],c='#659f00',lw=2.6,label='Measured chassis path' if lang=='en' else '실제 몸체 이동')
    for target in [0.,3.,15.]:
        row=min(samples,key=lambda s:abs(s['sim_time_s']-target));x,y,_=row['poses']['/World/Robot/Chassis'][0]
        ax.scatter(x,y,c='#659f00',s=18);ax.annotate(f'{int(target)} s',(x,y),xytext=(-24,9),textcoords='offset points',fontsize=9)
    ax.scatter(1.3,0,c='#263b31',marker='x',s=60)
    ax.set(xlim=(-2.,2.),ylim=(-3.2,1.7),xlabel='X (m)',ylabel='Y (m)',title='A moving box closes the first detour' if lang=='en' else '움직이는 상자가 첫 우회로를 막습니다')
    ax.set_aspect('equal');ax.grid(alpha=.15);ax.legend(loc='upper right',fontsize=9)
    for spine in ax.spines.values():spine.set_edgecolor('#ced6db')
    save(fig,f'cart.{lang}.svg')
test_navigation.pyGitHub에서 파일 보기
Python
"""Sensor-map behavior checks with no simulator or GPU dependencies."""
import math
import unittest
from navigation import Navigator


def scan(pose, wall_x=None):
    depths, points = [], []
    for i in range(180):
        angle = pose[2]-math.pi+2*math.pi*i/180
        dx, dy = math.cos(angle), math.sin(angle)
        distance = (wall_x-pose[0])/dx if wall_x is not None and dx > .0001 else 2.5
        hit = .05 <= distance < 2.5 and abs(pose[1]+distance*dy) < 2.
        depths.append(distance if hit else 2.5)
        points.append([wall_x, pose[1]+distance*dy, .48] if hit else [0., 0., 0.])
    return depths, points


class DynamicMapping(unittest.TestCase):
    def test_opened_barrier_is_cleared_and_route_recovers(self):
        nav = Navigator();pose = (-1., 0., 0.)
        nav.update(pose, *scan(pose, 0.), 1.)
        self.assertFalse(nav.path)
        self.assertGreater(len(nav.hits), 10)
        nav.update(pose, *scan(pose), 1.1)
        self.assertTrue(nav.path)
        self.assertGreater(nav.stats['cleared'], 10)
        self.assertFalse(nav.hits)

    def test_closer_surface_does_not_clear_occluded_old_surface(self):
        nav = Navigator();pose = (-1., 0., 0.)
        nav.update(pose, *scan(pose, .3), 1.)
        original = set(nav.hits)
        nav.update(pose, *scan(pose, -.2), 1.1)
        self.assertTrue(original.intersection(nav.hits))
        nav.update(pose, *scan(pose, -.2), 3.2)
        self.assertGreater(nav.stats['expired'], 0)

    def test_stale_or_repeated_frame_stops(self):
        nav = Navigator();pose = (-1., 0., 0.)
        nav.update(pose, *scan(pose), 1.)
        self.assertGreater(nav.command(pose, 2.5, .0)[0], 0)
        self.assertEqual(nav.command(pose, 2.5, .26), (0., 0., 'sensor-stop'))
        self.assertFalse(nav.update(pose, *scan(pose), 1.))
        self.assertEqual(nav.command(pose, 2.5, .01)[:2], (0., 0.))

    def test_new_close_obstacle_brakes_both_wheels(self):
        nav = Navigator();pose = (-1., 0., 0.)
        nav.update(pose, *scan(pose, -.3), 1.)
        self.assertEqual(nav.command(pose, .7, 0), (0., 0., 'obstacle-brake'))
        self.assertEqual(nav.command(pose, .7, .1)[:2], (0., 0.))

    def test_robot_can_leave_a_quantized_inflation_margin(self):
        nav = Navigator(goal=(-1.3, 0.));pose = (-.43, 0., math.pi)
        nav.update(pose, *scan(pose, 0.), 1.)
        self.assertTrue(nav.path)
        self.assertTrue(all(p[0] < 0 for p in nav.path))
        self.assertGreater(nav.command(pose, 2.5, 0)[0], 0)

    def test_terminal_approach_uses_axle_offset(self):
        nav = Navigator();pose = (1.2, 0., 0.)
        nav.update(pose, *scan(pose), 1.)
        v, _, phase = nav.command(pose, 2.5, 0)
        self.assertGreater(v, 0)
        self.assertEqual(phase, 'final-approach')


if __name__ == '__main__':
    unittest.main()
check.pyGitHub에서 파일 보기
Python
"""Check actual actor motion, measured standstill and route changes without GPU."""
import argparse,hashlib,json,math,runpy
from pathlib import Path

ROBOT='/World/Robot/Chassis'
MOVER='/World/MovingObstacle'


def distance_to_box(point, center, size):
    return math.hypot(*[max(abs(point[i]-center[i])-size[i]/2,0) for i in (0,1)])


def metrics(result):
    samples=result['samples'];size=result['moving_actor']['size']
    clearance=min(distance_to_box(s['poses'][ROBOT][0],s['poses'][MOVER][0],size)-.36 for s in samples)
    error=max(math.dist(s['poses'][MOVER][0],s['obstacle_target_xyz']) for s in samples)
    display=max(math.dist(s['poses'][MOVER][0],s['render_position_xyz']) for s in samples)
    return {'minimum_mover_clearance_bound_m':clearance,'max_actor_target_error_m':error,'max_physics_display_error_m':display}


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['moving_actor']['schedule_passed_to_controller'] is False
    assert r['navigation']['obstacle_transforms_as_input'] is False
    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']
    samples=r['samples'];positions=[s['poses'][ROBOT][0] for s in samples]
    assert all(.20<p[2]<.28 and abs(p[0])<2.1 and abs(p[1])<1.6 for p in positions)
    assert not r['crate_contact_events'],'Unexpected robot-obstacle contact'
    m=metrics(r);assert m['max_actor_target_error_m']<.002
    assert m['max_physics_display_error_m']<.002
    assert m['minimum_mover_clearance_bound_m']>.015
    ys=[s['poses'][MOVER][0][1] for s in samples]
    assert max(ys)-min(ys)>1.7,'The collider did not actually move'
    max_speed=1.5 if r['scenario']=='gate' else .875
    for old,new in zip(samples,samples[1:]):
        dt=new['sim_time_s']-old['sim_time_s']
        speed=math.dist(old['poses'][MOVER][0],new['poses'][MOVER][0])/dt
        assert speed<max_speed+.02,'Actor jumped instead of following a finite-speed target'
    assert len(scans)==r['sensor']['scan_frames'] and len(scans)>50
    for old,new in zip(scans,scans[1:]):assert new['sensor_time_s']>old['sensor_time_s']
    for scan in scans:
        assert len(scan['depths_m'])==len(scan['hit_positions_world'])==180
        assert all(math.isfinite(d) and .05<=d<=2.5001 for d in scan['depths_m'])
    if r['controller']=='static':
        assert r['scenario']=='gate' and not r['reached_goal']
        late=[s for s in samples if s['sim_time_s']>11]
        assert late and all(s['command']==[0.,0.] for s in late)
        assert abs(late[-1]['poses'][MOVER][0][1]-2.7)<.001
        assert math.dist(late[0]['poses'][ROBOT][0],late[-1]['poses'][ROBOT][0])<.01
        assert all(not s['planned_path_xy'] for s in scans if s['sim_time_s']>11)
        # Replaying only the final observation into a fresh copy finds a path.
        # The permanent memory, rather than a remaining physical wall, blocks it.
        fresh_class=runpy.run_path(str(Path(__file__).with_name('static_navigation.py')))['Navigator']
        fresh=fresh_class();last=scans[-1]
        fresh.update(last['pose_xyyaw'],last['depths_m'],last['hit_positions_world'],last['sensor_time_s'])
        assert fresh.path,'The last scan must show a traversable physical passage'
    else:
        assert r['reached_goal'] and r['map_cleared_total']>20
        final,rotation=r['final_poses'][ROBOT]
        assert math.dist(final[:2],(1.3,0))<.12
        qx,qy,qz,qw=rotation;yaw=math.atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz))
        assert abs(yaw)<.09
        if r['scenario']=='gate':
            waiting=[s for s in samples if 4.0<s['sim_time_s']<7.5]
            assert len(waiting)>20 and all(s['command']==[0.,0.] for s in waiting)
            assert max(math.dist(s['poses'][ROBOT][0],waiting[0]['poses'][ROBOT][0]) for s in waiting)<.01
            assert any(s['sim_time_s']>8 and s['command'][0]>.05 for s in samples)
        else:
            assert min(p[1] for p in positions)<-.4 and max(p[1] for p in positions)>.8
            assert min(p[1] for p in scans[0]['planned_path_xy'])<-.8
            upper=min(s['sim_time_s'] for s in scans if s['planned_path_xy'] and max(p[1] for p in s['planned_path_xy'])>.8)
            assert upper>=2.,'The robot changed sides before the blocker began moving'
            assert any(s['phase']=='obstacle-brake' for s in samples)
        if r['dropout']['duration_s']:
            stopped=[s for s in samples if s['phase']=='sensor-stop']
            assert len(stopped)>=5 and all(s['command']==[0.,0.] and s['sensor_age_s']>.25 for s in stopped)
            assert max(math.dist(s['poses'][ROBOT][0],stopped[-1]['poses'][ROBOT][0]) for s in stopped[2:])<.01
    print(folder.name,r['scenario'],r['controller'],'verified',json.dumps(m))
    return r


def main():
    parser=argparse.ArgumentParser();parser.add_argument('--run',type=Path);a=parser.parse_args()
    if a.run:check_run(a.run);return
    folder=Path(__file__).resolve().parent;root=folder.parents[1]
    record=json.loads((folder/'recorded.json').read_text())
    for case in record['runs']:check_run(folder/'evidence'/case)
    # The baseline really is the previously published implementation.
    assert (folder/'static_navigation.py').read_bytes()==(root/'labs/worldgen-navigation/navigation.py').read_bytes()
    for name,meta in record['files'].items():
        data=(root/name).read_bytes();assert len(data)==meta['bytes'] and hashlib.sha256(data).hexdigest()==meta['sha256'],name
    print('Motion, stops, clearing, reactive replanning, sensor gap and native media verified.')


if __name__=='__main__':main()