가상 카메라로 과자 서랍 제어하기

도망가는 알람, 카드를 인식하는 과자 서랍, 마우스까지 치우는 정리 규칙, 통로 폭을 비교하는 카트를 담았습니다.

  • Isaac Sim
  • PhysX
  • OpenUSD
  • OpenCV
책상 주변에서 해 본 실험
실행 영상실행 영상과 기록

동작 원리

데이터는 어떻게 흐를까요?
  1. 입력서랍 앞 카메라 입력카드·손·서랍 입구가 보이는 BGR 배열

    실습은 카메라가 볼 법한 이미지를 코드로 그려 제어 규칙을 시험합니다.

  2. 처리영상에서 움직일 조건 판단카드가 보이나? · 입구가 막혔나?

    OpenCV가 지정한 영역의 색을 확인합니다. 그 결과로 서랍을 움직일지 정합니다.

  3. 출력서랍을 움직이고 기록 남기기슬라이드 위치 · result.json

    시뮬레이터가 서랍이 미끄러지는 방향으로 관절을 움직입니다. 로그에서 언제 움직였는지 볼 수 있습니다.

카메라 영상을 세 가지 판단 값으로 바꾸기

실습 스크립트는 손·카드·장애물을 그린 컬러 배열을 만들어 카메라 입력으로 사용합니다. OpenCV는 영상 전체를 분류하는 대신, 손·카드·서랍 입구가 있을 위치를 잘라 관심 영역(ROI) 세 곳을 봅니다. 각 영역에서 지정한 BGR(파랑·초록·빨강) 범위에 들어온 픽셀만 마스크에 남깁니다. 예를 들어 카드 영역의 초록색 후보가 160픽셀을 초과하면 badge_seen을 참으로 기록합니다. 카메라 입력과 제어 코드 · OpenCV 색 범위 마스크

의사결정과 이동은 어떻게 연결되나요?

손을 발견하면 닫힘 동작을 검사하기 시작합니다. 그 뒤 초록색 카드가 보이거나 서랍 입구가 막혀 있으면 닫힘 목표를 0으로 유지합니다. 두 조건 모두 없으면 목표를 1로 바꾸고, 각 프레임에서 조금씩 움직입니다.

추적 필드 해석
badge_seen, slot_occupied 영상에서 찾은 카드·장애물 판정입니다. 같은 색의 카드를 든 사람은 같은 판정을 받습니다.
drawer_closed_fraction 닫힘 정도를 0~1로 기록합니다. 0은 열린 상태, 1은 닫힌 상태입니다.
drawer_final_m 영상 장면의 저장된 결과에 기록된 최종 슬라이드 이동량입니다. 카드가 없고 입구가 빈 사례는 약 0.305m 이동했습니다.

이 흐름을 다른 예제에도 적용할 수 있습니다. 카트에서는 영상 대신 통로 폭이 입력이고, 정리 예제에서는 물체를 버릴지 보관할지 정한 작업 규칙이 입력입니다. 각각 판단에 쓰이는 데이터를 하나만 바꿔 결과를 비교할 수 있습니다.

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

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에서 서랍 동작 보기

과자 서랍 USD를 File → Open으로 엽니다. 카메라를 FilmCamera로 바꾸고 Play를 누릅니다. 카드가 보일 때와 입구가 막혔을 때 서랍이 각각 어떻게 움직이는지 보세요.

2. 내 컴퓨터에서 판단 로그 만들기

터미널에서 이 레포로 이동합니다. Python, NumPy, OpenCV와 FFmpeg가 필요합니다. command -v ffmpeg에 설치 경로가 나오면 아래 블록을 실행합니다. 이 단계는 GPU 없이 할 수 있습니다.

Shell
python3 -m venv .venv-desk
source .venv-desk/bin/activate
python -m pip install numpy opencv-python-headless
python labs/everyday-robots/run_story_demos.py ./runs/stories "$(command -v ffmpeg)"

runs/stories/28-snack-guard/result/result.json을 텍스트 편집기로 엽니다. badge_seen은 카드를 봤는지, slot_occupied는 입구가 막혔는지, drawer_closed_fraction은 서랍을 얼마나 닫았는지 나타냅니다.

3. 다른 장면도 바꿔 보기

알람, 책상 정리, 카트도 같은 방법으로 열 수 있습니다. 카트의 workbench-asset.usda는 원래 폴더에 둡니다. 새 장면을 만들려면 서랍 물리·책상 정리는 Isaac Sim의 Python, 통로 폭 계산은 usd-core가 설치된 Python에서 실행합니다.

