Sts analysis

Examples/example_sts_analysis.py

@@ -0,0 +1,137 @@
+'''
+    ---------------------------------------------------------------------------
+    OpenCap processing: example_sts_analysis.py
+    ---------------------------------------------------------------------------
+    Copyright TBD
+
+    Author(s): Scott Uhlrich, RD Magruder
+
+    Licensed under the Apache License, Version 2.0 (the "License"); you may not
+    use this file except in compliance with the License. You may obtain a copy
+    of the License at http://www.apache.org/licenses/LICENSE-2.0
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+
+    Please contact us for any questions: https://www.opencap.ai/#contact
+
+    This example shows how to run a kinematic analysis of sit-to-stand data. It also
+    includes optional inverse dynamics, and you can compute scalar metrics.
+
+'''
+
+
+import os
+import sys
+sys.path.append("../")
+sys.path.append("../ActivityAnalyses")
+from ActivityAnalyses.sts_analysis import sts_analysis
+from utils import get_trial_id, download_trial
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+baseDir = os.path.join(os.getcwd(), '..')
+
+# %% Paths.
+dataFolder = os.path.join(baseDir, 'Data')
+
+# %% User-defined variables.
+session_id = 'f6d66b1a-ce0b-4c38-b6ef-eb48e8a4b49d'
+# trial_name = 'sts_normal' # Normal sit-to-stand; repetitions 3 and 4 are good for this trial
+trial_name = 'sts_lean' # Lean forward sit-to-stand
+# trial_name = 'sts_lean2' # Lean forward sit-to-stand
+# trial_name = 'sts_momentum' # Sit-to-stand with momentum
+
+# Select how many sit-to-stand cycles you'd like to analyze. Select -1 for all
+# sit-to-stand cycles detected in the trial.
+n_sts_cycles = -1
+
+# Select lowpass filter frequency for kinematics data.
+filter_frequency = 6
+
+# Select whether you want to run inverse dynamics, and select settings.
+run_inverse_dynamics = False
+case ='0' # Change this to compare across settings.
+motion_type = 'sit_to_stand'
+repetition = -1 # Select -1 for all repetitions
+
+scalar_names = {
+    'rise_time', 'sts_time', # Commonly reported metrics for sit-to-stand
+    'torso_orientation', 'torso_angular_velocity', # Torso kinematics PRIOR to liftoff; using world frame
+    'torso_orientation_liftoff', 'torso_angular_velocity_liftoff', # Torso kinematics AT liftoff; using world frame
+}
+
+# %% Download trial.
+trial_id = get_trial_id(session_id, trial_name)
+
+sessionDir = os.path.join(dataFolder, session_id)
+
+trialName = download_trial(trial_id, sessionDir, session_id=session_id)
+
+# %% Run sit-to-stand analysis.
+sts = sts_analysis(sessionDir, trialName, n_sts_cycles=n_sts_cycles,
+                   lowpass_cutoff_frequency_for_coordinate_values=filter_frequency)
+
+if run_inverse_dynamics:
+    sts_kinetics = sts.run_dynamic_simulation(baseDir, dataFolder, session_id, trial_name, case=case, repetition=repetition, verbose=True)
+
+# %% Plot sit-to-stand events.
+time = sts.coordinateValues['time']
+pelvis_Sig = sts.coordinateValues['pelvis_ty']
+torso_z = sts.body_angles['torso_z']
+velTorso_z = sts.body_angular_velocity['torso_z']
+
+plt.figure()
+plt.plot(time, pelvis_Sig)
+for c_v, val in enumerate(sts.stsEvents['endRisingIdx']):
+    plt.plot(time[val], pelvis_Sig[val], marker='o', markerfacecolor='k',
+             markeredgecolor='none', linestyle='none',
+             label='End Rising')
+    val2 = sts.stsEvents['startRisingIdx'][c_v]
+    plt.plot(time[val2], pelvis_Sig[val2], marker='o',
+             markerfacecolor='r', markeredgecolor='none',
+             linestyle='none', label='Rising start')
+    val3 = sts.stsEvents['sittingIdx'][c_v]
+    plt.plot(time[val3], pelvis_Sig[val3], marker='o',
+             markerfacecolor='g', markeredgecolor='none',
+             linestyle='none',
+             label='Sitting Time')
+    val4 = sts.stsEvents['forwardLeanIdx'][c_v]
+    plt.plot(time[val4], pelvis_Sig[val4], marker='o',
+             markerfacecolor='b', markeredgecolor='none',
+             linestyle='none', label='Forward Lean Start')
+    if c_v == 0:
+        plt.legend(loc='lower center', bbox_to_anchor=(0.5, -0.40), ncol=2)
+plt.xlabel('Time [s]')
+plt.ylabel('Position [m]')
+plt.title('Vertical pelvis position during sit-to-stand')
+ticks = np.arange(np.floor(np.min(time)), np.ceil(np.max(time)) + 1, 2)
+plt.xticks(ticks)
+plt.tight_layout()
+plt.show()
+
+# %% Display scalars of interest.
+stsResults = {'scalars': sts.compute_scalars(scalar_names)}
+print()
+print("Average STS Results:")
+for key, value in sorted(stsResults['scalars'].items()):
+    # Check if the value is a tuple of 2 values (e.g., left and right)
+    if isinstance(value['value'], tuple):
+        left_value, right_value = value['value']
+        left_value_rounded = round(left_value, 2)
+        right_value_rounded = round(right_value, 2)
+        print(f"{key}: Left = {left_value_rounded} {value['units']}, Right = {right_value_rounded} {value['units']}")
+    else:
+        rounded_value = round(value['value'], 2)
+        print(f"{key}: {rounded_value} {value['units']}")
+
+print()
+print("STS Results per cycle:")
+stsResults_cycles = sts.compute_scalars(scalar_names, return_all=True)
+for key, values in sorted(stsResults_cycles.items()):
+    rounded_values = [round(value, 4) for value in values['value']]
+    print(f"{key} ({values['units']}): {rounded_values}")
+

