로봇 경로 계획과 시연 생성

GR00T 집기 성공, cuRobo 장애물 우회 경로, AutoData 시연 재생, Newton에서 Microduck 보행 정책을 비교합니다.

  • Isaac Sim
  • cuRobo
  • AutoData
  • MimicGen
  • GR00T
  • Isaac Lab
  • Newton
로봇은 어떻게 움직일까
실행 영상실행 영상과 기록

동작 원리

데이터는 어떻게 흐를까요?
  1. 입력출발 자세, 목적지, 장애물관절 위치 + 목표 자세 + 장애물 상자

    출발과 목적지는 그대로 둡니다. 두 번째 계획에서만 경로 앞에 상자를 넣습니다.

  2. 처리팔이 갈 경로 계산cuRobo → 관절 위치의 순서

    cuRobo가 팔의 충돌 형상을 보며 목표 자세까지 갈 경로를 찾습니다.

  3. 출력두 경로를 비교motion.npz · result.json

    경로를 이루는 중간 지점과 팔 끝이 새 장애물 안을 지났는지 확인합니다.

팔 끝의 목표를 여러 관절의 경로로 바꾸기

목표는 팔 끝의 위치와 방향으로 정하고, 출발은 관절 값의 벡터로 정합니다. 로봇 모델을 통해 관절을 움직였을 때 팔 끝과 각 링크가 어디에 놓이는지 계산할 수 있습니다. cuRobo는 목표에 닿는 관절 자세와 도중에 충돌을 피할 경로를 찾습니다. 장애물을 모델에 추가하면 같은 목적지라도 허용되는 중간 자세가 달라집니다. cuRobo 공식 문서 · 이 실습의 계획 코드

출력 읽는 법
motion.npz의 baseline_q, obstacle_q 각 행이 경로의 한 시점입니다. 열은 로봇 모델 순서의 관절 값입니다.
baseline_tool, obstacle_tool 각 행은 팔 끝 위치 [x, y, z]입니다. 이 실습의 기준 경로는 24행, 우회 경로는 26행입니다.
result.json의 replanned_tool_enters_obstacle 저장된 팔 끝 표본이 새 장애물 상자 내부에 들어갔는지 알려줍니다. 팔 링크 전체의 충돌 판정은 플래너가 따로 수행합니다.

경로 계획과 시연 생성이 어디서 갈라지나요?

AutoData 실습은 별도로 실행합니다. MimicGen은 성공 시연에서 물체를 집고 놓는 구간을 재사용해 새로운 물체 배치에 맞게 시연을 만듭니다. 시뮬레이터에서 실행해 성공한 시연을 남깁니다. 기록의 num_attempts: 6, num_success: 3은 이 장면에서 생성한 시도와 성공 시연의 개수입니다. demo_0의 first_success_frame: 243은 성공을 처음 기록한 프레임이며 관절 경로의 길이와는 다른 지표입니다. MimicGen 공식 프로젝트

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

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. Isaac Sim에서 이미 만든 두 경로 보기

cuRobo 경로 장면을 File → Open으로 엽니다. 카메라를 FilmCamera로 바꾸고 Play를 누릅니다. 같은 목표를 향하는 두 경로 중 장애물을 비켜 가는 쪽을 찾아보세요. 이 단계는 경로 계산 환경 없이도 장면을 열 수 있습니다.

2. 터미널에서 새 경로 계산하기

RTX 머신에 cuRobo V2 71ce172 환경을 준비합니다. 그 Python이 활성화된 터미널에서 이 레포로 이동하고 아래를 실행합니다. motion.npz에는 경로 좌표 배열이, result.json에는 비교 결과가 남습니다.

Shell
python3 labs/cli.py --json doctor curobo-replan --python /path/to/curobo-env/bin/python
python3 labs/cli.py --json run curobo-replan \
  --python /path/to/curobo-env/bin/python --output ./runs/curobo-001
python3 labs/cli.py --json verify ./runs/curobo-001

Python 경로를 내 cuRobo 환경의 경로로 바꿔 넣으세요. 실행할 때마다 새 결과 폴더를 지정하면 해당 시도의 명령어와 측정값이 함께 보존됩니다. 실습 CLI 안내

