def compute(cls, pose1, pose2, dt):
assert pose1.skel == pose2.skel
data_local = []
data_global = []
assert dt > constants.EPSILON
for joint in pose1.skel.joints:
T1 = pose1.get_transform(joint, local=True)
T2 = pose2.get_transform(joint, local=True)
dR, dp = conversions.T2Rp(np.dot(math.invertT(T1), T2))
w, v = conversions.R2A(dR) / dt, dp / dt
data_local.append(np.hstack((w, v)))
T1 = pose1.get_transform(joint, local=False)
T2 = pose2.get_transform(joint, local=False)
dR, dp = conversions.T2Rp(np.dot(math.invertT(T1), T2))
w, v = conversions.R2A(dR) / dt, dp / dt
data_global.append(np.hstack((w, v)))
return np.array(data_local), np.array(data_global)
def set_transform(self, key, T, local, do_ortho_norm=True):
if local:
T1 = T
else:
T0 = self.skel.get_joint(key).xform_global
T1 = np.dot(math.invertT(T0), T)
if do_ortho_norm:
"""
This insures that the rotation part of
the given transformation is valid
"""
Q, p = conversions.T2Qp(T1)
Q = quaternion.Q_op(Q, op=["normalize"])
T1 = conversions.Qp2T(Q, p)
self.data[self.skel.get_index_joint(key)] = T1