owasp 网站开发,龙岗网络推广方式,柳州做网站,高新区建设局网站问题#xff1a;
在Isaac-sim中如果采用set_world_pose()和get_world_pose()得到的都是四元数#xff0c;如何将弧度转四元数#xff0c;或者将四元数转为弧度是需要解决的一个问题#xff0c; 这里的弧度是以x轴为0度#xff0c;y轴为90度#xff0c;逆时针方向逐渐增大…问题
在Isaac-sim中如果采用set_world_pose()和get_world_pose()得到的都是四元数如何将弧度转四元数或者将四元数转为弧度是需要解决的一个问题 这里的弧度是以x轴为0度y轴为90度逆时针方向逐渐增大
解决方案
采用scipy.spatial.transform中的Rotation库进行转换 在转换的时候涉及到角度的变换 即如果直接采用Rotation.from_euler(‘x’, angle_rad2, degreesFalse)进行转换得到的是以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, degreesFalse)# 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))
