def put_in_world_h36m(states):
joints = states[:, :-4]
root_x = states[:, -4]
root_y = states[:, -3]
root_z = states[:, -2]
root_r = states[:, -1]
joints = joints.reshape(joints.shape[:1] + (-1, 3))
joints[:, :, 0] = joints[:, :, 0] - joints[0:1, 0:1, 0]
joints[:, :, 2] = joints[:, :, 2] - joints[0:1, 0:1, 2]
rotation = Quaternions.id(1)
rotations = []
offsets = []
translation = np.array([[0, 0, 0]])
for i in range(len(joints)):
rotation = Quaternions.from_angle_axis(-root_r[i], np.array(
[0, 1, 0])) * rotation
rotations.append(rotation.qs[:, None, :])
joints[i, :, :] = rotation * joints[i]
joints[i, :, 0] = joints[i, :, 0] + translation[0, 0]
joints[i, :, 1] = joints[i, :, 1] + translation[0, 1]
joints[i, :, 2] = joints[i, :, 2] + translation[0, 2]
offsets.append(rotation * np.array([0, 0, 1]))
translation = translation + rotation * np.array(
[root_x[i], root_y[i], root_z[i]])
return joints[None], Quaternions(np.concatenate(rotations, axis=0))
def from_numpy(self, motions, frametime=None, quater=False):
if frametime is not None:
self.frametime = frametime
motions = motions.copy()
positions = motions[:, -3:]
self.anim.positions = positions[:, np.newaxis, :]
if quater:
rotations = motions[:, :-3].reshape(motions.shape[0], -1, 4)
norm = rotations[:, :,
0]**2 + rotations[:, :,
1]**2 + rotations[:, :,
2]**2 + rotations[:, :,
3]**2
norm = np.repeat(norm[:, :, np.newaxis], 4, axis=2)
rotations /= norm
rotations = Quaternions(rotations)
rotations = np.degrees(rotations.euler())
else:
rotations = motions[:, -3:].reshape(motions.shape[0], -1, 3)
rotations_full = np.zeros((rotations.shape[0], self.anim.shape[1], 3))
for i in range(self.anim.shape[1]):
if i in self.corps:
pt = self.corps.index(i)
rotations_full[:, i, :] = rotations[:, pt, :]
self.anim.rotations = rotations_full
def compute_joints_from_dofs(dofs, cam):
global j3d, root_rot
frame_num = 1
axis_angle = np.zeros((frame_num, 17, 3))
# 28 - (tx, ty, tz, rx, ry, rz) = 22
axis_angle[0, 0] = root_rot
axis_angle[0, 1] = dofs[0:3]
axis_angle[0, 2, 0] = dofs[3]
axis_angle[0, 4] = dofs[4:7]
axis_angle[0, 5, 0] = dofs[7]
axis_angle[0, 7] = dofs[8:11]
axis_angle[0, 9] = dofs[11:14]
axis_angle[0, 11] = dofs[14:17]
axis_angle[0, 12, 1] = dofs[17]
axis_angle[0, 14] = dofs[18:21]
axis_angle[0, 15, 1] = dofs[21]
quaternions = Quaternions.from_angle_axis(
np.sqrt(np.sum(axis_angle**2, axis=-1)), axis_angle)
# y-up, z-forward, x-right
offsets = input_tpose_j3d - input_tpose_j3d[j17_parents]
offsets[0] = j3d[0] * 100 # m -> cm
positions = offsets[np.newaxis].repeat(frame_num,
axis=0) # (frames, jointsNum, 3)
orients = Quaternions.id(0)
for i in range(offsets.shape[0]):
orients.qs = np.append(orients.qs, np.array([[1, 0, 0, 0]]), axis=0)
anim = Animation.Animation(quaternions, positions, orients, offsets,
j17_parents)
j3d_pre = Animation.positions_global(anim) / 100 # cm -> m
j2d_pre = np.dot(
np.divide(j3d_pre, j3d_pre[:, :, 2:], where=j3d_pre[:, :, 2:] != 0),
cam.T)[:, :, :2]
return j3d_pre[0], j2d_pre[0]
def posture_chain(postures_list):
nb_frames = len(postures_list)
positions = np.array([[[0.0] * 3] * 23] * nb_frames)
orients = Quaternions(np.array([1.0, 0.0, 0.0, 0.0] * 23))
offsets = np.array([[0.00000, 0.00000, 0.00000],
[0.00000, 10.3548, 0.00650000],
[0.00000, 10.04358, -0.0132000],
[0.00000, 10.03937, 0.00000],
[0.00000, 10.03831, 0.00000],
[0.00000, 10.56940, 0.00000],
[0.00000, 9.71100, -0.0280000],
[-3.15720, 10.03675, 0.00000],
[-10.44825, 0.00000, 0.00000],
[0.00000, -30.17078, 0.00000],
[0.00000, -20.56593, 0.00000],
[3.15720, 10.03675, 0.00000],
[10.44825, 0.00000, 0.00000],
[0.00000, -30.17078, 0.00000],
[0.00000, -20.56593, 0.00000],
[-8.36560, -0.0321000, -0.00320000],
[0.00000, -40.39800, 0.00310000],
[0.00000, -40.28828, 0.00520000],
[0.00000, -4.91550, 10.08263],
[8.36560, -0.0321000, -0.00320000],
[0.00000, -40.39800, 0.00310000],
[0.00000, -40.28828, 0.00520000],
[0.00000, -4.91550, 10.08263]])
parents = np.array([
-1, 0, 1, 2, 3, 4, 5, 4, 7, 8, 9, 4, 11, 12, 13, 0, 15, 16, 17, 0, 19,
20, 21
])
generated_anim = Animation.Animation(Quaternions(postures_list), positions,
orients, offsets, parents)
return generated_anim
def write(self,
rotations,
positions,
order,
path,
frametime=1.0 / 30,
offset=None,
root_y=None):
if order == 'quaternion':
norm = rotations[:, :,
0]**2 + rotations[:, :,
1]**2 + rotations[:, :,
2]**2 + rotations[:, :,
3]**2
norm = np.repeat(norm[:, :, np.newaxis], 4, axis=2)
rotations /= norm
rotations = Quaternions(rotations)
rotations = np.degrees(rotations.euler())
order = 'xyz'
rotations_full = np.zeros((rotations.shape[0], self.joint_num, 3))
for idx, edge in enumerate(self.edge2joint):
if edge != -1:
rotations_full[:, idx, :] = rotations[:, edge, :]
if root_y is not None: rotations_full[0, 0, 1] = root_y
if offset is None: offset = self.offset
return write_bvh(self.parent, offset, rotations_full, positions,
self.names, frametime, order, path)
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] # look at the translation vector inside a rotation matrix
anim_rotations = Quaternions.from_transforms(anim_transforms)
# self.animation.rotations[1,0]*(self.animation.offsets[1] + self.animation.rotations[1,1]*self.animation.offsets[2]) ==
# anim_positions[1, 2]
jdirs = anim_positions[:,c] - anim_positions[:,np.newaxis,j] # limb vectors given by animation
ddirs = self.positions[:,c] - self.positions[:,np.newaxis,j] # limb vectors given by input positions (target)
if len(c) > 1 and (jdirs==0).all() and (ddirs==0).all(): # there was an expansion of high degree vertices (expand_topology())
# self.animation.rotations[:, j] remains untouched
continue
jsums = np.sqrt(np.sum(jdirs**2.0, axis=-1)) + 1e-20
dsums = np.sqrt(np.sum(ddirs**2.0, axis=-1)) + 1e-20
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)
if jdirs.shape[1] == 1: # for a single child reconstruction should be exact
assert np.allclose(Quaternions.from_angle_axis(angles, np.cross(jdirs, ddirs)) * jdirs, ddirs)
axises = -anim_rotations[:,j,np.newaxis] * axises
# find out which of the given bones are not of zero length
# zero length bones produces a meaningless rotation
jdirs_positive = (jsums > 1e-4)
ddirs_positive = (dsums > 1e-4)
dirs_positive = jdirs_positive[0]
rotations = Quaternions.from_angle_axis(angles, axises)
if rotations.shape[1] == 1:
averages = rotations[:,0]
else:
averages = Quaternions.exp(rotations[:,dirs_positive].log().mean(axis=-2))
self.animation.rotations[:,j] = self.animation.rotations[:,j] * averages
if not self.silent:
anim_positions = Animation.positions_global(self.animation)
error = np.mean(np.sum((anim_positions - self.positions)**2.0, axis=-1)**0.5) # axis=-1)
print('[BasicInverseKinematics] Iteration %i Error: %f' % (i+1, error))
return self.animation
def unravel(cls, 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())
def nrot_for_forwardBVH(rot, skel=skel):
"""
input: new! rotations [J * 4 + 3 + 1, T]
output: global positions [T, J, 3] & rotation -> facing z+
"""
rot = np.swapaxes(rot, -1, -2)
rotations, rt_positions, rt_rotations = rot[..., :-4], rot[...,
-4:-1], rot[...,
-1:]
rotations = rotations.reshape(rotations.shape[:-1] +
(-1, 4)) # [..., J, 4]
norm = np.sqrt(np.sum(rotations**2, axis=-1, keepdims=True))
rotations = rotations / norm
"""
# based on distance
# 0: z+, 1: x+, 2: z-, 3: x-,
trail = np.zeros(4)
trail[1] = rt_positions[-1, 0] - rt_positions[0, 0]
trail[3] = -trail[1]
trail[0] = rt_positions[-1, 2] - rt_positions[0, 2]
trail[2] = -trail[0]
dir = np.argmax(trail)
"""
# based on forward direction in rt_rotations
# 0: z+, 1: x+, 2: z-, 3: x-,
trail = np.zeros(4)
for dir in range(4):
tmp = rt_rotations - dir * np.pi * 0.5 * np.ones(rt_rotations.shape)
tmp = np.abs(tmp)
tmp = np.minimum(tmp, 2.0 * np.pi * np.ones(rt_rotations.shape) - tmp)
trail[dir] = np.sum(tmp)
dir = np.argmin(trail)
rt_rotations = rt_rotations - dir * np.pi * 0.5 * np.ones(
rt_rotations.shape)
# np.set_printoptions(precision=3, suppress=True)
# print(rt_positions)
for d in range(dir):
tmp = rt_positions[..., 0].copy()
rt_positions[..., 0] = -rt_positions[..., 2].copy()
rt_positions[..., 2] = tmp
# print(rt_positions)
rt_rotations = Quaternions(np.array(Pivots(rt_rotations).quaternions()))
ori_rt_rotations = Quaternions(rotations)[:, 0:1]
ori_rt_rotations = rt_rotations * ori_rt_rotations
rotations[:, 0:1, :] = np.array(ori_rt_rotations) # [T, J, 4]
return _rot_to_pos(rotations, rtpos=rt_positions, trim=False), rotations
def import_fbx_as_anim(filename, pelvis_name=None):
joint_names, parents, offsets, qs, ts, qs_global, ts_global = import_fbx(
filename, pelvis_name)
orients = Quaternions.id(len(joint_names))
# anim = Animation(rotations=Quaternions.id((qs.shape[0], qs.shape[1])),
# positions=ts, orients=orients, offsets=offsets, parents=parents)
anim = Animation(rotations=Quaternions(qs),
positions=ts,
orients=orients,
offsets=offsets,
parents=parents)
return joint_names, parents, offsets, anim, ts_global
def recover_global_positions(processed, initRot, initTrans):
"""
Recovers the original global positions given the Holden form
:param processed: Holden data format gestures (F, 73)
:param initRot: intial rotation of the skeleton
:param initTrans: initial translation of the skeleton
:return:
"""
recovered_joints = []
for k in range(processed.shape[0]):
# split into joints and root velocities
joints = processed[k, :, :-7]
root_x, root_z, root_r = processed[k, :, -7], processed[
k, :, -6], processed[k, :, -5]
# reshape into the right format
joints = joints.reshape(len(joints), -1, 3)
# set initial rotation and translation
if initRot is None:
rotation = Quaternions.id(1)
else:
rotation = initRot[k]
if initTrans is None:
translation = np.array([[0, 0, 0]])
else:
translation = initTrans[k]
# maintain a list of the offsets
offsets = []
# integrate over time to recover original values
for i in range(len(joints)):
joints[i, :, :] = rotation * joints[i]
joints[i, :, 0] = joints[i, :, 0] + translation[0, 0]
joints[i, :, 2] = joints[i, :, 2] + translation[0, 2]
rotation = Quaternions.from_angle_axis(
-root_r[i], np.array([0, 1, 0])) * rotation
offsets.append(rotation * np.array([0, 0, 1]))
translation = translation + rotation * np.array(
[root_x[i], 0, root_z[i]])
joints = HoldenDataFormat.floor_skelton(joints[:, 1:])
# joints = joints[:, 1:]
recovered_joints.append(
filters.gaussian_filter1d(joints, 1, axis=0, mode='nearest'))
return np.array(recovered_joints)
def rotate(self, theta, axis):
q = Quaternions(np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)))
position = self.anim.positions[:, 0, :].copy()
rotation = self.anim.rotations[:, 0, :]
position[1:, ...] -= position[0:-1, ...]
q_position = Quaternions(np.hstack((np.zeros((position.shape[0], 1)), position)))
q_rotation = Quaternions.from_euler(np.radians(rotation))
q_rotation = q * q_rotation
q_position = q * q_position * (-q)
self.anim.rotations[:, 0, :] = np.degrees(q_rotation.euler())
position = q_position.imaginaries
for i in range(1, position.shape[0]):
position[i] += position[i-1]
self.anim.positions[:, 0, :] = position
def __call__(self):
children = AnimationStructure.children_list(self.animation.parents)
for i in range(self.iterations):
for j in AnimationStructure.joints(self.animation.parents):
c = np.array(children[j])
if len(c) == 0: continue
anim_transforms = Animation.transforms_global(self.animation)
anim_positions = anim_transforms[:, :, :3, 3]
anim_rotations = Quaternions.from_transforms(anim_transforms)
jdirs = anim_positions[:, c] - anim_positions[:, np.newaxis, j]
ddirs = self.positions[:, c] - anim_positions[:, np.newaxis, j]
jsums = np.sqrt(np.sum(jdirs**2.0, axis=-1)) + 1e-10
dsums = np.sqrt(np.sum(ddirs**2.0, axis=-1)) + 1e-10
jdirs = jdirs / jsums[:, :, np.newaxis]
ddirs = ddirs / dsums[:, :, np.newaxis]
angles = np.arccos(np.sum(jdirs * ddirs, axis=2).clip(-1, 1))
axises = np.cross(jdirs, ddirs)
axises = -anim_rotations[:, j, np.newaxis] * axises
rotations = Quaternions.from_angle_axis(angles, axises)
if rotations.shape[1] == 1:
averages = rotations[:, 0]
else:
averages = Quaternions.exp(rotations.log().mean(axis=-2))
self.animation.rotations[:,
j] = self.animation.rotations[:,
j] * averages
if not self.silent:
anim_positions = Animation.positions_global(self.animation)
error = np.mean(np.sum((anim_positions - self.positions)**2.0,
axis=-1)**0.5,
axis=-1)
print('[BasicInverseKinematics] Iteration %i Error: %f' %
(i + 1, error))
return self.animation
def get_windows(self, motions):
new_windows = []
for motion in motions:
self.total_frame += motion.shape[0]
motion = self.subsample(motion)
self.motion_length.append(motion.shape[0])
step_size = self.args.window_size // 2
window_size = step_size * 2
n_window = motion.shape[0] // step_size - 1
for i in range(n_window):
begin = i * step_size
end = begin + window_size
new = motion[begin:end, :]
if self.args.rotation == 'quaternion':
new = new.reshape(new.shape[0], -1, 3)
rotations = new[:, :-1, :]
rotations = Quaternions.from_euler(
np.radians(rotations)).qs
rotations = rotations.reshape(rotations.shape[0], -1)
positions = new[:, -1, :]
positions = np.concatenate(
(new, np.zeros((new.shape[0], new.shape[1], 1))),
axis=2)
new = np.concatenate(
(rotations, new[:, -1, :].reshape(new.shape[0], -1)),
axis=1)
new = new[np.newaxis, ...]
new_window = torch.tensor(new, dtype=torch.float32)
new_windows.append(new_window)
return torch.cat(new_windows)
def _rot_to_pos(rotations, rtpos=None, skel=skel, trim=True):
"""
input: rotations [(B, ) T, J, 4], rtpos [(B, ) T, 3]
output: pos [(B, ) T, J, 3]
"""
norm = np.sqrt(np.sum(rotations**2, axis=-1, keepdims=True))
norm = 1.0 / norm
rotations *= norm
# rotations = rotations / norm
transforms = Quaternions(rotations).transforms() # [..., J, 3, 3]
glb = np.zeros(rotations.shape[:-1] + (3, )) # [..., J, 3]
if rtpos is not None:
glb[..., 0, :] = rtpos
for i, pi in enumerate(skel.topology):
if pi == -1:
assert i == 0
continue
glb[..., i, :] = np.matmul(transforms[..., pi, :, :], skel.offset[i])
glb[..., i, :] += glb[..., pi, :]
transforms[..., i, :, :] = np.matmul(transforms[..., pi, :, :],
transforms[..., i, :, :])
if trim:
return glb[..., needed_joints, :]
else:
return glb
def yrot_from_glb(positions):
"""
input: positions [(B, )T, J, 3]
output: y-axis rotations in quaternions & pivots: [(B, ) T, 1, 4], [(B, )T, 1]
quaters to apply to motions, pivots for appending to the representation
"""
""" Remove root translations """
rt_positions = positions[..., 0:1, :].copy() # [..., 1, 3]
positions -= rt_positions # relative positions, [..., J, 3]
rt_positions -= rt_positions[0, :, :].copy() # set init pos to zero
""" Get forward direction """
# as long as the joints are all present (including root),
# it doesn't matter whether the positions passed are relative or not
across = across_from_glb(positions)
direction_filterwidth = 20
forward = np.cross(across, np.array([[0, 1, 0]]))
forward = filters.gaussian_filter1d(forward,
direction_filterwidth,
axis=0,
mode='nearest')
forward = forward / np.sqrt((forward**2).sum(axis=-1))[..., np.newaxis]
""" Remove Y Rotation """
target = np.tile(np.array([0, 0, 1]), forward.shape[:-1] + (1, ))
rotations = Quaternions.between(
forward, target)[..., np.newaxis, :] # from [B, T, 4] to [B, T, 1, 4]
# -rotation corresponds to the "global rotations"
rt_rotations = Pivots.from_quaternions(-rotations).ps # [...T, 1]
return rotations, rt_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 dofs_local2world(dofs):
frame = dofs.shape[0]
dofs_new = np.zeros((dofs.shape[0], 18, 3))
dofs_new[:, 0] = dofs[:, 0:3]
dofs_new[:, 1] = dofs[:, 3:6]
dofs_new[:, 2] = dofs[:, 6:9]
dofs_new[:, 3, 0] = dofs[:, 9]
dofs_new[:, 5] = dofs[:, 10:13]
dofs_new[:, 6, 0] = dofs[:, 13]
dofs_new[:, 8] = dofs[:, 14:17]
dofs_new[:, 10] = dofs[:, 17:20]
dofs_new[:, 12] = dofs[:, 20:23]
dofs_new[:, 13, 1] = dofs[:, 23]
dofs_new[:, 15] = dofs[:, 24:27]
dofs_new[:, 16, 1] = dofs[:, 27]
global_rot_mat = np.zeros((frame, 17, 3, 3))
axis_angle = dofs_new[:, 1:]
local_rot_mat = axis_angles_to_matrixs(axis_angle) # (frame, 17, 3, 3)
global_rot_mat[:, 0] = local_rot_mat[:, 0]
for i in range(1, axis_angle.shape[1]):
global_rot_mat[:, i] = ut.matrix_multiply(
global_rot_mat[:, j17_parents[i]], local_rot_mat[:, i])
angles, axis = Quaternions.from_transforms(global_rot_mat).angle_axis()
angle_axis_world = angles[..., np.newaxis] * axis
dofs_world = dofs_new
dofs_world[:, 1:] = angle_axis_world
return dofs_world
def add_noise(rots, level):
"""adds random noise to a rotation matrix."""
noised_rots = [[np.random.uniform(-level, level, 4)] * 23] * rots.shape[0]
noised_rots = Quaternions(np.array(noised_rots))
return rots + noised_rots
def get_Holden_Data_73(skel_list, ignore_root=False):
skel_list_output = []
footsteps_output = []
for ai in range(len(skel_list)):
anim = np.swapaxes(skel_list[ai].copy(), 0, 1) # frameNum x 73
joints, root_x, root_z, root_r = anim[:, :
-7], anim[:,
-7], anim[:,
-6], anim[:,
-5]
joints = joints.reshape(
(len(joints), -1, 3)) #(frameNum,66) -> (frameNum, 22, 3)
rotation = Quaternions.id(1)
offsets = []
translation = np.array([[0, 0, 0]])
if not ignore_root:
for i in range(len(joints)):
joints[i, :, :] = rotation * joints[i]
joints[i, :, 0] = joints[i, :, 0] + translation[0, 0]
joints[i, :, 2] = joints[i, :, 2] + translation[0, 2]
rotation = Quaternions.from_angle_axis(
-root_r[i], np.array([0, 1, 0])) * rotation
offsets.append(rotation * np.array([0, 0, 1]))
translation = translation + rotation * np.array(
[root_x[i], 0, root_z[i]])
#joints dim:(frameNum, 22, 3)
#Scaling
joints[:, :, :] = joints[:, :, :] * 5 #m -> cm
joints[:, :, 1] = joints[:, :, 1] * -1 #Flip Y axis
#Reshaping
joints = joints.reshape(joints.shape[0], joints.shape[1] *
joints.shape[2]) # frameNum x 66
joints = np.swapaxes(joints, 0, 1) # 66 x frameNum
skel_list_output.append(joints)
footsteps_output.append(anim[:, -4:])
skel_list_output = np.asarray(skel_list_output)
return skel_list_output
def quat_to_euler(rotations):
norm = rotations[:, :,
0]**2 + rotations[:, :,
1]**2 + rotations[:, :,
2]**2 + rotations[:, :,
3]**2
norm = np.repeat(norm[:, :, np.newaxis], 4, axis=2)
rotations /= norm
rotations = Quaternions(rotations)
local_to_word = Quaternions(np.array([[[-0.5, 0.5, 0.5, 0.5]]]))
rotations = local_to_word * rotations
# rotations[:,2] = -rotations[:,1]*rotations[:,2]
# rotations[:,1] = -rotations[:,0]*rotations[:,1]
# rotations[:,5] = -rotations[:,4]*rotations[:,5]
# rotations[:,4] = -rotations[:,3]*rotations[:,4]
rotations = np.degrees(rotations.euler())
return rotations
def make_animation(generated_rots, original_anim):
"""make a proper animation object from rotations by using a model from the real data for static parameters (orients, offsets ...) """
generated_anim = (Animation.Animation(Quaternions(generated_rots),
original_anim.positions,
original_anim.orients,
original_anim.offsets,
original_anim.parents))
return generated_anim
def set_new_root(self, new_root):
euler = torch.tensor(self.anim.rotations[:, 0, :], dtype=torch.float)
transform = ForwardKinematics.transform_from_euler(euler, 'xyz')
offset = torch.tensor(self.anim.offsets[new_root], dtype=torch.float)
new_pos = torch.matmul(transform, offset)
new_pos = new_pos.numpy() + self.anim.positions[:, 0, :]
self.anim.offsets[0] = -self.anim.offsets[new_root]
self.anim.offsets[new_root] = np.zeros((3, ))
self.anim.positions[:, new_root, :] = new_pos
rot0 = Quaternions.from_euler(np.radians(self.anim.rotations[:, 0, :]),
order='xyz')
rot1 = Quaternions.from_euler(np.radians(
self.anim.rotations[:, new_root, :]),
order='xyz')
new_rot1 = rot0 * rot1
new_rot0 = (-rot1)
new_rot0 = np.degrees(new_rot0.euler())
new_rot1 = np.degrees(new_rot1.euler())
self.anim.rotations[:, 0, :] = new_rot0
self.anim.rotations[:, new_root, :] = new_rot1
new_seq = []
vis = [0] * self.anim.rotations.shape[1]
new_idx = [-1] * len(vis)
new_parent = [0] * len(vis)
def relabel(x):
nonlocal new_seq, vis, new_idx, new_parent
new_idx[x] = len(new_seq)
new_seq.append(x)
vis[x] = 1
for y in range(len(vis)):
if not vis[y] and (self.anim.parents[x] == y
or self.anim.parents[y] == x):
relabel(y)
new_parent[new_idx[y]] = new_idx[x]
relabel(new_root)
self.anim.rotations = self.anim.rotations[:, new_seq, :]
self.anim.offsets = self.anim.offsets[new_seq]
names = self._names.copy()
for i, j in enumerate(new_seq):
self._names[i] = names[j]
self.anim.parents = np.array(new_parent, dtype=np.int)
def orients_global(anim):
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 jacobian(self, x, fp, fr, goal, weights, des_r, des_t):
""" Find parent rotations """
prs = fr[:, self.animation.parents]
prs[:, 0] = Quaternions.id((1))
""" Get partial rotations """
qys = Quaternions.from_angle_axis(x[:, 1:prs.shape[1] * 3:3],
np.array([[[0, 1, 0]]]))
qzs = Quaternions.from_angle_axis(x[:, 2:prs.shape[1] * 3:3],
np.array([[[0, 0, 1]]]))
""" Find axis of rotations """
es = np.empty((len(x), fr.shape[1] * 3, 3))
es[:, 0::3] = ((prs * qzs) * qys) * np.array([[[1, 0, 0]]])
es[:, 1::3] = ((prs * qzs) * np.array([[[0, 1, 0]]]))
es[:, 2::3] = ((prs * np.array([[[0, 0, 1]]])))
""" Construct Jacobian """
j = fp.repeat(3, axis=1)
j = des_r[np.newaxis, :, :, :,
np.newaxis] * (goal[:, np.newaxis, :, np.newaxis] -
j[:, :, np.newaxis, np.newaxis])
j = np.sum(j * weights[np.newaxis, np.newaxis, :, :, np.newaxis], 3)
j = self.cross(es[:, :, np.newaxis, :], j)
j = np.swapaxes(
j.reshape((len(x), fr.shape[1] * 3, goal.shape[1] * 3)), 1, 2)
if self.translate:
es = np.empty((len(x), fr.shape[1] * 3, 3))
es[:, 0::3] = prs * np.array([[[1, 0, 0]]])
es[:, 1::3] = prs * np.array([[[0, 1, 0]]])
es[:, 2::3] = prs * np.array([[[0, 0, 1]]])
jt = des_t[np.newaxis, :, :, :,
np.newaxis] * es[:, :, np.newaxis,
np.newaxis, :].repeat(goal.shape[1],
axis=2)
jt = np.sum(jt * weights[np.newaxis, np.newaxis, :, :, np.newaxis],
3)
jt = np.swapaxes(
jt.reshape((len(x), fr.shape[1] * 3, goal.shape[1] * 3)), 1, 2)
j = np.concatenate([j, jt], axis=-1)
return j
def ConvertTrajectory_velocityForm(posData, bodyNormalData):
traj_list=[]
initTrans_list=[]
initRot_list=[]
for i in range(len(posData)):
targetPos = np.swapaxes(posData[i],0,1) #framesx2
velocity = (targetPos[1:,:] - targetPos[:-1,:]).copy() #(frames,2)
frameLeng = velocity.shape[0]
velocity_3d = np.zeros( (frameLeng,3) )
velocity_3d[:,0] = velocity[:,0]
velocity_3d[:,2] = velocity[:,1] #(frames,3)
velocity_3d = np.expand_dims(velocity_3d,1) #(frames,1, 3)
""" Compute Rvelocity"""
targetNormal = np.swapaxes(bodyNormalData[i],0,1) #(frames, 2)
if targetNormal.shape[1] ==2:
forward = np.zeros( (targetNormal.shape[0],3) )
forward[:,0] = targetNormal[:,0]
forward[:,2] = targetNormal[:,1]
else:
forward = targetNormal
forward = forward / np.sqrt((forward**2).sum(axis=-1))[...,np.newaxis]
target = np.array([[0,0,1]]).repeat(len(forward), axis=0)
rotation = Quaternions.between(forward, target)[:,np.newaxis] #forward:(frame,3)
velocity_3d = rotation[1:] * velocity_3d
""" Get Root Rotation """
rvelocity = Pivots.from_quaternions(rotation[1:] * -rotation[:-1]).ps
""" Save output """
trajData = np.zeros( (frameLeng,3))
trajData[:,0] = velocity_3d[:,0,0] * 0.2 #0.2 to make holden data scale
trajData[:,1] = velocity_3d[:,0,2] * 0.2 #0.2 to make holden data scale
trajData[:,2] = rvelocity[:,0]
initTrans = np.zeros( (1,3) )
initTrans[0,0] = targetPos[0,0]
initTrans[0,2] = targetPos[0,1]
initTrans = initTrans*0.2
trajData = np.swapaxes(trajData,0,1)
traj_list.append(trajData)
initTrans_list.append(initTrans)
initRot = -rotation[0] #Inverse to move [0,0,1] -> original Forward
initRot_list.append(initRot)
return traj_list, initTrans_list,initRot_list
def nrot_to_glb_and_rot(rot, skel=skel):
"""
input: new! rotations [(B, ) J * 4 + 3 + 1, T]
output: global positions [(B, ) T, J, 3]
"""
rot = np.swapaxes(rot, -1, -2)
rotations, rt_positions, rt_rotations = rot[..., :-4], rot[...,
-4:-1], rot[...,
-1:]
rotations = rotations.reshape(rotations.shape[:-1] +
(-1, 4)) # [..., J, 4]
norm = np.sqrt(np.sum(rotations**2, axis=-1, keepdims=True))
rotations = rotations / norm
rt_rotations = Quaternions(np.array(Pivots(rt_rotations).quaternions()))
ori_rt_rotations = Quaternions(rotations)[:, 0:1]
ori_rt_rotations = rt_rotations * ori_rt_rotations
rotations[:, 0:1, :] = np.array(ori_rt_rotations) # [T, J, 4]
return _rot_to_pos(rotations, rtpos=rt_positions, trim=False), rotations
def jacobian(self, x, fp, fr, ts, dsc, tdsc):
""" Find parent rotations """
prs = fr[:, self.animation.parents]
prs[:, 0] = Quaternions.id((1))
""" Find global positions of target joints """
tps = fp[:, np.array(list(ts.keys()))]
""" Get partial rotations """
qys = Quaternions.from_angle_axis(x[:, 1:prs.shape[1] * 3:3],
np.array([[[0, 1, 0]]]))
qzs = Quaternions.from_angle_axis(x[:, 2:prs.shape[1] * 3:3],
np.array([[[0, 0, 1]]]))
""" Find axis of rotations """
es = np.empty((len(x), fr.shape[1] * 3, 3))
es[:, 0::3] = ((prs * qzs) * qys) * np.array([[[1, 0, 0]]])
es[:, 1::3] = ((prs * qzs) * np.array([[[0, 1, 0]]]))
es[:, 2::3] = ((prs * np.array([[[0, 0, 1]]])))
""" Construct Jacobian """
j = fp.repeat(3, axis=1)
j = dsc[np.newaxis, :, :,
np.newaxis] * (tps[:, np.newaxis, :] - j[:, :, np.newaxis])
j = self.cross(es[:, :, np.newaxis, :], j)
j = np.swapaxes(j.reshape((len(x), fr.shape[1] * 3, len(ts) * 3)), 1,
2)
if self.translate:
es = np.empty((len(x), fr.shape[1] * 3, 3))
es[:, 0::3] = prs * np.array([[[1, 0, 0]]])
es[:, 1::3] = prs * np.array([[[0, 1, 0]]])
es[:, 2::3] = prs * np.array([[[0, 0, 1]]])
jt = tdsc[np.newaxis, :, :,
np.newaxis] * es[:, :, np.newaxis, :].repeat(
tps.shape[1], axis=2)
jt = np.swapaxes(
jt.reshape((len(x), fr.shape[1] * 3, len(ts) * 3)), 1, 2)
j = np.concatenate([j, jt], axis=-1)
return j
def split_into_subjects(self, predictions, targets, batch):
# separate right and left seller
haggling = self.haggling
batch_size = int(targets.shape[0] / 2)
r_target = haggling.denormalize_data(
targets[0:batch_size].cpu().numpy())
r_prediction = haggling.denormalize_data(
predictions[0:batch_size].cpu().numpy())
l_target = haggling.denormalize_data(
targets[batch_size:].cpu().numpy())
l_prediction = haggling.denormalize_data(
predictions[batch_size:].cpu().numpy())
buyer = haggling.denormalize_data(
batch['buyer']['joints21'].cpu().numpy())
# get initRot and initTrans
initRotRightSeller = Quaternions(
batch['rightSeller']['initRot'].cpu().numpy())
initTransRightSeller = batch['rightSeller']['initTrans'].cpu().numpy()
initRotLeftSeller = Quaternions(
batch['leftSeller']['initRot'].cpu().numpy())
initTransLeftSeller = batch['leftSeller']['initTrans'].cpu().numpy()
initRotBuyer = Quaternions(batch['buyer']['initRot'].cpu().numpy())
initTransBuyer = batch['buyer']['initTrans'].cpu().numpy()
prediction_subjects = [
(r_prediction.copy(), initRotRightSeller, initTransRightSeller),
(l_prediction.copy(), initRotLeftSeller, initTransLeftSeller),
(buyer.copy(), initRotBuyer, initTransBuyer),
]
target_subjects = [
(r_target.copy(), initRotRightSeller, initTransRightSeller),
(l_target.copy(), initRotLeftSeller, initTransLeftSeller),
(buyer.copy(), initRotBuyer, initTransBuyer)
]
return prediction_subjects, target_subjects
def attention2Direction(targetPos, rightPos, leftPos, binaryAtten):
if targetPos.shape[0] == 2:
targetPos = data_2dTo3D(targetPos)
if rightPos.shape[0] == 2:
rightPos = data_2dTo3D(rightPos)
if leftPos.shape[0] == 2:
leftPos = data_2dTo3D(leftPos)
binaryAtten = binaryAtten[:targetPos.shape[1]]
pos2right = rightPos - targetPos #me->right (l->b): 0
pos2left = leftPos - targetPos
pos2right = np.swapaxes(normalize(pos2right, axis=0), 0, 1) #(frame,3)
pos2left = np.swapaxes(normalize(pos2left, axis=0), 0, 1) #(frame,3)
rotation_right2left = Quaternions.between(
pos2right, pos2left)[:, np.newaxis] #rot from 0->1
angle_right2left = Pivots.from_quaternions(
rotation_right2left).ps #(frame,1)
angleValue_right2FaceNormal = binaryAtten * np.squeeze(
angle_right2left) #This much angle from pos2Other_0
eurlers_right2FaceNormal = np.zeros(
(len(angleValue_right2FaceNormal), 3)) #(frame,3)
eurlers_right2FaceNormal[:, 1] = angleValue_right2FaceNormal #(frame,3)
rotation_right2FaceNormal = Quaternions.from_euler(
eurlers_right2FaceNormal) #(frame,)
faceNormal_byAtten = rotation_right2FaceNormal * pos2right #(frame,3)
faceNormal_byAtten = np.swapaxes(faceNormal_byAtten, 0, 1) #(3,frame)
return faceNormal_byAtten
def linear_interpolate(quaternions, tgt_len):
src_len = quaternions.shape[0]
src_points = np.linspace(0, 1, src_len)
tgt_points = np.linspace(0, 1, tgt_len)
result = np.zeros([tgt_len] + list(quaternions.shape[1:]) + [4])
for i, tgt_point in enumerate(tgt_points):
down_val = np.max(src_points[src_points <= tgt_point])
up_val = np.min(src_points[src_points >= tgt_point])
p = (tgt_point -
down_val) / float(up_val - down_val) if up_val > down_val else 0.0
quat1 = quaternions[src_points == down_val, ...]
quat2 = quaternions[src_points == up_val, ...]
result[i] = np.array(Quaternions.slerp(quat1, quat2, p))
return result