def rotXYZ(R, unit='deg'):
""" Compute Euler angles from matrix R using XYZ sequence."""
angles = np.zeros(3)
angles[0] = np.arctan2(R[2, 1], R[2, 2])
angles[1] = np.arctan2(-R[2, 0], np.sqrt(R[0, 0]**2 + R[1, 0]**2))
angles[2] = np.arctan2(R[1, 0], R[0, 0])
if unit[:3].lower() == 'deg': # convert from rad to degree
angles = np.rad2deg(angles)
return angles
评论列表
文章目录