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