3. 방금 계산한 경로를 장면으로 보기

NumPy·Pillow·usd-core가 설치된 Python으로 새 경로 배열을 USD 장면으로 만듭니다. 화면에 보이는 구들은 cuRobo가 팔의 충돌을 검사할 때 사용하는 형상입니다. 타임라인을 움직이면 저장된 위치를 따라 움직입니다.

Shell
python labs/robot-motion/make_scene.py ./runs/curobo-001 ./runs/curobo-scene-001

Isaac Sim에서 curobo-scene-001/curobo-replan.usda를 열고 FilmCamera를 선택합니다. 왼쪽은 기존 경로, 오른쪽은 빨간 장애물을 반영한 경로입니다. 로봇 정책을 새로 실행하는 화면이 아니라 계산된 충돌 형상을 재생합니다. Linux X11에서 Isaac Lab 환경으로 실제 GUI를 녹화하려면 다음 명령을 사용합니다.

Shell
python labs/physics/capture_gui.py ./runs/curobo-scene-001 ./runs/curobo-gui-001 \
  /path/to/ffmpeg --plan ./runs/curobo-scene-001/takes.json --viz kit

4. 새 시연 데이터가 필요하면 AutoData 실행하기

먼저 AutoData 공식 예제에서 4a3d1f3과 Isaac Lab 서브모듈, 동작 구간을 표시한 Franka HDF5를 준비합니다. HDF5는 여러 시도의 관절 값과 물체 위치를 한 파일에 담습니다. AutoData 환경을 켠 터미널에서 첫 두 줄을 내 경로로 바꾸고 실행하세요.

Shell
AUTODATA_DIR="$HOME/AutoData"
DEMO_FILE="$HOME/dataset_franka_annotated.hdf5"
bash labs/robot-motion/autodata.sh "$AUTODATA_DIR" "$DEMO_FILE" ./runs/autodata
python labs/robot-motion/audit.py ./runs/autodata/franka-generated.hdf5 ./runs/autodata-audit

AutoData 재생 장면으로 시연을 먼저 살펴봐도 됩니다. GR00T 집기·놓기 · Microduck 보행

테스트 결과

기존 경로는 끝단 중심이 새 장애물 안을 지났고, 재계획한 경로는 우회했습니다. 경유점은 24개와 26개였습니다. AutoData는 여섯 번 시도해 성공 시연 세 개를 생성했고, 재생에서도 세 개가 성공했습니다. HDF5 검사 코드에서 시연별 첫 성공 프레임을 확인할 수 있습니다. 촬영 당시 결과

11월 19일 재실행 기록에서는 저장된 경유점마다 팔 전체의 유효한 충돌 구체도 검사했습니다. 장애물과 가장 가까운 구체의 간격은 약 2.17mm였고, 두 경로의 최종 팔 끝 위치는 같았습니다. 경유점 사이의 충돌은 플래너가 별도로 검사합니다. 기존 경로와 재계획은 워밍업 조건이 달라서 두 실행 시간을 속도 비교로 쓰지는 않습니다.

실행 영상과 기록

같은 목적지, 새로 생긴 장애물

11월 19일 GPU에서 다시 계산한 경로입니다. 팔의 충돌 형상을 Isaac Sim에서 재생하며, 저장된 구체의 최소 간격은 약 2.17mm입니다.

실습 소스 코드

plan.pyGitHub에서 파일 보기
Python
import argparse
import json
import time
from pathlib import Path

import numpy as np
import torch

from curobo.examples.getting_started.motion_planning import _prepare_interpolated_trajectory
from curobo.motion_planner import MotionPlanner, MotionPlannerCfg
from curobo.scene import Cuboid, Scene
from curobo.types import JointState