테스트 결과

알람은 직선으로 후진할 때 책상 밖으로 떨어졌고, 가장자리에서 방향을 튼 경우는 책상 위에 남았습니다. 카드가 없는 경우에만 서랍이 약 30.5cm 닫혔습니다. 선별 정리는 과자만 수거함에 넣고 컵과 마우스를 남겼습니다. 통로는 필요한 폭이 80cm라 55cm와 75cm는 통과하지 못했고, 95cm에서 통과했습니다. 촬영 당시 결과

실행 영상과 기록

실습 소스 코드

run_story_demos.pyGitHub에서 파일 보기
Python
"""Run the mobile-alarm and color-badge drawer simulations and render their traces."""

import argparse
import json
import math
import subprocess
from pathlib import Path

import cv2
import numpy as np

FPS = 30
DARK = (21, 23, 25)
TEXT = (237, 241, 240)
GREEN = (114, 221, 158)
AMBER = (108, 185, 247)


def label(frame, message, x, y, size=.55, color=TEXT):
    cv2.putText(frame, message, (x, y), cv2.FONT_HERSHEY_SIMPLEX, size, color, 1, cv2.LINE_AA)


def encode(frames, destination, width, height, ffmpeg):
    with subprocess.Popen([
        str(ffmpeg), "-nostdin", "-hide_banner", "-loglevel", "error", "-y", "-f", "rawvideo",
        "-pix_fmt", "bgr24", "-s", f"{width}x{height}", "-r", str(FPS), "-i", "pipe:0",
        "-an", "-c:v", "libx264", "-preset", "fast", "-crf", "18", "-pix_fmt", "yuv420p",
        "-movflags", "+faststart", str(destination),
    ], stdin=subprocess.PIPE) as encoder:
        assert encoder.stdin is not None
        for frame in frames:
            assert frame.shape == (height, width, 3)
            encoder.stdin.write(frame.tobytes())
        encoder.stdin.close()
        assert encoder.wait() == 0


def wrap(angle):
    return math.atan2(math.sin(angle), math.cos(angle))


def alarm_trace(mode):
    wheel_radius, base = .03, .1125
    x, y, yaw, z = .53, .30, math.pi, .0
    waypoints = [(.93, .30), (.93, .47), (.70, .47)]
    index = 0
    triggered = False
    fall_at = press_at = None
    min_clear = math.inf
    trace = []
    for frame in range(180):
        t = frame / FPS
        hand = (.10 + min(t * .30, .97), .30)
        separation = math.hypot(x - hand[0], y - hand[1])
        if fall_at is None and press_at is None and separation <= .075:
            press_at = t
        triggered = triggered or separation <= .25
        v = turn = 0.0
        if fall_at is None and press_at is None and triggered:
            if mode == "reverse_only":
                v = -.40
            elif mode == "edge_aware" and index < len(waypoints):
                tx, ty = waypoints[index]
                if math.hypot(tx - x, ty - y) < .015:
                    index += 1
                else:
                    err = wrap(math.atan2(ty - y, tx - x) + math.pi - yaw)
                    turn = float(np.clip(5 * err, -3.5, 3.5))
                    if abs(err) < .10:
                        v = -min(.4, 5 * math.hypot(tx - x, ty - y))
        left, right = (v - turn * base / 2) / wheel_radius, (v + turn * base / 2) / wheel_radius
        if fall_at is None:
            speed, omega = wheel_radius * (left + right) / 2, wheel_radius * (right - left) / base
            x += speed * math.cos(yaw + omega / (2 * FPS)) / FPS
            y += speed * math.sin(yaw + omega / (2 * FPS)) / FPS
            yaw = wrap(yaw + omega / FPS)
            min_clear = min(min_clear, x - .075, 1.20 - x - .075, y - .075, .60 - y - .075)
            if not (0 <= x <= 1.20 and 0 <= y <= .60):
                fall_at = t
        else:
            z = -min(.8, 9.81 * (t - fall_at) ** 2 / 2)
        trace.append({"t": t, "pose": [x, y, yaw, z], "hand": hand, "wheel_rad_s": [left, right]})
    return {"mode": mode, "fall_s": fall_at, "pressed_s": press_at, "minimum_footprint_clearance_m": min_clear, "trace": trace}


