def st_gh_balance(ht_traj: PoseTrajectory, st_traj: PoseTrajectory,
gh_traj: PoseTrajectory):
num_frames = st_traj.rot_matrix.shape[0]
elev_axes = compute_elev_axis(ht_traj.rot_matrix)
hum_axes = ht_traj.rot_matrix[:, :, 1]
ht_diff = ht_traj.quat[1:] * np.conjugate(ht_traj.quat[:-1])
ht_proj = np.empty_like(ht_diff)
for i in range(num_frames - 1):
ht_proj[i] = quat_project(ht_diff[i], elev_axes[i])
ht_remain = ht_diff * np.conjugate(ht_proj)
ht_remain_float = q.as_float_array(ht_remain)
ht_remain_float = np.concatenate(
(ht_remain_float[:, 1:], ht_remain_float[:, 0][..., np.newaxis]), 1)
ht_remain_vec = Rotation.from_quat(ht_remain_float).as_rotvec()
ht_remain_angle = np.sqrt(extended_dot(ht_remain_vec, ht_remain_vec))
ht_remain_axis = ht_remain_vec / ht_remain_angle[..., np.newaxis]
ht_remain_angle = np.where(
extended_dot(ht_remain_axis, hum_axes[:-1]) > 0, ht_remain_angle,
-ht_remain_angle)
return np.cumsum(ht_remain_angle)
def raw_diff_scalar(self) -> np.ndarray:
"""Compute || vicon - biplane ||, where || . || indicates Euclidean norm.
Note that NaNs can still be present because for a particular frame a marker could be measured via biplane
fluoroscopy but not be visible to the Vicon cameras.
"""
return np.sqrt(extended_dot(self.raw_diff, self.raw_diff))
def compute_poe_axis(ht_traj: np.ndarray) -> np.ndarray:
"""Compute PoE rotation axis for each frame of ht_traj."""
long_axis = ht_traj[:, :, 1]
elev_axis = compute_elev_axis(ht_traj)
poe_axis = np.cross(elev_axis, long_axis)
poe_axis = poe_axis / np.sqrt(extended_dot(poe_axis, poe_axis))[...,
np.newaxis]
return poe_axis
def wrt_torso_traj_proj_ext(wrt_torso_traj: np.ndarray,
wrt_torso_axes: np.ndarray) -> np.ndarray:
"""Project angular velocity of trajectory that is expressed with respect to torso onto supplied axes
(also expressed with respect to torso) and integrate from start to end of motion."""
num_frames = wrt_torso_traj.shape[0]
# dt doesn't matter because we will integrate soon
av = ang_vel(wrt_torso_traj, dt=1)
av_x = (extended_dot(av, wrt_torso_traj[:, :, 0])[..., np.newaxis] *
wrt_torso_traj[:, :, 0])
av_y = (extended_dot(av, wrt_torso_traj[:, :, 1])[..., np.newaxis] *
wrt_torso_traj[:, :, 1])
av_z = (extended_dot(av, wrt_torso_traj[:, :, 2])[..., np.newaxis] *
wrt_torso_traj[:, :, 2])
av_proj_x = extended_dot(av_x, wrt_torso_axes)
av_proj_y = extended_dot(av_y, wrt_torso_axes)
av_proj_z = extended_dot(av_z, wrt_torso_axes)
av_proj = extended_dot(av, wrt_torso_axes)
induced = np.empty((num_frames, 4), dtype=np.float)
induced[:, 0] = cumtrapz(av_proj_x, dx=1, initial=0)
induced[:, 1] = cumtrapz(av_proj_y, dx=1, initial=0)
induced[:, 2] = cumtrapz(av_proj_z, dx=1, initial=0)
induced[:, 3] = cumtrapz(av_proj, dx=1, initial=0)
return induced
def st_induced_axial_rot_fha(st: PoseTrajectory,
ht: PoseTrajectory) -> np.ndarray:
"""Compute ST-induced HT axial rotation using finite helical axes."""
num_frames = st.rot_matrix.shape[0]
st_diff = st.quat[1:] * np.conjugate(st.quat[:-1])
st_fha = q.as_rotation_vector(st_diff)
st_fha_x = (
extended_dot(st_fha, st.rot_matrix[:-1, :, 0])[..., np.newaxis] *
st.rot_matrix[:-1, :, 0])
st_fha_y = (
extended_dot(st_fha, st.rot_matrix[:-1, :, 1])[..., np.newaxis] *
st.rot_matrix[:-1, :, 1])
st_fha_z = (
extended_dot(st_fha, st.rot_matrix[:-1, :, 2])[..., np.newaxis] *
st.rot_matrix[:-1, :, 2])
st_fha_proj_x = extended_dot(st_fha_x, ht.rot_matrix[:-1, :, 1])
st_fha_proj_y = extended_dot(st_fha_y, ht.rot_matrix[:-1, :, 1])
st_fha_proj_z = extended_dot(st_fha_z, ht.rot_matrix[:-1, :, 1])
st_fha_proj = extended_dot(st_fha, ht.rot_matrix[:-1, :, 1])
induced_axial_rot = np.empty((num_frames, 4), dtype=np.float)
induced_axial_rot[0, :] = 0
induced_axial_rot[1:, 0] = np.add.accumulate(st_fha_proj_x)
induced_axial_rot[1:, 1] = np.add.accumulate(st_fha_proj_y)
induced_axial_rot[1:, 2] = np.add.accumulate(st_fha_proj_z)
induced_axial_rot[1:, 3] = np.add.accumulate(st_fha_proj)
return induced_axial_rot
def st_induced_axial_rot_ang_vel(st: PoseTrajectory,
ht: PoseTrajectory) -> np.ndarray:
"""Compute ST-induced HT axial rotation using angular velocity."""
num_frames = st.rot_matrix.shape[0]
# The ST velocity is expressed in the torso coordinate system so we can't just use its x,y,z components.
# We need to first project it onto the x,y,z axes of the scapula. This is equivalent to expressing the ST velocity
# in its own body coordinates.
st_angvel_x = (
extended_dot(st.ang_vel, st.rot_matrix[:, :, 0])[..., np.newaxis] *
st.rot_matrix[:, :, 0])
st_angvel_y = (
extended_dot(st.ang_vel, st.rot_matrix[:, :, 1])[..., np.newaxis] *
st.rot_matrix[:, :, 1])
st_angvel_z = (
extended_dot(st.ang_vel, st.rot_matrix[:, :, 2])[..., np.newaxis] *
st.rot_matrix[:, :, 2])
st_angvel_proj_x = extended_dot(st_angvel_x, ht.rot_matrix[:, :, 1])
st_angvel_proj_y = extended_dot(st_angvel_y, ht.rot_matrix[:, :, 1])
st_angvel_proj_z = extended_dot(st_angvel_z, ht.rot_matrix[:, :, 1])
st_angvel_proj = extended_dot(st.ang_vel, ht.rot_matrix[:, :, 1])
induced_axial_rot = np.empty((num_frames, 4), dtype=np.float)
induced_axial_rot[:, 0] = cumtrapz(st_angvel_proj_x, dx=st.dt, initial=0)
induced_axial_rot[:, 1] = cumtrapz(st_angvel_proj_y, dx=st.dt, initial=0)
induced_axial_rot[:, 2] = cumtrapz(st_angvel_proj_z, dx=st.dt, initial=0)
induced_axial_rot[:, 3] = cumtrapz(st_angvel_proj, dx=st.dt, initial=0)
return induced_axial_rot
def wrt_torso_traj_proj(wrt_torso_traj: np.ndarray,
wrt_torso_axes: np.ndarray) -> np.ndarray:
"""Project angular velocity of trajectory that is expressed with respect to torso onto supplied axes
(also expressed with respect to torso) and integrate from start to end of motion."""
# dt doesn't matter because we will integrate soon
av = ang_vel(wrt_torso_traj, dt=1)
av_proj = extended_dot(av, wrt_torso_axes)
return cumtrapz(av_proj, dx=1, initial=0)
def compute_elev_axis(ht_traj: np.ndarray) -> np.ndarray:
"""Compute elevation rotation axis for each frame of ht_traj."""
num_frames = ht_traj.shape[0]
long_axis = -ht_traj[:, :, 1]
# compute axis that is perpendicular to the longitudinal axis projection
long_proj_perp = np.cross(np.tile(np.array([0, 1, 0]), (num_frames, 1)),
long_axis)
long_proj_perp = long_proj_perp / np.sqrt(
extended_dot(long_proj_perp, long_proj_perp))[..., np.newaxis]
return long_proj_perp
def gh_traj_proj(gh_traj: np.ndarray, st_traj: np.ndarray,
wrt_torso_axes: np.ndarray) -> np.ndarray:
"""Project angular velocity of GH trajectory onto supplied axes (expressed with respect to torso)
and integrate from start to end of motion."""
# dt doesn't matter because we will integrate soon
av = ang_vel(gh_traj, dt=1)
# express GH angular velocity in torso coordinate system
av_torso = np.squeeze(st_traj @ av[..., np.newaxis])
av_proj = extended_dot(av_torso, wrt_torso_axes)
return cumtrapz(av_proj, dx=1, initial=0)
def true_axial_rot_fha(traj):
num_frames = traj.rot_matrix.shape[0]
traj_diff = traj.quat[1:] * np.conjugate(traj.quat[:-1])
# Switch to using scipy.spatial.transform here because the quaternion package gives a rotation of close to 2*pi
# sometimes. This isn't incorrect, but it's not consistent. The return should be 2*pi-ret
# scipy.spatial.transform uses a scalar-last convention so account for that here
traj_diff_float = q.as_float_array(traj_diff)
traj_diff_float = np.concatenate((traj_diff_float[:, 1:], traj_diff_float[:, 0][..., np.newaxis]), axis=1)
traj_fha = R.from_quat(traj_diff_float).as_rotvec()
traj_fha_proj = extended_dot(traj_fha, traj.rot_matrix[:-1, :, 1])
true_axial_rot = np.empty((num_frames,), dtype=np.float)
true_axial_rot[0] = 0
true_axial_rot[1:] = np.add.accumulate(traj_fha_proj)
return true_axial_rot
def add_st_gh_contrib_ind(ht_joint: PoseTrajectory, st_joint: PoseTrajectory,
gh_joint: PoseTrajectory):
"""Compute HT, ST, and GH joint contributions for elevation, axial rotation, and PoE given the pose trajectories of
these joints."""
# Note that where these contributions are stored differs from the manuscript. To make it easy to compare to ISB
# PoE changes are stored in index 0, elevation changes are stored in index 1, and axial rotation changes are stored
# in index 2. The orthogonality of the frame is still respected just the index where these contributions are stored
# is changed.
poe_axes = compute_poe_axis(ht_joint.rot_matrix)
elev_axes = compute_elev_axis(ht_joint.rot_matrix)
axial_axes = compute_axial_axis(ht_joint.rot_matrix)
st_contribs = np.empty((st_joint.ang_vel.shape[0], 4))
st_contribs[:, 0] = wrt_torso_traj_proj(st_joint.rot_matrix, poe_axes)
st_contribs[:, 1] = wrt_torso_traj_proj(st_joint.rot_matrix, elev_axes)
st_contribs[:, 2] = wrt_torso_traj_proj(st_joint.rot_matrix, axial_axes)
st_ang_vel_mag = np.sqrt(extended_dot(st_joint.ang_vel, st_joint.ang_vel))
st_ang_vel_dir = st_joint.ang_vel / st_ang_vel_mag[..., np.newaxis]
st_ang_vel_mag = np.where(
extended_dot(st_ang_vel_dir, elev_axes) > 0, st_ang_vel_mag,
-st_ang_vel_mag)
st_contribs[:, 3] = cumtrapz(st_ang_vel_mag, dx=st_joint.dt, initial=0)
ht_contribs = np.empty((ht_joint.ang_vel.shape[0], 4))
ht_contribs[:, 0] = wrt_torso_traj_proj(ht_joint.rot_matrix, poe_axes)
ht_contribs[:, 1] = wrt_torso_traj_proj(ht_joint.rot_matrix, elev_axes)
ht_contribs[:, 2] = wrt_torso_traj_proj(ht_joint.rot_matrix, axial_axes)
ht_ang_vel_mag = np.sqrt(extended_dot(ht_joint.ang_vel, ht_joint.ang_vel))
ht_ang_vel_dir = ht_joint.ang_vel / ht_ang_vel_mag[..., np.newaxis]
ht_ang_vel_mag = np.where(
extended_dot(ht_ang_vel_dir, elev_axes) > 0, ht_ang_vel_mag,
-ht_ang_vel_mag)
ht_contribs[:, 3] = cumtrapz(ht_ang_vel_mag, dx=ht_joint.dt, initial=0)
gh_contribs = np.empty((gh_joint.ang_vel.shape[0], 4))
gh_contribs[:, 0] = gh_traj_proj(gh_joint.rot_matrix, st_joint.rot_matrix,
poe_axes)
gh_contribs[:, 1] = gh_traj_proj(gh_joint.rot_matrix, st_joint.rot_matrix,
elev_axes)
gh_contribs[:, 2] = gh_traj_proj(gh_joint.rot_matrix, st_joint.rot_matrix,
axial_axes)
gh_ang_vel_mag = np.sqrt(extended_dot(gh_joint.ang_vel, gh_joint.ang_vel))
gh_ang_vel_dir = (
np.squeeze(st_joint.rot_matrix @ gh_joint.ang_vel[..., np.newaxis]) /
ht_ang_vel_mag[..., np.newaxis])
gh_ang_vel_mag = np.where(
extended_dot(gh_ang_vel_dir, elev_axes) > 0, gh_ang_vel_mag,
-gh_ang_vel_mag)
gh_contribs[:, 3] = cumtrapz(gh_ang_vel_mag, dx=gh_joint.dt, initial=0)
return ht_contribs, st_contribs, gh_contribs
# logging
fileConfig(config_dir / 'logging.ini', disable_existing_loggers=False)
log = logging.getLogger(params.logger_name)
prepare_db(db,
params.torso_def,
params.scap_lateral,
params.dtheta_fine,
params.dtheta_coarse, [params.min_elev, params.max_elev],
should_fill=False,
should_clean=False)
trial = db.loc[params.trial_name]
#%%
# calculate st true axial rotation "manually"
st_angvel_proj = extended_dot(trial.st.ang_vel, trial.ht.rot_matrix[:, :,
1])
st_axial_rot_manual = cumtrapz(st_angvel_proj, dx=trial.st.dt, initial=0)
st_axial_rot = trial.ht.true_axial_rot - trial.gh.true_axial_rot
# plot
start_idx = trial.up_down_analysis.max_run_up_start_idx
end_idx = trial.up_down_analysis.max_run_up_end_idx
plt.close('all')
init_graphing(params.backend)
fig = plt.figure()
ax = fig.subplots()
ax.plot(np.rad2deg(st_axial_rot[start_idx:end_idx + 1]), label='HT-GH')
ax.plot(np.rad2deg(st_axial_rot_manual[start_idx:end_idx + 1]),
label='Manual',
ls='--')
ax.legend(loc='upper left')
tracking_markers = np.stack([
trial.filled[marker] for marker in StaticTorsoSegment.TRACKING_MARKERS
], 0)
torso_vicon = compute_trajectory(
trial.subject.torso.static_markers_intrinsic, tracking_markers)
torso_fluoro = change_cs(trial.subject.f_t_v, torso_vicon)
torso_truncated = torso_fluoro[trial.vicon_endpts[0]:trial.vicon_endpts[1]]
quat = quaternion.as_float_array(
quaternion.from_rotation_matrix(torso_truncated[:, :3, :3],
nonorthogonal=False))
torso_pos_quat = np.concatenate((torso_truncated[:, :3, 3], quat), 1)
# retrieve reference data
ref_data = pd.read_csv(
Path(params.matlab_threejs_dir) / subject_id /
(params.trial_name + '.csv'))
# since the negative of a quaternions represents the same orientation make the appropriate sign flip so graphs align
mask = extended_dot(torso_pos_quat[:, 3:],
ref_data.iloc[:, 3:7].to_numpy()) < 0
torso_pos_quat[mask, 3:] = -torso_pos_quat[mask, 3:]
# graph
init_graphing()
for n in range(7):
plt.figure(n + 1)
plt.plot(torso_pos_quat[:, n], 'r')
plt.plot(ref_data.iloc[:, n], 'g')
make_interactive()
plt.show()
ax_st = fig_st.subplots()
ax_st.xaxis.set_major_locator(plticker.MultipleLocator(base=10.0))
plot_utils.style_axes(ax_st, 'HT Elevation Angle', 'Alignment (deg)')
for trial_name, df_row in activity_df.iterrows():
age_grp = df_row['age_group']
ht = df_row['ht']
st = df_row['st']
gh = df_row['gh']
up_down = df_row['up_down_analysis']
start_idx = up_down.max_run_up_start_idx
end_idx = up_down.max_run_up_end_idx
sec = slice(start_idx, end_idx + 1)
ht_elev = -np.rad2deg(ht.euler.ht_isb[sec, 1])
elev_axis = compute_axial_axis(ht.rot_matrix)[sec]
st_ang_vel_axis = st.ang_vel[sec] / np.sqrt(extended_dot(st.ang_vel[sec], st.ang_vel[sec])[..., np.newaxis])
gh_ang_vel_axis = gh.ang_vel[sec] / np.sqrt(extended_dot(gh.ang_vel[sec], gh.ang_vel[sec])[..., np.newaxis])
gh_ang_vel_axis = np.squeeze(st.rot_matrix[sec] @ gh_ang_vel_axis[..., np.newaxis])
ang_st = extended_dot(st_ang_vel_axis, elev_axis)
ang_gh = extended_dot(gh_ang_vel_axis, elev_axis)
ax_gh.plot(ht_elev, ang_gh, label='_'.join(trial_name.split('_')[0:2]))
ax_st.plot(ht_elev, ang_st, label='_'.join(trial_name.split('_')[0:2]))
fig_gh.tight_layout()
fig_gh.subplots_adjust(bottom=0.2)
fig_gh.suptitle(activity + ' GH alignment')
fig_gh.legend(ncol=10, handlelength=0.75, handletextpad=0.25, columnspacing=0.5, loc='lower left',
fontsize=8)
plt.figure(fig_gh.number)
make_interactive()
prepare_db(db, params.torso_def, params.scap_lateral, params.dtheta_fine, params.dtheta_coarse,
[params.min_elev, params.max_elev], should_fill=False, should_clean=False)
trial = db.loc[params.trial_name]
#%%
end_idx = trial.up_down_analysis.max_run_up_end_idx
keep_every_vec = np.arange(6) + 1
indices = np.arange(end_idx+1)
gh_axial_rots = np.zeros(keep_every_vec.size)
for keep_every_idx, keep_every in enumerate(keep_every_vec):
indices_cur = indices[0::keep_every]
if indices_cur[-1] != indices[-1]:
indices_cur = np.concatenate((indices[0::keep_every], indices[-1][..., np.newaxis]))
time = trial.ht.frame_nums[indices_cur] * trial.ht.dt
gh_angvel_proj = extended_dot(np.squeeze(trial.st.rot_matrix[indices_cur] @
trial.gh.ang_vel[indices_cur][..., np.newaxis]),
trial.ht.rot_matrix[indices_cur, :, 1])
gh_axial_rot = cumtrapz(gh_angvel_proj, time, initial=0)
gh_axial_rots[keep_every_idx] = gh_axial_rot[-1]
# plot
plt.close('all')
init_graphing(params.backend)
fig = plt.figure()
ax = fig.subplots()
ax.plot(keep_every_vec, np.rad2deg(gh_axial_rots))
make_interactive()
plt.show()
def true_axial_rot_ang_vel(traj):
ang_vel_proj = extended_dot(traj.ang_vel, traj.rot_matrix[:, :, 1])
return cumtrapz(ang_vel_proj, dx=traj.dt, initial=0)
