def array_to_pose_data(self, skel, data, T_root_ref=None): assert len(data) == self._num_dofs + 6 T_root = conversions.Rp2T(conversions.A2R(data[3:6]), data[0:3]) if T_root_ref is not None: T_root = np.dot(T_root_ref, T_root) pose_data = [] idx = 6 for i in range(skel.num_joint()): joint = skel.joints[i] if joint == skel.root_joint: pose_data.append(T_root) else: j = self._char_info.bvh_map_inv[joint.name] if j is None: pose_data.append(constants.eye_T()) else: joint_type = self._joint_type[j] if joint_type == self._pb_client.JOINT_FIXED: pose_data.append(constants.eye_T()) elif joint_type == self._pb_client.JOINT_SPHERICAL: pose_data.append( conversions.R2T(conversions.A2R(data[idx:idx + 3]))) idx += 3 else: raise NotImplementedError() return pose_data
def test_R2A(self): R = test_utils.get_random_R() A = conversions.R2A(np.array([R])) np.testing.assert_almost_equal( conversions.R2A(np.array([R]))[0], conversions.R2A(R) ) np.testing.assert_almost_equal( conversions.A2R(np.array([A]))[0], conversions.A2R(A) ) R_test = conversions.A2R(A[0]) for r, r_test in zip(R.flatten(), R_test.flatten()): self.assertAlmostEqual(r, r_test)
def append_and_blend(motion1, motion2, blend_length=0): assert isinstance(motion1, motion_class.Motion) assert isinstance(motion2, motion_class.Motion) assert motion1.skel.num_joints() == motion2.skel.num_joints() combined_motion = copy.deepcopy(motion1) combined_motion.name = f"{motion1.name}+{motion2.name}" if motion1.num_frames() == 0: for i in range(motion2.num_frames()): combined_motion.poses.append(motion2.poses[i]) if hasattr(motion1, "velocities"): combined_motion.velocities.append(motion2.velocities[i]) return combined_motion frame_target = motion2.time_to_frame(blend_length) frame_source = motion1.time_to_frame(motion1.length() - blend_length) # Translate and rotate motion2 to location of frame_source pose1 = motion1.get_pose_by_frame(frame_source) pose2 = motion2.get_pose_by_frame(0) R1, p1 = conversions.T2Rp(pose1.get_root_transform()) R2, p2 = conversions.T2Rp(pose2.get_root_transform()) # Translation to be applied dp = p1 - p2 dp = dp - math.projectionOnVector(dp, motion1.skel.v_up_env) axis = motion1.skel.v_up_env # Rotation to be applied Q1 = conversions.R2Q(R1) Q2 = conversions.R2Q(R2) _, theta = quaternion.Q_closest(Q1, Q2, axis) dR = conversions.A2R(axis * theta) motion2 = translate(motion2, dp) motion2 = rotate(motion2, dR) t_total = motion1.fps * frame_source t_processed = 0.0 poses_new = [] for i in range(motion2.num_frames()): dt = 1 / motion2.fps t_total += dt t_processed += dt pose_target = motion2.get_pose_by_frame(i) # Blend pose for a moment if t_processed <= blend_length: alpha = t_processed / float(blend_length) pose_source = motion1.get_pose_by_time(t_total) pose_target = blend(pose_source, pose_target, alpha) poses_new.append(pose_target) del combined_motion.poses[frame_source + 1:] for i in range(len(poses_new)): combined_motion.add_one_frame(0, copy.deepcopy(poses_new[i].data)) return combined_motion
def slerp(R1, R2, t): """ Spherical linear interpolation (https://en.wikipedia.org/wiki/Slerp) between R1 and R2 with parameter t, 0 ≤ t ≤ 1 """ return np.dot( R1, conversions.A2R(t * conversions.R2A(np.dot(R1.transpose(), R2))) )
def random_rotation(mu_theta, sigma_theta, lower_theta, upper_theta): """ Generate a random position by a truncated normal districution """ theta = truncnorm( mu=mu_theta, sigma=sigma_theta, lower=lower_theta, upper=upper_theta ) return conversions.A2R(random_unit_vector() * theta)
def compute_target_pose(self, idx, action): agent = self._sim_agent[idx] char_info = agent._char_info ''' the current posture should be deepcopied because action will modify it ''' if self._action_type == Env.ActionMode.Relative: ref_pose = copy.deepcopy(self.get_current_pose_from_motion(idx)) else: ref_pose = copy.deepcopy( self._base_motion[idx].get_pose_by_frame(0)) a_real = self._action_space[idx].norm_to_real(action) dof_cnt = 0 for j in agent._joint_indices: joint_type = agent.get_joint_type(j) ''' Fixed joint will not be affected ''' if joint_type == self._pb_client.JOINT_FIXED: continue ''' If the joint do not have correspondance, use the reference posture itself''' if char_info.bvh_map[j] == None: continue if self._action_type == Env.ActionMode.Relative: T = ref_pose.get_transform(char_info.bvh_map[j], local=True) elif self._action_type == Env.ActionMode.Absolute: T = ref_pose.skel.get_joint( char_info.bvh_map[j]).xform_from_parent_joint else: raise NotImplementedError R, p = conversions.T2Rp(T) if joint_type == self._pb_client.JOINT_SPHERICAL: dR = conversions.A2R(a_real[dof_cnt:dof_cnt + 3]) dof_cnt += 3 elif joint_type == self._pb_client.JOINT_REVOLUTE: axis = agent.get_joint_axis(j) angle = a_real[dof_cnt:dof_cnt + 1] dR = conversions.A2R(axis * angle) dof_cnt += 1 else: raise NotImplementedError T_new = conversions.Rp2T(np.dot(R, dR), p) ref_pose.set_transform(char_info.bvh_map[j], T_new, do_ortho_norm=False, local=True) return ref_pose
def create_motion_from_amass_data(filename, bm, override_betas=None): bdata = np.load(filename) if override_betas is not None: betas = torch.Tensor(override_betas[:10][np.newaxis]).to("cpu") else: betas = torch.Tensor(bdata["betas"][:10][np.newaxis]).to("cpu") skel = create_skeleton_from_amass_bodymodel( bm, betas, len(joint_names), joint_names, ) fps = float(bdata["mocap_framerate"]) root_orient = bdata["poses"][:, :3] # controls the global root orientation pose_body = bdata["poses"][:, 3:66] # controls body joint angles trans = bdata["trans"][:, :3] # controls the finger articulation motion = motion_class.Motion(skel=skel, fps=fps) num_joints = skel.num_joints() parents = bm.kintree_table[0].long()[:num_joints] for frame in range(pose_body.shape[0]): pose_body_frame = pose_body[frame] root_orient_frame = root_orient[frame] root_trans_frame = trans[frame] pose_data = [] for j in range(num_joints): if j == 0: T = conversions.Rp2T(conversions.A2R(root_orient_frame), root_trans_frame) else: T = conversions.R2T( conversions.A2R(pose_body_frame[(j - 1) * 3:(j - 1) * 3 + 3])) pose_data.append(T) motion.add_one_frame(pose_data) return motion
def add_noise_to_pose_vel(self, agent, pose, vel=None, return_as_copied=True): ''' Add a little bit of noise to the given pose and velocity ''' ref_pose = copy.deepcopy(pose) if return_as_copied else pose if vel: ref_vel = copy.deepcopy(vel) if return_as_copied else vel dof_cnt = 0 for j in agent._joint_indices: joint_type = agent.get_joint_type(j) ''' Ignore fixed joints ''' if joint_type == self._pb_client.JOINT_FIXED: continue ''' Ignore if there is no corresponding joint ''' if agent._char_info.bvh_map[j] == None: continue T = ref_pose.get_transform(agent._char_info.bvh_map[j], local=True) R, p = conversions.T2Rp(T) if joint_type == self._pb_client.JOINT_SPHERICAL: dR = math.random_rotation( mu_theta=agent._char_info.noise_pose[j][0], sigma_theta=agent._char_info.noise_pose[j][1], lower_theta=agent._char_info.noise_pose[j][2], upper_theta=agent._char_info.noise_pose[j][3]) dof_cnt += 3 elif joint_type == self._pb_client.JOINT_REVOLUTE: theta = math.truncnorm(mu=agent._char_info.noise_pose[j][0], sigma=agent._char_info.noise_pose[j][1], lower=agent._char_info.noise_pose[j][2], upper=agent._char_info.noise_pose[j][3]) joint_axis = agent.get_joint_axis(j) dR = conversions.A2R(joint_axis * theta) dof_cnt += 1 else: raise NotImplementedError T_new = conversions.Rp2T(np.dot(R, dR), p) ref_pose.set_transform(agent._char_info.bvh_map[j], T_new, do_ortho_norm=False, local=True) if vel is not None: dw = math.truncnorm( mu=np.full(3, agent._char_info.noise_vel[j][0]), sigma=np.full(3, agent._char_info.noise_vel[j][1]), lower=np.full(3, agent._char_info.noise_vel[j][2]), upper=np.full(3, agent._char_info.noise_vel[j][3])) ref_vel.data_local[j][:3] += dw return ref_pose, ref_vel