def main():
    parser = argparse.ArgumentParser(description="Compare Franka motion plans before and after adding a cuboid.")
    parser.add_argument("output", type=Path)
    out = parser.parse_args().output
    out.mkdir(parents=True, exist_ok=True)
    table = Cuboid(name="workbench", pose=[0.0, 0.0, -0.055, 1., 0., 0., 0.], dims=[1.2, 0.75, 0.10])
    config = MotionPlannerCfg.create(
        robot="franka.yml",
        scene_model=Scene(cuboid=[table]),
        collision_cache={"cuboid": 4},
        graph_planner_algorithm="rrt",
        enable_trajopt=True,
    )
    planner = MotionPlanner(config)
    print(f"cuRobo | {torch.cuda.get_device_name(0)} | Franka Panda", flush=True)
    planner.warmup(enable_graph=True, num_warmup_iterations=2)
    start = JointState.from_position(planner.default_joint_state.position.unsqueeze(0), joint_names=planner.joint_names)
    goal_joints = start.clone()
    goal_joints.position[:, 0] += 1.60
    goal = planner.compute_kinematics(goal_joints).tool_poses.as_goal()

    def plan():
        planner.reset_seed()
        start_time = time.monotonic()
        result = planner.plan_pose(goal, start.clone(), max_attempts=8, enable_graph_attempt=0)
        torch.cuda.synchronize()
        ms = 1000 * (time.monotonic() - start_time)
        if result is None or not bool(result.success.any()):
            return None, ms
        return _prepare_interpolated_trajectory(result.get_interpolated_plan()), ms

    def kinematics(trajectory):
        points = planner.kinematics.get_active_js(trajectory).position[0].contiguous()
        state = planner.compute_kinematics(JointState.from_position(points, joint_names=planner.joint_names))
        tool = state.tool_poses.position.detach().cpu().numpy().reshape(len(points), -1, 3)[:, 0]
        spheres = state.robot_spheres.detach().cpu().numpy().reshape(len(points), -1, 4)
        return points.detach().cpu().numpy(), tool, spheres

    baseline, base_ms = plan()
    if baseline is None:
        raise RuntimeError("Baseline path failed dense validation")
    base_q, base_tool, base_spheres = kinematics(baseline)
    print(f"Baseline: validated | {len(base_tool)} waypoints | {base_ms:.1f} ms", flush=True)
    attempts = [
        (0.50, [0.04, 0.04, 0.08]),
        (0.57, [0.04, 0.04, 0.065]),
        (0.41, [0.035, 0.035, 0.065]),
        (0.50, [0.026, 0.026, 0.043]),
    ]
    for fraction, dims in attempts:
        center = base_tool[int((len(base_tool) - 1) * fraction)].copy()
        end_spheres = np.concatenate((base_spheres[0], base_spheres[-1]), axis=0)
        outside_box = np.maximum(np.abs(end_spheres[:, :3] - center) - np.asarray(dims) / 2, 0)
        if (np.linalg.norm(outside_box, axis=1) - end_spheres[:, 3] < 0.007).any():
            continue
        blocker = Cuboid(name="new_obstacle", pose=[*center.tolist(), 1., 0., 0., 0.], dims=dims)
        planner.update_world(Scene(cuboid=[table, blocker]))
        result, obstacle_ms = plan()
        print(f"Obstacle at {center.tolist()} | {dims} m | {'validated' if result is not None else 'no valid path'}", flush=True)
        if result is None:
            continue
        obstacle_q, obstacle_tool, obstacle_spheres = kinematics(result)
        is_inside = (np.abs(obstacle_tool - center) < np.asarray(dims) / 2).all(axis=1)
        if not is_inside.any():
            break
    else:
        raise RuntimeError("No obstacle case passed the planner and tool-center checks")
    assert np.allclose(base_q[0], obstacle_q[0], atol=1e-3)
    if not np.allclose(base_tool[-1], obstacle_tool[-1], atol=0.005):
        raise RuntimeError("Replanning changed the Cartesian goal by more than 5 mm")
    active = obstacle_spheres[..., 3] > 0
    distance_to_box = np.linalg.norm(
        np.maximum(np.abs(obstacle_spheres[..., :3] - center) - np.asarray(dims) / 2, 0), axis=-1
    ) - obstacle_spheres[..., 3]
    minimum_clearance = float(distance_to_box[active].min())
    if minimum_clearance < -0.003:
        raise RuntimeError(f"A sampled robot collision sphere overlaps the new obstacle: {minimum_clearance} m")
    np.savez_compressed(
        out / "motion.npz", baseline_tool=base_tool, obstacle_tool=obstacle_tool,
        baseline_spheres=base_spheres, obstacle_spheres=obstacle_spheres, baseline_q=base_q, obstacle_q=obstacle_q,
    )
    report = {
        "planner": "cuRoboV2 Franka Panda",
        "same_start_and_goal": True,
        "baseline_waypoints": len(base_tool),
        "obstacle_waypoints": len(obstacle_tool),
        "baseline_wall_ms": round(base_ms, 2),
        "obstacle_wall_ms": round(obstacle_ms, 2),
        "obstacle_center_m": center.tolist(),
        "obstacle_dimensions_m": dims,
        "baseline_tool_enters_new_obstacle": bool((np.abs(base_tool - center) < np.asarray(dims)/2).all(axis=1).any()),
        "replanned_tool_enters_obstacle": bool(is_inside.any()),
        "goal_difference_m": float(np.linalg.norm(base_tool[-1] - obstacle_tool[-1])),
        "sampled_robot_sphere_min_clearance_m": minimum_clearance,
        "clearance_scope": "All active collision spheres at stored trajectory samples; planner performs its own swept validation",
    }
    (out / "result.json").write_text(json.dumps(report, indent=2) + "\n")
    print(json.dumps(report, indent=2), flush=True)


