def action_to_targ_pose(self, action):
""" Converte action to PD controller target pose
Inputs:
action np.array of float, action which DeepMimicSim can take in
Outputs:
pose np.array of float, pose of character
"""
assert (action.size == self.dof - 7)
targ_pose = np.zeros(self.dof)
targ_pose[3] = 1
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
a_off = self.act_start[i]
if dof == 1: # revolute
targ_pose[p_off] = action[a_off]
elif dof == 4: # spherical
angle = action[a_off]
axis = action[a_off + 1:a_off + 4]
quata = Quaternion.fromAngleAxis(angle, axis)
targ_pose[p_off:p_off + 4] = quata.wxyz()
assert (np.isfinite(sum(targ_pose))), embed() #(action, targ_pose)
return targ_pose
def change_dir(frames):
# change direction to x+ axis, starting position at origin
ori_pos = frames[0, 1:4].copy()
ori_pos[1] = 0
vel_dir = frames[-1, 1:4] - frames[0, 1:4]
heading_theta = np.arctan2(-vel_dir[2], vel_dir[0])
inv_rot = Quaternion.fromAngleAxis(-heading_theta, np.array([0, 1, 0]))
for i in range(frames.shape[0]):
frame = frames[i]
frames[i, 1:4] = inv_rot.rotate(frame[1:4] - ori_pos)
root_rot = Quaternion.fromWXYZ(frame[4:8])
new_rot = inv_rot.mul(root_rot)
frames[i, 4:8] = new_rot.pos_wxyz()
return frames
def get_link_states(self):
angle = self.__wallAngle / 180 * np.pi
offset = np.array([np.cos(angle), 0, np.sin(angle)])
l_bar_pos = np.array(
self.__basePos) - offset * (3.0 - self.__vis_offset)
l_bar_pos[1] = 0
r_bar_pos = np.array(
self.__basePos) + offset * (3.0 + self.__vis_offset)
r_bar_pos[1] = 0
t_bar_quat = Quaternion.fromXYZW(self.__baseOri).mul(
Quaternion.fromAngleAxis(np.pi / 2, np.array([0, 0, 1])))
t_bar_pos = np.array(self.__basePos) + offset * self.__vis_offset
t_bar_pos[1] = self.height - 0.02
return [
l_bar_pos.tolist() + self.__baseOri,
r_bar_pos.tolist() + self.__baseOri,
t_bar_pos.tolist() + t_bar_quat.xyzw().tolist()
]