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)