if __name__ == "__main__":
    main()
autodata.shGitHub에서 파일 보기
Shell
#!/usr/bin/env bash
set -euo pipefail
if [[ $# != 3 ]]; then
  printf 'Usage: bash %s AUTODATA_CHECKOUT ANNOTATED_HDF5 OUTPUT_DIRECTORY\n' "$0" >&2
  exit 2
fi

checkout="$(cd "$1" && pwd -P)"
input="$(cd "$(dirname "$2")" && pwd -P)/$(basename "$2")"
mkdir -p "$3"
output="$(cd "$3" && pwd -P)"
test -s "$input"
cd "$checkout"

task=IsaacContrib-Stack-Cube-Franka-IK-Rel
python scripts/generate_dataset.py --viz kit --env_name "$task" --alg mimicgen \
  --generation_num_trials 3 --num_envs 3 \
  --task_descriptor autodata_examples/tasks/franka_cube_stack.yaml \
  --embodiment autodata_examples/embodiments/franka_ik_rel.yaml \
  --input_file "$input" --output_file "$output/franka-generated.hdf5" \
  --result_file "$output/run-result.json" --record_metric_inputs
python scripts/validate_dataset.py "$output/franka-generated.hdf5"
python submodules/IsaacLab-Arena/submodules/IsaacLab/scripts/tools/replay_demos.py \
  --viz kit --task "$task" --num_envs 1 \
  --dataset_file "$output/franka-generated.hdf5" --validate_success_rate
audit.pyGitHub에서 파일 보기
Python
"""Measure target tracking and completion frames in exported AutoData episodes."""

import argparse
import json
from pathlib import Path

import h5py
import numpy as np


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("dataset", type=Path)
    parser.add_argument("output", type=Path)
    args = parser.parse_args()
    args.output.mkdir(parents=True, exist_ok=True)
    episodes = []
    with h5py.File(args.dataset, "r") as stream:
        group = stream["data"]
        for name, sample in sorted(group.items()):
            obs = sample["obs"]
            pose = obs["datagen_info/eef_pose/franka"][:]
            target = obs["datagen_info/target_eef_pose/franka"][:]
            wrist = pose[:, :3, 3]
            desired = target[:, :3, 3]
            error = np.linalg.norm(wrist - desired, axis=1)
            grip = np.sum(obs["gripper_pos"][:], axis=1)
            cubes = obs["cube_positions"][:].reshape(len(wrist), 3, 3)
            successful = sample["step_success"][:].astype(bool)
            count = len(wrist)
            assert pose.shape == target.shape == (count, 4, 4)
            assert count == len(sample["actions"]) == len(grip) == len(successful) == sample.attrs["num_samples"]
            assert np.isfinite(error).all() and np.isfinite(grip).all() and np.isfinite(cubes).all()
            found = np.flatnonzero(successful)
            target_tracking = {"mean_error_mm": float(error.mean()*1000),
                               "p95_error_mm": float(np.percentile(error, 95)*1000),
                               "peak_error_mm": float(error.max()*1000)}
            row = {"episode": name, "recorded_frames": count, "episode_marked_success": bool(sample.attrs["success"]),
                   "first_success_frame": None if not len(found) else int(found[0]),
                   "positive_success_frames": int(len(found)), "tracking": target_tracking,
                   "wrist": wrist.tolist(), "target": desired.tolist(), "error_mm": (error*1000).tolist(),
                   "gripper_position_sum": grip.tolist(), "cube_heights_m": cubes[:, :, 2].tolist()}
            episodes.append(row)
            print(f"{name} | {count} frames | first success {row['first_success_frame']} | p95 target error {target_tracking['p95_error_mm']:.1f} mm", flush=True)
        assert sum(row["recorded_frames"] for row in episodes) == group.attrs["total"]
    (args.output / "result.json").write_text(json.dumps({
        "source": args.dataset.name, "frame_index_origin": 0,
        "tracking": "Euclidean distance between recorded datagen wrist and target translations within each sampled frame",
        "episodes": episodes,
    }, indent=2) + "\n")


if __name__ == "__main__":
    main()
make_scene.pyGitHub에서 파일 보기
Python
"""Build a cuRobo collision-geometry replay from a newly computed motion.npz.

CPU authoring only: requires numpy, usd-core and Pillow. This does not execute
Franka dynamics or a policy, and it does not use a full Franka visual mesh.
"""

import argparse
import hashlib
import json
from pathlib import Path

import numpy as np
from PIL import Image, ImageDraw, ImageFont
from pxr import Gf, Sdf, Usd, UsdGeom, UsdLux, UsdShade, Vt

FPS, END, LEAD, FINISH = 30, 300, 26, 234
COLORS = {
    "robot": (0.64, 0.70, 0.75), "base": (0.10, 0.14, 0.17),
    "baseline": (0.92, 0.49, 0.06), "obstacle": (0.35, 0.65, 0.03),
    "red": (0.82, 0.10, 0.07), "goal": (0.05, 0.42, 0.66),
    "table": (0.89, 0.91, 0.92), "floor": (0.97, 0.98, 0.98),
}


def sha256(path):
    return hashlib.sha256(path.read_bytes()).hexdigest()


def material(stage, name, color):
    mat = UsdShade.Material.Define(stage, "/World/Materials/" + name)
    shader = UsdShade.Shader.Define(stage, mat.GetPath().AppendChild("Surface"))
    shader.CreateIdAttr("UsdPreviewSurface")
    shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*color))
    shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.42)
    mat.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface")
    return mat


