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 blend(pose1, pose2, alpha=0.5): assert 0.0 <= alpha <= 1.0 pose_new = copy.deepcopy(pose1) for j in range(pose1.skel.num_joints()): R0, p0 = conversions.T2Rp(pose1.get_transform(j, local=True)) R1, p1 = conversions.T2Rp(pose2.get_transform(j, local=True)) R, p = math.slerp(R0, R1, alpha), math.lerp(p0, p1, alpha) pose_new.set_transform(j, conversions.Rp2T(R, p), local=True) return pose_new
def difference(self, node_A, node_B, frames_compare, num_comparison): """ Calculate the difference between the two given nodes. """ motion_idx_A = node_A["motion_idx"] frames_A = node_A["frame_start"] motion_idx_B = node_B["motion_idx"] frames_B = node_B["frame_start"] diff_pose = 0.0 diff_root_ee = 0.0 diff_trajectory = 0.0 for k in range(0, frames_compare + 1, (frames_compare + 1) // num_comparison): pose = self.motions[motion_idx_A].get_pose_by_frame(frames_A + k) vel = self.motions[motion_idx_A].get_velocity_by_frame(frames_A + k) pose_j = self.motions[motion_idx_B].get_pose_by_frame(frames_B + k) vel_j = self.motions[motion_idx_B].get_velocity_by_frame(frames_B + k) if k == 0: T_ref = pose.get_facing_transform() T_ref_j = pose_j.get_facing_transform() diff_pose += similarity.pose_similarity(pose, pose_j, vel, vel_j, self.w_joint_pos, self.w_joint_vel, self.w_joints) diff_root_ee += similarity.root_ee_similarity( pose, pose_j, vel, vel_j, self.w_root_pos, self.w_root_vel, self.w_ee_pos, self.w_ee_vel, T_ref, T_ref_j, ) if self.w_trajectory > 0.0: R, p = conversions.T2Rp(pose.get_facing_transform()) R_j, p_j = conversions.T2Rp(pose_j.get_facing_transform()) if k > 0: d = np.dot(R_prev.transpose(), p - p_prev) d_j = np.dot(R_j_prev.transpose(), p_j - p_j_prev) d = d - d_j diff_trajectory += np.dot(d, d) R_prev, p_prev = R, p R_j_prev, p_j_prev = R_j, p_j diff_pose /= num_comparison diff_root_ee /= num_comparison diff_trajectory /= num_comparison diff = diff_pose + diff_root_ee + diff_trajectory return diff
def calculate_metrics(pred_seqs, tgt_seqs): metric_frames = [6, 12, 18, 24] R_pred, _ = conversions.T2Rp(pred_seqs) R_tgt, _ = conversions.T2Rp(tgt_seqs) euler_error = metrics.euler_diff( R_pred[:, :, amass_dip.SMPL_MAJOR_JOINTS], R_tgt[:, :, amass_dip.SMPL_MAJOR_JOINTS], ) euler_error = np.mean(euler_error, axis=0) mae = {frame: np.sum(euler_error[:frame]) for frame in metric_frames} return mae
def interpolate(cls, pose1, pose2, alpha): skel = pose1.skel data = [] for j in skel.joints: R1, p1 = conversions.T2Rp(pose1.get_transform(j, local=True)) R2, p2 = conversions.T2Rp(pose2.get_transform(j, local=True)) R, p = ( math.slerp(R1, R2, alpha), math.lerp(p1, p2, alpha), ) data.append(conversions.Rp2T(R, p)) return Pose(pose1.skel, data)
def pose_similarity( pose1, pose2, vel1=None, vel2=None, w_joint_pos=0.9, w_joint_vel=0.1, w_joints=None, apply_root_correction=True, ): """ This only measure joint angle difference (i.e. root translation will not be considered). If 'apply_root_correction' is True, then pose2 will be rotated automatically in a way that its root rotation is closest to the root rotation of pose1, where'root_correction_axis' defines the geodesic curve. """ assert pose1.skel.num_joints() == pose2.skel.num_joints() skel = pose1.skel if vel1 is not None: assert vel2 is not None assert vel1.skel.num_joints() == vel2.skel.num_joints() if w_joints is None: w_joints = np.ones(skel.num_joints()) """ joint angle difference """ diff_pos = 0.0 if w_joint_pos > 0.0: root_idx = skel.get_index_joint(skel.root_joint) v_up_env = pose1.skel.v_up_env for j in range(skel.num_joints()): R1, p1 = conversions.T2Rp(pose1.get_transform(j, local=True)) R2, p2 = conversions.T2Rp(pose2.get_transform(j, local=True)) if apply_root_correction and j == root_idx: Q1 = conversions.R2Q(R1) Q2 = conversions.R2Q(R2) Q2, _ = quaternion.Q_closest(Q1, Q2, v_up_env) R2 = conversions.Q2R(Q2) # TODO: Verify if logSO3 is same as R2A dR = conversions.R2A(np.dot(np.transpose(R1), R2)) diff_pos += w_joints[j] * np.dot(dR, dR) """ joint angular velocity difference """ diff_vel = 0.0 if vel1 is not None and w_joint_vel > 0.0: skel = vel1.skel for j in range(skel.num_joints()): dw = vel2.get_angular(j, local=True) - vel1.get_angular(j, local=True) diff_vel += w_joints[j] * np.dot(dw, dw) return (w_joint_pos * diff_pos + w_joint_vel * diff_vel) / skel.num_joints()
def test_motion(self): motion = bvh.load(file=TEST_SINUSOIDAL_FILE) # Inspect 0th frame, root joint T = motion.get_pose_by_frame(0).get_transform(0, local=False) _, p = conversions.T2Rp(T) self.assertListEqual(list(p), [-3, 6, 5]) # Inspect 100th frame, root joint T = motion.get_pose_by_frame(100).get_transform(0, local=False) _, p = conversions.T2Rp(T) self.assertListEqual(list(p), [-3, 6, 5]) # Inspect 100th frame, "child2" joint T = motion.get_pose_by_frame(100).get_transform("child2", local=False) _, p = conversions.T2Rp(T) self.assertListEqual(list(p), [-8, 11, 5])
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 motion2 = translate(motion2, dp) t_total = motion1.fps * frame_source t_processed = 0.0 poses_new = [] dt = 1 / motion2.fps for i in range(motion2.num_frames()): 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(copy.deepcopy(poses_new[i].data)) return combined_motion
def test_R2T(self): T = test_utils.get_random_T() R, _ = conversions.T2Rp(T) np.testing.assert_almost_equal( conversions.R2T(np.array([R]))[0], conversions.R2T(R), )
def test_p2T(self): T = test_utils.get_random_T() _, p = conversions.T2Rp(T) np.testing.assert_almost_equal( conversions.p2T(np.array([p]))[0], conversions.p2T(p), )
def _write_hierarchy(motion, file, joint, scale=1.0, rot_order="XYZ", tab=""): def rot_order_to_str(order): if order == "xyz" or order == "XYZ": return "Xrotation Yrotation Zrotation" elif order == "zyx" or order == "ZYX": return "Zrotation Yrotation Xrotation" else: raise NotImplementedError joint_order = [joint.name] is_root_joint = joint.parent_joint is None if is_root_joint: file.write(tab + "ROOT %s\n" % joint.name) else: file.write(tab + "JOINT %s\n" % joint.name) file.write(tab + "{\n") R, p = conversions.T2Rp(joint.xform_from_parent_joint) p *= scale file.write(tab + "\tOFFSET %f %f %f\n" % (p[0], p[1], p[2])) if is_root_joint: file.write(tab + "\tCHANNELS 6 Xposition Yposition Zposition %s\n" % rot_order_to_str(rot_order)) else: file.write(tab + "\tCHANNELS 3 %s\n" % rot_order_to_str(rot_order)) for child_joint in joint.child_joints: child_joint_order = _write_hierarchy(motion, file, child_joint, scale, rot_order, tab + "\t") joint_order.extend(child_joint_order) if len(joint.child_joints) == 0: file.write(tab + "\tEnd Site\n") file.write(tab + "\t{\n") file.write(tab + "\t\tOFFSET %f %f %f\n" % (0.0, 0.0, 0.0)) file.write(tab + "\t}\n") file.write(tab + "}\n") return joint_order
def render_joint(joint, option): glPushAttrib(GL_LIGHTING) if option.lighting: glEnable(GL_LIGHTING) else: glDisable(GL_LIGHTING) j_type = joint.type() T0 = joint.child_bodynode.transform() T1 = joint.transform_from_child_body_node() T = T0.dot(T1) R, p = conversions.T2Rp(T) scale = option.scale if option.render_pos: gl_render.render_point(p, color=option.color_pos, scale=scale, radius=0.1) if option.render_ori: if j_type == "WeldJoint": pass elif j_type == "RevoluteJoint": axis1 = joint.axis_in_world_frame() gl_render.render_line(p, p + scale * axis1, color=option.color_ori[0]) elif j_type == "UniversalJoint": axis1 = joint.axis1_in_world_frame() axis2 = joint.axis2_in_world_frame() gl_render.render_line(p, p + scale * axis1, color=option.color_ori[0]) gl_render.render_line(p, p + scale * axis2, color=option.color_ori[1]) elif j_type == "EulerJoint": gl_render.render_transform(T, scale=scale, color_pos=option.color_pos, color_ori=option.color_ori) elif j_type == "BallJoint": gl_render.render_transform(T, scale=scale, color_pos=option.color_pos, color_ori=option.color_ori) elif j_type == "TranslationalJoint": gl_render.render_transform(T, scale=scale, color_pos=option.color_pos, color_ori=option.color_ori) elif j_type == "FreeJoint": gl_render.render_transform(T, scale=scale, color_pos=option.color_pos, color_ori=option.color_ori) else: raise NotImplementedError("Not Implemented") glPopAttrib(GL_LIGHTING)
def _state_body(self, agent, T_ref=None, include_com=True, include_p=True, include_Q=True, include_v=True, include_w=True, return_stacked=True): if T_ref is None: T_ref = agent.get_facing_transform(self.get_ground_height()) R_ref, p_ref = conversions.T2Rp(T_ref) R_ref_inv = R_ref.transpose() link_states = [] link_states.append(agent.get_root_state()) ps, Qs, vs, ws = agent.get_link_states() for j in agent._joint_indices: link_states.append((ps[j], Qs[j], vs[j], ws[j])) state = [] for i, s in enumerate(link_states): p, Q, v, w = s[0], s[1], s[2], s[3] if include_p: p_rel = np.dot(R_ref_inv, p - p_ref) state.append( p_rel) # relative position w.r.t. the reference frame if include_Q: Q_rel = conversions.R2Q(np.dot(R_ref_inv, conversions.Q2R(Q))) Q_rel = quaternion.Q_op(Q_rel, op=["normalize", "halfspace"]) state.append( Q_rel) # relative rotation w.r.t. the reference frame if include_v: v_rel = np.dot(R_ref_inv, v) state.append( v_rel) # relative linear vel w.r.t. the reference frame if include_w: w_rel = np.dot(R_ref_inv, w) state.append( w_rel) # relative angular vel w.r.t. the reference frame if include_com: if i == 0: p_com = agent._link_masses[i] * p v_com = agent._link_masses[i] * v else: p_com += agent._link_masses[i] * p v_com += agent._link_masses[i] * v if include_com: p_com /= agent._link_total_mass v_com /= agent._link_total_mass state.append(np.dot(R_ref_inv, p_com - p_ref)) state.append(np.dot(R_ref_inv, v_com)) if return_stacked: return np.hstack(state) else: return state
def compute(cls, pose1, pose2, dt): assert pose1.skel == pose2.skel data_local = [] data_global = [] assert dt > constants.EPSILON for joint in pose1.skel.joints: T1 = pose1.get_transform(joint, local=True) T2 = pose2.get_transform(joint, local=True) dR, dp = conversions.T2Rp(np.dot(math.invertT(T1), T2)) w, v = conversions.R2A(dR) / dt, dp / dt data_local.append(np.hstack((w, v))) T1 = pose1.get_transform(joint, local=False) T2 = pose2.get_transform(joint, local=False) dR, dp = conversions.T2Rp(np.dot(math.invertT(T1), T2)) w, v = conversions.R2A(dR) / dt, dp / dt data_global.append(np.hstack((w, v))) return np.array(data_local), np.array(data_global)
def set_pose(self, pose, vel=None): ''' Velocity should be represented w.r.t. local frame ''' # Root joint T = pose.get_transform(self._char_info.bvh_map[self._char_info.ROOT], local=False) Q, p = conversions.T2Qp(T) p *= self._ref_scale v, w = None, None if vel is not None: # Here we give a root orientation to get velocities represeted in world frame. R = conversions.Q2R(Q) w = vel.get_angular(self._char_info.bvh_map[self._char_info.ROOT], False, R) v = vel.get_linear(self._char_info.bvh_map[self._char_info.ROOT], False, R) v *= self._ref_scale bu.set_base_pQvw(self._pb_client, self._body_id, p, Q, v, w) # Other joints indices = [] state_pos = [] state_vel = [] for j in self._joint_indices: joint_type = self._joint_type[j] # When the target joint do not have dof, we simply ignore it if joint_type == self._pb_client.JOINT_FIXED: continue # When there is no matching between the given pose and the simulated character, # the character just tries to hold its initial pose if self._char_info.bvh_map[j] is None: state_pos.append(self._joint_pose_init[j]) state_vel.append(self._joint_vel_init[j]) else: T = pose.get_transform(self._char_info.bvh_map[j], local=True) if joint_type == self._pb_client.JOINT_SPHERICAL: Q, p = conversions.T2Qp(T) w = np.zeros(3) if vel is None else vel.get_angular( self._char_info.bvh_map[j], local=True) state_pos.append(Q) state_vel.append(w) elif joint_type == self._pb_client.JOINT_REVOLUTE: joint_axis = self.get_joint_axis(j) R, p = conversions.T2Rp(T) w = np.zeros(3) if vel is None else vel.get_angular( self._char_info.bvh_map[j], local=True) state_pos.append([math.project_rotation_1D(R, joint_axis)]) state_vel.append( [math.project_angular_vel_1D(w, joint_axis)]) else: raise NotImplementedError() indices.append(j) bu.set_joint_pv(self._pb_client, self._body_id, indices, state_pos, state_vel)
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
def test_Rp2T(self): T = test_utils.get_random_T() R, p = conversions.T2Rp(T) np.testing.assert_almost_equal( conversions.Rp2T(np.array([R]), np.array([p]))[0], conversions.Rp2T(R, p), ) T_test = conversions.Rp2T(R, p) for t, t_test in zip(T.flatten(), T_test.flatten()): self.assertAlmostEqual(t, t_test)
def save(motion, filename, scale=1.0, rot_order="XYZ", verbose=False): if verbose: print(" > > Save BVH file: %s" % filename) with open(filename, "w") as f: """ Write hierarchy """ if verbose: print(" > > > > Write BVH hierarchy") f.write("HIERARCHY\n") joint_order = _write_hierarchy( motion, f, motion.skel.root_joint, scale, rot_order ) """ Write data """ if verbose: print(" > > > > Write BVH data") t_start = 0 dt = 1.0 / motion.fps num_frames = motion.num_frames() f.write("MOTION\n") f.write("Frames: %d\n" % num_frames) f.write("Frame Time: %f\n" % dt) t = t_start for i in range(num_frames): if verbose and i % motion.fps == 0: print( "\r > > > > %d/%d processed (%d FPS)" % (i + 1, num_frames, motion.fps), end=" ", ) pose = motion.get_pose_by_frame(i) for joint_name in joint_order: joint = motion.skel.get_joint(joint_name) R, p = conversions.T2Rp(pose.get_transform(joint, local=True)) p *= scale R1, R2, R3 = conversions.R2E(R, order=rot_order, degrees=True) if joint == motion.skel.root_joint: f.write( "%f %f %f %f %f %f " % (p[0], p[1], p[2], R1, R2, R3) ) else: f.write("%f %f %f " % (R1, R2, R3)) f.write("\n") t += dt if verbose and i == num_frames - 1: print( "\r > > > > %d/%d processed (%d FPS)" % (i + 1, num_frames, motion.fps) ) f.close()
def render_transform( T, scale=1.0, line_width=1.0, point_size=0.05, render_pos=True, render_ori=[True, True, True], color_pos=[0, 0, 0, 1], color_ori=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], use_arrow=False, ): glLineWidth(line_width) R, p = conversions.T2Rp(T) glPushMatrix() glTranslated(p[0], p[1], p[2]) glScalef(scale, scale, scale) if render_pos: glColor(color_pos) glutSolidSphere(0.5 * point_size, 10, 10) if render_ori: o = np.zeros(3) if use_arrow: if render_ori[0]: render_arrow(o, o + R[:, 0], D=line_width * 0.02, color=color_ori[0]) if render_ori[1]: render_arrow(o, o + R[:, 1], D=line_width * 0.02, color=color_ori[1]) if render_ori[2]: render_arrow(o, o + R[:, 2], D=line_width * 0.02, color=color_ori[2]) else: if render_ori[0]: render_line(o, o + R[:, 0], color=color_ori[0]) if render_ori[1]: render_line(o, o + R[:, 1], color=color_ori[1]) if render_ori[2]: render_line(o, o + R[:, 2], color=color_ori[2]) glPopMatrix()
def state_imitation(self, sim_agent, kin_agent, poses, vels, include_abs, include_rel): assert len(poses) == len(vels) R_sim, p_sim = conversions.T2Rp( sim_agent.get_facing_transform(self.get_ground_height())) R_sim_inv = R_sim.transpose() state_sim = self._state_body(sim_agent, None, return_stacked=False) state = [] state_kin_orig = kin_agent.save_states() for pose, vel in zip(poses, vels): kin_agent.set_pose(pose, vel) state_kin = self._state_body(kin_agent, None, return_stacked=False) # Add pos/vel values if include_abs: state.append(np.hstack(state_kin)) # Add difference of pos/vel values if include_rel: for j in range(len(state_sim)): if len(state_sim[j]) == 3: state.append(state_sim[j] - state_kin[j]) elif len(state_sim[j]) == 4: state.append( self._pb_client.getDifferenceQuaternion( state_sim[j], state_kin[j])) else: raise NotImplementedError ''' Add facing frame differences ''' R_kin, p_kin = conversions.T2Rp( kin_agent.get_facing_transform(self.get_ground_height())) state.append(np.dot(R_sim_inv, p_kin - p_sim)) state.append(np.dot(R_sim_inv, kin_agent.get_facing_direction())) kin_agent.restore_states(state_kin_orig) return np.hstack(state)
def render_path(data, color=[0.0, 0.0, 0.0], scale=1.0, line_width=1.0, point_size=1.0): glColor(color) glLineWidth(line_width) glBegin(GL_LINE_STRIP) for d in data: R, p = conversions.T2Rp(d) glVertex3d(p[0], p[1], p[2]) glEnd() for d in data: render_transform(d, scale, line_width, point_size)
def transform(motion, T, local=False): """ Apply transform to all poses of a motion sequence. The operation is done in-place. Args: motion: Motion sequence to be transformed T: Transformation matrix of shape (4, 4) to be applied to poses of motion local: Optional; Set local=True if the transformations are to be applied locally, relative to parent of each joint. """ for pose_id in range(len(motion.poses)): R0, p0 = conversions.T2Rp(motion.poses[pose_id].get_root_transform()) R1, p1 = conversions.T2Rp(T) if local: R, p = np.dot(R0, R1), p0 + np.dot(R0, p1) else: R, p = np.dot(R1, R0), p0 + p1 motion.poses[pose_id].set_root_transform( conversions.Rp2T(R, p), local=False, ) return motion
def test_load_motion(self): # Load file motion = bvh.load(file=TEST_SINUSOIDAL_FILE) # Read pose data from all frames (in Euler angle) with open(TEST_SINUSOIDAL_FILE) as f: file_content = f.readlines() for frame_num, line in enumerate(file_content[-motion.num_frames():]): # Skip first 3 entries that store translation data, and read the # Euler angle data of joints angle_data = np.array(list(map(float, line.strip().split()))[3:]).reshape( -1, 3) for T, true_E in zip(motion.poses[frame_num].data, angle_data): R, _ = conversions.T2Rp(T) E = conversions.R2E(R, order="XYZ", degrees=True) np.testing.assert_almost_equal(E, true_E)
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 get_facing_direction_position(self, ground_height=0.0): R, p = conversions.T2Rp(self.get_root_transform()) d = np.dot(R, self._char_info.v_face) if np.allclose(d, self._char_info.v_up_env): msg = \ '\n+++++++++++++++++WARNING+++++++++++++++++++\n'+\ 'The facing direction is ill-defined ' +\ '(i.e. parellel to the world up-vector).\n'+\ 'A random direction will be assigned for the direction\n'+\ 'Be careful if your system is sensitive to the facing direction\n'+\ '+++++++++++++++++++++++++++++++++++++++++++\n' warnings.warn(msg) d = math.random_unit_vector() d = d - math.projectionOnVector(d, self._char_info.v_up_env) p = p - math.projectionOnVector(p, self._char_info.v_up_env) if ground_height != 0.0: p += ground_height * self._char_info.v_up_env return d / np.linalg.norm(d), p
def test_save_motion(self): # Load file motion = bvh.load(file=TEST_SINUSOIDAL_FILE) with tempfile.NamedTemporaryFile() as fp: # Save loaded file bvh.save(motion, fp.name, rot_order="XYZ") # Reload saved file and test if it is same as reference file # Read pose data from all frames (in Euler angle) with open(TEST_SINUSOIDAL_FILE) as f: orig_file = f.readlines() with open(fp.name) as f: saved_file = f.readlines() for orig_line, saved_line in zip( orig_file[-motion.num_frames():], saved_file[-motion.num_frames():], ): # Skip first 3 entries that store translation data, and read # the Euler angle data of joints orig_data, saved_data = [ np.array(list(map(float, line.strip().split()))) for line in [orig_line, saved_line] ] np.testing.assert_almost_equal(orig_data, saved_data) # Reload saved file and test if it has the same data as original # motion object with open(TEST_SINUSOIDAL_FILE) as f: file_content = f.readlines() for frame_num, line in enumerate( file_content[-motion.num_frames():]): # Skip first 3 entries that store translation data, and read # the Euler angle data of joints angle_data = np.array( list(map(float, line.strip().split()))[3:]).reshape(-1, 3) for T, true_E in zip(motion.poses[frame_num].data, angle_data): R, _ = conversions.T2Rp(T) E = conversions.R2E(R, order="XYZ", degrees=True) np.testing.assert_almost_equal(E, true_E)
def set_pose_by_xform(self, xform): assert len(xform) == len(self._char_info.bvh_map_inv) ''' Base ''' Q, p = conversions.T2Qp(xform[0]) p *= self._ref_scale bu.set_base_pQvw(self._pb_client, self._body_id, p, Q, None, None) ''' Others ''' indices = [] state_pos = [] state_vel = [] idx = -1 for k, j in self._char_info.bvh_map_inv.items(): idx += 1 if idx == 0: continue if j is None: continue joint_type = self._joint_type[j] if joint_type == self._pb_client.JOINT_FIXED: continue T = xform[idx] if joint_type == self._pb_client.JOINT_SPHERICAL: Q, p = conversions.T2Qp(T) w = np.zeros(3) state_pos.append(Q) state_vel.append(w) elif joint_type == self._pb_client.JOINT_REVOLUTE: joint_axis = self.get_joint_axis(j) R, p = conversions.T2Rp(T) w = np.zeros(3) state_pos.append(math.project_rotation_1D(R, joint_axis)) state_vel.append(math.project_angular_vel_1D(w, joint_axis)) else: raise NotImplementedError() indices.append(j) bu.set_joint_pv(self._pb_client, self._body_id, indices, state_pos, state_vel)
def get_facing_direction_position(self): R, p = conversions.T2Rp(self.get_root_transform()) d = np.dot(R, self.skel.v_face) d = d - math.projectionOnVector(d, self.skel.v_up_env) p = p - math.projectionOnVector(p, self.skel.v_up_env) return d / np.linalg.norm(d), p
def get_task_error(self, idx, data_prev, data_next, action): error = {} sim_agent = self._sim_agent[idx] char_info = sim_agent._char_info data = data_next[idx] sim_root_p, sim_root_Q, sim_root_v, sim_root_w = data['sim_root_pQvw'] sim_link_p, sim_link_Q, sim_link_v, sim_link_w = data['sim_link_pQvw'] sim_joint_p, sim_joint_v = data['sim_joint_pv'] sim_facing_frame = data['sim_facing_frame'] R_sim_f, p_sim_f = conversions.T2Rp(sim_facing_frame) R_sim_f_inv = R_sim_f.transpose() sim_com, sim_com_vel = data['sim_com'], data['sim_com_vel'] kin_root_p, kin_root_Q, kin_root_v, kin_root_w = data['kin_root_pQvw'] kin_link_p, kin_link_Q, kin_link_v, kin_link_w = data['kin_link_pQvw'] kin_joint_p, kin_joint_v = data['kin_joint_pv'] kin_facing_frame = data['kin_facing_frame'] R_kin_f, p_kin_f = conversions.T2Rp(kin_facing_frame) R_kin_f_inv = R_kin_f.transpose() kin_com, kin_com_vel = data['kin_com'], data['kin_com_vel'] indices = range(len(sim_joint_p)) if 'pose_pos' in self._reward_names[idx]: error['pose_pos'] = 0.0 for j in indices: joint_type = sim_agent.get_joint_type(j) if joint_type == self._pb_client.JOINT_FIXED: continue elif joint_type == self._pb_client.JOINT_SPHERICAL: dQ = self._pb_client.getDifferenceQuaternion( sim_joint_p[j], kin_joint_p[j]) _, diff_pose_pos = self._pb_client.getAxisAngleFromQuaternion( dQ) else: diff_pose_pos = sim_joint_p[j] - kin_joint_p[j] error['pose_pos'] += char_info.joint_weight[j] * np.dot( diff_pose_pos, diff_pose_pos) if len(indices) > 0: error['pose_pos'] /= len(indices) if 'pose_vel' in self._reward_names[idx]: error['pose_vel'] = 0.0 for j in indices: joint_type = sim_agent.get_joint_type(j) if joint_type == self._pb_client.JOINT_FIXED: continue else: diff_pose_vel = sim_joint_v[j] - kin_joint_v[j] error['pose_vel'] += char_info.joint_weight[j] * np.dot( diff_pose_vel, diff_pose_vel) if len(indices) > 0: error['pose_vel'] /= len(indices) if 'ee' in self._reward_names[idx]: error['ee'] = 0.0 for j in char_info.end_effector_indices: sim_ee_local = np.dot(R_sim_f_inv, sim_link_p[j] - p_sim_f) kin_ee_local = np.dot(R_kin_f_inv, kin_link_p[j] - p_kin_f) diff_pos = sim_ee_local - kin_ee_local error['ee'] += np.dot(diff_pos, diff_pos) if len(char_info.end_effector_indices) > 0: error['ee'] /= len(char_info.end_effector_indices) if 'root' in self._reward_names[idx]: diff_root_p = sim_root_p - kin_root_p _, diff_root_Q = self._pb_client.getAxisAngleFromQuaternion( self._pb_client.getDifferenceQuaternion( sim_root_Q, kin_root_Q)) diff_root_v = sim_root_v - kin_root_v diff_root_w = sim_root_w - kin_root_w error['root'] = 1.0 * np.dot(diff_root_p, diff_root_p) + \ 0.1 * np.dot(diff_root_Q, diff_root_Q) + \ 0.01 * np.dot(diff_root_v, diff_root_v) + \ 0.001 * np.dot(diff_root_w, diff_root_w) if 'com' in self._reward_names[idx]: diff_com = np.dot(R_sim_f_inv, sim_com - p_sim_f) - np.dot( R_kin_f_inv, kin_com - p_kin_f) diff_com_vel = sim_com_vel - kin_com_vel error['com'] = 1.0 * np.dot(diff_com, diff_com) + \ 0.1 * np.dot(diff_com_vel, diff_com_vel) return error
def compare_and_connect_edge( node_id, nodes, motions, frames_compare, w_joints, w_joint_pos, w_joint_vel, w_root_pos, w_root_vel, w_ee_pos, w_ee_vel, w_trajectory, diff_threshold, num_comparison, verbose, ): node = nodes[node_id] res = [] num_nodes = len(nodes) motion_idx = node["motion_idx"] frame_end = node["frame_end"] for j in range(num_nodes): motion_idx_j = nodes[j]["motion_idx"] frame_start_j = nodes[j]["frame_start"] diff_pose = 0.0 diff_root_ee = 0.0 diff_trajectory = 0.0 for k in range(0, frames_compare + 1, (frames_compare + 1) // num_comparison): pose = motions[motion_idx].get_pose_by_frame(frame_end + k) vel = motions[motion_idx].get_velocity_by_frame(frame_end + k) pose_j = motions[motion_idx_j].get_pose_by_frame(frame_start_j + k) vel_j = motions[motion_idx_j].get_velocity_by_frame(frame_start_j + k) if k == 0: T_ref = pose.get_facing_transform() T_ref_j = pose_j.get_facing_transform() diff_pose += similarity.pose_similarity(pose, pose_j, vel, vel_j, w_joint_pos, w_joint_vel, w_joints) diff_root_ee += similarity.root_ee_similarity( pose, pose_j, vel, vel_j, w_root_pos, w_root_vel, w_ee_pos, w_ee_vel, T_ref, T_ref_j, ) if w_trajectory > 0.0: R, p = conversions.T2Rp(pose.get_facing_transform()) R_j, p_j = conversions.T2Rp(pose_j.get_facing_transform()) if k > 0: d = np.dot(R_prev.transpose(), p - p_prev) d_j = np.dot(R_j_prev.transpose(), p_j - p_j_prev) d = d - d_j diff_trajectory += np.dot(d, d) R_prev, p_prev = R, p R_j_prev, p_j_prev = R_j, p_j diff_pose /= num_comparison diff_root_ee /= num_comparison diff_trajectory /= num_comparison diff = diff_pose + diff_root_ee + diff_trajectory if diff <= diff_threshold: res.append((diff, node_id, j)) return res