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