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_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)
def test_quat_to_mat_mujoco_random(self):
for _ in range(_NUM_RANDOM_SAMPLES):
quat = self._random_quaternion()
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)
def get_veloc_control(self, physics: 'mjcf.Physics'):
"""Velocity measurements in the prev root frame at the control timestep."""
del physics # physics unused by get_veloc_control.
rmat_prev = tr.quat_to_mat(self._walker_features_prev['quaternion'])[:3, :3]
veloc_world = (
self._walker_features['position'] -
self._walker_features_prev['position']) / self._control_timestep
return np.dot(veloc_world, rmat_prev)
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)
def test_quat_to_mat(self, quat, truemat):
"""Tests hard-coded quat-mat pairs generated from mujoco if mj not avail."""
mat = transformations.quat_to_mat(quat)
np.testing.assert_allclose(mat[0:3, 0:3], truemat, atol=1e-7)
