def updateValues(self, rotnextframe=False):
"""
Update the rotation values of the joints in the path from root to end effector.
Construct a matrix with theta values and rotate (old) local matrix with it;
Extract euler angles from this new rotation (local) matrix;
Repeat for the other joints down the hierarchy.
"""
i = 0
for joint in self.end_effector.pathFromDepthToJoint(self.depth):
drotation = mathutils.matrixR([
self.dtheta[i * 3], self.dtheta[i * 3 + 1],
self.dtheta[i * 3 + 2]
], joint.order)
local = mathutils.shape4ToShape3(
joint.getLocalTransform(self.currentframe))
new_local = np.dot(local, drotation)
new_theta, warning = mathutils.eulerFromMatrix(
new_local, joint.order)
if warning:
print(
'Warning raised from mathutils.eulerFromMatrix() at jacobian.updateValues()'
)
joint.rotation[self.currentframe] = np.asarray(new_theta)
i = i + 1
if rotnextframe:
try:
#May be the last frame
next_frame_local = mathutils.shape4ToShape3(
joint.getLocalTransform(self.currentframe + 1))
new_next_frame_local = np.dot(next_frame_local, drotation)
new_next_frame_theta, warning = mathutils.eulerFromMatrix(
new_next_frame_local, joint.order)
if warning:
print(
'Warning raised from mathutils.eulerFromMatrix() at jacobian.updateValues()'
)
joint.rotation[self.currentframe +
1] = np.asarray(new_next_frame_theta)
except:
pass
def PostureInitialization(ani_ava, ani_mocap, getpositions=False, headAlign = True, spineAlign = True, handAlign = True):
"""
Copy the rotation from the mocap joints to the corresponding avatar joint.
ani_ava: Avatar animation
ani_mocap: Mocap animation
"""
#Get mapping
mocapmap = ani_mocap.getskeletonmap()
avamap = ani_ava.getskeletonmap()
#
#Save the reference TPose
for joint in ani_ava.getlistofjoints():
joint.tposerot = joint.rotation[0]
joint.tposetrans = joint.translation[0]
start=time.time()
#Expand the number of frames of the avatar animation to match the mocap animation
ani_ava.expandFrames(mocapmap.root.translation.shape[0])
#Adapt pose each frame
print('Starting Posture Initialization')
for frame in range(ani_mocap.frames):
if np.mod(frame+1,100) == 0:
print('%i frames done. %s seconds.' % (int((frame+1)/100)*100,time.time()-start))
start=time.time()
#Get the Height of the root in the base position (Frame = 0)
#If the source animation (mocap) is not in the TPose, it will fail
if frame == 0:
ground_normal = np.array([0,1,0])
srcHHips = np.dot(mocapmap.hips.getPosition(0), ground_normal)
tgtHHips = np.dot(avamap.hips.getPosition(0), ground_normal)
ratio = tgtHHips/srcHHips
#Adjust roots/hips height
#Eray Molla Eq 1
srcPosHips = mocapmap.hips.getPosition(frame)
srcGroundHips = np.asarray([srcPosHips[0], 0, srcPosHips[2]])
tgtGroundHips = srcGroundHips*ratio
srcHHips = np.dot(mocapmap.hips.getPosition(frame), ground_normal)
tgtHHips = srcHHips*ratio
avamap.root.translation[frame] = [0,tgtHHips,0] + tgtGroundHips
if frame == 0:
mocapbones = []
avabones = []
if spineAlign:
mocapbones.append([mocapmap.hips, mocapmap.spine3])
avabones.append([avamap.hips, avamap.spine3])
if headAlign:
mocapbones.append([mocapmap.neck, mocapmap.head])
avabones.append([avamap.neck, avamap.head])
mocapbones = mocapbones + [[mocapmap.rarm, mocapmap.rforearm],[mocapmap.larm, mocapmap.lforearm],[mocapmap.rforearm, mocapmap.rhand],[mocapmap.lforearm, mocapmap.lhand],[mocapmap.rupleg, mocapmap.rlowleg],[mocapmap.lupleg, mocapmap.llowleg],[mocapmap.rlowleg, mocapmap.rfoot],[mocapmap.llowleg, mocapmap.lfoot]]
avabones = avabones + [[avamap.rarm, avamap.rforearm],[avamap.larm, avamap.lforearm],[avamap.rforearm, avamap.rhand],[avamap.lforearm, avamap.lhand],[avamap.rupleg, avamap.rlowleg],[avamap.lupleg, avamap.llowleg],[avamap.rlowleg, avamap.rfoot],[avamap.llowleg, avamap.lfoot]]
if handAlign and mocapmap.lhandmiddle and mocapmap.rhandmiddle and avamap.lhandmiddle and avamap.rhandmiddle:
mocapbones = mocapbones + [[mocapmap.rhand, mocapmap.rhandmiddle],[mocapmap.lhand, mocapmap.lhandmiddle]]
avabones = avabones + [[avamap.rhand, avamap.rhandmiddle],[avamap.lhand, avamap.lhandmiddle]]
for mocapbone, avabone in zip(mocapbones,avabones):
#Get source and target global transform and rotation matrices from the start of the bone
p0 = mocapbone[0].getPosition(0)
p1 = mocapbone[1].getPosition(0)
srcDirection = mathutils.unitVector(p1-p0)
#Get source and target global transform and rotation matrices from the end of the bone
p0 = avabone[0].getPosition(0)
p1 = avabone[1].getPosition(0)
tgtDirection = mathutils.unitVector(p1-p0)
#Align vectors
alignMat = mathutils.alignVectors(tgtDirection, srcDirection)
#Get new global rotation matrix
tgtGlbTransformMat = avabone[0].getGlobalTransform(frame)
tgtGlbRotationMat = mathutils.shape4ToShape3(tgtGlbTransformMat)
tgtNewGblRotationMat = np.dot(alignMat,tgtGlbRotationMat)
#Get new local rotation matrix
if not avabone[0] == avamap.root: #Does not have a parent, transform is already local
tgtParentGblRotationMat = mathutils.shape4ToShape3(avabone[0].parent.getGlobalTransform(frame))
tgtNewLclRotationMat = np.dot(tgtParentGblRotationMat.T, tgtNewGblRotationMat)
else:
tgtNewLclRotationMat = tgtNewGblRotationMat[:]
#Get new local rotation euler angles
tgtNewLclRotationEuler, warning = mathutils.eulerFromMatrix(tgtNewLclRotationMat, avabone[0].order)
avabone[0].rotation[frame] = tgtNewLclRotationEuler
else:
for joint_ava, joint_mocap in zip(avamap.getJointsNoRootHips(), mocapmap.getJointsNoRootHips()):
if joint_ava is not None and joint_mocap is not None:
previousframe = frame-1 if frame!= 0 else 0
#Get source and target global transform and rotation matrices
#Even if frame == 0 the matrices need to be recalculated
srcGlbTransformMat = joint_mocap.getGlobalTransform(frame)
srcGlbRotationMat = mathutils.shape4ToShape3(srcGlbTransformMat)
tgtGlbTransformMat = joint_ava.getGlobalTransform(previousframe)
tgtGlbRotationMat = mathutils.shape4ToShape3(tgtGlbTransformMat)
#Get previous source global transform and rotation matrices
srcPreviousGlbTransformMat = joint_mocap.getGlobalTransform(previousframe)
srcPreviousGlbRotationMat = mathutils.shape4ToShape3(srcPreviousGlbTransformMat)
#Get the transform of the source from the previous frame to the present frame
transform = np.dot(srcGlbRotationMat, srcPreviousGlbRotationMat.T)
#Apply transform
tgtNewGblRotationMat = np.dot(transform, tgtGlbRotationMat)
#Get new local rotation matrix
tgtParentGblRotationMat = mathutils.shape4ToShape3(joint_ava.parent.getGlobalTransform(frame))
tgtNewLclRotationMat = np.dot(tgtParentGblRotationMat.T, tgtNewGblRotationMat)
#Get new local rotation euler angles
tgtNewLclRotationEuler, warning = mathutils.eulerFromMatrix(tgtNewLclRotationMat, joint_ava.order)
joint_ava.rotation[frame] = tgtNewLclRotationEuler[:]
ani_ava.frames = ani_mocap.frames
ani_ava.frametime = ani_mocap.frametime
def getGlobalRotation(self, frame=0, includeBaseRotation=False):
return mathutils.eulerFromMatrix(
self.getGlobalTransform(frame, includeBaseRotation), self.order)