def z_angle(data=None, bone=None, method='quat'):
if data is None:
raise Exception("No data input")
if method == 'quat':
stuff = np.array(data[data_model.kin_bone_name(bone)])
return stuff.T[0]*stuff.T[0] - stuff.T[1]*stuff.T[1] - stuff.T[2]*stuff.T[2] + stuff.T[3]*stuff.T[3]
else: # kin is using savitzky-golay and model params are just residual moduli
for angle in ANGLES:
if angle[1] == bone[0]:
first_joint = np.array(data[angle[0]])
second_joint = np.array(data[angle[1]])
third_joint = np.array(data[angle[2]])
first_limb = first_joint - second_joint
second_limb = third_joint - second_joint
first_limb_normalized = normalize_rows(first_limb)
second_limb_normalized = normalize_rows(second_limb)
return np.clip(-1*np.sum((first_limb_normalized * second_limb_normalized), axis=-1), -1.0, 1.0)
def __init__(self, DM):
for kind in ['kin', 'qual']:
setattr(self, kind, HandlerObject())
for level in ['raw', 'smooth']:
setattr(getattr(self, kind), level, dict())
d = getattr(getattr(self, kind), level)
for bone in data_model.KIN_TREE:
if kind is 'kin' or bone in data_model.BONE_MAPPING:
if kind == 'kin' and DM.kin.smoothing == 'SG':# and bone != ('HipCenter', 'HipCenter'):
method = 'joint'
else:
method = 'quat'
handler = Handler(DM)
# stuff = getattr(getattr(DM, kind).data, level)[data_model.kin_bone_name(bone)]
handler.axis = AXES[kind]
# TEMP: FOLLOWING ASSUMES ACTING ON Z AXIS WITH NORMALIZED QUATERION
handler.raw_func = z_angle(data=getattr(getattr(DM, kind).data, level), bone=bone, method=method)
handler.times = getattr(DM, kind).data.raw['Time']
handler.time_shift = 0
handler.vis_func = degreeify#basic_degreeify
d[data_model.kin_bone_name(bone)] = handler
