mot to csv for Blender

Pose2Sim/Utilities/csv_from_mot_osim.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+
+'''
+    ##################################################
+    ## Build csv from mot and osim files            ##
+    ##################################################
+    
+    Build a csv file which stores locations and orientations of all bodies
+    calculated from a .mot motion file and a .osim model file.
+    
+    Equivalent to OpenSim Analysis -> BodyKinematics but without the bugs in 
+    orientations due to their use of Euler angle instead of homography matrices
+
+    Transforms from OpenSim's zup to Blender's yup
+    
+    Usage: 
+    from Pose2Sim.Utilities import csv_from_mot_osim; csv_from_mot_osim.csv_from_mot_osim_func(r'<input_mot_file>', r'<output_osim_file>', r'<output_csv_file>')
+    python -m csv_from_mot_osim -m input_mot_file -o input_osim_file
+    python -m csv_from_mot_osim -m input_mot_file -o input_osim_file -c output_csv_file
+'''
+
+
+## INIT
+import os
+import numpy as np
+import opensim as osim
+import argparse
+
+direction = 'zup'
+
+## AUTHORSHIP INFORMATION
+__author__ = "David Pagnon, Jonathan Camargo"
+__copyright__ = "Copyright 2023, BlendOSim & Sim2Blend"
+__credits__ = ["David Pagnon", "Jonathan Camargo"]
+__license__ = "MIT License"
+__version__ = "0.0.1"
+__maintainer__ = "David Pagnon"
+__email__ = "contact@david-pagnon.com"
+__status__ = "Development"
+
+
+def csv_from_mot_osim_func(*args):
+    '''
+    Build a csv file which stores locations and orientations of all bodies
+    calculated from a .mot motion file and a .osim model file.
+    
+    Equivalent to OpenSim Analysis -> BodyKinematics but without the bugs in 
+    orientations due to their use of Euler angle instead of homography matrices
+    
+    Usage: 
+    from Pose2Sim.Utilities import csv_from_mot_osim; csv_from_mot_osim.csv_from_mot_osim_func(r'<input_mot_file>', r'<output_osim_file>', r'<output_csv_file>')
+    python -m csv_from_mot_osim -m input_mot_file -o input_osim_file
+    python -m csv_from_mot_osim -m input_mot_file -o input_osim_file -t output_csv_file
+    '''
+    
+    try:
+        motion_path = args[0]['input_mot_file'] # invoked with argparse
+        osim_path = args[0]['input_osim_file']
+        if args[0]['csv_output_file'] == None:
+            output_csv_file = motion_path.replace('.mot', '.csv')
+        else:
+            output_csv_file = args[0]['csv_output_file']
+    except:
+        motion_path = args[0] # invoked as a function
+        osim_path = args[1]
+        try:
+            output_csv_file = args[2]
+        except:
+            output_csv_file = motion_path.replace('.mot', '.csv')
+    
+    
+    # Read model and motion files
+    model = osim.Model(input_osim_file)
+    motion_data = osim.TimeSeriesTable(input_mot_file)
+
+    # Model: get model coordinates and bodies
+    model_coordSet = model.getCoordinateSet()
+    coordinates = [model_coordSet.get(i) for i in range(model_coordSet.getSize())]
+    coordinateNames = [c.getName() for c in coordinates]
+    model_bodySet = model.getBodySet()
+    bodies = [model_bodySet.get(i) for i in range(model_bodySet.getSize())]
+    bodyNames = [b.getName() for b in bodies]
+
+    # Motion: read coordinates and convert to radians
+    times = motion_data.getIndependentColumn()
+    motion_data_np = motion_data.getMatrix().to_numpy()
+    for i in range(len(coordinates)):
+        if coordinates[i].getMotionType() == 1: # 1: rotation, 2: translation, 3: coupled
+            motion_data_np[:,i] = motion_data_np[:,i] * np.pi/180 # if rotation, convert to radians
+
+    # Animate model
+    state = model.initSystem()
+    loc_rot_frame_all = []
+    H_zup = np.array([[1,0,0,0], [0,0,-1,0], [0,1,0,0], [0,0,0,1]])
+    for n in range(motion_data.getNumRows()):
+        
+        # Set model struct in each time state
+        for c, coord in enumerate(coordinateNames):
+            model.getCoordinateSet().get(coord).setValue(state, motion_data_np[n,c])
+        
+        # Use state of model to get body coordinates in ground
+        loc_rot_frame = []
+        for b in bodies:
+            H_swig = b.getTransformInGround(state)
+            T = H_swig.T().to_numpy()
+            R_swig = H_swig.R()
+            R = np.array([[R_swig.get(0,0), R_swig.get(0,1), R_swig.get(0,2)],
+                [R_swig.get(1,0), R_swig.get(1,1), R_swig.get(1,2)],
+                [R_swig.get(2,0), R_swig.get(2,1), R_swig.get(2,2)]])
+            H = np.block([ [R,T.reshape(3,1)], [np.zeros(3), 1] ])
+            
+            # y-up to z-up
+            if direction=='zup':
+                H = H_zup @ H
+            
+            # Convert matrix to loc and rot, and export to csv
+            loc_x, loc_y, loc_z = H[0:3,3]
+            R_mat = H[0:3,0:3]
+            sy = np.sqrt(R_mat[1,0]**2 +  R_mat[0,0]**2) # singularity when y angle is +/- pi/2
+            if sy>1e-6:
+                rot_x = np.arctan2(R_mat[2,1], R_mat[2,2])
+                rot_y = np.arctan2(-R_mat[2,0], sy)
+                rot_z = np.arctan2(R_mat[1,0], R_mat[0,0])
+            else: # to be verified
+                rot_x = np.arctan2(-R_mat[1,2], R_mat[1,1])
+                rot_y = np.arctan2(-R[2,0], sy)
+                rot_z = 0
+            loc_rot_frame.extend([loc_x, loc_y, loc_z, rot_x, rot_y, rot_z])
+        
+        loc_rot_frame_all.append(loc_rot_frame)
+
+    # Export to csv
+    loc_rot_frame_all_np = np.array(loc_rot_frame_all)
+    loc_rot_frame_all_np = np.insert(loc_rot_frame_all_np, 0, times, axis=1) # insert time column
+    bodyHeader = 'times, ' + ''.join([f'{b}_x, {b}_y, {b}_z, {b}_rotx, {b}_roty, {b}_rotz, ' for b in bodyNames])[:-2]
+    np.savetxt(os.path.splitext(output_csv_file)[0]+'.csv', loc_rot_frame_all_np, delimiter=',', header=bodyHeader)
+    
+    
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-m', '--input_mot_file', required = True, help='input mot file')
+    parser.add_argument('-o', '--input_osim_file', required = True, help='input osim file')
+    parser.add_argument('-c', '--csv_output_file', required=False, help='csv output file')
+    args = vars(parser.parse_args())
+    
+    csv_from_mot_osim_func(args)

