16 lines
409 B
Python
16 lines
409 B
Python
|
|
from scipy.spatial.transform import Rotation as R
|
|
|
|
class PoseUtil:
|
|
@staticmethod
|
|
def get_quaternion_wxyz_from_rotation_matrix(rotation_matrix):
|
|
rot = R.from_matrix(rotation_matrix)
|
|
quat = rot.as_quat()
|
|
|
|
if quat.shape[0] == 4:
|
|
quaternions_wxyz = quat[[3, 0, 1, 2]]
|
|
else:
|
|
quaternions_wxyz = quat[:, [3, 0, 1, 2]]
|
|
return quaternions_wxyz
|
|
|