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)
