Add code to compute joint angles

compute_jcs.py

@@ -0,0 +1,141 @@
+""" 
+  compute_jcs.py
+
+  Usage: python compute_jcs.py <scs_dir> <subject_id> <wbct_to_xct_mc1_tfm> <wbct_to_xct_trp_tfm>
+
+  Writes to output files: outputs/transformed_<motion>/volumes/<bone>_cs_tfm_<subject_id>_<volume>.csv
+  where <motion> is one of ['abad', 'key', 'opp'], bone is one of ['mc1', "trp"]
+
+  Intermediate coordinate systems csvs are also written to outputs
+
+  Last Modified: Oct 2025, Erica Baldesarra
+
+  Note: the code relies on the folder structure of the DYNACT2/models directory in the 
+        Manske Lab network drive. If this changes, appropriate code changes will need to follow for this
+        code to work.
+  This code is designed to work on either Windows or MacOS systems.
+"""
+
+#example usage (MacOS):
+""" python compute_jcs.py "outputs/cs_result_csvs" 200 "/DYNACT2/models/DYNACT2_200/DYNACT2_200_MC1_WBCT_TO_XCT_REG.tfm" "DYNACT2/models/DYNACT2_200/DYNACT2_200_TRP_WBCT_TO_XCT_REG.tfm"
+"""
+
+import argparse
+import os
+import platform
+import SimpleITK as sitk
+import numpy as np
+import pandas as pd
+import pyvista as pv
+from tmc_jcs import sitk_to_pyvista_surface, save_coordinate_system_to_csv
+from visualize_bone import plot_bone
+
+# global constants
+bone_ids = ['TRP', 'MC1']
+
+def transform_scs(transform_path, scs, output_csv_path, inverse = False):
+  """
+  Transforms a set of coordinate system points using a given transform.
+  transform_path: path to transform file
+  scs: [[saddle] [x_axis] [y_axis] [z_axis]]
+  output_csv_path: path to scs output csv file
+  inverse: if True, applies the inverse of the transform. default = False
+
+  Returns: transformed scs
+  """
+  # work with copies of data to keep original scs unchanged, or in case of pre-processing needs
+  tfm_scs = np.copy(scs)
+
+  # todo: error-check tfm file path
+  transform = sitk.ReadTransform(transform_path)
+  if inverse:
+    transform.SetInverse()
+
+  # transform saddle point
+  tfm_scs_int = np.copy(tfm_scs)
+  tfm_scs_int[0] = np.array(transform.TransformPoint(tfm_scs[0]))
+
+  #transform axes as vectors
+  tfm_scs_int[1] = transform.TransformVector(tfm_scs[1], tfm_scs_int[0])
+  tfm_scs_int[2] = transform.TransformVector(tfm_scs[2], tfm_scs_int[0])
+  tfm_scs_int[3] = transform.TransformVector(tfm_scs[3], tfm_scs_int[0])
+
+  # create copy for output (todo: remove extra copy, used previously for postprocessing)
+  tfm_scs_final = np.copy(tfm_scs_int)
+
+  save_coordinate_system_to_csv(output_csv_path, tfm_scs_final[0], tfm_scs_final[1], tfm_scs_final[2], tfm_scs_final[3])
+
+  return tfm_scs_final
+
+def transform_to_motion(image_data_path, motion: str, mc1_scs, bone) -> None:
+  """ Transforms the coordinate systems to the first frame of the specified motion
+
+    image_data_path: path to image data (manskelab on ResearchFS)
+    motion: str, one of ['abad', 'key', 'opp']
+    scs: [[saddle] [x_axis] [y_axis] [z_axis]]
+    bone: current bone for coordinate system
+
+    prints to outputs folder in current directory
+  """
+
+  wbct_to_motion_tfm = os.path.join(image_data_path, f"DYNACT2_{args.subject_id}/DYNACT2_{args.subject_id}_WBCT/DYNACT2_{args.subject_id}_{bone.upper()}_WBCT_TO_{motion.upper()}_REG.tfm")
+
+  bone_to_motion_cs_path = f"outputs/transformed_{motion.lower()}/{bone.lower()}_cs_tfm_{args.subject_id}.csv"
+  scs_motion_start = transform_scs(wbct_to_motion_tfm, mc1_scs, bone_to_motion_cs_path, True)
+
+
+  for frame in range(2, 61):
+    motion_to_volume_tfm = os.path.join(image_data_path, f"DYNACT2_{args.subject_id}/DYNACT2_{args.subject_id}_{motion.upper()}/REGISTRATION/FinalTFMs/VOLUME_1_TO_{frame}_{bone.upper()}_REG.tfm")
+
+    bone_to_volume_cs_path = f"outputs/transformed_{motion.lower()}/volumes/{bone.lower()}_cs_tfm_{args.subject_id}_{frame}.csv"
+    scs_volume = transform_scs(motion_to_volume_tfm, scs_motion_start, bone_to_volume_cs_path, True)
+
+  return
+
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(
+      description="Compute and export joint coordinate systems for MC1 and TRP based on Halilaj, et al. 2013."
+  )
+  # note: bone coordinate system scs_dir is the output from tmc_jcs.py on xct images
+  parser.add_argument("scs_dir", help="Path to scs directory")
+  parser.add_argument("subject_id", help="Subject ID")
+  # todo: can change from argument to inline, as long as consistent folder structure
+  parser.add_argument("wbct_to_xct_mc1", help="Path to wbct to xct mc1 transform")
+  parser.add_argument("wbct_to_xct_trp", help="Path to wbct to xct trp transform")
+  parser.add_argument("--debug", default=False)
+
+  args = parser.parse_args()
+
+  # debug statements for image information. 
+  if args.debug:
+    print(sitk.ReadImage(args.mc1_bone).GetDirection())
+    print(sitk.ReadImage(args.mc1_bone).GetSpacing())
+    print(sitk.ReadImage(args.mc1_bone).GetOrigin())
+
+  # transform to dynact frame 1 
+  manskelab_dir = ''
+  if platform.system() == 'Windows': 
+    manskelab_dir = "Y:/DYNACT2/models"
+  elif platform.system() == "Darwin":
+    manskelab_dir = "/Volumes/ManskeLab/DYNACT2/models"
+
+  for bone in bone_ids:
+    # 1 transform to wbct
+    scs_path = os.path.join(args.scs_dir, f"{bone.lower()}_coordinate_system_{args.subject_id}.csv")
+
+    with open(scs_path, 'r') as f:
+      xct_scs = pd.read_csv(f, usecols = [1, 2, 3]).values
+
+    # do not use inverse here, tfm from fixed to moving (xct to wbct)
+    cs_wbct = transform_scs(args.wbct_to_xct_mc1, xct_scs, False)
+    cs_to_wbct_cs_path = f"outputs/transformed_wbct/{bone.lower()}_cs_tfm_{args.subject_id}.csv"
+
+    save_coordinate_system_to_csv(cs_to_wbct_cs_path, cs_wbct[0], cs_wbct[1], cs_wbct[2], cs_wbct[3])
+
+    # transform through motion frames
+    for motion in ['abad', 'key', 'opp']:
+      transform_to_motion(manskelab_dir, motion, cs_wbct, bone)
+
+  exit(0)
+

