def test_euler_to_rmat_special(self, angles): # Test for special values that often cause numerical issues. r1, r2, r3 = angles for ordering in transformations._eulermap.keys(): r = transformations.euler_to_rmat(np.array([r1, r2, r3]), ordering) euler_angles = transformations.rmat_to_euler(r, ordering) np.testing.assert_allclose(euler_angles, [r1, r2, r3])
def test_mat_to_quat_mujoco(self): subsamps = 10 rng = np.linspace(-np.pi, np.pi, subsamps) for euler_tup in itertools.product(rng, rng, rng): euler_vec = np.array(euler_tup, dtype=float) mat = transformations.euler_to_rmat(euler_vec, ordering='XYZ') mj_quat = np.empty(4, dtype=euler_vec.dtype) mjlib.mju_mat2Quat(mj_quat, mat.flatten()) tr_quat = transformations.mat_to_quat(mat) self.assertTrue( np.allclose(mj_quat, tr_quat) or np.allclose(mj_quat, -tr_quat))
def test_quat_to_mat_mujoco_special(self): # Test for special values that often cause numerical issues. rng = [-np.pi, np.pi / 2, 0, np.pi / 2, np.pi] for euler_tup in itertools.product(rng, rng, rng): euler_vec = np.array(euler_tup, dtype=np.float) mat = transformations.euler_to_rmat(euler_vec, ordering='XYZ') quat = transformations.mat_to_quat(mat) tr_mat = transformations.quat_to_mat(quat) mj_mat = np.zeros(9) mjlib.mju_quat2Mat(mj_mat, quat) mj_mat = mj_mat.reshape(3, 3) np.testing.assert_allclose(tr_mat[0:3, 0:3], mj_mat, atol=1e-10) np.testing.assert_allclose(tr_mat[0:3, 0:3], mat, atol=1e-10)