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)