def fit_limbs(self):
for limb in LIMBS:
data = self.data
lengths = self.lengths
A = np.array(data[limb[0]])
B = np.array(data[limb[1]])
C = np.array(data[limb[2]])
Y = normalize_rows(B - A)
X = normalize_rows(C - A)
Yn = normalize_rows(Y - np.sum((X * Y), axis=-1, keepdims=True) * X)
a = ((C - A) * X).sum(-1)
b = ((B - A) * X).sum(-1)
c = ((B - A) * Yn).sum(-1)
# will be dog slow for now
temp = {}
temp["start"] = []
temp["mid"] = []
temp["end"] = []
L1 = self.lengths[(limb[0], limb[1])]
L2 = self.lengths[(limb[1], limb[2])]
L = len(A)
for i in xrange(L):
S, M, E = triangle_min(L1, L2, a[i], b[i], c[i])
temp["start"].append(S)
temp["mid"].append([M[0], M[1]])
temp["end"].append([E[0], E[1]])
temp["start"] = np.array(temp["start"])
temp["end"] = np.array(temp["end"])
temp["mid"] = np.array(temp["mid"])
self.data[limb[0]] = A + (temp["start"].T[0]).reshape(L, 1) * X + (temp["start"].T[1]).reshape(L, 1) * Yn
self.data[limb[1]] = A + (temp["mid"].T[0]).reshape(L, 1) * X + (temp["mid"].T[1]).reshape(L, 1) * Yn
self.data[limb[2]] = A + (temp["end"].T[0]).reshape(L, 1) * X + (temp["end"].T[1]).reshape(L, 1) * Yn
def smooth(self, q):
"""Implement double exponential smoothing on generated timeseries."""
"""Copying most of MS code. Probably painfully slow in Python because
it is not vectorized."""
filt = np.zeros(q.shape)
trend = np.zeros(q.shape)
filt[0] = q[0]
trend[0] = np.array([1.0, 0.0, 0.0, 0.0])
filt[1] = slerp(q[0], filt[0], 0.5)
diff_started = rotation_between(filt[1], filt[0])
trend[1] = slerp(trend[0], diff_started, self.correction)
for i in range(2, len(q)):
diff_jitter = rotation_between(q[i], filt[i-1])
diff_val_jitter = np.abs(quaternion_angle(diff_jitter))
if diff_val_jitter <= self.jitter_radius:
filt[i] = slerp(filt[i-1], q[i], diff_val_jitter/self.jitter_radius)
else:
filt[i] = q[i]
filt[i] = normalize_rows(slerp(filt[i], prod(filt[i-1], trend[i-1]), self.smoothing))
diff_jitter = rotation_between(filt[i], filt[i-1])
trend[i] = normalize_rows(slerp(trend[i-1], diff_jitter, self.correction))
return np.array(filt)
def __init__(self, data=None):
if data is None:
with open("DataFiles/master/master.json", "rb") as file:
data = json.load(file)
self.coords = data
try:
self.kin = data['kin']
self.qual = data['qual']
except KeyError:
self.kin = data
for hinge in HINGES:
self.kin[hinge + 'Angle'] = angle_handlers.z_angle(data=self.kin,
bone=[hinge],
method='kin')
normal_fore_limb = None
normal_upper_limb = None
ball_bone = None
for seg in data_model.KIN_SEGMENTS:
bone = data_model.KIN_SEGMENTS[seg]
add = None
if hinge == bone[0]:
add = 'PostLength'
normal_fore_limb = normalize_rows(np.array(self.kin[bone[1]]) - np.array(self.kin[bone[0]]))
if hinge == bone[1]:
add = 'PreLength'
normal_upper_limb = normalize_rows(np.array(self.kin[bone[1]]) - np.array(self.kin[bone[0]]))
ball_bone = bone[0]
if add is not None:
self.kin[hinge + add] = correctors.limb_length(limb=bone, data=self.kin)
z = normal_upper_limb
x = normalize_rows(normal_fore_limb - np.dot((normal_fore_limb * normal_upper_limb).sum(-1).T, normal_upper_limb))
y = np.cross(z,x)
self.kin[ball_bone + "Axis"] = z
self.kin[ball_bone + "Normal"] = y
newlabels = []
for label in data_model.KIN_LABELS:
if label not in HINGES and label not in BALLS:
newlabels.append(label)
elif label in HINGES:
newlabels.append(label+'Angle')
newlabels.append(label+'PostLength')
newlabels.append(label+'PreLength')
elif label in BALLS:
newlabels.append(label+'Axis')
newlabels.append(label+'Normal')
self.newkin = {label: self.kin[label] for label in newlabels}
def axis_mover(z):
u = normalize_rows(np.column_stack((-1.0 * z.T[1], -1.0 * z.T[0])))
alpha = np.arccos(z.T[2])
if len(z.shape) == 1:
x = np.cos(0.5 * alpha)
w = 0.0
else:
x = np.cos(0.5 * alpha) * np.ones((z.shape[0],))
w = np.zeros((z.shape[0],))
return join_parts((x, np.sin(0.5 * alpha) * u.T[0], np.sin(0.5 * alpha) * u.T[1], w))
def __init__(self, name, limits=[0, 0], direction=np.array((0.01, 0, 1)), axial=0, sign=1.0):
self.name = name
self.limits = limits
# approx: equation for cone ax^2 + bz^2 >= y^2 constraining motion; outside certain cone.
self.direction = normalize_rows(
direction
) # point on 2-sphere giving (initial) direction of child relative to parent
# self.phi = 0 # rotation around x
# self.theta = 0 # rotation around z
self.axial = axial # rotation around bone axis
self.sign = sign
def greedy_fit_point(self, point, filter=True): # point should be made of numpy arrays....
new_point = sg_filter_data(point) if filter else point
fitted_point = dict()
fitted_point["HipCenter"] = new_point["HipCenter"]
visited = ["HipCenter"]
change = 1
while change > 0:
length = len(visited)
for edge in data_model.KIN_TREE:
if edge[0] in visited and edge[1] not in visited:
edge_length = self.lengths[edge]
dir = normalize_rows(np.array(new_point[edge[1]]) - np.array(fitted_point[edge[0]]))
fitted_point[edge[1]] = fitted_point[edge[0]] + edge_length * dir
visited.append(edge[1])
# print edge[1]
change = len(visited) - length
# other_keys = [label for label in new_point if label not in data_model.KIN_TREE]
# for label in other_keys:
# fitted_point[label] = new_point[label]
return fitted_point
def ingest_data(self, data, modeled=True, length_fit=False):
self.BM = BodyModel(data)
if length_fit:
self.BM.fit()
self.data = data
if modeled:
n = normalize_data({'kin': self.BM.data})['kin']
X = np.column_stack((n[key] for key in KL))
self.model_data = normalize_rows(X)
times = self.data['Time']
newtimes = np.linspace(times[0], times[-1], 1000)
self.times = times
self.newtimes = newtimes
floor = self.floor if self.floor else 0.5 * (np.mean(self.data['FootRight'].T[2]) + np.mean(self.data['FootLeft'].T[2]))
#if (floor == None): floor = 0.5 * (np.mean(self.data['FootRight'].T[2]) + np.mean(self.data['FootLeft'].T[2]))
vl = VL(self.BM.data, floor=floor)
modeled_vl = vl + VLm(self.model_data) if modeled else vl
subsampled_vl = UnivariateSpline(times, modeled_vl, s=0)(newtimes)
self.smoothed_VL = UnivariateSpline(newtimes, sav_gol.savgol_filter(subsampled_vl, 31, 3), s=0) # smooth pretty hard to stop wobbling, overest?
hl = HL(self.BM.data)
modeled_hl = hl + HLm(self.model_data) if modeled else hl
subsampled_hl = UnivariateSpline(times, modeled_hl, s=0)(newtimes)
self.smoothed_HL = UnivariateSpline(newtimes, sav_gol.savgol_filter(subsampled_hl, 31, 3), s=0) # smooth pretty hard to stop wobbling, overest?
aa = AA(self.BM.data)
modeled_aa = aa + AAm(self.model_data) if modeled else aa
subsampled_aa = UnivariateSpline(times, modeled_aa, s=0)(newtimes)
self.smoothed_AA = UnivariateSpline(newtimes, sav_gol.savgol_filter(subsampled_aa, 31, 3), s=0) # smooth pretty hard to stop wobbling, overest?
self.find_F()
# self.find_origins()
# self.find_ends()
self.find_V()
self.find_H()
self.find_D()
self.find_A()
def canonical(q):
z = conjugate(q, np.array([0.0, 0.0, 0.0, 1.0])).T[1:].T
z = normalize_rows(z)
return axis_mover(z)
def quaternion_angle(a):
b = normalize_rows(a)
return 2.0 * np.arccos((b.T)[2].T)
def normalize_data(data, center_floor = False):
"""Assume that data is aligned (so qual and kin have best fit)"""
qual_present = hasattr(data, 'qual')
k = data['kin']
kn = dict()
if qual_present:
q = data['qual']
qn = dict()
center = center_of_mass(k)
if center_floor:
center *= np.array((1,1,0))
# first translate center of mass to the origin
for key in k.keys():
if key in data_model.KIN_LABELS:
kn[key] = np.array(k[key]) - center
else:
kn[key] = k[key]
if qual_present:
for key in q.keys():
if len(q[key][0]) == 3:
qn[key] = np.array(q[key]) - center
else:
qn[key] = q[key]
# coordinate system to use: z axis, hip orientation vector (suitably
# gram schmidted) -- call this x, whatever is necessary to make right-handed system -- y
hr = kn['HipRight']
hl = kn['HipLeft']
n = normalize_rows(np.cross(hl, hr))
x = normalize_rows(n - n * np.array([0,0,1]))
y = normalize_rows(np.cross(np.array([0,0,1]), x))
outk = {}
if qual_present:
outq = {}
for key in kn.keys():
if key in data_model.KIN_LABELS:
outk[key] = np.column_stack(((kn[key] * x).sum(-1), (kn[key] * y).sum(-1), kn[key].T[2]))
else:
outk[key] = k[key]
if qual_present:
for key in qn.keys():
if len(q[key][0]) == 3:
outq[key] = np.column_stack(((qn[key] * x).sum(-1), (qn[key] * y).sum(-1), qn[key].T[2]))
elif key != 'Time' and key != 'HipCenterQ':
outq[key] = q[key]
elif key == 'HipCenterQ':
theta = np.arccos(x.T[0])
s = np.cos(0.5 * theta)
t = np.sin(0.5 * theta)
outq[key] = quaternions.conjugate(np.column_stack([s, np.zeros(len(s)), np.zeros(len(s)), t]), np.array(q[key]))
# try:
# outk['Time'] = k['Time']
# except KeyError:
# pass
if qual_present:
try:
outq['Time'] = q['Time']
except KeyError:
pass
if qual_present:
return {'unnormalized_kin': k, 'unnormalized_qual': q, 'kin': outk, 'qual': outq}
else:
return {'unnormalized_kin': k, 'kin': outk}
def proj(a,b):
c = normalize_rows(b)
return np.dot((a * c).sum(-1), c)
