def get_original_rotations(self, rt_rot=None):
if rt_rot is None:
rt_rot = self.rt_rot
yaxis_rotations = Quaternions(np.array(Pivots(rt_rot).quaternions()))
rt_rotations = Quaternions(self.rotations[:, :1]) # [T, 1, 4]
rt_rotations = np.array(yaxis_rotations * rt_rotations)
rt_rotations /= np.sqrt((rt_rotations ** 2).sum(axis=-1))[..., np.newaxis]
return np.concatenate((rt_rotations, self.rotations[:, 1:]), axis=1) # [T, J, 4]
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 rotations_global(anim):
"""
Global Animation Rotations
This relies on joint ordering
being incremental. That means a joint
J1 must not be a ancestor of J0 if
J0 appears before J1 in the joint
ordering.
Parameters
----------
anim : Animation
Input animation
Returns
-------
points : (F, J) Quaternions
global rotations for every frame F
and joint J
"""
joints = np.arange(anim.shape[1])
parents = np.arange(anim.shape[1])
locals = anim.rotations
globals = Quaternions.id(anim.shape)
globals[:, 0] = locals[:, 0]
for i in range(1, anim.shape[1]):
globals[:, i] = globals[:, anim.parents[i]] * locals[:, i]
return globals
def forward_rotations(skel, rotations, rtpos=None, trim=True, panda=False):
"""
input: rotations [T, J, 4], rtpos [T, 3]
output: positions [T, J, 3]
"""
transforms = Quaternions(rotations).transforms() # [..., J, 3, 3]
glb = np.zeros(rotations.shape[:-1] + (3, )) # [T, J, 3]
if rtpos is not None:
glb[..., 0, :] = rtpos
if panda:
# CHANGED: Removed the first bone as it is not recorded in the animation
topology = skel.topology[1:]
else:
topology = skel.topology
for i, pi in enumerate(topology):
if pi == -1:
continue
glb[..., i, :] = np.matmul(transforms[..., pi, :, :], skel.offset[i])
glb[..., i, :] += glb[..., pi, :]
transforms[..., i, :, :] = np.matmul(transforms[..., pi, :, :],
transforms[..., i, :, :])
if not panda and trim:
glb = glb[..., skel.chosen_joints, :]
return glb
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 orients_global(anim):
joints = np.arange(anim.shape[1])
parents = np.arange(anim.shape[1])
locals = anim.orients
globals = Quaternions.id(anim.shape[1])
globals[:, 0] = locals[:, 0]
for i in range(1, anim.shape[1]):
globals[:, i] = globals[:, anim.parents[i]] * locals[:, i]
return globals
def y_rotation_from_positions(positions, hips=(2, 6), sdrs=(14, 18)):
"""
input: positions [T, J, 3]
output: quaters: [T, 1, 4], quaternions that rotate the character around the y-axis to face [0, 0, 1]
pivots: [T, 1] in [0, 2pi], the angle from [0, 0, 1] to the current facing direction
"""
across = across_from_glb(positions, hips=hips, sdrs=sdrs)
direction_filterwidth = 20
forward = np.cross(across, np.array([[0, 1, 0]]))
forward = filters.gaussian_filter1d(forward, direction_filterwidth, axis=0, mode='nearest')
forward = forward / np.sqrt((forward ** 2).sum(axis=-1))[..., np.newaxis]
target = np.tile(np.array([0, 0, 1]), forward.shape[:-1] + (1, ))
quaters = Quaternions.between(forward, target)[..., np.newaxis, :] # [T, 4] -> [T, 1, 4]
pivots = Pivots.from_quaternions(-quaters).ps # from "target"[0, 0, 1] to current facing direction "forward"
return quaters, pivots
def forward_rotations(skel, rotations, rtpos=None, trim=True): #$$
"""
input: rotations [T, J, 4], rtpos [T, 3]
output: positions [T, J, 3]
"""
transforms = Quaternions(rotations).transforms() # [..., J, 3, 3]
glb = np.zeros(rotations.shape[:-1] + (3, )) # [T, J, 3]
if rtpos is not None:
glb[..., 0, :] = rtpos
for i, pi in enumerate(skel.topology):
if pi == -1:
continue
glb[..., i, :] = np.matmul(transforms[..., pi, :, :], skel.offset[i])
glb[..., i, :] += glb[..., pi, :]
transforms[..., i, :, :] = np.matmul(transforms[..., pi, :, :],
transforms[..., i, :, :])
if trim:
glb = glb[..., skel.chosen_joints, :]
return glb
def from_rotations_and_root_positions(cls, rotations, root_positions, skel=None, frametime=1/30):
"""
rotations: [T, J, 4]
root_positions: [T, 3]
"""
if skel is None:
skel = Skel()
rotations /= np.sqrt(np.sum(rotations ** 2, axis=-1))[..., np.newaxis]
global_positions = forward_rotations(skel, rotations, root_positions, trim=True)
foot_contact = foot_contact_from_positions(global_positions, fid_l=skel.fid_l, fid_r=skel.fid_r)
quaters, pivots = y_rotation_from_positions(global_positions, hips=skel.hips, sdrs=skel.sdrs)
root_rotations = Quaternions(rotations[:, 0:1, :].copy()) # [T, 1, 4]
root_rotations = quaters * root_rotations # facing [0, 0, 1]
root_rotations = np.array(root_rotations).reshape((-1, 1, 4)) # [T, 1, 4]
rotations[:, 0:1, :] = root_rotations
full = np.concatenate([rotations.reshape((len(rotations), -1)), root_positions, pivots, foot_contact], axis=-1)
return cls(full, skel, frametime)
def load_from_maya(root, start, end):
"""
Load Animation Object from Maya Joint Skeleton
Parameters
----------
root : PyNode
Root Joint of Maya Skeleton
start, end : int, int
Start and End frame index of Maya Animation
Returns
-------
animation : Animation
Loaded animation from maya
names : [str]
Joint names from maya
"""
import pymel.core as pm
original_time = pm.currentTime(q=True)
pm.currentTime(start)
""" Build Structure """
names, parents = AnimationStructure.load_from_maya(root)
descendants = AnimationStructure.descendants_list(parents)
orients = Quaternions.id(len(names))
offsets = np.array([pm.xform(j, q=True, translation=True) for j in names])
for j, name in enumerate(names):
scale = pm.xform(pm.PyNode(name), q=True, scale=True, relative=True)
if len(descendants[j]) == 0: continue
offsets[descendants[j]] *= scale
""" Load Animation """
eulers = np.zeros((end - start, len(names), 3))
positions = np.zeros((end - start, len(names), 3))
rotations = Quaternions.id((end - start, len(names)))
for i in range(end - start):
pm.currentTime(start + i + 1, u=True)
scales = {}
for j, name, parent in zip(range(len(names)), names, parents):
node = pm.PyNode(name)
if i == 0 and pm.hasAttr(node, 'jointOrient'):
ort = node.getOrientation()
orients[j] = Quaternions(
np.array([ort[3], ort[0], ort[1], ort[2]]))
if pm.hasAttr(node, 'rotate'):
eulers[i, j] = np.radians(pm.xform(node, q=True,
rotation=True))
if pm.hasAttr(node, 'translate'):
positions[i, j] = pm.xform(node, q=True, translation=True)
if pm.hasAttr(node, 'scale'):
scales[j] = pm.xform(node, q=True, scale=True, relative=True)
for j in scales:
if len(descendants[j]) == 0: continue
positions[i, descendants[j]] *= scales[j]
positions[i, 0] = pm.xform(root,
q=True,
translation=True,
worldSpace=True)
rotations = orients[np.newaxis] * Quaternions.from_euler(
eulers, order='xyz', world=True)
""" Done """
pm.currentTime(original_time)
return Animation(rotations, positions, orients, offsets, parents), names
def load_to_maya(anim, names=None, radius=0.5):
"""
Load Animation Object into Maya as Joint Skeleton
loads each frame as a new keyfame in maya.
If the animation is too slow or too fast perhaps
the framerate needs adjusting before being loaded
such that it matches the maya scene framerate.
Parameters
----------
anim : Animation
Animation to load into Scene
names : [str]
Optional list of Joint names for Skeleton
Returns
-------
List of Maya Joint Nodes loaded into scene
"""
import pymel.core as pm
joints = []
frames = range(1, len(anim) + 1)
if names is None:
names = ["joint_" + str(i) for i in range(len(anim.parents))]
for i, offset, orient, parent, name in zip(range(len(anim.offsets)),
anim.offsets, anim.orients,
anim.parents, names):
if parent < 0:
pm.select(d=True)
else:
pm.select(joints[parent])
joint = pm.joint(n=name, p=offset, relative=True, radius=radius)
joint.setOrientation([orient[1], orient[2], orient[3], orient[0]])
curvex = pm.nodetypes.AnimCurveTA(n=name + "_rotateX")
curvey = pm.nodetypes.AnimCurveTA(n=name + "_rotateY")
curvez = pm.nodetypes.AnimCurveTA(n=name + "_rotateZ")
jrotations = (-Quaternions(orient[np.newaxis]) *
anim.rotations[:, i]).euler()
curvex.addKeys(frames, jrotations[:, 0])
curvey.addKeys(frames, jrotations[:, 1])
curvez.addKeys(frames, jrotations[:, 2])
pm.connectAttr(curvex.output, joint.rotateX)
pm.connectAttr(curvey.output, joint.rotateY)
pm.connectAttr(curvez.output, joint.rotateZ)
offsetx = pm.nodetypes.AnimCurveTU(n=name + "_translateX")
offsety = pm.nodetypes.AnimCurveTU(n=name + "_translateY")
offsetz = pm.nodetypes.AnimCurveTU(n=name + "_translateZ")
offsetx.addKeys(frames, anim.positions[:, i, 0])
offsety.addKeys(frames, anim.positions[:, i, 1])
offsetz.addKeys(frames, anim.positions[:, i, 2])
pm.connectAttr(offsetx.output, joint.translateX)
pm.connectAttr(offsety.output, joint.translateY)
pm.connectAttr(offsetz.output, joint.translateZ)
joints.append(joint)
return joints
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)
# result: [fnum, J, 3]
positions = offsets[np.newaxis].repeat(fnum, axis=0)
# result: [fnum, len(orients), 3]
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:
# This should be root positions[0:1] & all rotations
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)
# fill in all positions
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 rotations_parents_global(anim):
rotations = rotations_global(anim)
rotations = rotations[:, anim.parents]
rotations[:, 0] = Quaternions.id(len(anim))
return rotations
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 rotationMatrixByQuaternion(rotation, order='xzy', world=False):
return np.array(
Quaternions.from_euler(np.radians(rotation), order=order, world=world))
def fix_foot_contact(input_file, foot_file, output_file, ref_height):
anim, name, ftime = BVH.load(input_file)
fid = get_ee_id_by_names(name)
contact = get_foot_contact(foot_file, ref_height)
glb = Animation.positions_global(anim) # [T, J, 3]
T = glb.shape[0]
for i, fidx in enumerate(fid): # fidx: index of the foot joint
fixed = contact[:, i] # [T]
s = 0
while s < T:
while s < T and fixed[s] == 0:
s += 1
if s >= T:
break
t = s
avg = glb[t, fidx].copy()
while t + 1 < T and fixed[t + 1] == 1:
t += 1
avg += glb[t, fidx].copy()
avg /= (t - s + 1)
for j in range(s, t + 1):
glb[j, fidx] = avg.copy()
s = t + 1
for s in range(T):
if fixed[s] == 1:
continue
l, r = None, None
consl, consr = False, False
for k in range(L):
if s - k - 1 < 0:
break
if fixed[s - k - 1]:
l = s - k - 1
consl = True
break
for k in range(L):
if s + k + 1 >= T:
break
if fixed[s + k + 1]:
r = s + k + 1
consr = True
break
if not consl and not consr:
continue
if consl and consr:
litp = lerp(alpha(1.0 * (s - l + 1) / (L + 1)), glb[s, fidx],
glb[l, fidx])
ritp = lerp(alpha(1.0 * (r - s + 1) / (L + 1)), glb[s, fidx],
glb[r, fidx])
itp = lerp(alpha(1.0 * (s - l + 1) / (r - l + 1)), ritp, litp)
glb[s, fidx] = itp.copy()
continue
if consl:
litp = lerp(alpha(1.0 * (s - l + 1) / (L + 1)), glb[s, fidx],
glb[l, fidx])
glb[s, fidx] = litp.copy()
continue
if consr:
ritp = lerp(alpha(1.0 * (r - s + 1) / (L + 1)), glb[s, fidx],
glb[r, fidx])
glb[s, fidx] = ritp.copy()
# glb is ready
anim = anim.copy()
rot = torch.tensor(anim.rotations.qs, dtype=torch.float)
pos = torch.tensor(anim.positions[:, 0, :], dtype=torch.float)
offset = torch.tensor(anim.offsets, dtype=torch.float)
glb = torch.tensor(glb, dtype=torch.float)
ik_solver = InverseKinematics(rot, pos, offset, anim.parents, glb)
print('Fixing foot contact using IK...')
for i in tqdm(range(50)):
ik_solver.step()
rotations = ik_solver.rotations.detach()
norm = torch.norm(rotations, dim=-1, keepdim=True)
rotations /= norm
anim.rotations = Quaternions(rotations.numpy())
anim.positions[:, 0, :] = ik_solver.position.detach().numpy()
BVH.save(output_file, anim, name, ftime)
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 __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))