def _calc_ref_pose_warmup(self):
"""Calculate default reference pose during warmup period."""
motion = self.get_active_motion()
pose0 = motion.calc_frame(0)
warmup_pose = self._default_pose.copy()
pose_root_rot = motion.get_frame_root_rot(pose0)
default_root_rot = motion.get_frame_root_rot(warmup_pose)
default_root_pos = motion.get_frame_root_pos(warmup_pose)
pose_heading = motion_util.calc_heading(pose_root_rot)
default_heading = motion_util.calc_heading(default_root_rot)
delta_heading = pose_heading - default_heading
delta_heading_rot = transformations.quaternion_about_axis(
delta_heading, [0, 0, 1])
default_root_pos = pose3d.QuaternionRotatePoint(
default_root_pos, delta_heading_rot)
default_root_rot = transformations.quaternion_multiply(
delta_heading_rot, default_root_rot)
motion.set_frame_root_pos(default_root_pos, warmup_pose)
motion.set_frame_root_rot(default_root_rot, warmup_pose)
return warmup_pose
def _reset_ref_motion(self):
"""Reset reference motion.
First randomly select a new reference motion from
the set of available motions, and then resets to a random point along the
selected motion.
"""
self._active_motion_id = self._sample_ref_motion()
self._origin_offset_rot = np.array([0, 0, 0, 1])
self._origin_offset_pos.fill(0.0)
self._reset_motion_time_offset()
motion = self.get_active_motion()
time = self._get_motion_time()
ref_pose = self._calc_ref_pose(time)
ref_root_pos = motion.get_frame_root_pos(ref_pose)
ref_root_rot = motion.get_frame_root_rot(ref_pose)
sim_root_pos = self._get_sim_base_position()
sim_root_rot = self._get_sim_base_rotation()
# move the root to the same position and rotation as simulated robot
self._origin_offset_pos = sim_root_pos - ref_root_pos
self._origin_offset_pos[2] = 0
ref_heading = motion_util.calc_heading(ref_root_rot)
sim_heading = motion_util.calc_heading(sim_root_rot)
delta_heading = sim_heading - ref_heading
self._origin_offset_rot = transformations.quaternion_about_axis(
delta_heading, [0, 0, 1])
self._ref_pose = self._calc_ref_pose(time)
self._ref_vel = self._calc_ref_vel(time)
self._update_ref_model()
self._prev_motion_phase = motion.calc_phase(time)
self._reset_clip_change_time()
return
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
def _calc_cycle_delta_heading(self):
"""Calculates the net change in the root heading after a cycle.
Returns:
Net change in heading.
"""
first_frame = self._frames[0]
last_frame = self._frames[-1]
rot_start = self.get_frame_root_rot(first_frame)
rot_end = self.get_frame_root_rot(last_frame)
inv_rot_start = transformations.quaternion_conjugate(rot_start)
drot = transformations.quaternion_multiply(rot_end, inv_rot_start)
cycle_delta_heading = motion_util.calc_heading(drot)
return cycle_delta_heading
def _calc_heading(self, root_rotation):
"""Returns the heading of a rotation q, specified as a quaternion.
The heading represents the rotational component of q along the vertical
axis (z axis). The heading is computing with respect to the robot's default
root orientation (self._default_root_rotation). This is because different
models have different coordinate systems, and some models may not have the z
axis as the up direction. This is similar to robot.GetTrueBaseOrientation(),
but is applied both to the robot and reference motion.
Args:
root_rotation: A quaternion representing the rotation of the robot's root.
Returns:
An angle representing the rotation about the z axis with respect to
the default orientation.
"""
inv_default_rotation = transformations.quaternion_conjugate(
self._get_default_root_rotation())
rel_rotation = transformations.quaternion_multiply(
root_rotation, inv_default_rotation)
heading = motion_util.calc_heading(rel_rotation)
return heading