README.md

@@ -708,7 +708,7 @@ A list of standalone tools (see [Utilities](https://github.com/perfanalytics/pos
 
 
 <details>
-  <summary><b>Converting files and Calibrating</b> (CLICK TO SHOW)</summary>
+  <summary><b>Converting calibration files</b> (CLICK TO SHOW)</summary>
     <pre>
 
 [Blazepose_runsave.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/Blazepose_runsave.py)
@@ -720,9 +720,6 @@ Converts a DeepLabCut (h5) 2D pose estimation file into OpenPose (json) files.
 [AlphaPose_to_OpenPose.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/AlphaPose_to_OpenPose.py)
 Converts AlphaPose single json file to OpenPose frame-by-frame files.
 
-[c3d_to_trc.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/c3d_to_trc.py)
-Converts 3D point data of a .c3d file to a .trc file compatible with OpenSim. No analog data (force plates, emg) nor computed data (angles, powers, etc) are retrieved.
-
 [calib_from_checkerboard.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/calib_from_checkerboard.py)
 Calibrates cameras with images or a video of a checkerboard, saves calibration in a Pose2Sim .toml calibration file.
 You should probably use Pose2Sim.calibration() instead, which is much easier and better.
@@ -736,8 +733,14 @@ Converts a Pose2Sim .toml calibration file (e.g., from a checkerboard) to a Qual
 [calib_easymocap_to_toml.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/calib_easymocap_to_toml.py)
 Converts EasyMocap intrinsic and extrinsic .yml calibration files to an OpenCV .toml calibration file.
 
-[calib_easymocap_to_yml.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/calib_toml_to_easymocap.py)
+[calib_toml_to_easymocap.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/calib_toml_to_easymocap.py)
 Converts an OpenCV .toml calibration file to EasyMocap intrinsic and extrinsic .yml calibration files.
+
+[calib_toml_to_opencap.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/calib_toml_to_opencap.py)
+Converts an OpenCV .toml calibration file to OpenCap .pickle calibration files.
+
+[calib_toml_to_opencap.py]( )
+To convert OpenCap calibration tiles to a .toml file, please use Pose2Sim.calibration() and set convert_from = 'opencap' in Config.toml.
    </pre>
 </details>
 
@@ -759,7 +762,10 @@ Displays X, Y, Z coordinates of each 3D keypoint of a TRC file in a different ma
 <details>
   <summary><b>Other trc tools</b> (CLICK TO SHOW)</summary>
     <pre>
-    
+
+[c3d_to_trc.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/c3d_to_trc.py)
+Converts 3D point data of a .c3d file to a .trc file compatible with OpenSim. No analog data (force plates, emg) nor computed data (angles, powers, etc) are retrieved.
+
 [trc_desample.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/trc_desample.py)
 Undersamples a trc file.
 
@@ -767,7 +773,7 @@ Undersamples a trc file.
 Changes Z-up system coordinates to Y-up system coordinates.
 
 [trc_filter.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/trc_filter.py)
-Filters trc files. Available filters: Butterworth, Butterworth on speed, Gaussian, LOESS, Median.
+Filters trc files. Available filters: Butterworth, Kalman, Butterworth on speed, Gaussian, LOESS, Median.
 
 [trc_gaitevents.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/trc_gaitevents.py)
 Detects gait events from point coordinates according to [Zeni et al. (2008)](https://www.sciencedirect.com/science/article/abs/pii/S0966636207001804?via%3Dihub).
@@ -777,6 +783,10 @@ Combine two trc files, for example a triangulated DeepLabCut trc file and a tria
 
 [trc_from_mot_osim.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/trc_from_mot_osim.py)
 Build a trc file from a .mot motion file and a .osim model file.
+
+[csv_from_mot_osim.py](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Utilities/csv_from_mot_osim.py)
+Converts a mot file to a .csv file with rotation and orientation of all segments.
+
    </pre>
 </details>