def bind(geometry, mat):
    UsdShade.MaterialBindingAPI.Apply(geometry.GetPrim()).Bind(mat)


def box(stage, path, position, size, mat):
    shape = UsdGeom.Cube.Define(stage, path)
    shape.CreateSizeAttr(1)
    shape.AddTranslateOp().Set(Gf.Vec3d(*map(float, position)))
    shape.AddScaleOp().Set(Gf.Vec3d(*map(float, size)))
    bind(shape, mat)
    return shape


def sphere(stage, path, position, radius, mat):
    shape = UsdGeom.Sphere.Define(stage, path)
    shape.CreateRadiusAttr(float(radius))
    transform = shape.AddTranslateOp()
    transform.Set(Gf.Vec3d(*map(float, position)))
    bind(shape, mat)
    return transform


def curve(stage, path, points, width, mat):
    shape = UsdGeom.BasisCurves.Define(stage, path)
    shape.CreateTypeAttr(UsdGeom.Tokens.linear)
    shape.CreateWrapAttr(UsdGeom.Tokens.nonperiodic)
    shape.CreateCurveVertexCountsAttr([len(points)])
    shape.CreatePointsAttr(Vt.Vec3fArray.FromNumpy(np.asarray(points, dtype=np.float32)))
    shape.CreateWidthsAttr([width])
    shape.SetWidthsInterpolation(UsdGeom.Tokens.constant)
    bind(shape, mat)
    return shape


