def test_rand(self): q = euldeg2quat(23, 82, 13) rotm_gt = euldeg2rotm(23, 82, 13) rotm = quat2rotm(q) assert np.all(np.isclose(rotm, rotm_gt)) q = euldeg2quat(-240, 428, -888) rotm_gt = euldeg2rotm(-240, 428, -888) rotm = quat2rotm(q) assert np.all(np.isclose(rotm, rotm_gt))
def test_rand(self): q_1 = euldeg2quat(23, 82, 13) rotm = quat2rotm(q_1) q_2 = rotm2quat(rotm) assert np.all(np.isclose(q_1, q_2)) or np.all(np.isclose(-q_1, q_2)) q_1 = euldeg2quat(-240, 428, -888) rotm = quat2rotm(q_1) q_2 = rotm2quat(rotm) assert np.all(np.isclose(q_1, q_2)) or np.all(np.isclose(-q_1, q_2))
def test_z90(self): quat = euldeg2quat(0, 0, 90) quat_gt = np.zeros(4) quat_gt[0] = 0.7071068 quat_gt[3] = 0.7071068 assert np.all(np.isclose(quat, quat_gt)) or np.all( np.isclose(-quat, quat_gt))
def test_rand(self): quat = euldeg2quat(23, 82, 13) quat_gt = np.zeros(4) quat_gt[0] = 0.74961123 quat_gt[1] = 0.07672064 quat_gt[2] = 0.65578898 quat_gt[3] = -0.04623594 assert np.all(np.isclose(quat, quat_gt)) or np.all( np.isclose(-quat, quat_gt)) quat = euldeg2quat(-240, 428, -888) quat_gt = np.zeros(4) quat_gt[0] = -0.43829334 quat_gt[1] = 0.35311284 quat_gt[2] = -0.68480871 quat_gt[3] = -0.46286856 assert np.all(np.isclose(quat, quat_gt)) or np.all( np.isclose(-quat, quat_gt))
def test_z180(self): quat = euldeg2quat(0, 0, 180) quat_gt = np.zeros(4) quat_gt[3] = 1 assert np.all(np.isclose(quat, quat_gt)) or np.all( np.isclose(-quat, quat_gt))
def test_unnormalized(self): q = euldeg2quat(2, -45, 21) * 5 rotm_gt = euldeg2rotm(2, -45, 21) rotm = quat2rotm(q) assert np.all(np.isclose(rotm, rotm_gt))
def test_z180(self): q = euldeg2quat(0, 0, 180) rotm_gt = euldeg2rotm(0, 0, 180) rotm = quat2rotm(q) assert np.all(np.isclose(rotm, rotm_gt))
def test_x90(self): q = euldeg2quat(90, 0, 0) rotm_gt = euldeg2rotm(90, 0, 0) rotm = quat2rotm(q) assert np.all(np.isclose(rotm, rotm_gt))
def test_identity(self): q = euldeg2quat(0, 0, 0) rotm_gt = euldeg2rotm(0, 0, 0) rotm = quat2rotm(q) assert np.all(np.isclose(rotm, rotm_gt))
def test_identity(self): quat = euldeg2quat(0, 0, 0) quat_gt = np.zeros(4) quat_gt[0] = 1 assert np.all(np.isclose(quat, quat_gt)) or np.all( np.isclose(-quat, quat_gt))