在Isaac-sim中弧度转四元数以及四元数转弧度的问题

问题：

在Isaac-sim中如果采用set_world_pose()和get_world_pose()得到的都是四元数，如何将弧度转四元数，或者将四元数转为弧度是需要解决的一个问题，
这里的弧度是以x轴为0度，y轴为90度，逆时针方向逐渐增大；

解决方案：

采用scipy.spatial.transform中的Rotation库，进行转换；
在转换的时候涉及到角度的变换，
即如果直接采用Rotation.from_euler(‘x’, angle_rad2, degrees=False)进行转换，得到的是以x轴负方向为0度，y轴为90度，顺时针方向逐渐增大；
因此需要在rad_to_quaternion函数中通过angle_rad2 = -angle_rad + math.pi方法进行转换，同样的在quaternion_to_angle函数中通过angle_rad = -angle_rad + math.pi将转换回来。

import math
from scipy.spatial.transform import Rotation
import torch

def rad_to_quaternion(angle_rad):
    if angle_rad > math.pi:
        angle_rad -= 2 * math.pi
    elif angle_rad < -math.pi:
        angle_rad += 2 * math.pi
    angle_rad2 = -angle_rad + math.pi
    rotation = Rotation.from_euler('x', angle_rad2, degrees=False)
    # print("rotation.as_quat()=%s" % (str(rotation.as_quat())))
    quaternion = rotation.as_quat()[:]  # 将旋转矩阵转换为四元数
    return quaternion, angle_rad, angle_rad2

def quaternion_to_angle(quaternion):
    rotation = Rotation.from_quat(quaternion)
    euler_angles = rotation.as_euler('xyz')
    angle_rad = euler_angles[0]
    angle_rad = -angle_rad + math.pi
    if angle_rad > math.pi:
        angle_rad -= 2 * math.pi
    elif angle_rad < -math.pi:
        angle_rad += 2 * math.pi
    return angle_rad

for i in range(361):
    rad = math.radians(i)
    quaternion, rad_ori, rad_new = rad_to_quaternion(rad)
    rad_bak = quaternion_to_angle(quaternion)

    theta0 = math.degrees(rad_ori)
    theta1 = math.degrees(rad_new)
    theta2 = math.degrees(rad_bak)
    print("原始角度为%s，经过变换后角度为%s，转换后的角度为%s"%(str(round(theta0)), str(round(theta1)), str(round(theta2))))

    # rad_new = math.floor(rad_new * 10000) / 10000
    # rad_bak = math.floor(rad_bak * 10000) / 10000
    # if rad_new != rad_bak:
    #     print("出错了rad_new=%s, rad_bak=%s"%(str(rad_new), str(rad_bak)))
    # print("rad=%.4f, rad_bak=%.4f"%(rad, rad_bak))