def set_walker(physics,
walker,
qpos,
qvel,
offset=0,
null_xyz_and_yaw=False,
position_shift=None,
rotation_shift=None):
"""Set the freejoint and walker's joints angles and velocities."""
qpos = np.array(qpos)
if null_xyz_and_yaw:
qpos[:3] = 0.
euler = tr.quat_to_euler(qpos[3:7], ordering='ZYX')
euler[0] = 0.
quat = tr.euler_to_quat(euler, ordering='ZYX')
qpos[3:7] = quat
qpos[:3] += offset
freejoint = mjcf.get_attachment_frame(walker.mjcf_model).freejoint
physics.bind(freejoint).qpos = qpos[:7]
physics.bind(freejoint).qvel = qvel[:6]
physics.bind(walker.mocap_joints).qpos = qpos[7:]
physics.bind(walker.mocap_joints).qvel = qvel[6:]
if position_shift is not None or rotation_shift is not None:
walker.shift_pose(physics,
position=position_shift,
quaternion=rotation_shift,
rotate_velocity=True)
def test_quat_rotate_mujoco_special(self):
# Test for special values that often cause numerical issues.
rng = [-np.pi, np.pi / 2, 0, np.pi / 2, np.pi]
vec = np.array([1, 0, 0], dtype=np.float64)
for euler_tup in itertools.product(rng, rng, rng):
euler_vec = np.array(euler_tup, dtype=np.float64)
quat = transformations.euler_to_quat(euler_vec, ordering='XYZ')
rotated_vec_tr = transformations.quat_rotate(quat, vec)
rotated_vec_mj = np.zeros(3)
mjlib.mju_rotVecQuat(rotated_vec_mj, vec, quat)
np.testing.assert_allclose(rotated_vec_tr, rotated_vec_mj, atol=1e-14)
def test_quat_mul_mujoco_special(self):
# Test for special values that often cause numerical issues.
rng = [-np.pi, np.pi / 2, 0, np.pi / 2, np.pi]
quat1 = np.array([1, 0, 0, 0], dtype=np.float64)
for euler_tup in itertools.product(rng, rng, rng):
euler_vec = np.array(euler_tup, dtype=np.float64)
quat2 = transformations.euler_to_quat(euler_vec, ordering='XYZ')
quat_prod_tr = transformations.quat_mul(quat1, quat2)
quat_prod_mj = np.zeros(4)
mjlib.mju_mulQuat(quat_prod_mj, quat1, quat2)
np.testing.assert_allclose(quat_prod_tr, quat_prod_mj, atol=1e-14)
quat1 = quat2