How it works
- InputStart, goal, and obstacle
arm joints + target pose + obstacle boxKeep the start and goal fixed. Add a box to the world for the second plan.
- ProcessFind a path for the arm
cuRobo → joint waypointsThe planner checks the arm’s collision shapes and searches for a path to the goal.
- OutputCompare two paths
motion.npz · result.jsonCompare waypoint arrays and check whether the tool center enters the new obstacle.
Turn a tool pose into a path for every joint
The goal specifies the tool’s position and orientation, and the starting state specifies the arm joints. The robot model maps joint values to the tool and link positions. cuRobo finds joint configurations that reach the goal and a path that avoids collision along the way. Adding an obstacle changes which intermediate configurations are feasible even when the goal stays fixed. cuRobo documentation · Lab planner
| Output | How to read it |
|---|---|
baseline_q and obstacle_q in motion.npz |
Each row is a waypoint. Columns are joint values in the robot model’s order. |
baseline_tool and obstacle_tool |
Each row contains tool position [x, y, z]. The recorded paths have 24 and 26 rows. |
replanned_tool_enters_obstacle in result.json |
Checks sampled tool positions against the new box. The planner separately checks the arm’s collision geometry. |
How does this differ from generating demonstrations?
AutoData runs as a separate exercise. MimicGen reuses segments of a successful demonstration, such as picking up and putting down an object, and adapts them to a changed object arrangement. The simulation checks the resulting attempts before saving successful examples. In the recorded result, num_attempts: 6 and num_success: 3 count attempts and successful demonstrations. first_success_frame: 243 for demo_0 marks the first successful frame in that demonstration. MimicGen project
First time? Get the lab files
Open a terminal on the Isaac Sim machine for GPU labs, or on your own computer for CPU-only steps. Press Enter after each line. You can copy a multi-line command block as a whole.
cd "$HOME"
git clone https://github.com/james-joobs/awesome-nvidia-physical-ai.git
cd awesome-nvidia-physical-ai
pwd
This example clones into your home folder. If you already cloned the repo, skip git clone and enter your project folder. In each new terminal, check that pwd ends in awesome-nvidia-physical-ai so commands can find the labs/ files.
Run it
1. View two paths in Isaac Sim
Open the cuRobo path scene via File → Open. Select FilmCamera and press Play. Identify which path goes around the box. The scene replays saved paths and does not need the planner installed.
2. Compute new paths in a terminal
Set up cuRobo V2 at 71ce172 on your RTX machine. Enter this repository in a terminal with the cuRobo Python environment active. The script writes waypoint arrays to motion.npz and the comparison to result.json.
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
Replace the Python path with your cuRobo environment. Each run needs a new output folder so the commands and measurements remain attached to that attempt. CLI reference
3. View the newly calculated paths
Use a Python environment with NumPy, Pillow and usd-core to turn the new path arrays into a review scene. The spheres are cuRobo's collision representation of the arm, and their saved positions are replayed on the timeline.
python labs/robot-motion/make_scene.py ./runs/curobo-001 ./runs/curobo-scene-001
Open curobo-scene-001/curobo-replan.usda in Isaac Sim and select FilmCamera. The left side shows the baseline path; the right shows the path planned with the red obstacle. These are calculated collision shapes, rather than a live robot policy. To record with an Isaac Lab environment on X11:
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. Generate demonstrations with AutoData if needed
Use the official AutoData example to set up revision 4a3d1f3, its Isaac Lab submodules, and the annotated Franka HDF5. HDF5 keeps a set of trials and their joint and object states together. In the AutoData environment, replace the paths in the first two lines before running the block.
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
You can also start by opening the AutoData replay scene. GR00T placement · Microduck
Check the result
In the recorded run, the original tool center entered the new obstacle and the replanned path avoided it. The paths had 24 and 26 waypoints with the same endpoints. AutoData got three successful demonstrations in six attempts. Its replay reported three successes, and the audit finds each first success frame. Recorded results
The November 19 rerun also checked all active collision spheres at the stored waypoints. The minimum sphere-to-box clearance was about 2.17 mm, and the two final tool positions matched. The planner separately performs swept collision validation. Warmup conditions differ between the baseline and replan, so their wall times are not a fair speed comparison.
Example run and recording
Same goal, a new obstacle
November 19 GPU replan. Isaac Sim replays the calculated arm collision geometry; sampled sphere clearance is about 2.17 mm.
Source code
plan.py
View files on GitHubimport 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.sh
View files on GitHub#!/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.py
View files on GitHub"""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.py
View files on GitHub"""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()

