def blend_frames(self, frame0, frame1, blend):
"""Linearly interpolate between two frames.
Args:
frame0: First frame to be blended corresponds to (blend = 0).
frame1: Second frame to be blended corresponds to (blend = 1).
blend: Float between [0, 1], specifying the interpolation between
the two frames.
Returns:
An interpolation of the two frames.
"""
root_pos0 = self.get_frame_root_pos(frame0)
root_pos1 = self.get_frame_root_pos(frame1)
root_rot0 = self.get_frame_root_rot(frame0)
root_rot1 = self.get_frame_root_rot(frame1)
joints0 = self.get_frame_joints(frame0)
joints1 = self.get_frame_joints(frame1)
blend_root_pos = (1.0 - blend) * root_pos0 + blend * root_pos1
blend_root_rot = transformations.quaternion_slerp(
root_rot0, root_rot1, blend)
blend_joints = (1.0 - blend) * joints0 + blend * joints1
blend_root_rot = motion_util.standardize_quaternion(blend_root_rot)
blend_frame = np.zeros(self.get_frame_size())
self.set_frame_root_pos(blend_root_pos, blend_frame)
self.set_frame_root_rot(blend_root_rot, blend_frame)
self.set_frame_joints(blend_joints, blend_frame)
return blend_frame
def _postprocess_frames(self, frames):
"""Postprocesses frames to ensure they satisfy certain properties,
such as normalizing and standardizing all quaternions.
Args:
frames: Array containing frames to be processed. Each row of the array
should represent a frame.
Returns: An array containing the post processed frames.
"""
num_frames = frames.shape[0]
if num_frames > 0:
first_frame = self._frames[0]
pos_start = self.get_frame_root_pos(first_frame)
for f in range(num_frames):
curr_frame = frames[f]
root_pos = self.get_frame_root_pos(curr_frame)
root_pos[0] -= pos_start[0]
root_pos[1] -= pos_start[1]
root_rot = self.get_frame_root_rot(curr_frame)
root_rot = pose3d.QuaternionNormalize(root_rot)
root_rot = motion_util.standardize_quaternion(root_rot)
self.set_frame_root_pos(root_pos, curr_frame)
self.set_frame_root_rot(root_rot, curr_frame)
return
def calc_frame(self, time):
"""Calculates the frame for a given point in time.
Args:
time: Time at which the frame is to be computed.
Return: An array containing the frame for the given point in time,
specifying the pose of the character.
"""
f0, f1, blend = self.calc_blend_idx(time)
frame0 = self.get_frame(f0)
frame1 = self.get_frame(f1)
blend_frame = self.blend_frames(frame0, frame1, blend)
blend_root_pos = self.get_frame_root_pos(blend_frame)
blend_root_rot = self.get_frame_root_rot(blend_frame)
cycle_count = self.calc_cycle_count(time)
cycle_offset_pos = self._calc_cycle_offset_pos(cycle_count)
cycle_offset_rot = self._calc_cycle_offset_rot(cycle_count)
blend_root_pos = pose3d.QuaternionRotatePoint(blend_root_pos,
cycle_offset_rot)
blend_root_pos += cycle_offset_pos
blend_root_rot = transformations.quaternion_multiply(
cycle_offset_rot, blend_root_rot)
blend_root_rot = motion_util.standardize_quaternion(blend_root_rot)
self.set_frame_root_pos(blend_root_pos, blend_frame)
self.set_frame_root_rot(blend_root_rot, blend_frame)
return blend_frame
def build_target_obs(self):
"""Constructs the target observations, consisting of a sequence of
target frames for future timesteps. The tartet poses to include is
specified by self._tar_frame_steps.
Returns:
An array containing the target frames.
"""
tar_poses = []
time0 = self._get_motion_time()
dt = self._env.env_time_step
motion = self.get_active_motion()
robot = self._env.robot
ref_base_pos = self._get_ref_base_position()
sim_base_rot = np.array(robot.GetBaseOrientation())
heading = motion_util.calc_heading(sim_base_rot)
if self._tar_obs_noise is not None:
heading += self._randn(0, self._tar_obs_noise[0])
inv_heading_rot = transformations.quaternion_about_axis(
-heading, [0, 0, 1])
for step in self._tar_frame_steps:
tar_time = time0 + step * dt
tar_pose = self._calc_ref_pose(tar_time)
tar_root_pos = motion.get_frame_root_pos(tar_pose)
tar_root_rot = motion.get_frame_root_rot(tar_pose)
tar_root_pos -= ref_base_pos
tar_root_pos = pose3d.QuaternionRotatePoint(
tar_root_pos, inv_heading_rot)
tar_root_rot = transformations.quaternion_multiply(
inv_heading_rot, tar_root_rot)
tar_root_rot = motion_util.standardize_quaternion(tar_root_rot)
motion.set_frame_root_pos(tar_root_pos, tar_pose)
motion.set_frame_root_rot(tar_root_rot, tar_pose)
tar_poses.append(tar_pose)
tar_obs = np.concatenate(tar_poses, axis=-1)
return tar_obs