def label_texture(path, title, subtitle, accent):
    image = Image.new("RGB", (1600, 310), "white")
    draw = ImageDraw.Draw(image)
    font_path = Path("/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf")
    bold_path = font_path.with_name("DejaVuSans-Bold.ttf")
    font = ImageFont.truetype(str(font_path), 44) if font_path.exists() else ImageFont.load_default(size=44)
    bold = ImageFont.truetype(str(bold_path), 76) if bold_path.exists() else ImageFont.load_default(size=76)
    draw.rounded_rectangle((0, 40, 14, 252), radius=7, fill=tuple(int(v * 255) for v in accent))
    draw.text((52, 40), title, font=bold, fill=(23, 31, 37))
    draw.text((54, 151), subtitle, font=font, fill=(79, 90, 99))
    draw.text((54, 220), "cuRobo / Franka collision geometry", font=font, fill=(108, 117, 124))
    image.save(path, optimize=True)


def billboard(stage, path, center, eye, texture):
    # Camera-facing geometry; the label is part of the actual USD scene.
    normal = (eye - center) / np.linalg.norm(eye - center)
    right = np.cross(np.array([0., 0., 1.]), normal)
    right /= np.linalg.norm(right)
    up = np.cross(normal, right)
    corners = [center + x * 0.69 * right + y * 0.134 * up for x, y in [(-1, -1), (1, -1), (1, 1), (-1, 1)]]
    mesh = UsdGeom.Mesh.Define(stage, path)
    mesh.CreatePointsAttr(Vt.Vec3fArray.FromNumpy(np.asarray(corners, dtype=np.float32)))
    mesh.CreateFaceVertexCountsAttr([4])
    mesh.CreateFaceVertexIndicesAttr([0, 1, 2, 3])
    mesh.CreateSubdivisionSchemeAttr(UsdGeom.Tokens.none)
    mesh.CreateDoubleSidedAttr(True)
    UsdGeom.PrimvarsAPI(mesh).CreatePrimvar("st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex).Set(
        [(0, 0), (1, 0), (1, 1), (0, 1)])
    mat = UsdShade.Material.Define(stage, path + "Material")
    uv = UsdShade.Shader.Define(stage, mat.GetPath().AppendChild("UV"))
    uv.CreateIdAttr("UsdPrimvarReader_float2")
    uv.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("st")
    tex = UsdShade.Shader.Define(stage, mat.GetPath().AppendChild("Texture"))
    tex.CreateIdAttr("UsdUVTexture")
    tex.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(Sdf.AssetPath("./" + texture))
    tex.CreateInput("sourceColorSpace", Sdf.ValueTypeNames.Token).Set("sRGB")
    tex.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource(uv.ConnectableAPI(), "result")
    surface = UsdShade.Shader.Define(stage, mat.GetPath().AppendChild("Surface"))
    surface.CreateIdAttr("UsdPreviewSurface")
    surface.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(0))
    surface.CreateInput("emissiveColor", Sdf.ValueTypeNames.Color3f).ConnectToSource(tex.ConnectableAPI(), "rgb")
    mat.CreateSurfaceOutput().ConnectToSource(surface.ConnectableAPI(), "surface")
    bind(mesh, mat)


