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 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 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)