OpenSimPipeline/JointReaction/computeJointLoading.py

@@ -75,7 +75,7 @@ def computeKAM(pathGenericTemplates, outputDir, modelPath, IDPath, IKPath,
         pController.setName(coord.getName() + '_controller') 
         pController.addActuator(newActuator)
         # Attach the function to the controller.
-        pController.prescribeControlForActuator(0,constFxn) 
+        pController.prescribeControlForActuator(newActuator.getName(),constFxn)
         model.addController(pController) 
 
     # Get controler set.
@@ -373,7 +373,7 @@ def computeMCF(pathGenericTemplates, outputDir, modelPath, activationsPath,
                 pController.setName(coordName + '_controller') 
                 pController.addActuator(newActuator) 
                 # Attach the function to the controller.
-                pController.prescribeControlForActuator(0,constFxn) 
+                pController.prescribeControlForActuator(newActuator.getName(),constFxn)
                 model.addController(pController) 
 
         controllerSet = model.getControllerSet()

UtilsDynamicSimulations/OpenSimAD/mainOpenSimAD.py

@@ -2194,6 +2194,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0',
             numpy_to_storage(labels, data, os.path.join(
                 pathResults, 'kinetics_{}_{}.mot'.format(trialName, case)),
                 datatype='ID')
+            torque_labels = labels[1:]
             # Grounds reaction forces (per sphere).
             labels = ['time']                    
             data = np.zeros((tgridf.T[:-1].shape[0], 1+nContactSpheres*9))
@@ -2696,7 +2697,8 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0',
             'muscles': bothSidesMuscles,
             'passive_limit_torques': pT_opt,
             'muscle_driven_joints': muscleDrivenJoints,
-            'limit_torques_joints': passiveTorqueJoints}             
+            'limit_torques_joints': passiveTorqueJoints,
+            'torque_labels': torque_labels}
         if computeKAM:
             optimaltrajectories[case]['KAM'] = KAM
             optimaltrajectories[case]['KAM_BWht'] = KAM_BWht

UtilsDynamicSimulations/OpenSimAD/utilsOpenSimAD.py

