Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
+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
|
||||
Reference in New Issue
Block a user