compute_motion.py

@@ -0,0 +1,151 @@
+""" compute_motion.py
+
+    Uses the computed scs csv files from compute_jcs.py to compute motion angles through dynamic frames
+
+    Usage: python compute_motion.py <subject_id>
+
+    Writes to outputs/motions/<subject_id>_<motion>_angles.csv
+
+    Note: relies on file structure defined in compute_jcs.py.
+
+    Last Modified: Oct 2025, Erica Baldesarra
+"""
+import argparse
+import numpy as np
+import os
+from utils import read_scs_csv, compute_jcs
+
+
+def compute_joint_translation(mc1_saddle: np.ndarray, trp_saddle: np.ndarray) -> np.ndarray:
+  """ Computes the translation of the TRP and MC1 relative to each other based on 
+
+    'Translation of the metacarpal with respect to the trapezium is defined as
+      the translation of the MC1 SCS origin with respect to the TPM SCS.'
+  """
+  return mc1_saddle - trp_saddle
+
+"""
+    Function to compute joint rotations
+    From compute_motion.py in ManskeLab/DYNACT2, chris branch:
+    https://github.com/ManskeLab/DYNACT2/blob/chris/dynact2/compute_motion.py
+
+    alpha (a) = abduction-adduction angle (X_MC1)
+    beta  (b) = axial rotation (Floating axis)
+    gama  (g) = flexion-extension (Z_TRP)
+
+    Relative rotation matrix is assumed to have the following form:
+    Rxyz = Rz * Ry * Rx
+    Rxyz = 
+        [ cos(b)cos(g)   (sin(a)sin(b)cos(g) + cos(a)sin(g))   (-cos(a)sin(b)cos(g) + sin(a)sin(g)) ]
+        [ -cos(b)sin(g)  (-sin(a)sin(b)sin(g) + cos(a)cos(g))   (cos(a)sin(b)sin(g) + sin(a)cos(g)) ]
+        [ sin(b)                    -sin(a)cos(b)                         cos(a)cos(b)              ]
+
+    Parameters
+    ----------
+    mc1_scs: MC1 SCS in format: [[saddle], [x_axis], [y-axis], [z-axis]]
+    trp_scs: TRP SCS in format: [[saddle], [x_axis], [y-axis], [z-axis]]
+
+    Returns
+    -------
+    angles : list in Degrees
+"""
+def compute_angles(mc1_scs, trp_scs):
+
+    # Matricies for axes should be formatted as follows:
+    # Rxyz = [Rz  Ry  Rx]
+    # Rxyz = [Zx  Yx  Xx]
+    #        [Zy  Yy  Xy]
+    #        [Zz  Yz  Xz]
+    M_scs_MC1 = np.array([mc1_scs[1].T, mc1_scs[2].T, mc1_scs[3].T])
+    M_scs_TRP = np.array([trp_scs[1].T, trp_scs[2].T, trp_scs[3].T])
+
+    # Calculate angle of MC1 relative to TRP
+    R_relative = np.linalg.inv(M_scs_TRP) * M_scs_MC1
+
+    beta = np.arcsin(R_relative[2, 0])
+    alpha = np.arcsin(-1 * R_relative[2, 1] / np.cos(beta))
+    gama = np.arcsin(-1 * R_relative[1, 0] / np.cos(beta))
+
+    angles = [np.rad2deg(alpha), np.rad2deg(beta), np.rad2deg(gama)]
+    return angles
+
+""" Compute the joint angles for each frame in the specified motion
+
+Args:
+    subject (int | str): subject ID number
+    motion (str): motion type: one of ['abad', 'key', 'opp']
+
+Returns:
+    list: a list with each element containing the frame motion array for the frame at that index (+1)
+"""
+def compute_frame_angles(subject, motion):
+  overall_motion_results = []
+
+  for frame in range(1, 61):
+    # compute motion array with values ["FlexExt", "AbAd", "Rot", "X", "Y", "Z"]
+    frame_motion_results = []
+
+    if frame == 1:
+      mc1_scs_csv = f"outputs/transformed_{motion.lower()}/mc1_cs_tfm_{subject}.csv"
+      trp_scs_csv = f"outputs/transformed_{motion.lower()}/trp_cs_tfm_{subject}.csv"
+    else:
+      mc1_scs_csv = f"outputs/transformed_{motion.lower()}/volumes/mc1_cs_tfm_{subject}_{frame}.csv"
+      trp_scs_csv = f"outputs/transformed_{motion.lower()}/volumes/trp_cs_tfm_{subject}_{frame}.csv"
+
+    mc1_scs = read_scs_csv(mc1_scs_csv)
+    trp_scs = read_scs_csv(trp_scs_csv)
+
+    euler_zyx = compute_jcs(mc1_scs, trp_scs)
+
+    angles = compute_angles(mc1_scs, trp_scs)
+    frame_motion_results.append(angles)
+
+    joint_xyz_translation = compute_joint_translation(mc1_scs[0], trp_scs[0])
+    frame_motion_results.append(joint_xyz_translation)
+    import itertools 
+    # flatten list
+    frame_motion_results = list(itertools.chain.from_iterable(frame_motion_results))
+
+    overall_motion_results.append(frame_motion_results)
+
+  return overall_motion_results
+
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(
+      description="Compute and export joint angles for for MC1 and TRP based on Halilaj, et al. 2013."
+  )
+
+  parser.add_argument("subject_id", help="Subject to compute")
+  args = parser.parse_args()
+
+
+  for motion in ['abad', 'key', 'opp']:
+    # setup motion-wide data
+    # Create an array for the roatation/translantion results
+    header_arr = np.array([["Frame", "AbAd", "Rot", "FlexExt", "X", "Y", "Z"]], dtype=object)
+    frame_arr = np.arange(60).astype(str)
+    frame_arr.shape = (60, 1)
+
+    print(
+        "Computing angles for "
+        + str(args.subject_id) 
+        + "\nMotion: "
+        + motion.upper()
+        + "..."
+    )
+
+    output_arr = frame_arr
+    motion_results = compute_frame_angles(args.subject_id, motion)
+    output_arr = np.hstack([output_arr, motion_results])
+    output_arr = np.vstack([header_arr, output_arr])
+    output_arr = output_arr.astype(str)
+
+    print("Writing out values to CSV...")
+    print()
+
+    output_path = "outputs/motions/"
+    output_csv = os.path.join(output_path, f"{str(args.subject_id)}_{motion.lower()}_angles_deg.csv")
+    np.savetxt(output_csv, output_arr, delimiter=",", fmt="%s")
+
+  exit(0)

