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)