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 = []
for k in range(len(bodies)):
bodies_quat_diff.append(
mjmath.mj_quatdiff(root_xquat, bodies_xquat[k])) # q1^-1 * q2
return np.reshape(np.stack(bodies_quat_diff, 0), -1)
def body_orientations_in_egocentric_frame(physics):
"""Compute relative orientation of the bodies."""
# Get the bodies
if hasattr(self._entity, 'mocap_bodies'):
bodies = self._entity.mocap_bodies
elif hasattr(self._entity, 'bodies'):
bodies = self._entity.bodies[1:] # remove presumed root
else:
bodies = self._entity.mjcf_model.find_all('body')
# Get the quaternions of all the bodies 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 torso frame
bodies_quat_diff = []
for k in range(len(bodies)):
bodies_quat_diff.append(
mjmath.mj_quatdiff(root_xquat, bodies_xquat[k])) # q1^-1 * q2
return np.reshape(np.stack(bodies_quat_diff, 0), -1)
def convert(file_name, physics, timestep):
"""Converts the parsed .amc values into qpos and qvel values and resamples.
Args:
file_name: The .amc file to be parsed and converted.
physics: The corresponding physics instance.
timestep: Desired output interval between resampled frames.
Returns:
A namedtuple with fields:
`qpos`, a numpy array containing converted positional variables.
`qvel`, a numpy array containing converted velocity variables.
`time`, a numpy array containing the corresponding times.
"""
frame_values = parse(file_name)
joint2index = {}
for name in physics.named.data.qpos.axes.row.names:
joint2index[name] = physics.named.data.qpos.axes.row.convert_key_item(
name)
index2joint = {}
for joint, index in joint2index.items():
if isinstance(index, slice):
indices = range(index.start, index.stop)
else:
indices = [index]
for ii in indices:
index2joint[ii] = joint
# Convert frame_values to qpos
amcvals2qpos_transformer = Amcvals2qpos(index2joint,
_CMU_MOCAP_JOINT_ORDER)
qpos_values = []
for frame_value in frame_values:
qpos_values.append(amcvals2qpos_transformer(frame_value))
qpos_values = np.stack(qpos_values) # Time by nq
# Interpolate/resample.
# Note: interpolate quaternions rather than euler angles (slerp).
# see https://en.wikipedia.org/wiki/Slerp
qpos_values_resampled = []
time_vals = np.arange(0, len(frame_values) * MOCAP_DT - 1e-8, MOCAP_DT)
time_vals_new = np.arange(0, len(frame_values) * MOCAP_DT, timestep)
while time_vals_new[-1] > time_vals[-1]:
time_vals_new = time_vals_new[:-1]
for i in range(qpos_values.shape[1]):
f = interpolate.splrep(time_vals, qpos_values[:, i])
qpos_values_resampled.append(interpolate.splev(time_vals_new, f))
qpos_values_resampled = np.stack(qpos_values_resampled) # nq by ntime
qvel_list = []
for t in range(qpos_values_resampled.shape[1] - 1):
p_tp1 = qpos_values_resampled[:, t + 1]
p_t = qpos_values_resampled[:, t]
qvel = [(p_tp1[:3] - p_t[:3]) / timestep,
mjmath.mj_quat2vel(mjmath.mj_quatdiff(p_t[3:7], p_tp1[3:7]),
timestep), (p_tp1[7:] - p_t[7:]) / timestep]
qvel_list.append(np.concatenate(qvel))
qvel_values_resampled = np.vstack(qvel_list).T
return Converted(qpos_values_resampled, qvel_values_resampled,
time_vals_new)
def testQuatDiff(self):
np.testing.assert_allclose(
mjmath.mj_quatdiff([0., 1., 0., 0.], [0., 0., 1., 0.]),
[0., 0., 0., -1.])