def alarm_frames(rows):
    names = {"stationary": "STATIONARY", "reverse_only": "REVERSE ONLY", "edge_aware": "EDGE-AWARE ROUTE"}
    for frame_idx in range(len(rows[0]["trace"])):
        frame = np.full((414, 1440, 3), DARK, dtype=np.uint8)
        label(frame, "RUNAWAY ALARM  /  SAME HAND TRAJECTORY", 20, 32, .72)
        for panel, row in enumerate(rows):
            x0, y0, scale = 38 + panel * 480, 124, 324
            cv2.rectangle(frame, (x0, y0), (x0 + 389, y0 + 194), (100, 113, 128), -1)
            cv2.rectangle(frame, (x0+24, y0+24), (x0+365, y0+170), (135, 149, 164), 1)
            label(frame, names[row["mode"]], x0, 89, .67)
            now = row["trace"][frame_idx]
            t = now["t"]
            x, y, yaw, z = now["pose"]
            xy = (int(x0 + x * scale), int(y0 + (.6 - y) * scale))
            line = [(int(x0 + p["pose"][0] * scale), int(y0 + (.6-p["pose"][1]) * scale)) for p in row["trace"][:frame_idx+1]]
            if len(line) > 1:
                cv2.polylines(frame, [np.array(line, np.int32)], False, GREEN, 2)
            hx, hy = now["hand"]
            cv2.circle(frame, (round(x0 + hx*scale), round(y0 + (.60-hy)*scale)), 13, (201, 202, 215), -1, cv2.LINE_AA)
            if z > -.7:
                cv2.circle(frame, xy, max(9, round(22 - abs(z)*10)), (52, 74, 76), -1, cv2.LINE_AA)
                direction = (round(xy[0] + math.cos(yaw)*27), round(xy[1] - math.sin(yaw)*27))
                cv2.arrowedLine(frame, xy, direction, AMBER, 4, cv2.LINE_AA)
            pressed = row["pressed_s"] is not None and t >= row["pressed_s"]
            fell = row["fall_s"] is not None and t >= row["fall_s"]
            state = "OVER EDGE" if fell else "BUTTON PRESSED" if pressed else "RUNNING"
            label(frame, state, x0, 358, .58, AMBER if fell else GREEN)
            label(frame, f"{t:.2f} s  |  wheel {now['wheel_rad_s'][0]:+.1f} / {now['wheel_rad_s'][1]:+.1f} rad/s", x0, 386, .48)
        yield frame


def camera_frame(t, badge, obstruction):
    image = np.full((244, 324, 3), (42, 46, 48), np.uint8)
    cv2.rectangle(image, (92, 53), (232, 205), (87, 101, 113), -1)
    cv2.rectangle(image, (115, 71), (210, 134), (29, 31, 35), -1)
    hand_y = max(88, int(235 - t * 92))
    cv2.circle(image, (163, hand_y), 22, (30, 108, 219), -1, cv2.LINE_AA)
    if badge:
        cv2.rectangle(image, (139, hand_y+15), (166, hand_y+41), (45, 195, 73), -1)
    if obstruction:
        cv2.rectangle(image, (168, 90), (193, 116), (219, 99, 28), -1)
    return image


def drawer_control(name, badge, obstruction):
    close, active = 0.0, False
    any_block_time = any_grant_time = None
    frames = []
    for index in range(108):
        t = index / FPS
        camera = camera_frame(t, badge, obstruction)
        zone = camera[78:156, 104:218]
        hand = int(cv2.countNonZero(cv2.inRange(zone, (10, 75, 180), (70, 145, 245)))) > 120
        # Card and slot are camera regions. Actuation does not read the scenario's input flags.
        card_roi = camera[80:205, 110:209]
        access = int(cv2.countNonZero(cv2.inRange(card_roi, (25, 155, 45), (65, 235, 115)))) > 160
        slot = camera[74:132, 115:208]
        occupied = int(cv2.countNonZero(cv2.inRange(slot, (170, 65, 5), (245, 136, 65)))) > 120
        active = active or hand
        if active:
            if access and any_grant_time is None:
                any_grant_time = t
            if not access and occupied and any_block_time is None:
                any_block_time = t
            goal = 0.0 if access or occupied else 1.0
            close += float(np.clip(goal - close, -.034, .034))
        frames.append({"t": t, "hand_present": hand, "badge_seen": access, "slot_occupied": occupied, "drawer_closed_fraction": close})
    return {"scenario": name, "input_badge": badge, "input_obstruction": obstruction,
            "grant_s": any_grant_time, "obstruction_interlock_s": any_block_time,
            "max_closed_fraction": max(item["drawer_closed_fraction"] for item in frames),
            "final_closed_fraction": frames[-1]["drawer_closed_fraction"], "trace": frames}


