def unravel(clas, anim, shape, parents): nf, nj = shape rotations = anim[nf * nj * 0:nf * nj * 3] positions = anim[nf * nj * 3:nf * nj * 6] orients = anim[nf * nj * 6 + nj * 0:nf * nj * 6 + nj * 3] offsets = anim[nf * nj * 6 + nj * 3:nf * nj * 6 + nj * 6] return cls(Quaternions.exp(rotations), positions, Quaternions.exp(orients), offsets, parents.copy())
def __call__(self): children = AnimationStructure.children_list(self.animation.parents) for i in range(self.iterations): for j in AnimationStructure.joints(self.animation.parents): c = np.array(children[j]) if len(c) == 0: continue anim_transforms = Animation.transforms_global(self.animation) anim_positions = anim_transforms[:, :, :3, 3] anim_rotations = Quaternions.from_transforms(anim_transforms) jdirs = anim_positions[:, c] - anim_positions[:, np.newaxis, j] ddirs = self.positions[:, c] - anim_positions[:, np.newaxis, j] jsums = np.sqrt(np.sum(jdirs**2.0, axis=-1)) + 1e-10 dsums = np.sqrt(np.sum(ddirs**2.0, axis=-1)) + 1e-10 jdirs = jdirs / jsums[:, :, np.newaxis] ddirs = ddirs / dsums[:, :, np.newaxis] angles = np.arccos(np.sum(jdirs * ddirs, axis=2).clip(-1, 1)) axises = np.cross(jdirs, ddirs) axises = -anim_rotations[:, j, np.newaxis] * axises rotations = Quaternions.from_angle_axis(angles, axises) if rotations.shape[1] == 1: averages = rotations[:, 0] else: averages = Quaternions.exp(rotations.log().mean(axis=-2)) self.animation.rotations[:, j] = self.animation.rotations[:, j] * averages if not self.silent: anim_positions = Animation.positions_global(self.animation) error = np.mean(np.sum((anim_positions - self.positions)**2.0, axis=-1)**0.5, axis=-1) print('[BasicInverseKinematics] Iteration %i Error: %f' % (i + 1, error)) return self.animation
def jacobian(self, x, fp, fr, goal, weights, des_r, des_t): """ Find parent rotations """ prs = fr[:, self.animation.parents] prs[:, 0] = Quaternions.id((1)) """ Get partial rotations """ qys = Quaternions.from_angle_axis(x[:, 1:prs.shape[1] * 3:3], np.array([[[0, 1, 0]]])) qzs = Quaternions.from_angle_axis(x[:, 2:prs.shape[1] * 3:3], np.array([[[0, 0, 1]]])) """ Find axis of rotations """ es = np.empty((len(x), fr.shape[1] * 3, 3)) es[:, 0::3] = ((prs * qzs) * qys) * np.array([[[1, 0, 0]]]) es[:, 1::3] = ((prs * qzs) * np.array([[[0, 1, 0]]])) es[:, 2::3] = ((prs * np.array([[[0, 0, 1]]]))) """ Construct Jacobian """ j = fp.repeat(3, axis=1) j = des_r[np.newaxis, :, :, :, np.newaxis] * (goal[:, np.newaxis, :, np.newaxis] - j[:, :, np.newaxis, np.newaxis]) j = np.sum(j * weights[np.newaxis, np.newaxis, :, :, np.newaxis], 3) j = self.cross(es[:, :, np.newaxis, :], j) j = np.swapaxes( j.reshape((len(x), fr.shape[1] * 3, goal.shape[1] * 3)), 1, 2) if self.translate: es = np.empty((len(x), fr.shape[1] * 3, 3)) es[:, 0::3] = prs * np.array([[[1, 0, 0]]]) es[:, 1::3] = prs * np.array([[[0, 1, 0]]]) es[:, 2::3] = prs * np.array([[[0, 0, 1]]]) jt = des_t[np.newaxis, :, :, :, np.newaxis] * es[:, :, np.newaxis, np.newaxis, :].repeat(goal.shape[1], axis=2) jt = np.sum(jt * weights[np.newaxis, np.newaxis, :, :, np.newaxis], 3) jt = np.swapaxes( jt.reshape((len(x), fr.shape[1] * 3, goal.shape[1] * 3)), 1, 2) j = np.concatenate([j, jt], axis=-1) return j
def jacobian(self, x, fp, fr, ts, dsc, tdsc): """ Find parent rotations """ prs = fr[:, self.animation.parents] prs[:, 0] = Quaternions.id((1)) """ Find global positions of target joints """ tps = fp[:, np.array(list(ts.keys()))] """ Get partial rotations """ qys = Quaternions.from_angle_axis(x[:, 1:prs.shape[1] * 3:3], np.array([[[0, 1, 0]]])) qzs = Quaternions.from_angle_axis(x[:, 2:prs.shape[1] * 3:3], np.array([[[0, 0, 1]]])) """ Find axis of rotations """ es = np.empty((len(x), fr.shape[1] * 3, 3)) es[:, 0::3] = ((prs * qzs) * qys) * np.array([[[1, 0, 0]]]) es[:, 1::3] = ((prs * qzs) * np.array([[[0, 1, 0]]])) es[:, 2::3] = ((prs * np.array([[[0, 0, 1]]]))) """ Construct Jacobian """ j = fp.repeat(3, axis=1) j = dsc[np.newaxis, :, :, np.newaxis] * (tps[:, np.newaxis, :] - j[:, :, np.newaxis]) j = self.cross(es[:, :, np.newaxis, :], j) j = np.swapaxes(j.reshape((len(x), fr.shape[1] * 3, len(ts) * 3)), 1, 2) if self.translate: es = np.empty((len(x), fr.shape[1] * 3, 3)) es[:, 0::3] = prs * np.array([[[1, 0, 0]]]) es[:, 1::3] = prs * np.array([[[0, 1, 0]]]) es[:, 2::3] = prs * np.array([[[0, 0, 1]]]) jt = tdsc[np.newaxis, :, :, np.newaxis] * es[:, :, np.newaxis, :].repeat( tps.shape[1], axis=2) jt = np.swapaxes( jt.reshape((len(x), fr.shape[1] * 3, len(ts) * 3)), 1, 2) j = np.concatenate([j, jt], axis=-1) return j
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 animation_plot(animations, filename=None, ignore_root=False, interval=33.33): footsteps = [] for ai in range(len(animations)): anim = np.swapaxes(animations[ai][0].copy(), 0, 1) joints, root_x, root_z, root_r = anim[:, : -7], anim[:, -7], anim[:, -6], anim[:, -5] joints = joints.reshape((len(joints), -1, 3)) rotation = Quaternions.id(1) offsets = [] translation = np.array([[0, 0, 0]]) if not ignore_root: for i in range(len(joints)): joints[i, :, :] = rotation * joints[i] joints[i, :, 0] = joints[i, :, 0] + translation[0, 0] joints[i, :, 2] = joints[i, :, 2] + translation[0, 2] rotation = Quaternions.from_angle_axis( -root_r[i], np.array([0, 1, 0])) * rotation offsets.append(rotation * np.array([0, 0, 1])) translation = translation + rotation * \ np.array([root_x[i], 0, root_z[i]]) animations[ai] = joints footsteps.append(anim[:, -4:]) """ offsets = np.array(offsets)[:,0] print(offsets) import matplotlib.pyplot as plt plt.plot(joints[::10,0,0], joints[::10,0,2], 'o') plt.plot(joints[:,0,0], joints[:,0,2]) for j in range(0, len(joints), 10): if footsteps[ai][j,0] > 0.5: plt.plot(joints[j,3,0], joints[j,3,2], '.', color='black') if footsteps[ai][j,2] > 0.5: plt.plot(joints[j,7,0], joints[j,7,2], '.', color='black') #plt.plot( # np.array([joints[j,0,0], joints[j,0,0] + 3 * offsets[j,0]]), # np.array([joints[j,0,2], joints[j,0,2] + 3 * offsets[j,2]]), color='red') plt.xlim([-30, 30]) plt.ylim([-10, 50]) plt.grid(False) plt.show() """ #raise Exception() footsteps = np.array(footsteps) scale = 1.25 * ((len(animations)) / 2) fig = plt.figure(figsize=(12, 8)) ax = fig.add_subplot(111, projection='3d') ax.set_xlim3d(-scale * 30, scale * 30) ax.set_zlim3d(0, scale * 60) ax.set_ylim3d(-scale * 30, scale * 30) ax.set_xticks([], []) ax.set_yticks([], []) ax.set_zticks([], []) ax.set_aspect('equal') acolors = list(sorted(colors.cnames.keys()))[::-1] lines = [] parents = np.array([ -1, 0, 1, 2, 3, 4, 1, 6, 7, 8, 1, 10, 11, 12, 12, 14, 15, 16, 12, 18, 19, 20 ]) for ai, anim in enumerate(animations): lines.append([ plt.plot([0, 0], [0, 0], [0, 0], color=acolors[ai], lw=2, path_effects=[ pe.Stroke(linewidth=3, foreground='black'), pe.Normal() ])[0] for _ in range(anim.shape[1]) ]) def animate(i): changed = [] for ai in range(len(animations)): offset = 25 * (ai - ((len(animations)) / 2)) for j in range(len(parents)): if parents[j] != -1: lines[ai][j].set_data([ animations[ai][i, j, 0] + offset, animations[ai][i, parents[j], 0] + offset ], [ -animations[ai][i, j, 2], -animations[ai][i, parents[j], 2] ]) lines[ai][j].set_3d_properties([ animations[ai][i, j, 1], animations[ai][i, parents[j], 1] ]) changed += lines return changed plt.tight_layout() ani = animation.FuncAnimation(fig, animate, np.arange(len(animations[0])), interval=interval) if filename != None: #ani.save(filename, fps=30, bitrate=13934) data = {} for i, a, f in zip(range(len(animations)), animations, footsteps): data['anim_%i' % i] = a data['anim_%i_footsteps' % i] = f np.savez_compressed(filename.replace('.mp4', '.npz'), **data) try: plt.show() except AttributeError as e: pass
def quaternions(self, plane='xz'): fa = self._ellipsis() axises = np.ones(self.ps.shape + (3, )) axises[fa + ("xyz".index(plane[0]), )] = 0.0 axises[fa + ("xyz".index(plane[1]), )] = 0.0 return Quaternions.from_angle_axis(self.ps, axises)
def load(filename, start=None, end=None, order=None, world=False): """ Reads a BVH file and constructs an animation Parameters ---------- filename: str File to be opened start : int Optional Starting Frame end : int Optional Ending Frame order : str Optional Specifier for joint order. Given as string E.G 'xyz', 'zxy' world : bool If set to true euler angles are applied together in world space rather than local space Returns ------- (animation, joint_names, frametime) Tuple of loaded animation and joint names """ f = open(filename, "r") i = 0 active = -1 end_site = False names = [] orients = Quaternions.id(0) offsets = np.array([]).reshape((0, 3)) parents = np.array([], dtype=int) for line in f: if "HIERARCHY" in line: continue if "MOTION" in line: continue rmatch = re.match(r"ROOT (\w+)", line) if rmatch: names.append(rmatch.group(1)) offsets = np.append(offsets, np.array([[0, 0, 0]]), axis=0) orients.qs = np.append( orients.qs, np.array([[1, 0, 0, 0]]), axis=0) parents = np.append(parents, active) active = (len(parents) - 1) continue if "{" in line: continue if "}" in line: if end_site: end_site = False else: active = parents[active] continue offmatch = re.match( r"\s*OFFSET\s+([\-\d\.e]+)\s+([\-\d\.e]+)\s+([\-\d\.e]+)", line) if offmatch: if not end_site: offsets[active] = np.array( [list(map(float, offmatch.groups()))]) continue chanmatch = re.match(r"\s*CHANNELS\s+(\d+)", line) if chanmatch: channels = int(chanmatch.group(1)) if order is None: channelis = 0 if channels == 3 else 3 channelie = 3 if channels == 3 else 6 parts = line.split()[2 + channelis:2 + channelie] if any([p not in channelmap for p in parts]): continue order = "".join([channelmap[p] for p in parts]) continue jmatch = re.match("\s*JOINT\s+(\w+)", line) if jmatch: names.append(jmatch.group(1)) offsets = np.append(offsets, np.array([[0, 0, 0]]), axis=0) orients.qs = np.append( orients.qs, np.array([[1, 0, 0, 0]]), axis=0) parents = np.append(parents, active) active = (len(parents) - 1) continue if "End Site" in line: end_site = True continue fmatch = re.match("\s*Frames:\s+(\d+)", line) if fmatch: if start and end: fnum = (end - start) - 1 else: fnum = int(fmatch.group(1)) jnum = len(parents) positions = offsets[np.newaxis].repeat(fnum, axis=0) rotations = np.zeros((fnum, len(orients), 3)) continue fmatch = re.match("\s*Frame Time:\s+([\d\.]+)", line) if fmatch: frametime = float(fmatch.group(1)) continue if (start and end) and (i < start or i >= end - 1): i += 1 continue dmatch = line.strip().split(' ') if dmatch: data_block = np.array(list(map(float, dmatch))) N = len(parents) fi = i - start if start else i if channels == 3: positions[fi, 0:1] = data_block[0:3] rotations[fi, :] = data_block[3:].reshape(N, 3) elif channels == 6: data_block = data_block.reshape(N, 6) positions[fi, :] = data_block[:, 0:3] rotations[fi, :] = data_block[:, 3:6] elif channels == 9: positions[fi, 0] = data_block[0:3] data_block = data_block[3:].reshape(N - 1, 9) rotations[fi, 1:] = data_block[:, 3:6] positions[fi, 1:] += data_block[:, 0:3] * data_block[:, 6:9] else: raise Exception("Too many channels! %i" % channels) i += 1 f.close() rotations = Quaternions.from_euler( np.radians(rotations), order=order, world=world) return (Animation(rotations, positions, orients, offsets, parents), names, frametime)
def __call__(self, descendants=None, maxjoints=4, gamma=1.0, transpose=False): """ Calculate Masses """ if self.weights is None: self.weights = np.ones(self.animation.shape[1]) if self.weights_translate is None: self.weights_translate = np.ones(self.animation.shape[1]) nf = len(self.animation) nj = self.animation.shape[1] nv = self.goal.shape[1] weightids = np.argsort(-self.vweights, axis=1)[:, :maxjoints] weightvls = np.array( list(map(lambda w, i: w[i], self.vweights, weightids))) weightvls = weightvls / weightvls.sum(axis=1)[..., np.newaxis] if descendants is None: self.descendants = AnimationStructure.descendants_mask( self.animation.parents) else: self.descendants = descendants des_r = np.eye(nj) + self.descendants des_r = des_r[:, weightids].repeat(3, axis=0) des_t = np.eye(nj) + self.descendants des_t = des_t[:, weightids].repeat(3, axis=0) if not self.silent: curr = Animation.skin(self.animation, self.rest, self.vweights, self.mesh, maxjoints=maxjoints) error = np.mean(np.sqrt(np.sum((curr - self.goal)**2.0, axis=-1))) print('[ICP] Start | Error: %f' % error) for i in range(self.iterations): """ Get Global Rotations & Positions """ gt = Animation.transforms_global(self.animation) gp = gt[:, :, :, 3] gp = gp[:, :, :3] / gp[:, :, 3, np.newaxis] gr = Quaternions.from_transforms(gt) x = self.animation.rotations.euler().reshape(nf, -1) w = self.weights.repeat(3) if self.translate: x = np.hstack([x, self.animation.positions.reshape(nf, -1)]) w = np.hstack([w, self.weights_translate.repeat(3)]) """ Get Current State """ curr = Animation.skin(self.animation, self.rest, self.vweights, self.mesh, maxjoints=maxjoints) """ Find Cloest Points """ if self.find_closest: mapping = np.argmin((curr[:, :, np.newaxis] - self.goal[:, np.newaxis, :])**2.0, axis=2) e = gamma * (np.array( list(map(lambda g, m: g[m], self.goal, mapping))) - curr).reshape(nf, -1) else: e = gamma * (self.goal - curr).reshape(nf, -1) """ Generate Jacobian """ if self.recalculate or i == 0: j = self.jacobian(x, gp, gr, self.goal, weightvls, des_r, des_t) """ Update Variables """ l = self.damping * (1.0 / (w + 1e-10)) d = (l * l) * np.eye(x.shape[1]) if transpose: x += np.array(list(map(lambda jf, ef: jf.T.dot(ef), j, e))) else: x += np.array( list( map( lambda jf, ef: linalg.lu_solve( linalg.lu_factor(jf.T.dot(jf) + d), jf.T.dot( ef)), j, e))) """ Set Back Rotations / Translations """ self.animation.rotations = Quaternions.from_euler( x[:, :nj * 3].reshape((nf, nj, 3)), order='xyz', world=True) if self.translate: self.animation.positions = x[:, nj * 3:].reshape((nf, nj, 3)) if not self.silent: curr = Animation.skin(self.animation, self.rest, self.vweights, self.mesh) error = np.mean( np.sqrt(np.sum((curr - self.goal)**2.0, axis=-1))) print('[ICP] Iteration %i | Error: %f' % (i + 1, error))
def __call__(self, descendants=None, gamma=1.0): self.descendants = descendants """ Calculate Masses """ if self.weights is None: self.weights = np.ones(self.animation.shape[1]) if self.weights_translate is None: self.weights_translate = np.ones(self.animation.shape[1]) """ Calculate Descendants """ if self.descendants is None: self.descendants = AnimationStructure.descendants_mask( self.animation.parents) self.tdescendants = np.eye(self.animation.shape[1]) + self.descendants self.first_descendants = self.descendants[:, np.array( list(self.targets.keys()) )].repeat(3, axis=0).astype(int) self.first_tdescendants = self.tdescendants[:, np.array( list(self.targets.keys( )))].repeat( 3, axis=0).astype(int) """ Calculate End Effectors """ self.endeff = np.array(list(self.targets.values())) self.endeff = np.swapaxes(self.endeff, 0, 1) if not self.references is None: self.second_descendants = self.descendants.repeat( 3, axis=0).astype(int) self.second_tdescendants = self.tdescendants.repeat( 3, axis=0).astype(int) self.second_targets = dict([ (i, self.references[:, i]) for i in xrange(self.references.shape[1]) ]) nf = len(self.animation) nj = self.animation.shape[1] if not self.silent: gp = Animation.positions_global(self.animation) gp = gp[:, np.array(list(self.targets.keys()))] error = np.mean(np.sqrt(np.sum((self.endeff - gp)**2.0, axis=2))) print('[JacobianInverseKinematics] Start | Error: %f' % error) for i in range(self.iterations): """ Get Global Rotations & Positions """ gt = Animation.transforms_global(self.animation) gp = gt[:, :, :, 3] gp = gp[:, :, :3] / gp[:, :, 3, np.newaxis] gr = Quaternions.from_transforms(gt) x = self.animation.rotations.euler().reshape(nf, -1) w = self.weights.repeat(3) if self.translate: x = np.hstack([x, self.animation.positions.reshape(nf, -1)]) w = np.hstack([w, self.weights_translate.repeat(3)]) """ Generate Jacobian """ if self.recalculate or i == 0: j = self.jacobian(x, gp, gr, self.targets, self.first_descendants, self.first_tdescendants) """ Update Variables """ l = self.damping * (1.0 / (w + 0.001)) d = (l * l) * np.eye(x.shape[1]) e = gamma * ( self.endeff.reshape(nf, -1) - gp[:, np.array(list(self.targets.keys()))].reshape(nf, -1)) x += np.array( list( map( lambda jf, ef: linalg.lu_solve( linalg.lu_factor(jf.T.dot(jf) + d), jf.T.dot(ef)), j, e))) """ Generate Secondary Jacobian """ if self.references is not None: ns = np.array( list( map( lambda jf: np.eye(x.shape[1]) - linalg.solve( jf.T.dot(jf) + d, jf.T.dot(jf)), j))) if self.recalculate or i == 0: j2 = self.jacobian(x, gp, gr, self.second_targets, self.second_descendants, self.second_tdescendants) e2 = self.secondary * (self.references.reshape(nf, -1) - gp.reshape(nf, -1)) x += np.array( list( map( lambda nsf, j2f, e2f: nsf.dot( linalg.lu_solve( linalg.lu_factor(j2f.T.dot(j2f) + d), j2f.T.dot(e2f))), ns, j2, e2))) """ Set Back Rotations / Translations """ self.animation.rotations = Quaternions.from_euler( x[:, :nj * 3].reshape((nf, nj, 3)), order='xyz', world=True) if self.translate: self.animation.positions = x[:, nj * 3:].reshape((nf, nj, 3)) """ Generate Error """ if not self.silent: gp = Animation.positions_global(self.animation) gp = gp[:, np.array(list(self.targets.keys()))] error = np.mean(np.sum((self.endeff - gp)**2.0, axis=2)**0.5) print('[JacobianInverseKinematics] Iteration %i | Error: %f' % (i + 1, error))
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