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)