def fill_traj(traj, frames_to_avg, frames_to_fill, fill_down=True):
dt = traj.dt
# compute averages
ang_vel_avg_up = np.mean(traj.ang_vel[0:frames_to_avg, :], axis=0)
ang_vel_avg_up_angle = np.linalg.norm(ang_vel_avg_up)
ang_vel_avg_up_axis = ang_vel_avg_up / ang_vel_avg_up_angle
ang_vel_avg_down = np.mean(traj.ang_vel[-frames_to_avg:, :], axis=0)
ang_vel_avg_down_angle = np.linalg.norm(ang_vel_avg_down)
ang_vel_avg_down_axis = ang_vel_avg_down / ang_vel_avg_down_angle
vel_avg_up = np.mean(traj.vel[0:frames_to_avg, :], axis=0)
vel_avg_down = np.mean(traj.vel[-frames_to_avg:, :], axis=0)
# add additional frames
pos_up_filled = np.stack([
vel_avg_up * dt * i + traj.pos[0]
for i in range(-frames_to_fill, 0)
], 0)
pos_down_filled = np.stack([
vel_avg_down * dt * i + traj.pos[-1]
for i in range(1, frames_to_fill + 1)
], 0)
quat_up_filled = np.stack([
create_quat(ang_vel_avg_up_angle * dt * i, ang_vel_avg_up_axis) *
traj.quat[0] for i in range(-frames_to_fill, 0)
], 0)
quat_down_filled = np.stack([
create_quat(ang_vel_avg_down_angle * dt * i, ang_vel_avg_down_axis)
* traj.quat[-1] for i in range(1, frames_to_fill + 1)
], 0)
# create new trajectory
new_frame_nums = np.concatenate(
(np.arange(traj.frame_nums[0] - frames_to_fill,
traj.frame_nums[0]), traj.frame_nums,
np.arange(traj.frame_nums[-1] + 1,
traj.frame_nums[-1] + frames_to_fill + 1)))
if new_frame_nums[0] < 0:
new_frame_nums = new_frame_nums + (-new_frame_nums[0])
if fill_down:
pos = np.concatenate((pos_up_filled, traj.pos, pos_down_filled),
axis=0)
quat = q.as_float_array(
np.concatenate((quat_up_filled, traj.quat, quat_down_filled),
axis=0))
return PoseTrajectory.from_quat(pos, quat, dt, new_frame_nums)
else:
pos = np.concatenate((pos_up_filled, traj.pos), axis=0)
quat = q.as_float_array(
np.concatenate((quat_up_filled, traj.quat), axis=0))
return PoseTrajectory.from_quat(pos, quat, dt,
new_frame_nums[:-frames_to_fill])
def interp_quat_traj(x_orig: np.ndarray, quat_orig: np.ndarray,
x: np.ndarray) -> PoseTrajectory:
"""Return interpolated biokinepy.trajectory.PoseTrajectory (SLERP) with NaNs outside of region defined in x_orig.
NOTE: only orientation is interpolated, and the position data is instantiated with zeros."""
quat_interp = interp_traj_slerp(x_orig, quat_orig, x)
return PoseTrajectory.from_quat(np.zeros((quat_interp.size, 3)),
q.as_float_array(quat_interp))
def clean_up_trials(db: pd.DataFrame) -> None:
"""Exclude frames from trials where subject has not gotten ready to perform thumb-up elevation yet."""
# the subject is axially rotating their arm to prepare for a thumb up arm elevation in the first 47 frames for SA,
# and first 69 frames for CA
clean_up_tasks = {'O45_009_SA_t01': 47, 'O45_009_CA_t01': 69}
for trial_name, start_frame in clean_up_tasks.items():
ht = db.loc[trial_name, 'ht']
gh = db.loc[trial_name, 'gh']
st = db.loc[trial_name, 'st']
db.loc[trial_name,
'ht'] = PoseTrajectory.from_ht(ht.ht[start_frame:, :, :], ht.dt,
ht.frame_nums[start_frame:])
db.loc[trial_name,
'gh'] = PoseTrajectory.from_ht(gh.ht[start_frame:, :, :], gh.dt,
gh.frame_nums[start_frame:])
db.loc[trial_name,
'st'] = PoseTrajectory.from_ht(st.ht[start_frame:, :, :], st.dt,
st.frame_nums[start_frame:])
def create_st_traj_ludewig(df: pd.DataFrame) -> PoseTrajectory:
"""Create ST PoseTrajectory from Elevation, PoE, Axial Rotation contained in df."""
q_repro = q.from_rotation_vector(
np.deg2rad(df['ReProtraction'].to_numpy())[..., np.newaxis] *
np.array([0, 1, 0]))
q_latmed = q.from_rotation_vector(
np.deg2rad(df['LatMedRot'].to_numpy())[..., np.newaxis] *
np.array([1, 0, 0]))
q_tilt = q.from_rotation_vector(
np.deg2rad(df['Tilt'].to_numpy())[..., np.newaxis] *
np.array([0, 0, 1]))
quat_traj = q.as_float_array(q_repro * q_latmed * q_tilt)
pos = np.zeros((q_repro.size, 3))
traj = PoseTrajectory.from_quat(pos, quat_traj)
traj.long_axis = np.array([0, 0, 1])
return traj
def create_gh_traj_ludewig(df: pd.DataFrame) -> PoseTrajectory:
"""Create GH PoseTrajectory from Elevation, PoE, Axial Rotation contained in df."""
q_elev = q.from_rotation_vector(
np.deg2rad(df['Elevation'].to_numpy())[..., np.newaxis] *
np.array([1, 0, 0]))
q_poe = q.from_rotation_vector(
np.deg2rad(df['PoE'].to_numpy())[..., np.newaxis] *
np.array([0, 0, 1]))
q_axial = q.from_rotation_vector(
np.deg2rad(df['Axial'].to_numpy())[..., np.newaxis] *
np.array([0, 1, 0]))
quat_traj = q.as_float_array(q_elev * q_poe * q_axial)
pos = np.zeros((q_elev.size, 3))
traj = PoseTrajectory.from_quat(pos, quat_traj)
traj.long_axis = np.array([0, 1, 0])
return traj
def summary_plotter(shoulder_trajs, traj_def, x_ind_def, y_ind_def, x_cmn_def,
y_cmn_def, decomp_method, sub_rot, ax, ind_plotter_fnc,
avg_color, quat_avg_color):
# plot individual trajectories
ind_traj_plot_lines = shoulder_trajs.apply(
ind_plotter_fnc,
args=[traj_def, x_ind_def, y_ind_def, decomp_method, sub_rot, ax])
# common x-axis : only look at the first trajectory because it will be the same for all
x_cmn = getattr(shoulder_trajs.iloc[0], x_cmn_def)
# Extract the y-values spanning the common x-axis. This goes to each quaternion interpolated trajectory, and applies
# decomp_method and sub_rot. The results are then averaged below. This is technically not correct because
# mathematically it doesn't make sense to average PoE, Elevation, etc. but this is how other papers handle this step
y_cmn_all = np.stack(
shoulder_trajs.apply(
extract_sub_rot,
args=[traj_def, y_cmn_def, decomp_method, sub_rot]), 0)
traj_mean, traj_std, traj_se = traj_stats(y_cmn_all)
agg_lines = ax.errorbar(x_cmn,
np.rad2deg(traj_mean),
yerr=np.rad2deg(traj_se),
capsize=2,
color=avg_color,
zorder=4,
lw=2)
# So here the individual interpolated trajectories are averaged via quaternions. This is mathematically correct.
# Then the averaged trajectory is decomposed according to decomp_method and sub_rot. One could then use this to
# compute SD and SE, but I don't go that far since almost always the quaternion mean and the mean as computed above
# match very well. But I do overlay the quaternion mean as a sanity check
if 'euler' in decomp_method:
mean_traj_quat = quat_mean_trajs(
np.stack(shoulder_trajs.apply(extract_interp_quat_traj,
args=[traj_def, y_cmn_def]),
axis=0))
mean_traj_pos = np.zeros((mean_traj_quat.size, mean_traj_quat.size))
mean_traj_pose = PoseTrajectory.from_quat(
mean_traj_pos, q.as_float_array(mean_traj_quat))
mean_y_quat = rgetattr(mean_traj_pose, decomp_method)[:, sub_rot]
quat_mean_lines = ax.plot(x_cmn,
np.rad2deg(mean_y_quat),
color=quat_avg_color,
zorder=5,
lw=2)
else:
quat_mean_lines = None
return ind_traj_plot_lines, agg_lines, quat_mean_lines
def get_trajs(trial: BiplaneViconTrial, dt: float, torso_def: str, scap_lateral: str = 'GC') \
-> Tuple[PoseTrajectory, PoseTrajectory, PoseTrajectory]:
"""Return HT, GH, and ST trajectory from a given trial."""
torso, scap, hum = trajectories_from_trial(trial, dt, torso_def=torso_def)
# this is not efficient because I recompute the scapula CS for each trial, but it can be done at the subject level
# however, in the grand scheme of things, this computation is trivial
scap_gc = scapula_cs_isb(trial.subject.scapula_landmarks['GC'],
trial.subject.scapula_landmarks['IA'],
trial.subject.scapula_landmarks['TS'])
scap_lat = scapula_cs_isb(trial.subject.scapula_landmarks[scap_lateral],
trial.subject.scapula_landmarks['IA'],
trial.subject.scapula_landmarks['TS'])
scap_gc_lat = ht_inv(scap_gc) @ scap_lat
scap = PoseTrajectory.from_ht(scap.ht @ scap_gc_lat[np.newaxis, ...],
dt=scap.dt,
frame_nums=scap.frame_nums)
ht = hum.in_trajectory(torso)
gh = hum.in_trajectory(scap)
st = scap.in_trajectory(torso)
return ht, gh, st
def trajectories_from_trial(trial: BiplaneViconTrial, dt: float, smoothed: bool = True, base_cs: str = 'vicon',
torso_def: str = 'isb', frame_sync: bool = True) -> Tuple[PoseTrajectory, PoseTrajectory,
PoseTrajectory]:
"""Create torso, scapula, and humerus trajectories from a trial."""
assert(np.array_equal(trial.humerus_frame_nums, trial.scapula_frame_nums))
scap_quat_field = 'scapula_quat_fluoro_avg_smooth' if smoothed else 'scapula_quat_fluoro'
scap_pos_field = 'scapula_pos_fluoro_avg_smooth' if smoothed else 'scapula_pos_fluoro'
hum_quat_field = 'humerus_quat_fluoro_avg_smooth' if smoothed else 'humerus_quat_fluoro'
hum_pos_field = 'humerus_pos_fluoro_avg_smooth' if smoothed else 'humerus_pos_fluoro'
def get_torso_pos_quat(t, thorax_def):
if thorax_def == 'isb':
return t.torso_pos_vicon, t.torso_quat_vicon
elif thorax_def == 'v3d':
return t.torso_v3d_pos_vicon, t.torso_v3d_quat_vicon
else:
raise ValueError('torso_def must be either isb or v3d.')
if base_cs == 'vicon':
# scapula
scap_rot_mat = quaternion.as_rotation_matrix(quaternion.from_float_array(getattr(trial, scap_quat_field)))
scap_traj_fluoro = ht_r(scap_rot_mat, getattr(trial, scap_pos_field))
scap_traj_vicon = change_cs(ht_inv(trial.subject.f_t_v), scap_traj_fluoro)
scap_traj = PoseTrajectory.from_ht(scap_traj_vicon, dt, trial.scapula_frame_nums)
# humerus
hum_rot_mat = quaternion.as_rotation_matrix(quaternion.from_float_array(getattr(trial, hum_quat_field)))
hum_traj_fluoro = ht_r(hum_rot_mat, getattr(trial, hum_pos_field))
hum_traj_vicon = change_cs(ht_inv(trial.subject.f_t_v), hum_traj_fluoro)
hum_traj = PoseTrajectory.from_ht(hum_traj_vicon, dt, trial.humerus_frame_nums)
# torso
torso_pos_vicon, torso_quat_vicon = get_torso_pos_quat(trial, torso_def)
if frame_sync:
torso_pos_vicon_sync = (torso_pos_vicon[trial.vicon_endpts[0]:
trial.vicon_endpts[1]])[trial.humerus_frame_nums - 1]
torso_quat_vicon_sync = (torso_quat_vicon[trial.vicon_endpts[0]:
trial.vicon_endpts[1]])[trial.humerus_frame_nums - 1]
torso_traj = PoseTrajectory.from_quat(torso_pos_vicon_sync, torso_quat_vicon_sync, dt,
trial.humerus_frame_nums)
else:
torso_traj = PoseTrajectory.from_quat(torso_pos_vicon, torso_quat_vicon, dt,
np.arange(torso_pos_vicon.shape[0]) + 1)
elif base_cs == 'fluoro':
scap_traj = PoseTrajectory.from_quat(getattr(trial, scap_pos_field), getattr(trial, scap_quat_field), dt,
trial.scapula_frame_nums)
hum_traj = PoseTrajectory.from_quat(getattr(trial, hum_pos_field), getattr(trial, hum_quat_field), dt,
trial.humerus_frame_nums)
torso_pos_vicon, torso_quat_vicon = get_torso_pos_quat(trial, torso_def)
torso_rot_mat = quaternion.as_rotation_matrix(quaternion.from_float_array(torso_quat_vicon))
torso_traj_vicon = ht_r(torso_rot_mat, torso_pos_vicon)
torso_traj_fluoro = change_cs(trial.subject.f_t_v, torso_traj_vicon)
if frame_sync:
torso_traj_fluoro_sync = (torso_traj_fluoro[trial.vicon_endpts[0]:
trial.vicon_endpts[1]])[trial.humerus_frame_nums - 1]
torso_traj = PoseTrajectory.from_ht(torso_traj_fluoro_sync, dt, trial.humerus_frame_nums)
else:
torso_traj = PoseTrajectory.from_ht(torso_traj_fluoro, dt, np.arange(torso_traj_fluoro.shape[0]) + 1)
else:
raise ValueError('base_cs must be either vicon or fluoro.')
return torso_traj, scap_traj, hum_traj
def set_long_axis_hum(traj: PoseTrajectory) -> None:
"""Set the longitudinal axis on humeral trajectory according to ISB specifications."""
traj.long_axis = np.array([0, 1, 0])
def __init__(self, traj: PoseTrajectory, shoulder_traj: ShoulderTrajInterp,
long_axis_fnc):
self.traj = traj
self.shoulder_traj = shoulder_traj
pos = np.zeros((self.traj.quat.size, 3))
pos_up = shoulder_traj.get_up(pos)
pos_down = shoulder_traj.get_down(pos)
quat_up = shoulder_traj.get_up(self.traj.quat)
quat_down = shoulder_traj.get_down(self.traj.quat)
self.up = PoseTrajectory.from_quat(pos_up, q.as_float_array(quat_up))
self.down = PoseTrajectory.from_quat(pos_down,
q.as_float_array(quat_down))
self.sym_fine_up = interp_quat_traj(shoulder_traj.ht_ea_up, quat_up,
shoulder_traj.sym_ht_range_fine)
self.sym_fine_down = interp_quat_traj(shoulder_traj.ht_ea_down,
quat_down,
shoulder_traj.sym_ht_range_fine)
self.common_fine_up = interp_quat_traj(
shoulder_traj.ht_ea_up, quat_up,
shoulder_traj.common_ht_range_fine)
self.common_fine_down = interp_quat_traj(
shoulder_traj.ht_ea_down, quat_down,
shoulder_traj.common_ht_range_fine)
self.common_coarse_up = interp_quat_traj(
shoulder_traj.ht_ea_up, quat_up,
shoulder_traj.common_ht_range_coarse)
self.common_coarse_down = interp_quat_traj(
shoulder_traj.ht_ea_down, quat_down,
shoulder_traj.common_ht_range_coarse)
long_axis_fnc(self.up)
long_axis_fnc(self.down)
long_axis_fnc(self.sym_fine_up)
long_axis_fnc(self.sym_fine_down)
long_axis_fnc(self.common_fine_up)
long_axis_fnc(self.common_fine_down)
long_axis_fnc(self.common_coarse_up)
long_axis_fnc(self.common_coarse_down)
# compute axial rotation interpolation - this must be done separately than the computations above because
# axial rotation is path dependent
self.true_axial_rot = traj.true_axial_rot
self.true_axial_rot_up = shoulder_traj.get_up(self.true_axial_rot)
self.true_axial_rot_down = shoulder_traj.get_down(self.true_axial_rot)
self.true_axial_rot_sym_fine_up = interp_scalar_traj(
shoulder_traj.ht_ea_up, self.true_axial_rot_up,
shoulder_traj.sym_ht_range_fine)
self.true_axial_rot_sym_fine_down = interp_scalar_traj(
shoulder_traj.ht_ea_down, self.true_axial_rot_down,
shoulder_traj.sym_ht_range_fine)
self.true_axial_rot_common_fine_up = interp_scalar_traj(
shoulder_traj.ht_ea_up, self.true_axial_rot_up,
shoulder_traj.common_ht_range_fine)
self.true_axial_rot_common_fine_down = interp_scalar_traj(
shoulder_traj.ht_ea_down, self.true_axial_rot_down,
shoulder_traj.common_ht_range_fine)
self.true_axial_rot_common_coarse_up = interp_scalar_traj(
shoulder_traj.ht_ea_up, self.true_axial_rot_up,
shoulder_traj.common_ht_range_coarse)
self.true_axial_rot_common_coarse_down = interp_scalar_traj(
shoulder_traj.ht_ea_down, self.true_axial_rot_down,
shoulder_traj.common_ht_range_coarse)
def set_long_axis_scap(traj: PoseTrajectory) -> None:
"""Set the third axis of rotation on the scapula trajectory according to ISB specifications."""
traj.long_axis = np.array([0, 0, 1])