def redirect(positions, joint0, joint1, forward='z'): forwarddir = { 'x': np.array([[[1, 0, 0]]]), 'y': np.array([[[0, 1, 0]]]), 'z': np.array([[[0, 0, 1]]]), }[forward] direction = (positions[:, joint0] - positions[:, joint1]).mean(axis=0)[np.newaxis, np.newaxis] direction = direction / np.sqrt(np.sum(direction**2)) rotation = Quaternions.between(direction, forwarddir).constrained_y() return rotation * positions
def process_data(anim, phase, gait, type='flat'): """ Do FK """ global_xforms = Animation.transforms_global(anim) global_positions = global_xforms[:, :, :3, 3] / global_xforms[:, :, 3:, 3] # global_rotations = Quaternions.from_transforms_toDM( # global_xforms, adj_axis='y', adj_degree=90) global_rotations = Quaternions.from_transforms(global_xforms) """ Extract Forward Direction """ sdr_l, sdr_r, hip_l, hip_r = 6, 3, 13, 9 across = ((global_positions[:, sdr_l] - global_positions[:, sdr_r]) + (global_positions[:, hip_l] - global_positions[:, hip_r])) across = across / np.sqrt((across**2).sum(axis=-1))[..., np.newaxis] """ Smooth Forward Direction """ direction_filterwidth = 20 forward = filters.gaussian_filter1d(np.cross(across, np.array([[0, 1, 0]])), direction_filterwidth, axis=0, mode='nearest') forward = forward / np.sqrt((forward**2).sum(axis=-1))[..., np.newaxis] root_rotation = Quaternions.between( forward, np.array([[0, 0, 1]]).repeat(len(forward), axis=0))[:, np.newaxis] """ Local Space """ local_positions = global_positions.copy() local_positions[:, :, 0] = local_positions[:, :, 0] - \ local_positions[:, 0:1, 0] local_positions[:, :, 2] = local_positions[:, :, 2] - \ local_positions[:, 0:1, 2] local_positions = root_rotation[:-1] * local_positions[:-1] local_velocities = root_rotation[:-1] * \ (global_positions[1:] - global_positions[:-1]) local_rotations = abs((root_rotation[:-1] * global_rotations[:-1])).log() root_velocity = root_rotation[:-1] * \ (global_positions[1:, 0:1] - global_positions[:-1, 0:1]) root_rvelocity = Pivots.from_quaternions(root_rotation[1:] * -root_rotation[:-1]).ps """ Foot Contacts """ fid_l, fid_r = np.array([15, 16]), np.array([11, 12]) velfactor = np.array([0.02, 0.02]) feet_l_x = (global_positions[1:, fid_l, 0] - global_positions[:-1, fid_l, 0])**2 feet_l_y = (global_positions[1:, fid_l, 1] - global_positions[:-1, fid_l, 1])**2 feet_l_z = (global_positions[1:, fid_l, 2] - global_positions[:-1, fid_l, 2])**2 feet_l = (((feet_l_x + feet_l_y + feet_l_z) < velfactor)).astype(np.float) feet_r_x = (global_positions[1:, fid_r, 0] - global_positions[:-1, fid_r, 0])**2 feet_r_y = (global_positions[1:, fid_r, 1] - global_positions[:-1, fid_r, 1])**2 feet_r_z = (global_positions[1:, fid_r, 2] - global_positions[:-1, fid_r, 2])**2 feet_r = (((feet_r_x + feet_r_y + feet_r_z) < velfactor)).astype(np.float) """ Phase """ dphase = phase[1:] - phase[:-1] dphase[dphase < 0] = (1.0 - phase[:-1] + phase[1:])[dphase < 0] """ Adjust Crouching Gait Value """ if type == 'flat': crouch_low, crouch_high = 80, 130 head = 16 gait[:-1, 3] = 1 - \ np.clip((global_positions[:-1, head, 1] - 80) / (130 - 80), 0, 1) gait[-1, 3] = gait[-2, 3] """ Start Windows """ Pc, Xc, Yc = [], [], [] for i in range(window, len(anim) - window - 1, 1): rootposs = root_rotation[i:i + 1, 0] * ( global_positions[i - window:i + window:10, 0] - global_positions[i:i + 1, 0]) rootdirs = root_rotation[i:i + 1, 0] * \ forward[i - window:i + window:10] rootgait = gait[i - window:i + window:10] Pc.append(phase[i]) Xc.append( np.hstack([ rootposs[:, 0].ravel(), rootposs[:, 2].ravel(), # Trajectory Pos rootdirs[:, 0].ravel(), rootdirs[:, 2].ravel(), # Trajectory Dir rootgait[:, 0].ravel(), rootgait[:, 1].ravel(), # Trajectory Gait rootgait[:, 2].ravel(), rootgait[:, 3].ravel(), rootgait[:, 4].ravel(), rootgait[:, 5].ravel(), local_positions[i - 1].ravel(), # Joint Pos local_velocities[i - 1].ravel(), # Joint Vel ])) rootposs_next = root_rotation[i + 1:i + 2, 0] * ( global_positions[i + 1:i + window + 1:10, 0] - global_positions[i + 1:i + 2, 0]) rootdirs_next = root_rotation[i + 1:i + 2, 0] * forward[i + 1:i + window + 1:10] Yc.append( np.hstack([ root_velocity[i, 0, 0].ravel(), # Root Vel X root_velocity[i, 0, 2].ravel(), # Root Vel Y root_rvelocity[i].ravel(), # Root Rot Vel dphase[i], # Change in Phase np.concatenate([feet_l[i], feet_r[i]], axis=-1), # Contacts rootposs_next[:, 0].ravel(), rootposs_next[:, 2].ravel(), # Next Trajectory Pos rootdirs_next[:, 0].ravel(), rootdirs_next[:, 2].ravel(), # Next Trajectory Dir local_positions[i].ravel(), # Joint Pos local_velocities[i].ravel(), # Joint Vel local_rotations[i].ravel() # Joint Rot ])) return np.array(Pc), np.array(Xc), np.array(Yc)
def process_heights(anim, nsamples=10, type='flat'): """ Do FK """ global_xforms = Animation.transforms_global(anim) global_positions = global_xforms[:, :, :3, 3] / global_xforms[:, :, 3:, 3] global_rotations = Quaternions.from_transforms(global_xforms) """ Extract Forward Direction """ sdr_l, sdr_r, hip_l, hip_r = 6, 3, 13, 9 across = ((global_positions[:, sdr_l] - global_positions[:, sdr_r]) + (global_positions[:, hip_l] - global_positions[:, hip_r])) across = across / np.sqrt((across**2).sum(axis=-1))[..., np.newaxis] """ Smooth Forward Direction """ direction_filterwidth = 20 forward = filters.gaussian_filter1d(np.cross(across, np.array([[0, 1, 0]])), direction_filterwidth, axis=0, mode='nearest') forward = forward / np.sqrt((forward**2).sum(axis=-1))[..., np.newaxis] root_rotation = Quaternions.between( forward, np.array([[0, 0, 1]]).repeat(len(forward), axis=0))[:, np.newaxis] """ Foot Contacts """ fid_l, fid_r = np.array([15, 16]), np.array([11, 12]) velfactor = np.array([0.02, 0.02]) feet_l_x = (global_positions[1:, fid_l, 0] - global_positions[:-1, fid_l, 0])**2 feet_l_y = (global_positions[1:, fid_l, 1] - global_positions[:-1, fid_l, 1])**2 feet_l_z = (global_positions[1:, fid_l, 2] - global_positions[:-1, fid_l, 2])**2 feet_l = (((feet_l_x + feet_l_y + feet_l_z) < velfactor)) feet_r_x = (global_positions[1:, fid_r, 0] - global_positions[:-1, fid_r, 0])**2 feet_r_y = (global_positions[1:, fid_r, 1] - global_positions[:-1, fid_r, 1])**2 feet_r_z = (global_positions[1:, fid_r, 2] - global_positions[:-1, fid_r, 2])**2 feet_r = (((feet_r_x + feet_r_y + feet_r_z) < velfactor)) # left = feet_l_x + feet_l_y + feet_l_z # right = feet_r_x + feet_r_y + feet_r_z # plt.plot(left) # plt.plot(right) # plt.show() feet_l = np.concatenate([feet_l, feet_l[-1:]], axis=0) feet_r = np.concatenate([feet_r, feet_r[-1:]], axis=0) # print(feet_l, feet_r) """ Toe and Heel Heights """ toe_h, heel_h = 4.0, 5.0 """ Foot Down Positions """ feet_down = np.concatenate([ global_positions[feet_l[:, 0], fid_l[0]] - np.array([0, heel_h, 0]), global_positions[feet_l[:, 1], fid_l[1]] - np.array([0, toe_h, 0]), global_positions[feet_r[:, 0], fid_r[0]] - np.array([0, heel_h, 0]), global_positions[feet_r[:, 1], fid_r[1]] - np.array([0, toe_h, 0]) ], axis=0) """ Foot Up Positions """ feet_up = np.concatenate([ global_positions[~feet_l[:, 0], fid_l[0]] - np.array([0, heel_h, 0]), global_positions[~feet_l[:, 1], fid_l[1]] - np.array([0, toe_h, 0]), global_positions[~feet_r[:, 0], fid_r[0]] - np.array([0, heel_h, 0]), global_positions[~feet_r[:, 1], fid_r[1]] - np.array([0, toe_h, 0]) ], axis=0) """ Down Locations """ feet_down_xz = np.concatenate([feet_down[:, 0:1], feet_down[:, 2:3]], axis=-1) feet_down_xz_mean = feet_down_xz.mean(axis=0) feet_down_y = feet_down[:, 1:2] feet_down_y_mean = feet_down_y.mean(axis=0) feet_down_y_std = feet_down_y.std(axis=0) """ Up Locations """ feet_up_xz = np.concatenate([feet_up[:, 0:1], feet_up[:, 2:3]], axis=-1) feet_up_y = feet_up[:, 1:2] if len(feet_down_xz) == 0: """ No Contacts """ def terr_func(Xp): return np.zeros_like(Xp)[:, :1][np.newaxis].repeat(nsamples, axis=0) elif type == 'flat': """ Flat """ def terr_func(Xp): return np.zeros_like(Xp)[:, :1][np.newaxis].repeat( nsamples, axis=0) + feet_down_y_mean else: """ Terrain Heights """ terr_down_y = patchfunc(patches, feet_down_xz - feet_down_xz_mean) terr_down_y_mean = terr_down_y.mean(axis=1) terr_down_y_std = terr_down_y.std(axis=1) terr_up_y = patchfunc(patches, feet_up_xz - feet_down_xz_mean) """ Fitting Error """ terr_down_err = 0.1 * ( ((terr_down_y - terr_down_y_mean[:, np.newaxis]) - (feet_down_y - feet_down_y_mean)[np.newaxis])**2)[..., 0].mean(axis=1) terr_up_err = (np.maximum( (terr_up_y - terr_down_y_mean[:, np.newaxis]) - (feet_up_y - feet_down_y_mean)[np.newaxis], 0.0)**2)[..., 0].mean(axis=1) """ Jumping Error """ if type == 'jumpy': terr_over_minh = 5.0 terr_over_err = (np.maximum( ((feet_up_y - feet_down_y_mean)[np.newaxis] - terr_over_minh) - (terr_up_y - terr_down_y_mean[:, np.newaxis]), 0.0)**2)[..., 0].mean(axis=1) else: terr_over_err = 0.0 """ Fitting Terrain to Walking on Beam """ if type == 'beam': beam_samples = 1 beam_min_height = 40.0 beam_c = global_positions[:, 0] beam_c_xz = np.concatenate([beam_c[:, 0:1], beam_c[:, 2:3]], axis=-1) beam_c_y = patchfunc(patches, beam_c_xz - feet_down_xz_mean) beam_o = (beam_c.repeat(beam_samples, axis=0) + np.array([50, 0, 50]) * rng.normal(size=(len(beam_c) * beam_samples, 3))) beam_o_xz = np.concatenate([beam_o[:, 0:1], beam_o[:, 2:3]], axis=-1) beam_o_y = patchfunc(patches, beam_o_xz - feet_down_xz_mean) beam_pdist = np.sqrt(((beam_o[:, np.newaxis] - beam_c[np.newaxis, :])**2).sum(axis=-1)) beam_far = (beam_pdist > 15).all(axis=1) terr_beam_err = (np.maximum( beam_o_y[:, beam_far] - (beam_c_y.repeat(beam_samples, axis=1)[:, beam_far] - beam_min_height), 0.0)**2)[..., 0].mean(axis=1) else: terr_beam_err = 0.0 """ Final Fitting Error """ terr = terr_down_err + terr_up_err + terr_over_err + terr_beam_err """ Best Fitting Terrains """ terr_ids = np.argsort(terr)[:nsamples] terr_patches = patches[terr_ids] def terr_basic_func(Xp): return ((patchfunc(terr_patches, Xp - feet_down_xz_mean) - terr_down_y_mean[terr_ids][:, np.newaxis]) + feet_down_y_mean) """ Terrain Fit Editing """ terr_residuals = feet_down_y - terr_basic_func(feet_down_xz) terr_fine_func = [ RBF(smooth=0.1, function='linear') for _ in range(nsamples) ] for i in range(nsamples): terr_fine_func[i].fit(feet_down_xz, terr_residuals[i]) def terr_func(Xp): return (terr_basic_func(Xp) + np.array([ff(Xp) for ff in terr_fine_func])) """ Get Trajectory Terrain Heights """ root_offsets_c = global_positions[:, 0] root_offsets_r = (-root_rotation[:, 0] * np.array([[+25, 0, 0]])) + root_offsets_c root_offsets_l = (-root_rotation[:, 0] * np.array([[-25, 0, 0]])) + root_offsets_c root_heights_c = terr_func(root_offsets_c[:, np.array([0, 2])])[..., 0] root_heights_r = terr_func(root_offsets_r[:, np.array([0, 2])])[..., 0] root_heights_l = terr_func(root_offsets_l[:, np.array([0, 2])])[..., 0] """ Find Trajectory Heights at each Window """ root_terrains = [] root_averages = [] for i in range(window, len(anim) - window, 1): root_terrains.append( np.concatenate([ root_heights_r[:, i - window:i + window:10], root_heights_c[:, i - window:i + window:10], root_heights_l[:, i - window:i + window:10] ], axis=1)) root_averages.append(root_heights_c[:, i - window:i + window:10].mean(axis=1)) root_terrains = np.swapaxes(np.array(root_terrains), 0, 1) root_averages = np.swapaxes(np.array(root_averages), 0, 1) return root_terrains, root_averages