def post_update(self):
self.t += self._sim_step
if self.checkpoint:
pose, vel = self.character.get_pose()
self.recorder.append('character', pose)
if self.checkpoint and self.t > self.cut_off:
pose, vel = self.character.get_pose()
pose, vel = self._skeleton.toLocalFrame(pose, vel)
with open('data/states/%s.npy' % self.name, 'wb') as f:
np.save(f, pose)
np.save(f, vel)
frames = self.recorder.get('character')
last_frame = frames[-1]
inv_ori_rot = self._skeleton.buildHeadingTrans(
np.array(last_frame[3:7]))
offset = np.array(last_frame[0:3])
offset[1] = 0
for frame in frames:
pos = np.array(frame[0:3])
rot = Quaternion.fromWXYZ(np.array(frame[3:7]))
newpos = inv_ori_rot.rotate(pos - offset)
newrot = inv_ori_rot.mul(rot)
frame[0:3] = newpos.tolist()
frame[3:7] = newrot.wxyz().tolist()
self.recorder.save('data/records/%s.yaml' % self.name)
print(self.t)
print(pose)
print(vel)
print('finish data saving')
input()
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 targ_pose_to_exp(self, pose):
""" Convert target pose to exponential map, used as initialization of
Actor reference memory
Inputs:
pose np.array of float, pose of character
Outputs:
exp np.array of float, action represented in exponential map
"""
assert (pose.size == self.dof)
expmap = np.zeros(self.expdof)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
e_off = self.exp_start[i]
if dof == 1: # revolute
expmap[e_off] = pose[p_off]
elif dof == 4: # spherical
quata = Quaternion.fromWXYZ(pose[p_off:p_off + 4])
expmap[e_off:e_off + 3] = quata.expmap()
assert (np.isfinite(sum(expmap))), embed() #(action, targ_pose)
return expmap
def computeVel(self, pose0, pose1, dt, padding=True):
""" Compute velocity between two poses
Inputs:
pose0 np.array of float, start pose
pose1 np.array of float, end pose
dt float, duraction between two poses
Outputs:
avg_vel np.array of float, vel (w/ or w/o padding)
"""
assert (pose0.size == self.dof)
assert (pose1.size == self.dof)
avg_vel = np.zeros_like(pose0)
root0 = pose0[:3]
root1 = pose1[:3]
avg_vel[:3] = (root1 - root0) / dt
offset = 3
# root angular velocity is in world coordinate
dof = self.joint_dof[0]
quat0 = Quaternion.fromWXYZ(pose0[offset:offset + dof])
quat1 = Quaternion.fromWXYZ(pose1[offset:offset + dof])
avg_vel[offset:offset + 3] = computeAngVel(quat0, quat1, dt)
offset += dof
# other joints
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
if dof == 1: # revolute
theta0 = pose0[offset]
theta1 = pose1[offset]
avg_vel[offset] = (theta1 - theta0) / dt
elif dof == 4: # spherical
quat0 = Quaternion.fromWXYZ(pose0[offset:offset + dof])
quat1 = Quaternion.fromWXYZ(pose1[offset:offset + dof])
avg_vel[offset:offset + 3] = computeAngVelRel(quat0, quat1, dt)
offset += dof
if padding is False:
avg_vel = avg_vel[self.comp2pad]
return avg_vel
def calc_reward(self):
if self.is_fail:
return 0
count, pose, vel = self._mocap.slerp(self.t)
vel[0] *= self.preset.speed_multiplier; vel[2] *= self.preset.speed_multiplier
self._curr_kin_pose = pose
self._curr_kin_vel = vel
local_sim_pose, local_sim_vel = self._skeleton.toLocalFrame(self._curr_sim_pose, self._curr_sim_vel)
local_kin_pose, local_kin_vel = self._skeleton.toLocalFrame(self._curr_kin_pose, self._curr_kin_vel)
reward = self._skeleton.get_reward(local_sim_pose, local_sim_vel, local_kin_pose, local_kin_vel)
if self.is_episode_end():
pose, vel = self.character.get_pose()
pose, vel = self._skeleton.toLocalFrame(pose, vel)
bonus = 10.0*self._skeleton.get_reward2(local_sim_pose, local_sim_vel, local_kin_pose, local_kin_vel)
bonus *= np.exp(-1.0 * np.mean(np.abs(vel[6:] - local_kin_vel[6:])))
bonus *= np.exp(-1.0 * np.mean(np.abs(vel[3:6] - self.preset.angular_v)))
bonus *= bell(vel[2], self.preset.linear_v_z, 1.0)
if self.checkpoint:
print('v[2]', vel[2])
print(pose)
print(vel)
print('writing to %s ...' % (self.checkpoint + '.npy'))
with open(self.checkpoint + '.npy', 'wb') as f:
np.save(f, pose)
np.save(f, vel)
print('processing recording, please wait ...')
frames = self.motion_recorder.get('character')
last_frame = frames[-1]
inv_ori_rot = self._skeleton.buildHeadingTrans(np.array(last_frame[3:7]))
offset = np.array(last_frame[0:3])
offset[1] = 0
for frame in frames:
pos = np.array(frame[0:3])
rot = Quaternion.fromWXYZ(np.array(frame[3:7]))
newpos = inv_ori_rot.rotate(pos - offset)
newrot = inv_ori_rot.mul(rot)
frame[0:3] = newpos.tolist()
frame[3:7] = newrot.wxyz().tolist()
self.motion_recorder.save(self.checkpoint + '.yaml')
print('writing done')
return bonus + reward
return reward
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 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
def normalize_target_pose(self, pose):
"""
normalize given pose to make quaternions norm as 1
"""
assert (pose.size == self.dof)
norm_pose = pose.copy()
for i in range(self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
if dof == 4: # spherical
quata = Quaternion.fromWXYZ(pose[p_off:p_off + 4])
norm_pose[p_off:p_off + 4] = quata.pos_wxyz()
assert (np.isfinite(sum(norm_pose))), embed() #(action, targ_pose)
return norm_pose
def generalized_falling_rwd(self):
avg_penalty = self.offset_penalty / self.num_step
rwd = 1.0 - np.clip(
np.power(avg_penalty / self.offset_penalty_coeff, 2), 0, 1)
avg_ang_penalty = self.angv_penalty / self.num_step
rwd *= np.exp(-self.angv_penalty_coeff * avg_ang_penalty)
if rwd > 0:
q = Quaternion.fromWXYZ(self.root_quat)
min_angle = np.pi
for qb in self.q_bases:
q_base = Quaternion.fromWXYZ(np.array(qb))
q_diff = q.mul(q_base.conjugate())
angle = np.abs(q_diff.angle())
min_angle = np.min([min_angle, angle])
rwd *= np.clip(min_angle / (np.pi / 2), 0.01, 1)
#print(min_angle)
#print(self.root_quat)
if self.head_contact:
rwd *= 0.7
return rwd
def toLocalFrame(self, pose, vel, ori_pos=None, ori_rot=None):
""" Convert pose and vel from world frame to local frame,
the local frame heading direction is rotated x-axis
Inputs:
pose np.array of float, character pose
vel np.array of float, character vel w/ padding
ori_pos np.array of float, 3 dim, position of local coordinate origin
ori_rot np.array of float, 4 dim, (w, x, y, z) quat of local coordinate orientation
Outputs:
local_pose
local_vel
"""
if ori_pos is None:
ori_pos = pose[:3]
if ori_rot is None:
ori_rot = pose[3:7]
# heading theta
inv_ori_rot = self.buildHeadingTrans(ori_rot)
local_pos = pose.copy()
local_vel = vel.copy()
# ignore y difference, because local cooridnate shares xoz plane with world
local_pos[0] -= ori_pos[0] # root x pos
local_pos[2] -= ori_pos[2] # root y pos
ori_rot = Quaternion.fromWXYZ(ori_rot)
ori_rot = inv_ori_rot.mul(ori_rot)
local_pos[3:7] = ori_rot.pos_wxyz() # root orientation
local_vel[:3] = inv_ori_rot.rotate(vel[:3]) # root velocity
local_vel[3:6] = inv_ori_rot.rotate(vel[3:6]) # root angular velocity
return local_pos, local_vel
def targ_pose_to_action(self, pose):
""" Convert desired pose to action
Inputs:
pose np.array of float, pose of character
Outputs:
action np.array of float, action which DeepMimicSim can take in
"""
assert (pose.size == self.dof)
action = np.zeros(self.dof - 7)
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
action[a_off] = pose[p_off]
elif dof == 4: # spherical
quata = Quaternion.fromWXYZ(pose[p_off:p_off + 4])
action[a_off:a_off + 4] = quata.angaxis()
assert (np.isfinite(sum(action))), embed() #(action, targ_pose)
return action
def __init__(self, skeleton, mocap_file, extend_none_wrap=True):
self._skeleton = skeleton # HumanoidSkeleton
self._durations = None
self._frames = None
self._cycletime = None # total time of mocap
self._spf = None # seconds per frame
self._cyc_offset = None # cycle offset of base
self._is_wrap = None
with open(mocap_file, 'r') as f:
data = json.load(f)
frame_data = np.array(data["Frames"])
loop_type = data["Loop"]
self._is_wrap = (loop_type == "wrap")
# assert frame duration should be uniform
self._durations = frame_data[:, 0]
tmp = self._durations[:-1]
assert (abs(tmp - tmp.mean()).max() < 1e-10), tmp
self._spf = self._durations[0]
# if is non-wrap motion, extend final frame 0.5 seconds
if not self._is_wrap and extend_none_wrap:
n_ext = int(0.5 / self._spf)
frame_data[-1, 0] = self._spf
repeated_frames = np.repeat([frame_data[-1]], n_ext, axis=0)
frame_data = np.concatenate([frame_data, repeated_frames])
frame_data[-1, 0] = 0
self._durations = frame_data[:, 0]
# calculate cycle time and other information
self._cycletime = np.sum(self._durations)
self._frames = frame_data[:, 1:]
self.num_frames = frame_data.shape[0]
self._cyc_offset = self._frames[-1][:3] - self._frames[0][:3]
self._cyc_offset[1] = 0
assert (self._frames.shape[1] == self._skeleton.dof)
# normalize quaternions
for offset, dof in zip(self._skeleton.pos_start,
self._skeleton.joint_dof):
if dof == 4:
for i in range(self.num_frames):
quat = Quaternion.fromWXYZ(self._frames[i,
offset:offset + 4])
self._frames[i, offset:offset + 4] = quat.wxyz()
# precompute velocity
self._vels_pad = np.zeros((self.num_frames, self._skeleton.dof))
for i in range(self._vels_pad.shape[0] - 1):
curr_frame = self._frames[i]
next_frame = self._frames[i + 1]
dt = self._durations[i]
self._vels_pad[i] = self._skeleton.computeVel(
curr_frame, next_frame, dt, True)
self._vels_pad[-1] = self._vels_pad[-2]
self._vels_pad_noise = self._vels_pad.copy()
self._vels_pad = self._butterworth_filter(self._vels_pad)
self._vels_comp_noise = self._vels_pad_noise[:,
self._skeleton.comp2pad]
self._vels_comp = self._vels_pad[:, self._skeleton.comp2pad]
assert (np.isfinite(
self._vels_pad.sum())), print("infinite vels in mocap file")
# precompute com pose and smooth
self._com = []
self._com_vel = []
for p, v in zip(self._frames, self._vels_pad):
self._skeleton.set_pose(p)
self._skeleton.set_vel(v)
self._com.append(self._skeleton.get_com_pos())
self._com_vel.append(self._skeleton.get_com_vel())
self._com = np.array(self._com)
self._com_vel = np.array(self._com_vel)
self._com_smooth = self._com.copy()
self._com_smooth = self._butterworth_filter(self._com_smooth)