def guard_frames(rows):
    for i in range(len(rows[0]["trace"])):
        frame = np.full((370, 1440, 3), DARK, np.uint8)
        label(frame, "SNACK GUARD  /  WHAT THE RGB CAMERA SEES", 20, 32, .72)
        for col, row in enumerate(rows):
            x = 18 + 360*col
            item = row["trace"][i]
            image = camera_frame(item["t"], row["input_badge"], row["input_obstruction"])
            cv2.rectangle(image, (104, 78), (218, 156), (178, 183, 188), 1)
            frame[75:319, x:x+324] = image
            label(frame, row["scenario"], x, 63, .51)
            fraction = item["drawer_closed_fraction"]
            cv2.rectangle(frame, (x, 326), (x + round(fraction*310), 336), AMBER, -1)
            label(frame, f"DRAWER CLOSED {fraction:.0%}", x, 356, .49, AMBER if fraction > .5 else GREEN)
        yield frame


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("root", type=Path)
    parser.add_argument("ffmpeg", type=Path)
    args = parser.parse_args()
    alarms = [alarm_trace(name) for name in ("stationary", "reverse_only", "edge_aware")]
    assert alarms[0]["pressed_s"] is not None and alarms[1]["fall_s"] is not None
    assert alarms[2]["pressed_s"] is None and alarms[2]["fall_s"] is None and alarms[2]["minimum_footprint_clearance_m"] > 0
    guards = [drawer_control("OWNER / NO CARD", False, False), drawer_control("OWNER / GREEN CARD", True, False),
              drawer_control("GUEST / SAME GREEN CARD", True, False), drawer_control("OBJECT IN SLOT", False, True)]
    assert guards[0]["final_closed_fraction"] == 1
    assert guards[1]["grant_s"] is not None and guards[2]["grant_s"] is not None
    assert guards[3]["obstruction_interlock_s"] is not None and guards[3]["max_closed_fraction"] == 0
    for name, rows, media, dimensions in (("27-runaway-alarm", alarms, alarm_frames, (1440, 414)),
                                           ("28-snack-guard", guards, guard_frames, (1440, 370))):
        folder = args.root / name / "result"
        folder.mkdir(parents=True, exist_ok=True)
        (folder / "result.json").write_text(json.dumps({"fps": FPS, "cases": rows}, indent=2) + "\n")
        encode(media(rows), folder / "comparison.mp4", *dimensions, args.ffmpeg)
        for row in rows:
            print(name, {key: value for key, value in row.items() if key != "trace"}, flush=True)


if __name__ == "__main__":
    main()
physics.pyGitHub에서 파일 보기
Python
"""Compute the falling alarm and the drawer drive response with Isaac Sim PhysX."""

import json
import math
import sys
import traceback
from pathlib import Path

from isaacsim import SimulationApp

app = SimulationApp({"headless": True, "fast_shutdown": True})

from omni.physx import get_physx_interface, get_physx_simulation_interface
from pxr import Gf, Usd, UsdGeom, UsdPhysics, UsdUtils

DT = 1 / 120


def make_stage():
    stage = Usd.Stage.CreateInMemory()
    UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
    UsdGeom.SetStageMetersPerUnit(stage, 1)
    world = UsdGeom.Xform.Define(stage, "/World").GetPrim()
    stage.SetDefaultPrim(world)
    scene = UsdPhysics.Scene.Define(stage, "/World/Physics")
    scene.CreateGravityDirectionAttr(Gf.Vec3f(0, 0, -1))
    scene.CreateGravityMagnitudeAttr(9.81)
    return stage


def box(stage, name, center, size):
    shape = UsdGeom.Cube.Define(stage, name)
    shape.CreateSizeAttr(1.0)
    shape.AddTranslateOp().Set(Gf.Vec3d(*center))
    shape.AddScaleOp().Set(Gf.Vec3f(*size))
    UsdPhysics.CollisionAPI.Apply(shape.GetPrim())
    return shape


