Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,675 @@
|
||||
import operator
|
||||
|
||||
import numpy as np
|
||||
import numpy.core.umath as ut # (for numpy 1.25)
|
||||
# import numpy.core.umath_tests as ut
|
||||
|
||||
import AnimationStructure
|
||||
from Quaternions import Quaternions
|
||||
|
||||
|
||||
class Animation:
|
||||
"""
|
||||
Animation is a numpy-like wrapper for animation data
|
||||
|
||||
Animation data consists of several arrays consisting
|
||||
of F frames and J joints.
|
||||
|
||||
The animation is specified by
|
||||
|
||||
rotations : (F, J) Quaternions | Joint Rotations
|
||||
positions : (F, J, 3) ndarray | Joint Positions
|
||||
|
||||
The base pose is specified by
|
||||
|
||||
orients : (J) Quaternions | Joint Orientations
|
||||
offsets : (J, 3) ndarray | Joint Offsets
|
||||
|
||||
And the skeletal structure is specified by
|
||||
|
||||
parents : (J) ndarray | Joint Parents
|
||||
"""
|
||||
|
||||
def __init__(self, rotations, positions, orients, offsets, parents):
|
||||
|
||||
self.rotations = rotations
|
||||
self.positions = positions
|
||||
self.orients = orients
|
||||
self.offsets = offsets
|
||||
self.parents = parents
|
||||
|
||||
def __op__(self, op, other):
|
||||
return Animation(
|
||||
op(self.rotations, other.rotations),
|
||||
op(self.positions, other.positions),
|
||||
op(self.orients, other.orients),
|
||||
op(self.offsets, other.offsets),
|
||||
op(self.parents, other.parents))
|
||||
|
||||
def __iop__(self, op, other):
|
||||
self.rotations = op(self.roations, other.rotations)
|
||||
self.positions = op(self.roations, other.positions)
|
||||
self.orients = op(self.orients, other.orients)
|
||||
self.offsets = op(self.offsets, other.offsets)
|
||||
self.parents = op(self.parents, other.parents)
|
||||
return self
|
||||
|
||||
def __sop__(self, op):
|
||||
return Animation(
|
||||
op(self.rotations),
|
||||
op(self.positions),
|
||||
op(self.orients),
|
||||
op(self.offsets),
|
||||
op(self.parents))
|
||||
|
||||
def __add__(self, other): return self.__op__(operator.add, other)
|
||||
def __sub__(self, other): return self.__op__(operator.sub, other)
|
||||
def __mul__(self, other): return self.__op__(operator.mul, other)
|
||||
def __div__(self, other): return self.__op__(operator.div, other)
|
||||
|
||||
def __abs__(self): return self.__sop__(operator.abs)
|
||||
def __neg__(self): return self.__sop__(operator.neg)
|
||||
|
||||
def __iadd__(self, other): return self.__iop__(operator.iadd, other)
|
||||
def __isub__(self, other): return self.__iop__(operator.isub, other)
|
||||
def __imul__(self, other): return self.__iop__(operator.imul, other)
|
||||
def __idiv__(self, other): return self.__iop__(operator.idiv, other)
|
||||
|
||||
def __len__(self): return len(self.rotations)
|
||||
|
||||
def __getitem__(self, k):
|
||||
if isinstance(k, tuple):
|
||||
return Animation(
|
||||
self.rotations[k],
|
||||
self.positions[k],
|
||||
self.orients[k[1:]],
|
||||
self.offsets[k[1:]],
|
||||
self.parents[k[1:]])
|
||||
else:
|
||||
return Animation(
|
||||
self.rotations[k],
|
||||
self.positions[k],
|
||||
self.orients,
|
||||
self.offsets,
|
||||
self.parents)
|
||||
|
||||
def __setitem__(self, k, v):
|
||||
if isinstance(k, tuple):
|
||||
self.rotations.__setitem__(k, v.rotations)
|
||||
self.positions.__setitem__(k, v.positions)
|
||||
self.orients.__setitem__(k[1:], v.orients)
|
||||
self.offsets.__setitem__(k[1:], v.offsets)
|
||||
self.parents.__setitem__(k[1:], v.parents)
|
||||
else:
|
||||
self.rotations.__setitem__(k, v.rotations)
|
||||
self.positions.__setitem__(k, v.positions)
|
||||
self.orients.__setitem__(k, v.orients)
|
||||
self.offsets.__setitem__(k, v.offsets)
|
||||
self.parents.__setitem__(k, v.parents)
|
||||
|
||||
@property
|
||||
def shape(self): return (self.rotations.shape[0], self.rotations.shape[1])
|
||||
|
||||
def copy(self): return Animation(
|
||||
self.rotations.copy(), self.positions.copy(),
|
||||
self.orients.copy(), self.offsets.copy(),
|
||||
self.parents.copy())
|
||||
|
||||
def repeat(self, *args, **kw):
|
||||
return Animation(
|
||||
self.rotations.repeat(*args, **kw),
|
||||
self.positions.repeat(*args, **kw),
|
||||
self.orients, self.offsets, self.parents)
|
||||
|
||||
def ravel(self):
|
||||
return np.hstack([
|
||||
self.rotations.log().ravel(),
|
||||
self.positions.ravel(),
|
||||
self.orients.log().ravel(),
|
||||
self.offsets.ravel()])
|
||||
|
||||
@classmethod
|
||||
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())
|
||||
|
||||
|
||||
""" Maya Interaction """
|
||||
|
||||
|
||||
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_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
|
||||
|
||||
# local transformation matrices
|
||||
|
||||
|
||||
def transforms_local(anim):
|
||||
"""
|
||||
Computes Animation Local Transforms
|
||||
|
||||
As well as a number of other uses this can
|
||||
be used to compute global joint transforms,
|
||||
which in turn can be used to compete global
|
||||
joint positions
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
anim : Animation
|
||||
Input animation
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
transforms : (F, J, 4, 4) ndarray
|
||||
|
||||
For each frame F, joint local
|
||||
transforms for each joint J
|
||||
"""
|
||||
|
||||
transforms = anim.rotations.transforms()
|
||||
transforms = np.concatenate([transforms, np.zeros(transforms.shape[:2] + (3, 1))], axis=-1)
|
||||
transforms = np.concatenate([transforms, np.zeros(transforms.shape[:2] + (1, 4))], axis=-2)
|
||||
# the last column is filled with the joint positions!
|
||||
transforms[:, :, 0:3, 3] = anim.positions
|
||||
transforms[:, :, 3:4, 3] = 1.0
|
||||
return transforms
|
||||
|
||||
|
||||
def transforms_multiply(t0s, t1s):
|
||||
"""
|
||||
Transforms Multiply
|
||||
|
||||
Multiplies two arrays of animation transforms
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
t0s, t1s : (F, J, 4, 4) ndarray
|
||||
Two arrays of transforms
|
||||
for each frame F and each
|
||||
joint J
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
transforms : (F, J, 4, 4) ndarray
|
||||
Array of transforms for each
|
||||
frame F and joint J multiplied
|
||||
together
|
||||
"""
|
||||
|
||||
return ut.matrix_multiply(t0s, t1s)
|
||||
|
||||
|
||||
def transforms_inv(ts):
|
||||
fts = ts.reshape(-1, 4, 4)
|
||||
fts = np.array(list(map(lambda x: np.linalg.inv(x), fts)))
|
||||
return fts.reshape(ts.shape)
|
||||
|
||||
|
||||
def transforms_blank(anim):
|
||||
"""
|
||||
Blank Transforms
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
anim : Animation
|
||||
Input animation
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
transforms : (F, J, 4, 4) ndarray
|
||||
Array of identity transforms for
|
||||
each frame F and joint J
|
||||
"""
|
||||
|
||||
ts = np.zeros(anim.shape + (4, 4))
|
||||
ts[:, :, 0, 0] = 1.0
|
||||
ts[:, :, 1, 1] = 1.0
|
||||
ts[:, :, 2, 2] = 1.0
|
||||
ts[:, :, 3, 3] = 1.0
|
||||
return ts
|
||||
|
||||
# global transformation matrices
|
||||
|
||||
|
||||
def transforms_global(anim):
|
||||
"""
|
||||
Global Animation Transforms
|
||||
|
||||
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
|
||||
------
|
||||
|
||||
transforms : (F, J, 4, 4) ndarray
|
||||
Array of global transforms for
|
||||
each frame F and joint J
|
||||
"""
|
||||
|
||||
joints = np.arange(anim.shape[1])
|
||||
parents = np.arange(anim.shape[1])
|
||||
locals = transforms_local(anim)
|
||||
globals = transforms_blank(anim)
|
||||
|
||||
globals[:, 0] = locals[:, 0]
|
||||
|
||||
for i in range(1, anim.shape[1]):
|
||||
globals[:, i] = transforms_multiply(globals[:, anim.parents[i]], locals[:, i])
|
||||
|
||||
return globals
|
||||
|
||||
# !!! useful!
|
||||
|
||||
|
||||
def positions_global(anim):
|
||||
"""
|
||||
Global Joint Positions
|
||||
|
||||
Given an animation compute the global joint
|
||||
positions at at every frame
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
anim : Animation
|
||||
Input animation
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
positions : (F, J, 3) ndarray
|
||||
Positions for every frame F
|
||||
and joint position J
|
||||
"""
|
||||
|
||||
# get the last column -- corresponding to the coordinates
|
||||
positions = transforms_global(anim)[:, :, :, 3]
|
||||
return positions[:, :, :3] / positions[:, :, 3, np.newaxis]
|
||||
|
||||
|
||||
""" Rotations """
|
||||
|
||||
|
||||
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 rotations_parents_global(anim):
|
||||
rotations = rotations_global(anim)
|
||||
rotations = rotations[:, anim.parents]
|
||||
rotations[:, 0] = Quaternions.id(len(anim))
|
||||
return rotations
|
||||
|
||||
|
||||
def rotations_load_to_maya(rotations, positions, names=None):
|
||||
"""
|
||||
Load Rotations into Maya
|
||||
|
||||
Loads a Quaternions array into the scene
|
||||
via the representation of axis
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
rotations : (F, J) Quaternions
|
||||
array of rotations to load
|
||||
into the scene where
|
||||
F = number of frames
|
||||
J = number of joints
|
||||
|
||||
positions : (F, J, 3) ndarray
|
||||
array of positions to load
|
||||
rotation axis at where:
|
||||
F = number of frames
|
||||
J = number of joints
|
||||
|
||||
names : [str]
|
||||
List of joint names
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
maxies : Group
|
||||
Grouped Maya Node of all Axis nodes
|
||||
"""
|
||||
|
||||
import pymel.core as pm
|
||||
|
||||
if names is None:
|
||||
names = ["joint_" + str(i) for i in range(rotations.shape[1])]
|
||||
|
||||
maxis = []
|
||||
frames = range(1, len(positions)+1)
|
||||
for i, name in enumerate(names):
|
||||
|
||||
name = name + "_axis"
|
||||
axis = pm.group(
|
||||
pm.curve(p=[(0, 0, 0), (1, 0, 0)], d=1, n=name+'_axis_x'),
|
||||
pm.curve(p=[(0, 0, 0), (0, 1, 0)], d=1, n=name+'_axis_y'),
|
||||
pm.curve(p=[(0, 0, 0), (0, 0, 1)], d=1, n=name+'_axis_z'),
|
||||
n=name)
|
||||
|
||||
axis.rotatePivot.set((0, 0, 0))
|
||||
axis.scalePivot.set((0, 0, 0))
|
||||
axis.childAtIndex(0).overrideEnabled.set(1)
|
||||
axis.childAtIndex(0).overrideColor.set(13)
|
||||
axis.childAtIndex(1).overrideEnabled.set(1)
|
||||
axis.childAtIndex(1).overrideColor.set(14)
|
||||
axis.childAtIndex(2).overrideEnabled.set(1)
|
||||
axis.childAtIndex(2).overrideColor.set(15)
|
||||
|
||||
curvex = pm.nodetypes.AnimCurveTA(n=name + "_rotateX")
|
||||
curvey = pm.nodetypes.AnimCurveTA(n=name + "_rotateY")
|
||||
curvez = pm.nodetypes.AnimCurveTA(n=name + "_rotateZ")
|
||||
|
||||
arotations = rotations[:, i].euler()
|
||||
curvex.addKeys(frames, arotations[:, 0])
|
||||
curvey.addKeys(frames, arotations[:, 1])
|
||||
curvez.addKeys(frames, arotations[:, 2])
|
||||
|
||||
pm.connectAttr(curvex.output, axis.rotateX)
|
||||
pm.connectAttr(curvey.output, axis.rotateY)
|
||||
pm.connectAttr(curvez.output, axis.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, positions[:, i, 0])
|
||||
offsety.addKeys(frames, positions[:, i, 1])
|
||||
offsetz.addKeys(frames, positions[:, i, 2])
|
||||
|
||||
pm.connectAttr(offsetx.output, axis.translateX)
|
||||
pm.connectAttr(offsety.output, axis.translateY)
|
||||
pm.connectAttr(offsetz.output, axis.translateZ)
|
||||
|
||||
maxis.append(axis)
|
||||
|
||||
return pm.group(*maxis, n='RotationAnimation')
|
||||
|
||||
|
||||
""" Offsets & Orients """
|
||||
|
||||
|
||||
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 offsets_transforms_local(anim):
|
||||
|
||||
transforms = anim.orients[np.newaxis].transforms()
|
||||
transforms = np.concatenate([transforms, np.zeros(transforms.shape[:2] + (3, 1))], axis=-1)
|
||||
transforms = np.concatenate([transforms, np.zeros(transforms.shape[:2] + (1, 4))], axis=-2)
|
||||
transforms[:, :, 0:3, 3] = anim.offsets[np.newaxis]
|
||||
transforms[:, :, 3:4, 3] = 1.0
|
||||
return transforms
|
||||
|
||||
|
||||
def offsets_transforms_global(anim):
|
||||
|
||||
joints = np.arange(anim.shape[1])
|
||||
parents = np.arange(anim.shape[1])
|
||||
locals = offsets_transforms_local(anim)
|
||||
globals = transforms_blank(anim)
|
||||
|
||||
globals[:, 0] = locals[:, 0]
|
||||
|
||||
for i in range(1, anim.shape[1]):
|
||||
globals[:, i] = transforms_multiply(globals[:, anim.parents[i]], locals[:, i])
|
||||
|
||||
return globals
|
||||
|
||||
|
||||
def offsets_global(anim):
|
||||
offsets = offsets_transforms_global(anim)[:, :, :, 3]
|
||||
return offsets[0, :, :3] / offsets[0, :, 3, np.newaxis]
|
||||
|
||||
|
||||
""" Lengths """
|
||||
|
||||
|
||||
def offset_lengths(anim):
|
||||
return np.sum(anim.offsets[1:]**2.0, axis=1)**0.5
|
||||
|
||||
|
||||
def position_lengths(anim):
|
||||
return np.sum(anim.positions[:, 1:]**2.0, axis=2)**0.5
|
||||
|
||||
|
||||
""" Skinning """
|
||||
|
||||
|
||||
def skin(anim, rest, weights, mesh, maxjoints=4):
|
||||
|
||||
full_transforms = transforms_multiply(
|
||||
transforms_global(anim),
|
||||
transforms_inv(transforms_global(rest[0:1])))
|
||||
|
||||
weightids = np.argsort(-weights, axis=1)[:, :maxjoints]
|
||||
weightvls = np.array(list(map(lambda w, i: w[i], weights, weightids)))
|
||||
weightvls = weightvls / weightvls.sum(axis=1)[..., np.newaxis]
|
||||
|
||||
verts = np.hstack([mesh, np.ones((len(mesh), 1))])
|
||||
verts = verts[np.newaxis, :, np.newaxis, :, np.newaxis]
|
||||
verts = transforms_multiply(full_transforms[:, weightids], verts)
|
||||
verts = (verts[:, :, :, :3] / verts[:, :, :, 3:4])[:, :, :, :, 0]
|
||||
|
||||
return np.sum(weightvls[np.newaxis, :, :, np.newaxis] * verts, axis=2)
|
||||
@@ -0,0 +1,451 @@
|
||||
import numpy as np
|
||||
import scipy.sparse as sparse
|
||||
import Animation as Animation
|
||||
|
||||
|
||||
""" Maya Functions """
|
||||
|
||||
def load_from_maya(root):
|
||||
"""
|
||||
Load joint parents and names from maya
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
root : PyNode
|
||||
Root Maya Node
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
(names, parents) : ([str], (J) ndarray)
|
||||
List of joint names and array
|
||||
of indices representing the parent
|
||||
joint for each joint J.
|
||||
|
||||
Joint index -1 is used to represent
|
||||
that there is no parent joint
|
||||
"""
|
||||
|
||||
import pymel.core as pm
|
||||
|
||||
names = []
|
||||
parents = []
|
||||
|
||||
def unload_joint(j, parents, par):
|
||||
|
||||
id = len(names)
|
||||
names.append(j)
|
||||
parents.append(par)
|
||||
|
||||
children = [c for c in j.getChildren() if
|
||||
isinstance(c, pm.nt.Transform) and
|
||||
not isinstance(c, pm.nt.Constraint) and
|
||||
not any(pm.listRelatives(c, s=True)) and
|
||||
(any(pm.listRelatives(c, ad=True, ap=False, type='joint')) or isinstance(c, pm.nt.Joint))]
|
||||
|
||||
map(lambda c: unload_joint(c, parents, id), children)
|
||||
|
||||
unload_joint(root, parents, -1)
|
||||
|
||||
return (names, parents)
|
||||
|
||||
|
||||
""" Family Functions """
|
||||
|
||||
def joints(parents):
|
||||
"""
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
joints : (J) ndarray
|
||||
Array of joint indices
|
||||
"""
|
||||
return np.arange(len(parents), dtype=int)
|
||||
|
||||
def joints_list(parents):
|
||||
"""
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
joints : [ndarray]
|
||||
List of arrays of joint idices for
|
||||
each joint
|
||||
"""
|
||||
return list(joints(parents)[:,np.newaxis])
|
||||
|
||||
def parents_list(parents):
|
||||
"""
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
parents : [ndarray]
|
||||
List of arrays of joint idices for
|
||||
the parents of each joint
|
||||
"""
|
||||
return list(parents[:,np.newaxis])
|
||||
|
||||
|
||||
def children_list(parents):
|
||||
"""
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
children : [ndarray]
|
||||
List of arrays of joint indices for
|
||||
the children of each joint
|
||||
"""
|
||||
|
||||
def joint_children(i):
|
||||
return [j for j, p in enumerate(parents) if p == i]
|
||||
|
||||
return list(map(lambda j: np.array(joint_children(j)), joints(parents)))
|
||||
|
||||
|
||||
def descendants_list(parents):
|
||||
"""
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
descendants : [ndarray]
|
||||
List of arrays of joint idices for
|
||||
the descendants of each joint
|
||||
"""
|
||||
|
||||
children = children_list(parents)
|
||||
|
||||
def joint_descendants(i):
|
||||
return sum([joint_descendants(j) for j in children[i]], list(children[i]))
|
||||
|
||||
return list(map(lambda j: np.array(joint_descendants(j)), joints(parents)))
|
||||
|
||||
|
||||
def ancestors_list(parents):
|
||||
"""
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
ancestors : [ndarray]
|
||||
List of arrays of joint idices for
|
||||
the ancestors of each joint
|
||||
"""
|
||||
|
||||
decendants = descendants_list(parents)
|
||||
|
||||
def joint_ancestors(i):
|
||||
return [j for j in joints(parents) if i in decendants[j]]
|
||||
|
||||
return list(map(lambda j: np.array(joint_ancestors(j)), joints(parents)))
|
||||
|
||||
|
||||
""" Mask Functions """
|
||||
|
||||
def mask(parents, filter):
|
||||
"""
|
||||
Constructs a Mask for a give filter
|
||||
|
||||
A mask is a (J, J) ndarray truth table for a given
|
||||
condition over J joints. For example there
|
||||
may be a mask specifying if a joint N is a
|
||||
child of another joint M.
|
||||
|
||||
This could be constructed into a mask using
|
||||
`m = mask(parents, children_list)` and the condition
|
||||
of childhood tested using `m[N, M]`.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
filter : (J) ndarray -> [ndarray]
|
||||
function that outputs a list of arrays
|
||||
of joint indices for some condition
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
mask : (N, N) ndarray
|
||||
boolean truth table of given condition
|
||||
"""
|
||||
m = np.zeros((len(parents), len(parents))).astype(bool)
|
||||
jnts = joints(parents)
|
||||
fltr = filter(parents)
|
||||
for i,f in enumerate(fltr): m[i,:] = np.any(jnts[:,np.newaxis] == f[np.newaxis,:], axis=1)
|
||||
return m
|
||||
|
||||
def joints_mask(parents): return np.eye(len(parents)).astype(bool)
|
||||
def children_mask(parents): return mask(parents, children_list)
|
||||
def parents_mask(parents): return mask(parents, parents_list)
|
||||
def descendants_mask(parents): return mask(parents, descendants_list)
|
||||
def ancestors_mask(parents): return mask(parents, ancestors_list)
|
||||
|
||||
""" Search Functions """
|
||||
|
||||
def joint_chain_ascend(parents, start, end):
|
||||
chain = []
|
||||
while start != end:
|
||||
chain.append(start)
|
||||
start = parents[start]
|
||||
chain.append(end)
|
||||
return np.array(chain, dtype=int)
|
||||
|
||||
|
||||
""" Constraints """
|
||||
|
||||
def constraints(anim, **kwargs):
|
||||
"""
|
||||
Constraint list for Animation
|
||||
|
||||
This constraint list can be used in the
|
||||
VerletParticle solver to constrain
|
||||
a animation global joint positions.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
anim : Animation
|
||||
Input animation
|
||||
|
||||
masses : (F, J) ndarray
|
||||
Optional list of masses
|
||||
for joints J across frames F
|
||||
defaults to weighting by
|
||||
vertical height
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
constraints : [(int, int, (F, J) ndarray, (F, J) ndarray, (F, J) ndarray)]
|
||||
A list of constraints in the format:
|
||||
(Joint1, Joint2, Masses1, Masses2, Lengths)
|
||||
|
||||
"""
|
||||
|
||||
masses = kwargs.pop('masses', None)
|
||||
|
||||
children = children_list(anim.parents)
|
||||
constraints = []
|
||||
|
||||
points_offsets = Animation.offsets_global(anim)
|
||||
points = Animation.positions_global(anim)
|
||||
|
||||
if masses is None:
|
||||
masses = 1.0 / (0.1 + np.absolute(points_offsets[:,1]))
|
||||
masses = masses[np.newaxis].repeat(len(anim), axis=0)
|
||||
|
||||
for j in xrange(anim.shape[1]):
|
||||
|
||||
""" Add constraints between all joints and their children """
|
||||
for c0 in children[j]:
|
||||
|
||||
dists = np.sum((points[:, c0] - points[:, j])**2.0, axis=1)**0.5
|
||||
constraints.append((c0, j, masses[:,c0], masses[:,j], dists))
|
||||
|
||||
""" Add constraints between all children of joint """
|
||||
for c1 in children[j]:
|
||||
if c0 == c1: continue
|
||||
|
||||
dists = np.sum((points[:, c0] - points[:, c1])**2.0, axis=1)**0.5
|
||||
constraints.append((c0, c1, masses[:,c0], masses[:,c1], dists))
|
||||
|
||||
return constraints
|
||||
|
||||
""" Graph Functions """
|
||||
|
||||
def graph(anim):
|
||||
"""
|
||||
Generates a weighted adjacency matrix
|
||||
using local joint distances along
|
||||
the skeletal structure.
|
||||
|
||||
Joints which are not connected
|
||||
are assigned the weight `0`.
|
||||
|
||||
Joints which actually have zero distance
|
||||
between them, but are still connected, are
|
||||
perturbed by some minimal amount.
|
||||
|
||||
The output of this routine can be used
|
||||
with the `scipy.sparse.csgraph`
|
||||
routines for graph analysis.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
anim : Animation
|
||||
input animation
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
graph : (N, N) ndarray
|
||||
weight adjacency matrix using
|
||||
local distances along the
|
||||
skeletal structure from joint
|
||||
N to joint M. If joints are not
|
||||
directly connected are assigned
|
||||
the weight `0`.
|
||||
"""
|
||||
|
||||
graph = np.zeros(anim.shape[1], anim.shape[1])
|
||||
lengths = np.sum(anim.offsets**2.0, axis=1)**0.5 + 0.001
|
||||
|
||||
for i,p in enumerate(anim.parents):
|
||||
if p == -1: continue
|
||||
graph[i,p] = lengths[p]
|
||||
graph[p,i] = lengths[p]
|
||||
|
||||
return graph
|
||||
|
||||
|
||||
def distances(anim):
|
||||
"""
|
||||
Generates a distance matrix for
|
||||
pairwise joint distances along
|
||||
the skeletal structure
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
anim : Animation
|
||||
input animation
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
distances : (N, N) ndarray
|
||||
array of pairwise distances
|
||||
along skeletal structure
|
||||
from some joint N to some
|
||||
joint M
|
||||
"""
|
||||
|
||||
distances = np.zeros((anim.shape[1], anim.shape[1]))
|
||||
generated = distances.copy().astype(bool)
|
||||
|
||||
joint_lengths = np.sum(anim.offsets**2.0, axis=1)**0.5
|
||||
joint_children = children_list(anim)
|
||||
joint_parents = parents_list(anim)
|
||||
|
||||
def find_distance(distances, generated, prev, i, j):
|
||||
|
||||
""" If root, identity, or already generated, return """
|
||||
if j == -1: return (0.0, True)
|
||||
if j == i: return (0.0, True)
|
||||
if generated[i,j]: return (distances[i,j], True)
|
||||
|
||||
""" Find best distances along parents and children """
|
||||
par_dists = [(joint_lengths[j], find_distance(distances, generated, j, i, p)) for p in joint_parents[j] if p != prev]
|
||||
out_dists = [(joint_lengths[c], find_distance(distances, generated, j, i, c)) for c in joint_children[j] if c != prev]
|
||||
|
||||
""" Check valid distance and not dead end """
|
||||
par_dists = [a + d for (a, (d, f)) in par_dists if f]
|
||||
out_dists = [a + d for (a, (d, f)) in out_dists if f]
|
||||
|
||||
""" All dead ends """
|
||||
if (out_dists + par_dists) == []: return (0.0, False)
|
||||
|
||||
""" Get minimum path """
|
||||
dist = min(out_dists + par_dists)
|
||||
distances[i,j] = dist; distances[j,i] = dist
|
||||
generated[i,j] = True; generated[j,i] = True
|
||||
|
||||
for i in xrange(anim.shape[1]):
|
||||
for j in xrange(anim.shape[1]):
|
||||
find_distance(distances, generated, -1, i, j)
|
||||
|
||||
return distances
|
||||
|
||||
def edges(parents):
|
||||
"""
|
||||
Animation structure edges
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
edges : (M, 2) ndarray
|
||||
array of pairs where each
|
||||
pair contains two indices of a joints
|
||||
which corrisponds to an edge in the
|
||||
joint structure going from parent to child.
|
||||
"""
|
||||
|
||||
return np.array(list(zip(parents, joints(parents)))[1:])
|
||||
|
||||
|
||||
def incidence(parents):
|
||||
"""
|
||||
Incidence Matrix
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
parents : (J) ndarray
|
||||
parents array
|
||||
|
||||
Returns
|
||||
-------
|
||||
|
||||
incidence : (N, M) ndarray
|
||||
|
||||
Matrix of N joint positions by
|
||||
M edges which each entry is either
|
||||
1 or -1 and multiplication by the
|
||||
joint positions returns the an
|
||||
array of vectors along each edge
|
||||
of the structure
|
||||
"""
|
||||
|
||||
es = edges(parents)
|
||||
|
||||
inc = np.zeros((len(parents)-1, len(parents))).astype(np.int)
|
||||
for i, e in enumerate(es):
|
||||
inc[i,e[0]] = 1
|
||||
inc[i,e[1]] = -1
|
||||
|
||||
return inc.T
|
||||
|
||||
+471
@@ -0,0 +1,471 @@
|
||||
from Quaternions import Quaternions
|
||||
from Animation import Animation
|
||||
import re
|
||||
import numpy as np
|
||||
import sys
|
||||
|
||||
|
||||
channelmap = {
|
||||
'Xrotation': 'x',
|
||||
'Yrotation': 'y',
|
||||
'Zrotation': 'z'
|
||||
}
|
||||
|
||||
channelmap_inv = {
|
||||
'x': 'Xrotation',
|
||||
'y': 'Yrotation',
|
||||
'z': 'Zrotation',
|
||||
}
|
||||
|
||||
ordermap = {
|
||||
'x': 0,
|
||||
'y': 1,
|
||||
'z': 2,
|
||||
}
|
||||
|
||||
|
||||
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 load_bfa(filename, start=None, end=None, order=None, world=False):
|
||||
"""
|
||||
Reads a BVH file and constructs an animation
|
||||
|
||||
!!! Read from bfa, will replace the end sites of arms by two joints (w/ unit rotation)
|
||||
|
||||
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
|
||||
|
||||
hand_idx = [9, 14]
|
||||
|
||||
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()))])
|
||||
"""
|
||||
else:
|
||||
print("active = ", active)
|
||||
if active in hand_idx:
|
||||
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:
|
||||
if active + 1 in hand_idx:
|
||||
print("parent:", names[-1])
|
||||
name = "LeftHandIndex" if active + 1 == hand_idx[0] else "RightHandIndex"
|
||||
names.append(name)
|
||||
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)
|
||||
else:
|
||||
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]
|
||||
tmp = data_block[3:].reshape(N - 2, 3)
|
||||
tmp = np.concatenate([tmp[:hand_idx[0]],
|
||||
np.array([[0, 0, 0]]),
|
||||
tmp[hand_idx[0]: hand_idx[1] - 1],
|
||||
np.array([[0, 0, 0]]),
|
||||
tmp[hand_idx[1] - 1:]], axis=0)
|
||||
rotations[fi, :] = tmp.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 save(filename, anim, names=None, frametime=1.0/24.0, order='zyx', positions=False, orients=True):
|
||||
"""
|
||||
Saves an Animation to file as BVH
|
||||
|
||||
Parameters
|
||||
----------
|
||||
filename: str
|
||||
File to be saved to
|
||||
|
||||
anim : Animation
|
||||
Animation to save
|
||||
|
||||
names : [str]
|
||||
List of joint names
|
||||
|
||||
order : str
|
||||
Optional Specifier for joint order.
|
||||
Given as string E.G 'xyz', 'zxy'
|
||||
|
||||
frametime : float
|
||||
Optional Animation Frame time
|
||||
|
||||
positions : bool
|
||||
Optional specfier to save bone
|
||||
positions for each frame
|
||||
|
||||
orients : bool
|
||||
Multiply joint orients to the rotations
|
||||
before saving.
|
||||
|
||||
"""
|
||||
|
||||
if names is None:
|
||||
names = ["joint_" + str(i) for i in range(len(anim.parents))]
|
||||
|
||||
with open(filename, 'w') as f:
|
||||
|
||||
t = ""
|
||||
f.write("%sHIERARCHY\n" % t)
|
||||
f.write("%sROOT %s\n" % (t, names[0]))
|
||||
f.write("%s{\n" % t)
|
||||
t += '\t'
|
||||
|
||||
f.write("%sOFFSET %f %f %f\n" % (t, anim.offsets[0, 0], anim.offsets[0, 1], anim.offsets[0, 2]))
|
||||
f.write("%sCHANNELS 6 Xposition Yposition Zposition %s %s %s \n" %
|
||||
(t, channelmap_inv[order[0]], channelmap_inv[order[1]], channelmap_inv[order[2]]))
|
||||
|
||||
for i in range(anim.shape[1]):
|
||||
if anim.parents[i] == 0:
|
||||
t = save_joint(f, anim, names, t, i, order=order, positions=positions)
|
||||
|
||||
t = t[:-1]
|
||||
f.write("%s}\n" % t)
|
||||
|
||||
f.write("MOTION\n")
|
||||
f.write("Frames: %i\n" % anim.shape[0])
|
||||
f.write("Frame Time: %f\n" % frametime)
|
||||
|
||||
# if orients:
|
||||
# rots = np.degrees((-anim.orients[np.newaxis] * anim.rotations).euler(order=order[::-1]))
|
||||
# else:
|
||||
# rots = np.degrees(anim.rotations.euler(order=order[::-1]))
|
||||
rots = np.degrees(anim.rotations.euler(order=order[::-1]))
|
||||
poss = anim.positions
|
||||
|
||||
for i in range(anim.shape[0]):
|
||||
for j in range(anim.shape[1]):
|
||||
|
||||
if positions or j == 0:
|
||||
|
||||
f.write("%f %f %f %f %f %f " % (
|
||||
poss[i, j, 0], poss[i, j, 1], poss[i, j, 2],
|
||||
rots[i, j, ordermap[order[0]]], rots[i, j, ordermap[order[1]]], rots[i, j, ordermap[order[2]]]))
|
||||
|
||||
else:
|
||||
|
||||
f.write("%f %f %f " % (
|
||||
rots[i, j, ordermap[order[0]]], rots[i, j, ordermap[order[1]]], rots[i, j, ordermap[order[2]]]))
|
||||
|
||||
f.write("\n")
|
||||
|
||||
|
||||
def save_joint(f, anim, names, t, i, order='zyx', positions=False):
|
||||
|
||||
f.write("%sJOINT %s\n" % (t, names[i]))
|
||||
f.write("%s{\n" % t)
|
||||
t += '\t'
|
||||
|
||||
f.write("%sOFFSET %f %f %f\n" % (t, anim.offsets[i, 0], anim.offsets[i, 1], anim.offsets[i, 2]))
|
||||
|
||||
if positions:
|
||||
f.write("%sCHANNELS 6 Xposition Yposition Zposition %s %s %s \n" % (t,
|
||||
channelmap_inv[order[0]], channelmap_inv[order[1]], channelmap_inv[order[2]]))
|
||||
else:
|
||||
f.write("%sCHANNELS 3 %s %s %s\n" % (t,
|
||||
channelmap_inv[order[0]], channelmap_inv[order[1]], channelmap_inv[order[2]]))
|
||||
|
||||
end_site = True
|
||||
|
||||
for j in range(anim.shape[1]):
|
||||
if anim.parents[j] == i:
|
||||
t = save_joint(f, anim, names, t, j, order=order, positions=positions)
|
||||
end_site = False
|
||||
|
||||
if end_site:
|
||||
f.write("%sEnd Site\n" % t)
|
||||
f.write("%s{\n" % t)
|
||||
t += '\t'
|
||||
f.write("%sOFFSET %f %f %f\n" % (t, 0.0, 0.0, 0.0))
|
||||
t = t[:-1]
|
||||
f.write("%s}\n" % t)
|
||||
|
||||
t = t[:-1]
|
||||
f.write("%s}\n" % t)
|
||||
|
||||
return t
|
||||
@@ -0,0 +1,496 @@
|
||||
import numpy as np
|
||||
import scipy.linalg as linalg
|
||||
|
||||
import Animation
|
||||
import AnimationStructure
|
||||
|
||||
from Quaternions import Quaternions
|
||||
|
||||
|
||||
class BasicInverseKinematics:
|
||||
"""
|
||||
Basic Inverse Kinematics Solver
|
||||
|
||||
This is an extremely simple full body IK
|
||||
solver.
|
||||
|
||||
It works given the following conditions:
|
||||
|
||||
* All joint targets must be specified
|
||||
* All joint targets must be in reach
|
||||
* All joint targets must not differ
|
||||
extremely from the starting pose
|
||||
* No bone length constraints can be violated
|
||||
* The root translation and rotation are
|
||||
set to good initial values
|
||||
|
||||
It works under the observation that if the
|
||||
_directions_ the joints are pointing toward
|
||||
match the _directions_ of the vectors between
|
||||
the target joints then the pose should match
|
||||
that of the target pose.
|
||||
|
||||
Therefore it iterates over joints rotating
|
||||
each joint such that the vectors between it
|
||||
and it's children match that of the target
|
||||
positions.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
animation : Animation
|
||||
animation input
|
||||
|
||||
positions : (F, J, 3) ndarray
|
||||
target positions for each frame F
|
||||
and each joint J
|
||||
|
||||
iterations : int
|
||||
Optional number of iterations.
|
||||
If the above conditions are met
|
||||
1 iteration should be enough,
|
||||
therefore the default is 1
|
||||
|
||||
silent : bool
|
||||
Optional if to suppress output
|
||||
defaults to False
|
||||
"""
|
||||
|
||||
def __init__(self, animation, positions, iterations=1, silent=True):
|
||||
|
||||
self.animation = animation
|
||||
self.positions = positions
|
||||
self.iterations = iterations
|
||||
self.silent = silent
|
||||
|
||||
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
|
||||
|
||||
|
||||
class JacobianInverseKinematics:
|
||||
"""
|
||||
Jacobian Based Full Body IK Solver
|
||||
|
||||
This is a full body IK solver which
|
||||
uses the dampened least squares inverse
|
||||
jacobian method.
|
||||
|
||||
It should remain fairly stable and effective
|
||||
even for joint positions which are out of
|
||||
reach and it can also take any number of targets
|
||||
to treat as end effectors.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
animation : Animation
|
||||
animation to solve inverse problem on
|
||||
|
||||
targets : {int : (F, 3) ndarray}
|
||||
Dictionary of target positions for each
|
||||
frame F, mapping joint index to
|
||||
a target position
|
||||
|
||||
references : (F, 3)
|
||||
Optional list of J joint position
|
||||
references for which the result
|
||||
should bias toward
|
||||
|
||||
iterations : int
|
||||
Optional number of iterations to
|
||||
compute. More iterations results in
|
||||
better accuracy but takes longer to
|
||||
compute. Default is 10.
|
||||
|
||||
recalculate : bool
|
||||
Optional if to recalcuate jacobian
|
||||
each iteration. Gives better accuracy
|
||||
but slower to compute. Defaults to True
|
||||
|
||||
damping : float
|
||||
Optional damping constant. Higher
|
||||
damping increases stability but
|
||||
requires more iterations to converge.
|
||||
Defaults to 5.0
|
||||
|
||||
secondary : float
|
||||
Force, or bias toward secondary target.
|
||||
Defaults to 0.25
|
||||
|
||||
silent : bool
|
||||
Optional if to suppress output
|
||||
defaults to False
|
||||
"""
|
||||
|
||||
def __init__(self, animation, targets,
|
||||
references=None, iterations=10,
|
||||
recalculate=True, damping=2.0,
|
||||
secondary=0.25, translate=False,
|
||||
silent=False, weights=None,
|
||||
weights_translate=None):
|
||||
|
||||
self.animation = animation
|
||||
self.targets = targets
|
||||
self.references = references
|
||||
|
||||
self.iterations = iterations
|
||||
self.recalculate = recalculate
|
||||
self.damping = damping
|
||||
self.secondary = secondary
|
||||
self.translate = translate
|
||||
self.silent = silent
|
||||
self.weights = weights
|
||||
self.weights_translate = weights_translate
|
||||
|
||||
def cross(self, a, b):
|
||||
o = np.empty(b.shape)
|
||||
o[..., 0] = a[..., 1]*b[..., 2] - a[..., 2]*b[..., 1]
|
||||
o[..., 1] = a[..., 2]*b[..., 0] - a[..., 0]*b[..., 2]
|
||||
o[..., 2] = a[..., 0]*b[..., 1] - a[..., 1]*b[..., 0]
|
||||
return o
|
||||
|
||||
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
|
||||
|
||||
# @profile(immediate=True)
|
||||
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 range(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))
|
||||
|
||||
j = np.nan_to_num(j)
|
||||
e = np.nan_to_num(e)
|
||||
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))
|
||||
|
||||
|
||||
class BasicJacobianIK:
|
||||
"""
|
||||
Same interface as BasicInverseKinematics
|
||||
but uses the Jacobian IK Solver Instead
|
||||
"""
|
||||
|
||||
def __init__(self, animation, positions, iterations=10, silent=True, **kw):
|
||||
|
||||
targets = dict([(i, positions[:, i]) for i in range(positions.shape[1])])
|
||||
self.ik = JacobianInverseKinematics(animation, targets, iterations=iterations, silent=silent, **kw)
|
||||
|
||||
def __call__(self, **kw):
|
||||
return self.ik(**kw)
|
||||
|
||||
|
||||
class ICP:
|
||||
|
||||
def __init__(self,
|
||||
anim, rest, weights, mesh, goal,
|
||||
find_closest=True, damping=10,
|
||||
iterations=10, silent=True,
|
||||
translate=True, recalculate=True,
|
||||
weights_translate=None):
|
||||
|
||||
self.animation = anim
|
||||
self.rest = rest
|
||||
self.vweights = weights
|
||||
self.mesh = mesh
|
||||
self.goal = goal
|
||||
self.find_closest = find_closest
|
||||
self.iterations = iterations
|
||||
self.silent = silent
|
||||
self.translate = translate
|
||||
self.damping = damping
|
||||
self.weights = None
|
||||
self.weights_translate = weights_translate
|
||||
self.recalculate = recalculate
|
||||
|
||||
def cross(self, a, b):
|
||||
o = np.empty(b.shape)
|
||||
o[..., 0] = a[..., 1]*b[..., 2] - a[..., 2]*b[..., 1]
|
||||
o[..., 1] = a[..., 2]*b[..., 0] - a[..., 0]*b[..., 2]
|
||||
o[..., 2] = a[..., 0]*b[..., 1] - a[..., 1]*b[..., 0]
|
||||
return o
|
||||
|
||||
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
|
||||
|
||||
# @profile(immediate=True)
|
||||
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))
|
||||
@@ -0,0 +1,91 @@
|
||||
import numpy as np
|
||||
|
||||
from Quaternions import Quaternions
|
||||
|
||||
|
||||
class Pivots:
|
||||
"""
|
||||
Pivots is an ndarray of angular rotations
|
||||
|
||||
This wrapper provides some functions for
|
||||
working with pivots.
|
||||
|
||||
These are particularly useful as a number
|
||||
of atomic operations (such as adding or
|
||||
subtracting) cannot be achieved using
|
||||
the standard arithmatic and need to be
|
||||
defined differently to work correctly
|
||||
"""
|
||||
|
||||
def __init__(self, ps): self.ps = np.array(ps)
|
||||
def __str__(self): return "Pivots(" + str(self.ps) + ")"
|
||||
def __repr__(self): return "Pivots(" + repr(self.ps) + ")"
|
||||
|
||||
def __add__(self, other): return Pivots(np.arctan2(np.sin(self.ps + other.ps), np.cos(self.ps + other.ps)))
|
||||
def __sub__(self, other): return Pivots(np.arctan2(np.sin(self.ps - other.ps), np.cos(self.ps - other.ps)))
|
||||
def __mul__(self, other): return Pivots(self.ps * other.ps)
|
||||
def __div__(self, other): return Pivots(self.ps / other.ps)
|
||||
def __mod__(self, other): return Pivots(self.ps % other.ps)
|
||||
def __pow__(self, other): return Pivots(self.ps ** other.ps)
|
||||
|
||||
def __lt__(self, other): return self.ps < other.ps
|
||||
def __le__(self, other): return self.ps <= other.ps
|
||||
def __eq__(self, other): return self.ps == other.ps
|
||||
def __ne__(self, other): return self.ps != other.ps
|
||||
def __ge__(self, other): return self.ps >= other.ps
|
||||
def __gt__(self, other): return self.ps > other.ps
|
||||
|
||||
def __abs__(self): return Pivots(abs(self.ps))
|
||||
def __neg__(self): return Pivots(-self.ps)
|
||||
|
||||
def __iter__(self): return iter(self.ps)
|
||||
def __len__(self): return len(self.ps)
|
||||
|
||||
def __getitem__(self, k): return Pivots(self.ps[k])
|
||||
def __setitem__(self, k, v): self.ps[k] = v.ps
|
||||
|
||||
def _ellipsis(self): return tuple(map(lambda x: slice(None), self.shape))
|
||||
|
||||
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 directions(self, plane='xz'):
|
||||
dirs = np.zeros((len(self.ps), 3))
|
||||
dirs[..., "xyz".index(plane[0])] = np.sin(self.ps)
|
||||
dirs[..., "xyz".index(plane[1])] = np.cos(self.ps)
|
||||
return dirs
|
||||
|
||||
def normalized(self):
|
||||
xs = np.copy(self.ps)
|
||||
while np.any(xs > np.pi):
|
||||
xs[xs > np.pi] = xs[xs > np.pi] - 2 * np.pi
|
||||
while np.any(xs < -np.pi):
|
||||
xs[xs < -np.pi] = xs[xs < -np.pi] + 2 * np.pi
|
||||
return Pivots(xs)
|
||||
|
||||
def interpolate(self, ws):
|
||||
dir = np.average(self.directions, weights=ws, axis=0)
|
||||
return np.arctan2(dir[2], dir[0])
|
||||
|
||||
def copy(self):
|
||||
return Pivots(np.copy(self.ps))
|
||||
|
||||
@property
|
||||
def shape(self):
|
||||
return self.ps.shape
|
||||
|
||||
@classmethod
|
||||
def from_quaternions(cls, qs, forward='z', plane='xz'):
|
||||
ds = np.zeros(qs.shape + (3,))
|
||||
ds[..., 'xyz'.index(forward)] = 1.0
|
||||
return Pivots.from_directions(qs * ds, plane=plane)
|
||||
|
||||
@classmethod
|
||||
def from_directions(cls, ds, plane='xz'):
|
||||
ys = ds[..., 'xyz'.index(plane[0])]
|
||||
xs = ds[..., 'xyz'.index(plane[1])]
|
||||
return Pivots(np.arctan2(ys, xs))
|
||||
@@ -0,0 +1,481 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Quaternions:
|
||||
"""
|
||||
Quaternions is a wrapper around a numpy ndarray
|
||||
that allows it to act as if it were an narray of
|
||||
a quaternion data type.
|
||||
|
||||
Therefore addition, subtraction, multiplication,
|
||||
division, negation, absolute, are all defined
|
||||
in terms of quaternion operations such as quaternion
|
||||
multiplication.
|
||||
|
||||
This allows for much neater code and many routines
|
||||
which conceptually do the same thing to be written
|
||||
in the same way for point data and for rotation data.
|
||||
|
||||
The Quaternions class has been desgined such that it
|
||||
should support broadcasting and slicing in all of the
|
||||
usual ways.
|
||||
"""
|
||||
|
||||
def __init__(self, qs):
|
||||
if isinstance(qs, np.ndarray):
|
||||
|
||||
if len(qs.shape) == 1:
|
||||
qs = np.array([qs])
|
||||
self.qs = qs
|
||||
return
|
||||
|
||||
if isinstance(qs, Quaternions):
|
||||
self.qs = qs.qs
|
||||
return
|
||||
|
||||
raise TypeError('Quaternions must be constructed from iterable, numpy array, or Quaternions, not %s' % type(qs))
|
||||
|
||||
def __str__(self): return "Quaternions(" + str(self.qs) + ")"
|
||||
def __repr__(self): return "Quaternions(" + repr(self.qs) + ")"
|
||||
""" Helper Methods for Broadcasting and Data extraction """
|
||||
|
||||
@classmethod
|
||||
def _broadcast(cls, sqs, oqs, scalar=False):
|
||||
if isinstance(oqs, float):
|
||||
return sqs, oqs * np.ones(sqs.shape[:-1])
|
||||
|
||||
ss = np.array(sqs.shape) if not scalar else np.array(sqs.shape[:-1])
|
||||
os = np.array(oqs.shape)
|
||||
|
||||
if len(ss) != len(os):
|
||||
raise TypeError('Quaternions cannot broadcast together shapes %s and %s' % (sqs.shape, oqs.shape))
|
||||
|
||||
if np.all(ss == os):
|
||||
return sqs, oqs
|
||||
|
||||
if not np.all((ss == os) | (os == np.ones(len(os))) | (ss == np.ones(len(ss)))):
|
||||
raise TypeError('Quaternions cannot broadcast together shapes %s and %s' % (sqs.shape, oqs.shape))
|
||||
|
||||
sqsn, oqsn = sqs.copy(), oqs.copy()
|
||||
|
||||
for a in np.where(ss == 1)[0]:
|
||||
sqsn = sqsn.repeat(os[a], axis=a)
|
||||
for a in np.where(os == 1)[0]:
|
||||
oqsn = oqsn.repeat(ss[a], axis=a)
|
||||
|
||||
return sqsn, oqsn
|
||||
|
||||
""" Adding Quaterions is just Defined as Multiplication """
|
||||
|
||||
def __add__(self, other): return self * other
|
||||
def __sub__(self, other): return self / other
|
||||
""" Quaterion Multiplication """
|
||||
|
||||
def __mul__(self, other):
|
||||
"""
|
||||
Quaternion multiplication has three main methods.
|
||||
|
||||
When multiplying a Quaternions array by Quaternions
|
||||
normal quaternion multiplication is performed.
|
||||
|
||||
When multiplying a Quaternions array by a vector
|
||||
array of the same shape, where the last axis is 3,
|
||||
it is assumed to be a Quaternion by 3D-Vector
|
||||
multiplication and the 3D-Vectors are rotated
|
||||
in space by the Quaternions.
|
||||
|
||||
When multipplying a Quaternions array by a scalar
|
||||
or vector of different shape it is assumed to be
|
||||
a Quaternions by Scalars multiplication and the
|
||||
Quaternions are scaled using Slerp and the identity
|
||||
quaternions.
|
||||
"""
|
||||
|
||||
""" If Quaternions type do Quaternions * Quaternions """
|
||||
if isinstance(other, Quaternions):
|
||||
sqs, oqs = Quaternions._broadcast(self.qs, other.qs)
|
||||
|
||||
q0 = sqs[..., 0]
|
||||
q1 = sqs[..., 1]
|
||||
q2 = sqs[..., 2]
|
||||
q3 = sqs[..., 3]
|
||||
r0 = oqs[..., 0]
|
||||
r1 = oqs[..., 1]
|
||||
r2 = oqs[..., 2]
|
||||
r3 = oqs[..., 3]
|
||||
|
||||
qs = np.empty(sqs.shape)
|
||||
qs[..., 0] = r0 * q0 - r1 * q1 - r2 * q2 - r3 * q3
|
||||
qs[..., 1] = r0 * q1 + r1 * q0 - r2 * q3 + r3 * q2
|
||||
qs[..., 2] = r0 * q2 + r1 * q3 + r2 * q0 - r3 * q1
|
||||
qs[..., 3] = r0 * q3 - r1 * q2 + r2 * q1 + r3 * q0
|
||||
|
||||
return Quaternions(qs)
|
||||
|
||||
""" If array type do Quaternions * Vectors """
|
||||
if isinstance(other, np.ndarray) and other.shape[-1] == 3:
|
||||
vs = Quaternions(np.concatenate([np.zeros(other.shape[:-1] + (1,)), other], axis=-1))
|
||||
|
||||
return (self * (vs * -self)).imaginaries
|
||||
|
||||
""" If float do Quaternions * Scalars """
|
||||
if isinstance(other, np.ndarray) or isinstance(other, float):
|
||||
return Quaternions.slerp(Quaternions.id_like(self), self, other)
|
||||
|
||||
raise TypeError('Cannot multiply/add Quaternions with type %s' % str(type(other)))
|
||||
|
||||
def __div__(self, other):
|
||||
"""
|
||||
When a Quaternion type is supplied, division is defined
|
||||
as multiplication by the inverse of that Quaternion.
|
||||
|
||||
When a scalar or vector is supplied it is defined
|
||||
as multiplicaion of one over the supplied value.
|
||||
Essentially a scaling.
|
||||
"""
|
||||
|
||||
if isinstance(other, Quaternions):
|
||||
return self * (-other)
|
||||
if isinstance(other, np.ndarray):
|
||||
return self * (1.0 / other)
|
||||
if isinstance(other, float):
|
||||
return self * (1.0 / other)
|
||||
raise TypeError('Cannot divide/subtract Quaternions with type %s' + str(type(other)))
|
||||
|
||||
def __eq__(self, other): return self.qs == other.qs
|
||||
def __ne__(self, other): return self.qs != other.qs
|
||||
|
||||
def __neg__(self):
|
||||
""" Invert Quaternions """
|
||||
return Quaternions(self.qs * np.array([[1, -1, -1, -1]]))
|
||||
|
||||
def __abs__(self):
|
||||
""" Unify Quaternions To Single Pole """
|
||||
qabs = self.normalized().copy()
|
||||
top = np.sum((qabs.qs) * np.array([1, 0, 0, 0]), axis=-1)
|
||||
bot = np.sum((-qabs.qs) * np.array([1, 0, 0, 0]), axis=-1)
|
||||
qabs.qs[top < bot] = -qabs.qs[top < bot]
|
||||
return qabs
|
||||
|
||||
def __iter__(self): return iter(self.qs)
|
||||
def __len__(self): return len(self.qs)
|
||||
|
||||
def __getitem__(self, k): return Quaternions(self.qs[k])
|
||||
def __setitem__(self, k, v): self.qs[k] = v.qs
|
||||
|
||||
@property
|
||||
def lengths(self):
|
||||
return np.sum(self.qs**2.0, axis=-1)**0.5
|
||||
|
||||
@property
|
||||
def reals(self):
|
||||
return self.qs[..., 0]
|
||||
|
||||
@property
|
||||
def imaginaries(self):
|
||||
return self.qs[..., 1:4]
|
||||
|
||||
@property
|
||||
def shape(self): return self.qs.shape[:-1]
|
||||
|
||||
def repeat(self, n, **kwargs):
|
||||
return Quaternions(self.qs.repeat(n, **kwargs))
|
||||
|
||||
def normalized(self):
|
||||
return Quaternions(self.qs / self.lengths[..., np.newaxis])
|
||||
|
||||
def log(self):
|
||||
norm = abs(self.normalized())
|
||||
imgs = norm.imaginaries
|
||||
lens = np.sqrt(np.sum(imgs**2, axis=-1))
|
||||
lens = np.arctan2(lens, norm.reals) / (lens + 1e-10)
|
||||
return imgs * lens[..., np.newaxis]
|
||||
|
||||
def constrained(self, axis):
|
||||
|
||||
rl = self.reals
|
||||
im = np.sum(axis * self.imaginaries, axis=-1)
|
||||
|
||||
t1 = -2 * np.arctan2(rl, im) + np.pi
|
||||
t2 = -2 * np.arctan2(rl, im) - np.pi
|
||||
|
||||
top = Quaternions.exp(axis[np.newaxis] * (t1[:, np.newaxis] / 2.0))
|
||||
bot = Quaternions.exp(axis[np.newaxis] * (t2[:, np.newaxis] / 2.0))
|
||||
img = self.dot(top) > self.dot(bot)
|
||||
|
||||
ret = top.copy()
|
||||
ret[img] = top[img]
|
||||
ret[~img] = bot[~img]
|
||||
return ret
|
||||
|
||||
def constrained_x(self): return self.constrained(np.array([1, 0, 0]))
|
||||
def constrained_y(self): return self.constrained(np.array([0, 1, 0]))
|
||||
def constrained_z(self): return self.constrained(np.array([0, 0, 1]))
|
||||
|
||||
def dot(self, q): return np.sum(self.qs * q.qs, axis=-1)
|
||||
|
||||
def copy(self): return Quaternions(np.copy(self.qs))
|
||||
|
||||
def reshape(self, s):
|
||||
self.qs.reshape(s)
|
||||
return self
|
||||
|
||||
def interpolate(self, ws):
|
||||
return Quaternions.exp(np.average(abs(self).log, axis=0, weights=ws))
|
||||
|
||||
def euler(self, order='xyz'):
|
||||
|
||||
q = self.normalized().qs
|
||||
q0 = q[..., 0]
|
||||
q1 = q[..., 1]
|
||||
q2 = q[..., 2]
|
||||
q3 = q[..., 3]
|
||||
es = np.zeros(self.shape + (3,))
|
||||
|
||||
if order == 'xyz':
|
||||
es[..., 0] = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
|
||||
es[..., 1] = np.arcsin((2 * (q0 * q2 - q3 * q1)).clip(-1, 1))
|
||||
es[..., 2] = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
|
||||
elif order == 'yzx':
|
||||
es[..., 0] = np.arctan2(2 * (q1 * q0 - q2 * q3), -q1 * q1 + q2 * q2 - q3 * q3 + q0 * q0)
|
||||
es[..., 1] = np.arctan2(2 * (q2 * q0 - q1 * q3), q1 * q1 - q2 * q2 - q3 * q3 + q0 * q0)
|
||||
es[..., 2] = np.arcsin((2 * (q1 * q2 + q3 * q0)).clip(-1, 1))
|
||||
else:
|
||||
raise NotImplementedError('Cannot convert from ordering %s' % order)
|
||||
|
||||
"""
|
||||
|
||||
# These conversion don't appear to work correctly for Maya.
|
||||
# http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
|
||||
|
||||
if order == 'xyz':
|
||||
es[fa + (0,)] = np.arctan2(2 * (q0 * q3 - q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
|
||||
es[fa + (1,)] = np.arcsin((2 * (q1 * q3 + q0 * q2)).clip(-1,1))
|
||||
es[fa + (2,)] = np.arctan2(2 * (q0 * q1 - q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
|
||||
elif order == 'yzx':
|
||||
es[fa + (0,)] = np.arctan2(2 * (q0 * q1 - q2 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
|
||||
es[fa + (1,)] = np.arcsin((2 * (q1 * q2 + q0 * q3)).clip(-1,1))
|
||||
es[fa + (2,)] = np.arctan2(2 * (q0 * q2 - q1 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
|
||||
elif order == 'zxy':
|
||||
es[fa + (0,)] = np.arctan2(2 * (q0 * q2 - q1 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
|
||||
es[fa + (1,)] = np.arcsin((2 * (q0 * q1 + q2 * q3)).clip(-1,1))
|
||||
es[fa + (2,)] = np.arctan2(2 * (q0 * q3 - q1 * q2), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
|
||||
elif order == 'xzy':
|
||||
es[fa + (0,)] = np.arctan2(2 * (q0 * q2 + q1 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
|
||||
es[fa + (1,)] = np.arcsin((2 * (q0 * q3 - q1 * q2)).clip(-1,1))
|
||||
es[fa + (2,)] = np.arctan2(2 * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
|
||||
elif order == 'yxz':
|
||||
es[fa + (0,)] = np.arctan2(2 * (q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)
|
||||
es[fa + (1,)] = np.arcsin((2 * (q0 * q1 - q2 * q3)).clip(-1,1))
|
||||
es[fa + (2,)] = np.arctan2(2 * (q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
|
||||
elif order == 'zyx':
|
||||
es[fa + (0,)] = np.arctan2(2 * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)
|
||||
es[fa + (1,)] = np.arcsin((2 * (q0 * q2 - q1 * q3)).clip(-1,1))
|
||||
es[fa + (2,)] = np.arctan2(2 * (q0 * q3 + q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)
|
||||
else:
|
||||
raise KeyError('Unknown ordering %s' % order)
|
||||
|
||||
"""
|
||||
|
||||
# https://github.com/ehsan/ogre/blob/master/OgreMain/src/OgreMatrix3.cpp
|
||||
# Use this class and convert from matrix
|
||||
|
||||
return es
|
||||
|
||||
def average(self):
|
||||
|
||||
if len(self.shape) == 1:
|
||||
|
||||
import numpy.core.umath_tests as ut
|
||||
system = ut.matrix_multiply(self.qs[:, :, np.newaxis], self.qs[:, np.newaxis, :]).sum(axis=0)
|
||||
w, v = np.linalg.eigh(system)
|
||||
qiT_dot_qref = (self.qs[:, :, np.newaxis] * v[np.newaxis, :, :]).sum(axis=1)
|
||||
return Quaternions(v[:, np.argmin((1.-qiT_dot_qref**2).sum(axis=0))])
|
||||
|
||||
else:
|
||||
|
||||
raise NotImplementedError('Cannot average multi-dimensionsal Quaternions')
|
||||
|
||||
def angle_axis(self):
|
||||
|
||||
norm = self.normalized()
|
||||
s = np.sqrt(1 - (norm.reals**2.0))
|
||||
s[s == 0] = 0.001
|
||||
|
||||
angles = 2.0 * np.arccos(norm.reals)
|
||||
axis = norm.imaginaries / s[..., np.newaxis]
|
||||
|
||||
return angles, axis
|
||||
|
||||
def transforms(self):
|
||||
|
||||
qw = self.qs[..., 0]
|
||||
qx = self.qs[..., 1]
|
||||
qy = self.qs[..., 2]
|
||||
qz = self.qs[..., 3]
|
||||
|
||||
x2 = qx + qx
|
||||
y2 = qy + qy
|
||||
z2 = qz + qz
|
||||
xx = qx * x2
|
||||
yy = qy * y2
|
||||
wx = qw * x2
|
||||
xy = qx * y2
|
||||
yz = qy * z2
|
||||
wy = qw * y2
|
||||
xz = qx * z2
|
||||
zz = qz * z2
|
||||
wz = qw * z2
|
||||
|
||||
m = np.empty(self.shape + (3, 3))
|
||||
m[..., 0, 0] = 1.0 - (yy + zz)
|
||||
m[..., 0, 1] = xy - wz
|
||||
m[..., 0, 2] = xz + wy
|
||||
m[..., 1, 0] = xy + wz
|
||||
m[..., 1, 1] = 1.0 - (xx + zz)
|
||||
m[..., 1, 2] = yz - wx
|
||||
m[..., 2, 0] = xz - wy
|
||||
m[..., 2, 1] = yz + wx
|
||||
m[..., 2, 2] = 1.0 - (xx + yy)
|
||||
|
||||
return m
|
||||
|
||||
def ravel(self):
|
||||
return self.qs.ravel()
|
||||
|
||||
@classmethod
|
||||
def id(cls, n):
|
||||
|
||||
if isinstance(n, tuple):
|
||||
qs = np.zeros(n + (4,))
|
||||
qs[..., 0] = 1.0
|
||||
return Quaternions(qs)
|
||||
|
||||
if isinstance(n, int):
|
||||
qs = np.zeros((n, 4))
|
||||
qs[:, 0] = 1.0
|
||||
return Quaternions(qs)
|
||||
|
||||
raise TypeError('Cannot Construct Quaternion from %s type' % str(type(n)))
|
||||
|
||||
@classmethod
|
||||
def id_like(cls, a):
|
||||
qs = np.zeros(a.shape + (4,))
|
||||
qs[..., 0] = 1.0
|
||||
return Quaternions(qs)
|
||||
|
||||
@classmethod
|
||||
def exp(cls, ws):
|
||||
|
||||
ts = np.sum(ws**2.0, axis=-1)**0.5
|
||||
ts[ts == 0] = 0.001
|
||||
ls = np.sin(ts) / ts
|
||||
|
||||
qs = np.empty(ws.shape[:-1] + (4,))
|
||||
qs[..., 0] = np.cos(ts)
|
||||
qs[..., 1] = ws[..., 0] * ls
|
||||
qs[..., 2] = ws[..., 1] * ls
|
||||
qs[..., 3] = ws[..., 2] * ls
|
||||
|
||||
return Quaternions(qs).normalized()
|
||||
|
||||
@classmethod
|
||||
def slerp(cls, q0s, q1s, a):
|
||||
|
||||
fst, snd = cls._broadcast(q0s.qs, q1s.qs)
|
||||
fst, a = cls._broadcast(fst, a, scalar=True)
|
||||
snd, a = cls._broadcast(snd, a, scalar=True)
|
||||
|
||||
len = np.sum(fst * snd, axis=-1)
|
||||
|
||||
neg = len < 0.0
|
||||
len[neg] = -len[neg]
|
||||
snd[neg] = -snd[neg]
|
||||
|
||||
amount0 = np.zeros(a.shape)
|
||||
amount1 = np.zeros(a.shape)
|
||||
|
||||
linear = (1.0 - len) < 0.01
|
||||
omegas = np.arccos(len[~linear])
|
||||
sinoms = np.sin(omegas)
|
||||
|
||||
amount0[linear] = 1.0 - a[linear]
|
||||
amount1[linear] = a[linear]
|
||||
amount0[~linear] = np.sin((1.0 - a[~linear]) * omegas) / sinoms
|
||||
amount1[~linear] = np.sin(a[~linear] * omegas) / sinoms
|
||||
|
||||
return Quaternions(
|
||||
amount0[..., np.newaxis] * fst +
|
||||
amount1[..., np.newaxis] * snd)
|
||||
|
||||
@classmethod
|
||||
def between(cls, v0s, v1s):
|
||||
a = np.cross(v0s, v1s)
|
||||
w = np.sqrt((v0s**2).sum(axis=-1) * (v1s**2).sum(axis=-1)) + (v0s * v1s).sum(axis=-1)
|
||||
return Quaternions(np.concatenate([w[..., np.newaxis], a], axis=-1)).normalized()
|
||||
|
||||
@classmethod
|
||||
def from_angle_axis(cls, angles, axis):
|
||||
axis = axis / (np.sqrt(np.sum(axis**2, axis=-1)) + 1e-10)[..., np.newaxis]
|
||||
sines = np.sin(angles / 2.0)[..., np.newaxis]
|
||||
cosines = np.cos(angles / 2.0)[..., np.newaxis]
|
||||
return Quaternions(np.concatenate([cosines, axis * sines], axis=-1))
|
||||
|
||||
@classmethod
|
||||
def from_euler(cls, es, order='xyz', world=False):
|
||||
|
||||
axis = {
|
||||
'x': np.array([1, 0, 0]),
|
||||
'y': np.array([0, 1, 0]),
|
||||
'z': np.array([0, 0, 1]),
|
||||
}
|
||||
|
||||
q0s = Quaternions.from_angle_axis(es[..., 0], axis[order[0]])
|
||||
q1s = Quaternions.from_angle_axis(es[..., 1], axis[order[1]])
|
||||
q2s = Quaternions.from_angle_axis(es[..., 2], axis[order[2]])
|
||||
|
||||
return (q2s * (q1s * q0s)) if world else (q0s * (q1s * q2s))
|
||||
|
||||
@classmethod
|
||||
def from_transforms(cls, ts):
|
||||
|
||||
d0, d1, d2 = ts[..., 0, 0], ts[..., 1, 1], ts[..., 2, 2]
|
||||
|
||||
q0 = (d0 + d1 + d2 + 1.0) / 4.0
|
||||
q1 = (d0 - d1 - d2 + 1.0) / 4.0
|
||||
q2 = (-d0 + d1 - d2 + 1.0) / 4.0
|
||||
q3 = (-d0 - d1 + d2 + 1.0) / 4.0
|
||||
|
||||
q0 = np.sqrt(q0.clip(0, None))
|
||||
q1 = np.sqrt(q1.clip(0, None))
|
||||
q2 = np.sqrt(q2.clip(0, None))
|
||||
q3 = np.sqrt(q3.clip(0, None))
|
||||
|
||||
c0 = (q0 >= q1) & (q0 >= q2) & (q0 >= q3)
|
||||
c1 = (q1 >= q0) & (q1 >= q2) & (q1 >= q3)
|
||||
c2 = (q2 >= q0) & (q2 >= q1) & (q2 >= q3)
|
||||
c3 = (q3 >= q0) & (q3 >= q1) & (q3 >= q2)
|
||||
|
||||
q1[c0] *= np.sign(ts[c0, 2, 1] - ts[c0, 1, 2])
|
||||
q2[c0] *= np.sign(ts[c0, 0, 2] - ts[c0, 2, 0])
|
||||
q3[c0] *= np.sign(ts[c0, 1, 0] - ts[c0, 0, 1])
|
||||
|
||||
q0[c1] *= np.sign(ts[c1, 2, 1] - ts[c1, 1, 2])
|
||||
q2[c1] *= np.sign(ts[c1, 1, 0] + ts[c1, 0, 1])
|
||||
q3[c1] *= np.sign(ts[c1, 0, 2] + ts[c1, 2, 0])
|
||||
|
||||
q0[c2] *= np.sign(ts[c2, 0, 2] - ts[c2, 2, 0])
|
||||
q1[c2] *= np.sign(ts[c2, 1, 0] + ts[c2, 0, 1])
|
||||
q3[c2] *= np.sign(ts[c2, 2, 1] + ts[c2, 1, 2])
|
||||
|
||||
q0[c3] *= np.sign(ts[c3, 1, 0] - ts[c3, 0, 1])
|
||||
q1[c3] *= np.sign(ts[c3, 2, 0] + ts[c3, 0, 2])
|
||||
q2[c3] *= np.sign(ts[c3, 2, 1] + ts[c3, 1, 2])
|
||||
|
||||
qs = np.empty(ts.shape[:-2] + (4,))
|
||||
qs[..., 0] = q0
|
||||
qs[..., 1] = q1
|
||||
qs[..., 2] = q2
|
||||
qs[..., 3] = q3
|
||||
|
||||
return cls(qs)
|
||||
@@ -0,0 +1,4 @@
|
||||
import sys
|
||||
import os
|
||||
BASEPATH = os.path.dirname(__file__)
|
||||
sys.path.insert(0, BASEPATH)
|
||||
Reference in New Issue
Block a user