Generate some valid orientation data for a tolerance of +/- (0.5, 0.5, 0.5) on either Euler angles or a rotation vector. (scipy==1.4.1)

create_test_data.py

import numpy as np

from scipy.spatial.transform import Rotation
from acrobotics.path.util import create_grid
from pyquaternion import Quaternion

def rotvec_to_mat(v):
    return Rotation.from_rotvec(v).as_dcm()

def mat_to_rotvec(mat):
    r = Rotation.from_dcm(mat)
    return r.as_rotvec()

def xyz_intrinsic_to_mat(v):
    return Rotation.from_euler("XYZ", v).as_dcm()

def mat_to_xyz_intrinsic(mat):
    r = Rotation.from_dcm(mat)
    return r.as_euler("XYZ")


def decide_xyz_rot_angles(rotation_matrix, lower, upper):
    v = mat_to_xyz_intrinsic(rotation_matrix)
    return np.all(np.logical_and(v >= lower, v <= upper))

def decide_rotvec(rotation_matrix, lower, upper):
    v = mat_to_rotvec(rotation_matrix)
    return np.all(np.logical_and(v >= lower, v <= upper))


if __name__ == "__main__":
    l, u = np.array(3 * [-0.5]), np.array(3 * [0.5])
    dxyz = lambda r: decide_xyz_rot_angles(r, l, u)
    drv = lambda r: decide_rotvec(r, l, u)

    angle_range = np.linspace(-0.49, 0.49, 3)
    samples = create_grid(3 * [angle_range])

    print("valid xyz euler data:")
    for s in samples:
        r = xyz_intrinsic_to_mat(s)
        q = Quaternion(matrix=r)
        s1, s2 = dxyz(r), drv(r)
        assert(s1)
        if s1 and not s2:
            print("{}, {}, {}, {}".format(q.x, q.y, q.z, q.w))

    print("Valid rotation vector data:")
    for s in samples:
        r = rotvec_to_mat(s)
        q = Quaternion(matrix=r)
        s1, s2 = dxyz(r), drv(r)
        #print(s1, s2)
        if s2 and not s1:
            #print(s)
            print("{}, {}, {}, {}".format(q.x, q.y, q.z, q.w))