def validate_trace(trace, report):
    for name in ("baseline", "obstacle"):
        for suffix in ("tool", "q", "spheres"):
            data = trace[name + "_" + suffix]
            if not np.isfinite(data).all() or len(data) < 2:
                raise ValueError("Non-finite/incomplete trajectory: " + name + "_" + suffix)
        spheres = trace[name + "_spheres"]
        if spheres.ndim != 3 or spheres.shape[2] != 4:
            raise ValueError("Expected collision sphere array [steps, spheres, xyz+radius]")
        active = spheres[0, :, 3] > 0
        if active.sum() < 10 or not (spheres[:, active, 3] > 0).all():
            raise ValueError("Collision sphere activity changed or is incomplete")
    if not np.allclose(trace["baseline_q"][0], trace["obstacle_q"][0], atol=.001):
        raise ValueError("The two plans do not share a start")
    error = float(np.linalg.norm(trace["baseline_tool"][-1] - trace["obstacle_tool"][-1]))
    if error > .01:
        raise ValueError("The two plans do not share a tool goal")
    if report.get("baseline_tool_enters_new_obstacle") is not True or report.get("replanned_tool_enters_obstacle") is not False:
        raise ValueError("Result does not show a successful obstacle replan")
    center, dims = np.asarray(report["obstacle_center_m"]), np.asarray(report["obstacle_dimensions_m"])
    spheres = trace["obstacle_spheres"]
    active = spheres[0, :, 3] > 0
    outside = np.maximum(np.abs(spheres[:, active, :3] - center) - dims / 2, 0)
    clearance = float((np.linalg.norm(outside, axis=2) - spheres[:, active, 3]).min())
    if clearance < -1e-5:
        raise ValueError("A sampled robot collision sphere intersects the obstacle")
    return {"goal_difference_m": error, "sampled_sphere_min_clearance_m": clearance,
            "active_collision_spheres": int(active.sum())}


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("run_directory", type=Path)
    parser.add_argument("scene_directory", type=Path)
    args = parser.parse_args()
    source, output = args.run_directory.resolve(), args.scene_directory.resolve()
    output.mkdir(parents=True, exist_ok=True)
    scene_path = output / "curobo-replan.usda"
    names = ["curobo-replan.usda", "takes.json", "scene-report.json", "baseline-label.png", "obstacle-label.png"]
    if any((output / name).exists() for name in names):
        raise FileExistsError("Use a new scene directory; scene outputs already exist")
    report = json.loads((source / "result.json").read_text())
    with np.load(source / "motion.npz", allow_pickle=False) as archive:
        trace = {name: archive[name] for name in archive.files}
    checks = validate_trace(trace, report)
    stage = Usd.Stage.CreateNew(str(scene_path))
    stage.SetStartTimeCode(0)
    stage.SetEndTimeCode(END)
    stage.SetTimeCodesPerSecond(FPS)
    stage.SetFramesPerSecond(FPS)
    UsdGeom.SetStageMetersPerUnit(stage, 1.)
    UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
    world = UsdGeom.Xform.Define(stage, "/World")
    stage.SetDefaultPrim(world.GetPrim())
    stage.GetRootLayer().customLayerData = {
        "evidence_mode": "collision-geometry-trajectory-replay",
        "description": "Saved cuRobo collision spheres and tool paths; not live physics, a live policy, or a full Franka mesh.",
        "motion_sha256": sha256(source / "motion.npz"), "result_sha256": sha256(source / "result.json"),
    }
    mats = {name: material(stage, name, color) for name, color in COLORS.items()}
    box(stage, "/World/Floor", (0, .15, -.035), (200, 200, .05), mats["floor"])
    eye, target = np.array([1.10, -3.70, 2.95]), np.array([0, .08, 1.17])
    camera = UsdGeom.Camera.Define(stage, "/World/FilmCamera")
    camera.AddTransformOp().Set(Gf.Matrix4d().SetLookAt(Gf.Vec3d(*eye), Gf.Vec3d(*target), Gf.Vec3d(0, 0, 1)).GetInverse())
    camera.CreateFocalLengthAttr(42.)
    camera.CreateHorizontalApertureAttr(36.)
    camera.CreateVerticalApertureAttr(20.25)
    camera.CreateClippingRangeAttr(Gf.Vec2f(.01, 1000))
    dome = UsdLux.DomeLight.Define(stage, "/World/Dome")
    dome.CreateIntensityAttr(650)
    dome.CreateColorAttr(Gf.Vec3f(1))
    sun = UsdLux.DistantLight.Define(stage, "/World/Key")
    sun.CreateIntensityAttr(1800)
    sun.CreateAngleAttr(0.8)
    sun.AddRotateXYZOp().Set(Gf.Vec3f(315, 15, 20))

    for name, x, title, subtitle in (
        ("baseline", -.86, "BASELINE", "Before adding the obstacle"),
        ("obstacle", .86, "REPLANNED", "Same goal. New obstacle."),
    ):
        root = "/World/" + name
        offset = np.array([x, 0, .82])
        box(stage, root + "/Table", (x, .10, .78), (1.50, 1.16, .08), mats["table"])
        box(stage, root + "/Accent", (x, -.477, .79), (1.50, .018, .055), mats[name])
        box(stage, root + "/Plinth", (x - .04, 0, .84), (.32, .31, .035), mats["base"])
        for lx in (-.6, .6):
            box(stage, root + ("/LegL" if lx < 0 else "/LegR"), (x + lx, .15, .36), (.065, .8, .72), mats["base"])
        points = trace[name + "_tool"] + offset
        curve(stage, root + "/ToolPath", points, .009, mats[name])
        marker = sphere(stage, root + "/Tool", points[0], .021, mats[name])
        sphere(stage, root + "/Start", points[0], .016, mats["goal"])
        sphere(stage, root + "/Goal", points[-1], .025, mats["goal"])
        # Goal cross makes the shared endpoint visible even when the tool arrives.
        for axis in range(3):
            delta = np.eye(3)[axis] * .045
            curve(stage, root + f"/GoalAxis{axis}", [points[-1] - delta, points[-1] + delta], .004, mats["goal"])
        spheres = trace[name + "_spheres"]
        active = np.flatnonzero(spheres[0, :, 3] > 0)
        instancer = UsdGeom.PointInstancer.Define(stage, root + "/CollisionSpheres")
        prototype = UsdGeom.Sphere.Define(stage, root + "/CollisionSpheres/Prototype")
        prototype.CreateRadiusAttr(1.)
        bind(prototype, mats["robot"])
        instancer.CreatePrototypesRel().SetTargets([prototype.GetPath()])
        instancer.CreateProtoIndicesAttr([0] * len(active))
        instancer.CreateScalesAttr([Gf.Vec3f(float(r)) for r in spheres[0, active, 3]])
        instancer.CreatePositionsAttr(Vt.Vec3fArray.FromNumpy((spheres[0, active, :3] + offset).astype(np.float32)))
        for step, sample in enumerate(spheres):
            frame = LEAD + (FINISH - LEAD) * step / (len(spheres) - 1)
            instancer.GetPositionsAttr().Set(Vt.Vec3fArray.FromNumpy((sample[active, :3] + offset).astype(np.float32)), frame)
            marker.Set(Gf.Vec3d(*map(float, points[step])), frame)
        if name == "obstacle":
            center = np.asarray(report["obstacle_center_m"]) + offset
            dims = np.asarray(report["obstacle_dimensions_m"])
            box(stage, root + "/NewObstacle", center, dims, mats["red"])
            # Thin stand is illustrative support, not part of the planning world.
            height = center[2] - dims[2] / 2 - .82
            box(stage, root + "/DisplayStand", (center[0], center[1], .82 + height / 2), (.009, .009, height), mats["base"])
        texture = name + "-label.png"
        label_texture(output / texture, title, subtitle, COLORS[name])
        billboard(stage, root + "/Label", np.array([x - .02, .04, 1.91]), eye, texture)
    stage.GetRootLayer().Save()
    check_stage = Usd.Stage.Open(str(scene_path))
    if not check_stage.GetPrimAtPath("/World/FilmCamera"):
        raise RuntimeError("Saved scene is missing its recording camera")
    for name in ("baseline", "obstacle"):
        attr = check_stage.GetPrimAtPath("/World/" + name + "/CollisionSpheres").GetAttribute("positions")
        if len(attr.GetTimeSamples()) != len(trace[name + "_spheres"]):
            raise RuntimeError("Saved sphere animation is incomplete")
    (output / "takes.json").write_text(json.dumps([["curobo-replan", "curobo-replan", "FilmCamera", 0, 1, 10.6]], indent=2) + "\n")
    summary = {"scene": scene_path.name, "evidence_mode": "collision-geometry-trajectory-replay",
               "description": "Animated exact-radius active collision spheres and saved tool paths; no full Franka visual mesh or live dynamics.",
               "illustrative_geometry": "Tables, labels, goal markers and thin obstacle stand are presentation geometry, not additional planner collision objects.",
               "input_hashes": {name: sha256(source / name) for name in ("motion.npz", "result.json")},
               "generator_sha256": sha256(Path(__file__)), "camera": "/World/FilmCamera",
               "frames": END, "fps": FPS, "source_waypoints": {name: len(trace[name + "_tool"]) for name in ("baseline", "obstacle")},
               "checks": checks, "validated": True}
    (output / "scene-report.json").write_text(json.dumps(summary, indent=2) + "\n")
    print(json.dumps(summary, indent=2))


if __name__ == "__main__":
    main()