def disturb_pose(self, pose, noise):
""" uniform disturb pose
pose
noise float, deviation angle in radius
"""
new_pose = pose.copy()
noise3d = noise / np.sqrt(3)
new_pose[:3] += uniform(-noise3d, noise3d, 3)
for jstar, jdof in zip(self.pos_start, self.joint_dof):
if jdof == 1:
new_pose[jstar] += uniform(-noise, noise)
elif jdof == 4:
noise_quat = Quaternion.fromExpMap(
uniform(-noise3d, noise3d, 3))
ori_quat = Quaternion.fromWXYZ(pose[jstar:jstar + 4])
new_quat = noise_quat.mul(ori_quat)
new_pose[jstar:jstar + 4] = new_quat.wxyz()
elif jdof == 0:
pass
else:
assert (False and "not support jdof other than 1 and 4")
return new_pose
def exp_to_action(self, expmap):
""" Convert action represented in exponential map to angle-axis action
which deepmimic_sim can take in
Inputs:
expmap np.array of float, action represented in exponential map
Outputs:
action np.array of float, action for deepmimic environment
"""
assert (expmap.size == self.expdof)
action = np.zeros(self.dof - 7)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
e_off = self.exp_start[i]
a_off = self.act_start[i]
if dof == 1: # revolute
action[a_off] = expmap[e_off]
elif dof == 4: # spherical
quata = Quaternion.fromExpMap(expmap[e_off:e_off + 3])
action[a_off:a_off + 4] = quata.angaxis()
assert (np.isfinite(sum(action))), embed() #(action, targ_pose)
return action
def exp_to_targ_pose_old(self, expmap):
""" Convert action represented in exponential map to angle-axis action
which deepmimic_sim can take in
Inputs:
expmap np.array of float, action represented in exponential map
Outputs:
pose np.array of float, pose of character
"""
assert (expmap.size == self.expdof)
pose = np.zeros(self.dof)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
e_off = self.exp_start[i]
p_off = self.pos_start[i]
if dof == 1: # revolute
pose[p_off] = expmap[e_off]
elif dof == 4: # spherical
quata = Quaternion.fromExpMap(expmap[e_off:e_off + 3])
pose[p_off:p_off + 4] = quata.wxyz()
assert (np.isfinite(sum(pose))), embed() #(action, targ_pose)
return pose
def buildHeadingTrans(self, rot):
""" Build the rotation that rotate to local coordinate
rot np.array of float, 4 dim, (w, x, y, z) quat of coordinate orientation
"""
theta = self._kin_core.getHeadingTheta(rot)
inv_rot = Quaternion.fromExpMap(np.array([0, -theta, 0]))
return inv_rot
def disturb_root_pose(self, pose, pos_noise, ori_noise):
""" uniform disturb pose root
"""
new_pose = pose.copy()
noise3d = pos_noise / np.sqrt(3)
new_pose[:3] += uniform(-noise3d, noise3d, 3)
noise3d = ori_noise / np.sqrt(3)
noise_quat = Quaternion.fromExpMap(uniform(-noise3d, noise3d, 3))
ori_quat = Quaternion.fromWXYZ(pose[3:7])
new_quat = noise_quat.mul(ori_quat)
new_pose[3:7] = new_quat.wxyz()
return new_pose