def body(stage, name, center, size, mass):
    prim = UsdGeom.Xform.Define(stage, name)
    prim.AddTranslateOp().Set(Gf.Vec3d(*center))
    box(stage, name + "/Collision", (0, 0, 0), size)
    UsdPhysics.MassAPI.Apply(prim.GetPrim()).CreateMassAttr(mass)
    return UsdPhysics.RigidBodyAPI.Apply(prim.GetPrim())


def simulate(stage, count, read, before=None):
    cache = UsdUtils.StageCache.Get()
    handle = cache.Insert(stage)
    driver = get_physx_simulation_interface()
    info = get_physx_interface()
    driver.attach_stage(handle.ToLongInt())
    try:
        for i in range(count):
            if before:
                before(i)
            driver.simulate(DT, i * DT)
            driver.fetch_results()
            if i % 4 == 0:
                read(info)
    finally:
        driver.detach_stage()
        cache.Erase(stage)


def alarm(output):
    stage = make_stage()
    z = .844
    box(stage, "/World/Floor", (0, 0, -.06), (20, 20, .12))
    # The drive trace ends 1 cm beyond the remaining tabletop edge.
    box(stage, "/World/Table", (-.61, 0, .78), (1.2, .60, .04))
    buzz = body(stage, "/World/Alarm", (0, 0, z), (.137, .14, .083), .35)
    buzz.CreateVelocityAttr(Gf.Vec3f(.40, 0, 0))
    samples = [[0, z, 0]]

    def read(info):
        pose = info.get_rigidbody_transformation("/World/Alarm")
        assert pose["ret_val"]
        x, _, height = pose["position"]
        qx, qy, qz, qw = pose["rotation"]
        pitch = math.degrees(math.asin(max(-1, min(1, 2 * (qw*qy-qz*qx)))))
        samples.append([float(x), float(height), float(pitch)])

    simulate(stage, 272, read)
    assert min(row[1] for row in samples) < .12
    (output / "alarm-physx.json").write_text(json.dumps({
        "engine": "Isaac Sim PhysX", "samples_fps": 30, "start_height_m": z,
        "resting_center_z_m": samples[-1][1], "samples": samples,
    }, indent=2) + "\n")
    print(f"FALL_OK: starts at {z:.3f} m, settles at {samples[-1][1]:.3f} m", flush=True)


def drawers(input_trace, output):
    inputs = json.loads(input_trace.read_text())["cases"]
    stage = make_stage()
    drives, samples = [], [[] for _ in inputs]
    for index, _ in enumerate(inputs):
        x = index * .82
        name = f"/World/Drawer_{index}"
        body(stage, name, (x, 0, .85), (.55, .35, .059), .8)
        joint = UsdPhysics.PrismaticJoint.Define(stage, f"/World/Slide_{index}")
        joint.CreateBody1Rel().SetTargets([name])
        joint.CreateAxisAttr("Y")
        joint.CreateLocalPos0Attr(Gf.Vec3f(x, 0, .85))
        joint.CreateLocalPos1Attr(Gf.Vec3f(0))
        joint.CreateLowerLimitAttr(0.0)
        joint.CreateUpperLimitAttr(.309)
        drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "linear")
        drive.CreateTypeAttr("force")
        drive.CreateStiffnessAttr(650)
        drive.CreateDampingAttr(38)
        drive.CreateMaxForceAttr(95)
        drives.append(drive.CreateTargetPositionAttr(0))

    def before(step):
        if step % 4 == 0:
            for drive, case in zip(drives, inputs, strict=True):
                drive.Set(.305 * case["trace"][step // 4]["drawer_closed_fraction"])

    def read(info):
        for index, row in enumerate(samples):
            pose = info.get_rigidbody_transformation(f"/World/Drawer_{index}")
            assert pose["ret_val"]
            row.append(max(0.0, float(pose["position"][1])))

    count = len(inputs[0]["trace"])
    simulate(stage, count * 4, read, before)
    assert all(len(row) == count for row in samples)
    assert samples[0][-1] > .285 and all(max(row) < .012 for row in samples[1:])
    cases = [{"scenario": case["scenario"], "final_position_m": row[-1], "position_m": row}
             for case, row in zip(inputs, samples, strict=True)]
    (output / "drawer-physx.json").write_text(json.dumps({
        "engine": "Isaac Sim PhysX prismatic drive", "sample_rate_hz": 30, "cases": cases,
    }, indent=2) + "\n")
    print(f"DRAWERS_OK: final positions {[(row['scenario'], round(row['final_position_m'], 4)) for row in cases]}", flush=True)


try:
    working = Path(sys.argv[1])
    working.mkdir(parents=True, exist_ok=True)
    alarm(working)
    drawers(Path(sys.argv[2]), working)
except Exception:
    traceback.print_exc()
    raise
finally:
    app.close()
desk_cleanup.pyGitHub에서 파일 보기
Python
"""Compare two cleanup objectives with Isaac Sim and record the PhysX poses."""

import argparse
import json
from pathlib import Path

from isaacsim import SimulationApp

parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("output", type=Path)
output = parser.parse_args().output.resolve()
output.mkdir(parents=True, exist_ok=True)
app = SimulationApp({"headless": True, "fast_shutdown": True})

import carb
import numpy as np
from omni.physx import get_physx_interface, get_physx_simulation_interface
from pxr import Gf, Sdf, Usd, UsdGeom, UsdLux, UsdPhysics, UsdShade, UsdUtils


def material(stage, name, color, friction=0.24):
    result = UsdShade.Material.Define(stage, f"/World/Looks/{name}")
    shader = UsdShade.Shader.Define(stage, result.GetPath().AppendChild("Shader"))
    shader.CreateIdAttr("UsdPreviewSurface")
    shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*color))
    shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(.45)
    result.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface")
    physics = UsdPhysics.MaterialAPI.Apply(result.GetPrim())
    physics.CreateStaticFrictionAttr(friction)
    physics.CreateDynamicFrictionAttr(friction)
    physics.CreateRestitutionAttr(0)
    return result


