def test_quat_diff_random(self): for _ in range(_NUM_RANDOM_SAMPLES): source = self._random_quaternion() target = self._random_quaternion() np.testing.assert_allclose( transformations.quat_diff(source, target), transformations.quat_mul(transformations.quat_conj(source), target))
def test_quat_mul_mujoco_random(self): for _ in range(_NUM_RANDOM_SAMPLES): quat1 = self._random_quaternion() quat2 = self._random_quaternion() quat_prod_tr = transformations.quat_mul(quat1, quat2) quat_prod_mj = np.zeros(4) mjlib.mju_mulQuat(quat_prod_mj, quat1, quat2) np.testing.assert_allclose(quat_prod_tr, quat_prod_mj)
def test_quat_diff_random_batched(self): source = np.stack( [self._random_quaternion() for _ in range(_NUM_RANDOM_SAMPLES)], axis=0) target = np.stack( [self._random_quaternion() for _ in range(_NUM_RANDOM_SAMPLES)], axis=0) np.testing.assert_allclose( transformations.quat_diff(source, target), transformations.quat_mul(transformations.quat_conj(source), target))
def test_quat_mul_vs_mat_mul_random(self): for _ in range(_NUM_RANDOM_SAMPLES): quat1 = self._random_quaternion() quat2 = self._random_quaternion() rmat1 = transformations.quat_to_mat(quat1)[0:3, 0:3] rmat2 = transformations.quat_to_mat(quat2)[0:3, 0:3] quat_prod = transformations.quat_mul(quat1, quat2) rmat_prod_q = transformations.quat_to_mat(quat_prod)[0:3, 0:3] rmat_prod = rmat1.dot(rmat2) np.testing.assert_allclose(rmat_prod, rmat_prod_q)
def test_quat_mul_mujoco_random_batched(self): quat1 = np.stack( [self._random_quaternion() for _ in range(_NUM_RANDOM_SAMPLES)], axis=0) quat2 = np.stack( [self._random_quaternion() for _ in range(_NUM_RANDOM_SAMPLES)], axis=0) quat_prod_tr = transformations.quat_mul(quat1, quat2) for k in range(quat1.shape[0]): quat_prod_mj = np.zeros(4) mjlib.mju_mulQuat(quat_prod_mj, quat1[k], quat2[k]) np.testing.assert_allclose(quat_prod_tr[k], quat_prod_mj)
def test_quat_mul_mujoco_special(self): # Test for special values that often cause numerical issues. rng = [-np.pi, np.pi / 2, 0, np.pi / 2, np.pi] quat1 = np.array([1, 0, 0, 0], dtype=np.float64) for euler_tup in itertools.product(rng, rng, rng): euler_vec = np.array(euler_tup, dtype=np.float64) quat2 = transformations.euler_to_quat(euler_vec, ordering='XYZ') quat_prod_tr = transformations.quat_mul(quat1, quat2) quat_prod_mj = np.zeros(4) mjlib.mju_mulQuat(quat_prod_mj, quat1, quat2) np.testing.assert_allclose(quat_prod_tr, quat_prod_mj, atol=1e-14) quat1 = quat2
def test_quat_mul_vs_mat_mul_random_batched(self): quat1 = np.stack( [self._random_quaternion() for _ in range(_NUM_RANDOM_SAMPLES)], axis=0) quat2 = np.stack( [self._random_quaternion() for _ in range(_NUM_RANDOM_SAMPLES)], axis=0) quat_prod = transformations.quat_mul(quat1, quat2) for k in range(_NUM_RANDOM_SAMPLES): rmat1 = transformations.quat_to_mat(quat1[k])[0:3, 0:3] rmat2 = transformations.quat_to_mat(quat2[k])[0:3, 0:3] rmat_prod_q = transformations.quat_to_mat(quat_prod[k])[0:3, 0:3] rmat_prod = rmat1.dot(rmat2) np.testing.assert_allclose(rmat_prod, rmat_prod_q)