def set_xyz_action_rotz(self, action):
action[:3] = np.clip(action[:3], -1, 1)
pos_delta = action[:3] * self.action_scale
new_mocap_pos = self.data.mocap_pos + pos_delta[None]
new_mocap_pos[0, :] = np.clip(
new_mocap_pos[0, :],
self.mocap_low,
self.mocap_high,
)
self.data.set_mocap_pos('mocap', new_mocap_pos)
zangle_delta = action[3] * self.action_rot_scale
new_mocap_zangle = quat_to_zangle(self.data.mocap_quat[0]) + zangle_delta
# new_mocap_zangle = action[3]
new_mocap_zangle = np.clip(
new_mocap_zangle,
-3.0,
3.0,
)
if new_mocap_zangle < 0:
new_mocap_zangle += 2 * np.pi
self.data.set_mocap_quat('mocap', zangle_to_quat(new_mocap_zangle))
def test_quaternions():
num_samps = 1000
for k in range(num_samps):
# test construction from axis+angle
ax1 = random_axis()
ang1 = random_angle()
q11 = Quaternion(axis=ax1, angle=ang1)
q12 = env_util.quat_create(ax1, ang1)
assert_close(q11.elements, q12)
# test multiplication
ax2 = random_axis()
ang2 = random_angle()
q21 = Quaternion(axis=ax2, angle=ang2)
q22 = env_util.quat_create(ax2, ang2)
prod1 = q11 * q21
prod2 = env_util.quat_mul(q12, q22)
assert_close(prod1.elements, prod2)
# test inversion
assert_close(q11.inverse.elements, env_util.quat_inv(q12))
assert_close(q21.inverse.elements, env_util.quat_inv(q22))
# test conversion to angle
exp_ang = quat_to_zangle_old(q12)
ang = env_util.quat_to_zangle(q12)
assert np.abs(exp_ang - ang) < 1e-6, "exp_ang={}, ang={}".format(exp_ang, ang)
# test conversion from angle
exp_q = zangle_to_quat_old(ang1)
q = env_util.zangle_to_quat(ang1)
assert_close(exp_q, q)