当前位置：首页 > news >正文

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

news 2025/8/22 17:34:38

问题：

在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 torchdef rad_to_quaternion(angle_rad):if angle_rad > math.pi:angle_rad -= 2 * math.pielif angle_rad < -math.pi:angle_rad += 2 * math.piangle_rad2 = -angle_rad + math.pirotation = 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_rad2def 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.piif angle_rad > math.pi:angle_rad -= 2 * math.pielif angle_rad < -math.pi:angle_rad += 2 * math.pireturn angle_radfor 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))