import numpy as np
from scipy.spatial.transform import Rotation as R

# # 欧拉角
# rx, ry, rz = -1.433, 0.114, -0.430

# # 尝试不同的旋转顺序
# orders = ['xyz', 'zyx', 'xzy', 'yxz', 'yzx', 'zyx']

# print("尝试不同旋转顺序的结果：")
# for order in orders:
#     try:
#         rot = R.from_euler(order, [rx, ry, rz], degrees=False)
#         quat = rot.as_quat()  # [x, y, z, w]
#         print(f"顺序 {order}: [{quat[0]:.8f}, {quat[1]:.8f}, {quat[2]:.8f}, {quat[3]:.8f}]")
#     except:
#         continue

# # 特别检查XYZ顺序
# print("\nXYZ顺序的详细计算：")
# rot_xyz = R.from_euler('xyz', [rx, ry, rz], degrees=False)
# quat_xyz = rot_xyz.as_quat()
# print(f"XYZ顺序: [{quat_xyz[0]:.8f}, {quat_xyz[1]:.8f}, {quat_xyz[2]:.8f}, {quat_xyz[3]:.8f}]")


# 输入欧拉角 (rx, ry, rz)，单位弧度
# euler_angles_rad = [-1.433, 0.114, -0.430]
# euler_angles_rad = [-1.622, -0.016, -0.4]
# euler_angles_rad = [-0.972, -0.557, -0.901]
# euler_angles_rad = [-1.674, 0.223, -0.747]
# euler_angles_rad = [-1.484, 0.152, -0.26]
# euler_angles_rad = [-1.691, -0.443, -0.452]
# euler_angles_rad = [-1.72, -0.189, -0.223]
# euler_angles_rad = [-2.213, -0.202, -0.391]
# euler_angles_rad = [-1.358, -0.284, -0.255]
# euler_angles_rad = [-1.509, -0.234, -0.416]
# euler_angles_rad = [-1.79, -0.447, -0.133]
euler_angles_rad = [-1.468, 0.199, -0.361]

# 创建旋转对象，指定欧拉角顺序为 'xyz'
rot = R.from_euler('xyz', euler_angles_rad, degrees=False)

# 转换为四元数，格式为 [x, y, z, w]
quaternion = rot.as_quat()

print(f"四元数 (x, y, z, w): [{quaternion[0]:.8f}, {quaternion[1]:.8f}, {quaternion[2]:.8f}, {quaternion[3]:.8f}]")