def set_xyz_action_rot(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,
)
rot_axis = action[4:] / np.linalg.norm(action[4:])
action[3] = action[3] * self.action_rot_scale
self.data.set_mocap_pos('mocap', new_mocap_pos)
# replace this with learned rotation
quat = quat_mul(quat_create(np.array([0, 1., 0]), np.pi),
quat_create(np.array(rot_axis), action[3]))
self.data.set_mocap_quat('mocap', quat)
def _set_obj_xyz_quat(self, pos, angle):
quat = quat_create(np.array([0, 0, .1]), angle)
qpos = self.data.qpos.flat.copy()
qvel = self.data.qvel.flat.copy()
qpos[9:12] = pos.copy()
qpos[12:16] = quat.copy()
qvel[9:15] = 0
self.set_state(qpos, qvel)
def set_xyz_action(self, action):
action = np.clip(action, -1, 1)
pos_delta = action * 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)
# self.data.set_mocap_quat('mocap', np.array([1, 0, 1, 0]))
quat = quat_mul(quat_create(np.array([1., 0, 0]), np.pi),
quat_create(np.array([0, 0, 1.]),
np.pi / 2)) #ref 기준 x축 180, z축 90순
# quat = quat_create(np.array([1., 0, 0]), -np.pi)
self.data.set_mocap_quat('mocap', quat) #w v 순인듯
def _reset_hand(self):
#10번씩 하는건 gripper 닫는 시간때문
for _ in range(10):
self.data.set_mocap_pos('mocap', self.hand_init_pos)
# self.data.set_mocap_quat('mocap', np.array([1, 0, 1, 0])) #w v 순인듯
# self.data.set_mocap_quat('mocap', np.array([1, 0, 0, 0])) #w v 순인듯
quat = quat_mul(quat_create(np.array([1., 0, 0]), np.pi),
quat_create(np.array([0, 0, 1.]),
np.pi / 2)) #ref 기준 x축 180, z축 90순
# quat = quat_create(np.array([1., 0, 0]), -np.pi)
self.data.set_mocap_quat('mocap', quat) #w v 순인듯
# self.do_simulation([-1,1], self.frame_skip)
self.do_simulation([0, 0], self.frame_skip)
rightFinger, leftFinger = self.get_site_pos(
'rightEndEffector'), self.get_site_pos('leftEndEffector')
self.init_fingerCOM = (rightFinger + leftFinger) / 2
self.pickCompleted = False
def set_xyz_action(self, action):
action = np.clip(action, -1, 1)
pos_delta = action * 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)
if self.rotMode == 'vertical_fixed':
quat = quat_mul(quat_create(np.array([1., 0, 0]), np.pi),
quat_create(np.array([0, 0, 1.]),
np.pi / 2)) #ref 기준 x축 180, z축 90순
elif self.rotMode == 'horizontal_fixed':
quat = quat_mul(quat_create(np.array([0, 0, 1.]), np.pi),
quat_create(np.array([0, 1., 0]),
np.pi / 2)) #ref 기준 z축 180, y축 90순
self.data.set_mocap_quat('mocap', quat) #w v 순인듯
def _reset_hand(self):
#10번씩 하는건 gripper 닫는 시간때문
for _ in range(10):
self.data.set_mocap_pos('mocap', self.hand_init_pos)
# self.data.set_mocap_quat('mocap', np.array([1, 0, 1, 0])) #w v 순인듯
# self.data.set_mocap_quat('mocap', np.array([1, 0, 0, 0])) #w v 순인듯
if self.rotMode == 'vertical_fixed':
quat = quat_mul(quat_create(np.array([1., 0, 0]), np.pi),
quat_create(np.array([0, 0, 1.]),
np.pi / 2)) #ref 기준 x축 180, z축 90순
elif self.rotMode == 'horizontal_fixed':
quat = quat_mul(quat_create(np.array([0, 0, 1.]), np.pi),
quat_create(np.array([0, 1., 0]),
np.pi / 2)) #ref 기준 z축 180, y축 90순
else: #그 외 경우도 vertically initialize
quat = quat_mul(quat_create(np.array([1., 0, 0]), np.pi),
quat_create(np.array([0, 0, 1.]),
np.pi / 2)) #ref 기준 x축 180, z축 90순
self.data.set_mocap_quat('mocap', quat)
# self.do_simulation([-1,1], self.frame_skip)
self.do_simulation([0, 0], self.frame_skip)
rightFinger, leftFinger = self.get_site_pos(
'rightEndEffector'), self.get_site_pos('leftEndEffector')
self.init_fingerCOM = (rightFinger + leftFinger) / 2
self.pickCompleted = False
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)