@@ -2335,7 +2335,7 @@ def processInputsOpenSimAD(baseDir, dataFolder, session_id, trial_name,
         if motion_type == 'squats':
             times_window = segment_squats(pathMotionFile, visualize=True)
         elif motion_type == 'sit_to_stand':
-            _, _, times_window = segment_STS(pathMotionFile, visualize=True)
+            times_window, _ = segment_STS(session_id, trial_name, dataFolder)
         time_window = times_window[repetition]
         settings['repetition'] = repetition
     else:

utilsKinematics.py

@@ -445,49 +445,69 @@ def get_center_of_mass_accelerations(self, lowpass_cutoff_frequency=-1):
 
         return com_accelerations 
 
-    def get_body_angular_velocity(self, body_names=None, lowpass_cutoff_frequency=-1,
-                                  expressed_in='body'):
-
+    def get_body_orientation(self, body_names=None, lowpass_cutoff_frequency=-1,
+                           expressed_in='body'):
         body_set = self.model.getBodySet()
         if body_names is None:
             body_names = []
             for i in range(body_set.getSize()):
-                print(i)
                 body = body_set.get(i)
                 body_names.append(body.getName())
-        
-        bodies = [body_set.get(body_name) for body_name in body_names]           
+
+        bodies = [body_set.get(body_name) for body_name in body_names]
         ground = self.model.getGround()
 
-        angular_velocity = np.ndarray((self.table.getNumRows(),
-                              len(body_names)*3)) # time x bodies x dim
-
-        for i_time in range(self.table.getNumRows()): # loop over time
+        angular_position = np.ndarray((self.table.getNumRows(),
+                              len(body_names)*3))
+
+        # Body orientation using atan2
+        for i_time in range(self.table.getNumRows()):  # Loop over time
             state = self.stateTrajectory()[i_time]
-            self.model.realizeVelocity(state)
-
-
-            for i_body,body in enumerate(bodies):
-                ang_vel_in_ground = body.getAngularVelocityInGround(state)
+            self.model.realizePosition(state)
+
+            for i_body, body in enumerate(bodies):
+                R = body.getTransformInGround(state).R()  # Get rotation matrix
+                roll = np.arctan2(R.get(2, 1), R.get(2, 2))  # atan2(R21, R22)
+                pitch = np.arctan2(-R.get(2, 0),
+                                   np.sqrt(R.get(2, 1) ** 2 + R.get(2, 2) ** 2))  # atan2(-R20, sqrt(R21^2 + R22^2))
+                yaw = np.arctan2(R.get(1, 0), R.get(0, 0))  # atan2(R10, R00)
+
                 if expressed_in == 'body':
-                    angular_velocity[i_time, i_body*3:i_body*3+3] = ground.expressVectorInAnotherFrame(
-                                                          state, ang_vel_in_ground, body
-                                                          ).to_numpy()
+                    angular_position[i_time, i_body * 3:i_body * 3 + 3] = ground.expressVectorInAnotherFrame(
+                        state, opensim.osim.Vec3(roll, pitch, yaw), body
+                    ).to_numpy()
                 elif expressed_in == 'ground':
-                    angular_velocity[i_time, i_body*3:i_body*3+3] = ang_vel_in_ground.to_numpy()
+                    angular_position[i_time, i_body * 3:i_body * 3 + 3] = np.array([roll, pitch, yaw])
                 else:
-                    raise Exception (expressed_in + ' is not a valid frame to express angular' + 
-                                     ' velocity.')
-
-        angular_velocity_filtered = lowPassFilter(self.time, angular_velocity, lowpass_cutoff_frequency)
-
+                    raise Exception(f"{expressed_in} is not a valid frame to express angular position.")
+
+        if lowpass_cutoff_frequency > 0:
+            angular_position_filtered = lowPassFilter(self.time, angular_position, lowpass_cutoff_frequency)
+        else:
+            angular_position_filtered = angular_position
+
         # Put into a dataframe
-        data = np.concatenate((np.expand_dims(self.time, axis=1), angular_velocity_filtered), axis=1)
+        data = np.concatenate((np.expand_dims(self.time, axis=1), angular_position_filtered), axis=1)
         columns = ['time']
         for i, body_name in enumerate(body_names):
             columns += [f'{body_name}_x', f'{body_name}_y', f'{body_name}_z']
-        angular_velocity_df = pd.DataFrame(data=data, columns=columns)
-
+        angular_position_df = pd.DataFrame(data=data, columns=columns)
+
+        return angular_position_df
+
+    def get_body_angular_velocity(self, body_names=None, lowpass_cutoff_frequency=-1, expressed_in='body'):
+        angular_position_df = self.get_body_orientation(body_names=body_names, lowpass_cutoff_frequency=lowpass_cutoff_frequency, expressed_in=expressed_in)
+
+        time = angular_position_df['time'].to_numpy()
+        angular_position = angular_position_df.iloc[:, 1:].to_numpy()  # Exclude time column
+
+        dt = np.diff(time)[:, None]  # Time step differences, shape (N-1, 1)
+        angular_velocity = np.diff(angular_position, axis=0) / dt
+        angular_velocity = np.vstack((np.zeros((1, angular_velocity.shape[1])), angular_velocity))
+
+        # Filtering commented out since angular_position includes filtering
+        angular_velocity_df = pd.DataFrame(data=np.column_stack((time, angular_velocity)),columns=angular_position_df.columns)
+
         return angular_velocity_df
 
     def get_ranges_of_motion(self, in_degrees=True, lowpass_cutoff_frequency=-1):