def cube(stage, path, location, size, material, collision=False):
    shape = UsdGeom.Cube.Define(stage, path)
    shape.CreateSizeAttr(1)
    shape.AddTranslateOp().Set(Gf.Vec3d(*location))
    shape.AddScaleOp().Set(Gf.Vec3d(*size))
    binding = UsdShade.MaterialBindingAPI.Apply(shape.GetPrim())
    binding.Bind(material)
    if collision:
        UsdPhysics.CollisionAPI.Apply(shape.GetPrim())
        binding.Bind(material, materialPurpose="physics")
    return shape


def main():
    carb.settings.get_settings().set_bool("/physics/updateToUsd", True)
    stage = Usd.Stage.CreateInMemory()
    UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
    UsdGeom.SetStageMetersPerUnit(stage, 1)
    stage.SetDefaultPrim(UsdGeom.Xform.Define(stage, "/World").GetPrim())
    physics = UsdPhysics.Scene.Define(stage, "/World/Physics")
    physics.CreateGravityDirectionAttr(Gf.Vec3f(0, 0, -1))
    physics.CreateGravityMagnitudeAttr(9.81)
    table_contact = material(stage, "DeskContact", (.48, .31, .19))
    colors = {"snack": material(stage, "Snack", (.93, .22, .045)), "mouse": material(stage, "Mouse", (.08, .17, .30)),
              "cup": material(stage, "Cup", (.66, .91, .92)), "bin": material(stage, "Bin", (.20, .52, .26), .45),
              "floor": material(stage, "Floor", (.1, .12, .14)), "white": material(stage, "White", (.94, .92, .80))}
    cube(stage, "/World/Floor", (0, 0, -.04), (16, 16, .08), colors["floor"], collision=True)
    items = {"snack": (-.36, (.11, .14, .04)), "cup": (0, (.09, .09, .13)), "mouse": (.36, (.10, .16, .05))}
    scenarios = {}
    for name, center in (("ClearEverything", -.82), ("SortTrashOnly", .82)):
        prefix = f"/World/{name}"
        bench = UsdGeom.Xform.Define(stage, prefix + "/Desk")
        bench.GetPrim().GetReferences().AddReference(str(Path(__file__).with_name("workbench-asset.usda").resolve()))
        bench.AddTranslateOp().Set(Gf.Vec3d(center, 0, 0))
        tabletop = stage.GetPrimAtPath(prefix + "/Desk/Workbench/Tabletop")
        assert tabletop.IsValid()
        UsdShade.MaterialBindingAPI.Apply(tabletop).Bind(table_contact, materialPurpose="physics")
        cube(stage, prefix + "/BinFloor", (center, .79, .42), (1.22, .74, .06), colors["bin"], collision=True)
        for index, x in enumerate((center - .62, center + .62)):
            cube(stage, prefix + f"/BinSide{index}", (x, .79, .52), (.025, .74, .22), colors["bin"], collision=True)
        for index, y in enumerate((.42, 1.16)):
            cube(stage, prefix + f"/BinEnd{index}", (center, y, .52), (1.22, .025, .22), colors["bin"], collision=True)
        body_paths = {}
        for item, (x, size) in items.items():
            path = prefix + "/" + item.capitalize()
            body = UsdGeom.Xform.Define(stage, path)
            body.AddTranslateOp().Set(Gf.Vec3d(center + x, -.065, .803 + size[2] / 2))
            cube(stage, path + "/Shape", (0, 0, 0), size, colors[item], collision=True)
            accent_z = size[2] / 2 + .001
            cube(stage, path + "/Accent", (0, 0, accent_z), (size[0] * .46, size[1] * .34, .003), colors["white"])
            rigid = UsdPhysics.RigidBodyAPI.Apply(body.GetPrim())
            rigid.CreateVelocityAttr(Gf.Vec3f(0, 2.30 if name == "ClearEverything" or item == "snack" else 0, 0))
            UsdPhysics.MassAPI.Apply(body.GetPrim()).CreateMassAttr(.15)
            body_paths[item] = path
        scenarios[name] = (center, body_paths)

    camera = UsdGeom.Camera.Define(stage, "/World/Camera")
    pose = Gf.Matrix4d(1).SetLookAt(Gf.Vec3d(0, 4.25, 3.05), Gf.Vec3d(0, .24, .62), Gf.Vec3d(0, 0, 1)).GetInverse()
    camera.AddTransformOp().Set(pose)
    camera.CreateFocalLengthAttr(24)
    ambient = UsdLux.DomeLight.Define(stage, "/World/Ambient")
    ambient.CreateIntensityAttr(420)
    light = UsdLux.DistantLight.Define(stage, "/World/Key")
    light.CreateIntensityAttr(1350)
    light.CreateAngleAttr(14)
    light.AddRotateXYZOp().Set(Gf.Vec3f(34, -17, 0))
    print("DESK_READY | Two CAD-defined workbenches | Same initial objects", flush=True)
    cache = UsdUtils.StageCache.Get()
    stage_id = cache.Insert(stage).ToLongInt()
    driver = get_physx_simulation_interface()
    query = get_physx_interface()
    body_order = [path for _, body_paths in scenarios.values() for path in body_paths.values()]
    poses = []
    driver.attach_stage(stage_id)
    try:
        for frame in range(150):
            driver.simulate(1 / 60, frame / 60)
            driver.fetch_results()
            positions = []
            for path in body_order:
                pose = query.get_rigidbody_transformation(path)
                assert pose["ret_val"], path
                positions.append(list(pose["position"]) + list(pose["rotation"]))
            poses.append(positions)
            if frame in (16, 33, 59, 149):
                print(f"PHYSX_STEP {frame + 1}/150", flush=True)
        results = {}
        for name, (center, body_paths) in scenarios.items():
            objects = {}
            for item, path in body_paths.items():
                pose = query.get_rigidbody_transformation(path)
                assert pose["ret_val"], path
                x, y, z = list(pose["position"])
                on_desk = abs(x - center) < .60 and abs(y) < .30 and z >= .80
                in_bin = abs(x - center) < .60 and .42 < y < 1.16 and .44 < z < .66
                objects[item] = {"xyz_m": [x, y, z], "on_desk": on_desk, "in_bin": in_bin}
            results[name] = {"objects": objects, "remaining_on_desk": sum(entry["on_desk"] for entry in objects.values()),
                             "task_passed": objects["snack"]["in_bin"] and objects["mouse"]["on_desk"] and objects["cup"]["on_desk"]}
        record = {"physics": "Isaac Sim 6.2.0 / PhysX", "simulated_seconds": 2.5,
                  "actuation": "initial linear velocities on selected objects", "scenarios": results,
                  "sample_rate_hz": 60, "body_paths": body_order,
                  "geometry_sizes_m": {kind: list(size) for kind, (_, size) in items.items()}}
        assert np.asarray(poses).shape == (150, 6, 7)
        assert np.isfinite(poses).all()
        np.save(output / "poses.npy", np.asarray(poses, dtype=np.float32))
        (output / "result.json").write_text(json.dumps(record, indent=2) + "\n")
        print(f"RESULT {json.dumps(record)}", flush=True)
    finally:
        driver.detach_stage()
        cache.Erase(stage)