plot_angles.py

@@ -0,0 +1,40 @@
+"""
+Plot joint angles per motion type and per manually found ROMs
+"""
+from utils import plot_angles, plot_ROMs, movement_types
+import matplotlib.pyplot as plt
+import argparse
+import pandas as pd
+import os
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(
+                    prog='Plot angles')
+  parser.add_argument('subject', type=int, help='Subject ID num')
+  parser.add_argument('useable_frames_fp', type=str, help='File path to folder with useable frames data')
+  parser.add_argument('angle_dir', type=str, help="path to angle dir")
+  parser.add_argument('--output_dir', type=str, help="path to output dir", default=os.path.join(os.path.curdir, 'outputs', 'angle_plots'))
+
+  args = parser.parse_args()
+  subject_id = int(args.subject)
+  useable_frames = pd.read_excel(args.useable_frames_fp)
+
+  angle_dirs = ['FlexExt', 'AbAd', 'Rot']
+
+  for movement in movement_types:
+    fig, axs = plt.subplots(1, 3)
+    angle_data_fp = os.path.join(args.angle_dir, f"{subject_id}_{movement.lower()}_angles_deg.csv")
+    angle_data = pd.read_csv(angle_data_fp)
+    #print(f"DEBUG: {angle_data}")
+    for index, angle_dir in enumerate(angle_dirs):
+      angles = angle_data[angle_dir].values
+      plot_angles(axs[index], angles, angle_dir)
+    fig.tight_layout()
+    fig.set_figwidth(12)
+    plt.subplots_adjust(wspace=0.3)
+    outfile = os.path.join(args.output_dir, f"{subject_id}_angle_plot_{movement.lower()}")
+    fig.savefig(outfile)
+    #plt.show()
+
+
+

