from omni.isaac.kit import SimulationApp
# 启动 Isaac Sim，Headless 模式
simulation_app = SimulationApp({"headless": True})

from omni.isaac.sensor import Camera
from omni.isaac.core import World
import numpy as np
from omni.isaac.core.utils.prims import is_prim_path_valid

# 初始化世界
world = World()
world.reset()

# 相机Prim路径（必须在 /World 下）
sensor_cam_path = "/World/TestCam"

print(f"创建 Camera Prim: {sensor_cam_path}")
cam = Camera(prim_path=sensor_cam_path, frequency=30, resolution=(640, 480))
cam.initialize()

# Step一次，让USD真正实例化camera prim
world.step(render=True)

# 检查Prim是否有效
if not is_prim_path_valid(sensor_cam_path):
    raise RuntimeError(f"❌ Camera Prim 路径 {sensor_cam_path} 不存在！")

print("✅ Camera Prim 存在，准备设定Pose")

# 安全的位姿
pos = np.array([0.0, 0.0, 0.5], dtype=np.float32)
rot = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32)

print("设置 set_world_pose ...")
cam.set_world_pose(position=pos, orientation=rot)

# 多步渲染，看看是否稳定
for i in range(10):
    world.step(render=True)
    print(f"Step {i+1} 完成")

simulation_app.close()
print("✅ 脚本执行结束，无崩溃")