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)