utils.py

@@ -0,0 +1,51 @@
+""" Utility module for the tmc_jcs repository
+
+    last Modified: Sept 2025, Erica Baldesarra
+"""
+
+import numpy as np
+import pandas as pd
+
+movement_types = ['ABAD', 'KEY', 'OPP']
+
+def read_scs_csv(csv_path):
+  with open(csv_path, 'r') as f:
+    scs = pd.read_csv(f, usecols = [1, 2, 3]).values
+
+  return scs
+
+
+def compute_jcs(mc1_scs, trp_scs):
+  """
+    Computes JCS from SCS based on Halilaj, et al. 2013.
+    Requires MC1 and TRP .csv output with saddle points and x, y, z axes for SCS
+    Returns e1, e2, e3 unit vectors for JCS where:
+
+    e1 = z_trp
+    e2 = e1 x e3 ('floating axis')
+    e3 = x_mc1
+  """
+  e1 = trp_scs[3]
+  e3 = mc1_scs[1]
+  e2 = np.cross(e1,e3)
+
+  return [e1, e2, e3]
+
+
+def plot_angles(ax, angles, motion_type, frames = 60):
+  x_values = np.arange(frames)
+  ax.plot(x_values, angles)
+  ax.set_xlabel('Frame')
+  ax.set_ylabel(f'Angle Between MC1 and TRP ({motion_type}, Deg)')
+
+  return ax
+
+def plot_ROMs(plot, values, roms = 2):
+  for rom in range(len(roms)):
+    motion = rom + 1
+    len_motion = roms[rom][1] - roms[rom][0] + 1
+    plot(values[rom], range(len_motion), label=f"Angles through movement {motion}")
+
+  return plot
+
+