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