try:
    main()
finally:
    app.close()
aisle.pyGitHub에서 파일 보기
Python
"""Compare three OpenUSD layouts using the measured bounds of the CAD workbench."""

import argparse
import hashlib
import json
from pathlib import Path

from pxr import Gf, Sdf, Usd, UsdGeom


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("source", type=Path)
    parser.add_argument("output", type=Path)
    args = parser.parse_args()
    root = args.output
    root.mkdir(parents=True, exist_ok=True)
    digest = hashlib.sha256(args.source.read_bytes()).hexdigest()
    source = Usd.Stage.Open(str(args.source))
    asset_layer = Sdf.Layer.CreateAnonymous()
    for path in ("/World", "/World/Looks", "/World/Workbench"):
        Sdf.CreatePrimInLayer(asset_layer, path)
    for path in ("/World/Looks", "/World/Workbench"):
        Sdf.CopySpec(source.GetRootLayer(), path, asset_layer, path)
    asset = Usd.Stage.Open(asset_layer)
    asset.GetPrimAtPath("/World").SetSpecifier(Sdf.SpecifierDef)
    asset.SetDefaultPrim(asset.GetPrimAtPath("/World"))
    UsdGeom.SetStageUpAxis(asset, UsdGeom.Tokens.z)
    UsdGeom.SetStageMetersPerUnit(asset, 1.0)
    asset.Export(str(root / "workbench-asset.usda"))

    stage = Usd.Stage.CreateInMemory()
    scene = UsdGeom.Xform.Define(stage, "/Cell").GetPrim()
    stage.SetDefaultPrim(scene)
    UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
    UsdGeom.SetStageMetersPerUnit(stage, 1)
    fixed = UsdGeom.Xform.Define(stage, "/Cell/Fixed")
    moved = UsdGeom.Xform.Define(stage, "/Cell/Moved")
    # Resolve both references from a file-backed version of the stage once authored.
    for bench in (fixed, moved):
        bench.GetPrim().GetReferences().AddReference(str((root / "workbench-asset.usda").resolve()))
    variants = scene.GetVariantSets().AddVariantSet("aisle")
    for name, separation in (("narrow", 1.15), ("borderline", 1.35), ("wide", 1.55)):
        variants.AddVariant(name)
        variants.SetVariantSelection(name)
        with variants.GetVariantEditContext():
            UsdGeom.Xformable(moved).AddTranslateOp().Set(Gf.Vec3d(0, separation, 0))
    variants.SetVariantSelection("wide")
    stage.GetRootLayer().Export(str(root / "aisle-layouts.usda"))
    path = root / "aisle-layouts.usda"
    path.write_text(path.read_text().replace(str((root / "workbench-asset.usda").resolve()), "./workbench-asset.usda"))

    measured = Usd.Stage.Open(str(path))
    variants = measured.GetDefaultPrim().GetVariantSets().GetVariantSet("aisle")
    measurements = []
    required = 0.8  # 0.60 m demo cart plus 0.10 m clearance per side.
    for name in ("narrow", "borderline", "wide"):
        variants.SetVariantSelection(name)
        bounds = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_])
        boxes = []
        for prim_path in ("/Cell/Fixed/Workbench", "/Cell/Moved/Workbench"):
            prim = measured.GetPrimAtPath(prim_path)
            assert prim.IsValid(), prim_path
            world = bounds.ComputeWorldBound(prim).ComputeAlignedRange()
            boxes.append([list(world.GetMin()), list(world.GetMax())])
        gap = boxes[1][0][1] - boxes[0][1][1]
        row = {"variant": name, "clear_m": round(gap, 5), "fits_demo_cart": gap >= required, "bounds_m": boxes}
        print(f"{name:11s} | clear {gap:.3f} m | {'PASS' if row['fits_demo_cart'] else 'FAIL'}", flush=True)
        measurements.append(row)
    assert [item["fits_demo_cart"] for item in measurements] == [False, False, True]
    assert digest == hashlib.sha256(args.source.read_bytes()).hexdigest()
    (root / "result.json").write_text(json.dumps({
        "asset_sha256": digest, "cart_width_m": 0.60, "clearance_per_side_m": 0.10,
        "required_clearance_m": required, "layouts": measurements,
    }, indent=2) + "\n")


if __name__ == "__main__":
    main()