From d027d16e3f334af8a27a221519a2c14305159992 Mon Sep 17 00:00:00 2001 From: davidpagnon Date: Wed, 18 Oct 2023 18:53:06 +0200 Subject: [PATCH] mot to csv for Blender --- Pose2Sim/Utilities/csv_from_mot_osim.py | 148 ++++++++++++++++++++++++ README.md | 24 ++-- 2 files changed, 165 insertions(+), 7 deletions(-) create mode 100644 Pose2Sim/Utilities/csv_from_mot_osim.py diff --git a/Pose2Sim/Utilities/csv_from_mot_osim.py b/Pose2Sim/Utilities/csv_from_mot_osim.py new file mode 100644 index 0000000..b757a8d --- /dev/null +++ b/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'', r'', r'') + 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'', r'', r'') + 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) \ No newline at end of file diff --git a/README.md b/README.md index f4e975b..8f2bb45 100644 --- a/README.md +++ b/README.md @@ -708,7 +708,7 @@ A list of standalone tools (see [Utilities](https://github.com/perfanalytics/pos
- Converting files and Calibrating (CLICK TO SHOW) + Converting calibration files (CLICK TO SHOW) 
 
 [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.
@@ -759,7 +762,10 @@ Displays X, Y, Z coordinates of each 3D keypoint of a TRC file in a different ma
Other trc tools (CLICK TO SHOW) 
-    
+
+[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.
+