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_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 bodies_orientations_in_egocentric_frame(physics): """Compute relative orientation of the bodies.""" # Get the bodies bodies = self._entity.bodies # Get the quaternions of all the bodies &root in the global frame bodies_xquat = physics.bind(bodies).xquat root_xquat = physics.bind(self._entity.root_body).xquat # Compute the relative quaternion of the bodies in the root frame bodies_quat_diff = transformations.quat_diff( np.tile(root_xquat, len(bodies)).reshape(-1, 4), bodies_xquat) # q1^-1 * q2 return np.reshape(bodies_quat_diff, -1)