# 如果你在 Script Editor 里运行，不需要 SimulationApp 这一段
# 如果你是放到 .py 独立运行（headless），取消下面注释：
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": True})

import omni.usd
from pxr import UsdGeom
from scipy.spatial.transform import Rotation as R
import numpy as np

# 1. 打开USD场景
usd_path = "/media/sig/1CC4CB86C4CB611E1/sig/qianluo/usdz/839920.usda"
success = omni.usd.get_context().open_stage(usd_path)
if not success:
    print(f"无法打开 USD 文件: {usd_path}")
    simulation_app.close()
    exit()

# 2. 获取 Stage 对象
stage = omni.usd.get_context().get_stage()

prim_path = "/World/Camera"  # 你要查看的相机 prim 路径

prim = stage.GetPrimAtPath(prim_path)
if prim.IsValid():
    xform = UsdGeom.Xformable(prim)
    world_xform = xform.ComputeLocalToWorldTransform(0)  # 0 表示 Default Timecode

    # 提取位置
    position = [world_xform.ExtractTranslation()[i] for i in range(3)]

    # 提取四元数（USD顺序是 x, y, z, w）
    rot = world_xform.ExtractRotationQuat()
    quat_usd = np.array([
        rot.GetImaginary()[0],
        rot.GetImaginary()[1],
        rot.GetImaginary()[2],
        rot.GetReal()
    ])

    # 打印信息
    print("==== USD 世界变换 ====")
    print(f"Position: {position}")
    print(f"Quaternion [x,y,z,w]: {quat_usd.tolist()}")

    euler_xyz = R.from_quat(quat_usd).as_euler('xyz', degrees=True)
    print(f"Euler angles (xyz deg): {euler_xyz.tolist()}")

else:
    print(f"找不到 prim: {prim_path}")

# 如果是独立 headless 运行，记得在最后 simulation_app.close()
# simulation_app.close()