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