-
Notifications
You must be signed in to change notification settings - Fork 56
/
Copy pathbodykin_from_mot_osim.py
174 lines (144 loc) · 7.14 KB
/
bodykin_from_mot_osim.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
#!/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 yup to Blender's zup unless you set direction = 'yup'
Beware, it can be quite slow depending on the ccomplexity
of the model and on the number of frames.
Usage:
from Pose2Sim.Utilities import bodykin_from_mot_osim; bodykin_from_mot_osim.bodykin_from_mot_osim_func(r'<input_mot_file>', r'<output_osim_file>', r'<output_csv_file>')
python -m bodykin_from_mot_osim -m input_mot_file -o input_osim_file
python -m bodykin_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' # 'zup' or 'yup'
## AUTHORSHIP INFORMATION
__author__ = "David Pagnon, Jonathan Camargo"
__copyright__ = "Copyright 2023, BlendOSim & Sim2Blend"
__credits__ = ["David Pagnon", "Jonathan Camargo"]
__license__ = "MIT License"
__version__ = "0.9.4"
__maintainer__ = "David Pagnon"
__email__ = "[email protected]"
__status__ = "Development"
def bodykin_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 bodykin_from_mot_osim; bodykin_from_mot_osim.bodykin_from_mot_osim_func(r'<input_mot_file>', r'<output_osim_file>', r'<output_csv_file>')
python -m bodykin_from_mot_osim -m input_mot_file -o input_osim_file
python -m bodykin_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(osim_path)
motion_data = osim.TimeSeriesTable(motion_path)
# # Degrees or radians
# with open(motion_path) as m_p:
# while True:
# line = m_p.readline()
# if 'inDegrees' in line:
# break
# if 'yes' in line:
# in_degrees = True
# else:
# in_degrees = False
# Model: get model coordinates and bodies
model_coordSet = model.getCoordinateSet()
# coordinates = [model_coordSet.get(i) for i in range(model_coordSet.getSize())]
# coordinates = [c for c in coordinates if '_beta' not in c.getName()]
# coordinateNames = [c.getName() for c in coordinates]
coordinateNames = motion_data.getColumnLabels()
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, c in enumerate(coordinateNames):
if model_coordSet.get(c).getMotionType() == 1: # 1: rotation, 2: translation, 3: coupled
if motion_data.getTableMetaDataAsString('inDegrees') == 'yes':
# if in_degrees:
# 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]])
print('Time frame:')
for n in range(motion_data.getNumRows()):
print(times[n], 's')
# Set model struct in each time state
for c, coord in enumerate(coordinateNames): ## PROBLEME QUAND HEADERS DE MOTION_DATA_NP ET COORDINATENAMES SONT PAS DANS LE MEME ORDRE
try:
model.getCoordinateSet().get(coord).setValue(state, motion_data_np[n,c], enforceContraints=False)
except:
pass
# model.assemble(state)
model.realizePosition(state) # much faster (IK already done, no need to compute it again)
# 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())
bodykin_from_mot_osim_func(args)