def test_TRCFileAdapter(self):
adapter = osim.TRCFileAdapter()
table = adapter.read(
os.path.join(test_dir, 'futureOrientationInverseKinematics.trc'))
assert table.getNumRows() == 1202
assert table.getNumColumns() == 2
table = adapter.read(os.path.join(test_dir, 'TRCFileWithNANs.trc'))
assert table.getNumRows() == 5
assert table.getNumColumns() == 14
def to_trc(self, filename):
"""
Write a trc file from a Markers3dOsim
Parameters
----------
filename : string
path of the file to write
"""
filename = Path(filename)
# Make sure the directory exists, otherwise create it
if not filename.parents[0].is_dir():
filename.parents[0].mkdir()
# Make sure the metadata are set
if not self.get_rate:
raise ValueError(
'get_rate is empty. Please fill with `your_variable.get_rate = 100.0` for example'
)
if not self.get_unit:
raise ValueError(
'get_unit is empty. Please fill with `your_variable.get_unit = "mm"` for example'
)
if not self.get_labels:
raise ValueError(
'get_labels is empty. Please fill with `your_variable.get_labels = ["M1", "M2"]` for example'
)
table = osim.TimeSeriesTableVec3()
# set metadata
table.setColumnLabels(self.get_labels)
table.addTableMetaDataString('DataRate', str(self.get_rate))
table.addTableMetaDataString('Units', self.get_unit)
time_vector = np.arange(start=0,
stop=1 / self.get_rate * self.shape[2],
step=1 / self.get_rate)
for iframe in range(self.shape[-1]):
a = np.round(self.get_frame(iframe)[:-1, ...], decimals=4)
row = osim.RowVectorOfVec3([
osim.Vec3(a[0, i], a[1, i], a[2, i])
for i in range(a.shape[-1])
])
table.appendRow(time_vector[iframe], row)
adapter = osim.TRCFileAdapter()
adapter.write(table, str(filename))
def addVirtualMarkersStatic(staticTRC = None, outputTRC = 'static_withVirtualMarkers.trc'):
# Convenience function for adding virtual markers to static trial.
#
# Input: staticTRC - .trc filename for static trial to add markers to
# outputTRC - optional input for filename for output .trc file
#
# Hip joint centres are placed according to the method outlined by
# Harrington et al. (2007), J Biomech, 40: 595-602.
#
# Knee joint centres are placed at the mid point of the femoral epicondyle
# markers.
#
# Pseudo ankle joint centres are placed at the mid points of the malleoli
# markers.
#
# A marker at the mid point of the two metatarsal markers.
#
# Markers around the foot and ankle are projected to the floor to assist
# in aligning the scaling parameters with the axis they are relevant for
# scaling.
#
# Markers at the middle of the set of upper torso and pelvis markers used
# to help scale torso and pelvis length.
#
# Note that the application of this is only relevant to the markerset used
# with this data. Different labelling of markers will require adaptating
# this function to make it work.
#Check for input
if staticTRC is None:
raise ValueError('Input of staticTRC is required')
#Use the Vec3 TimeSeriesTable to read the Vec3 type data file.
staticTable = osim.TimeSeriesTableVec3(staticTRC)
#Convert to more easily readable object for easier manipulation
#Get number of column labels and data rows
nLabels = staticTable.getNumColumns()
nRows = staticTable.getNumRows()
#Pre-allocate numpy array based on data size
dataArray = np.zeros((nRows,nLabels*3))
#Loop through labels and rows and get data
for iLabel in range(0,nLabels):
#Get current column label
currLabel = staticTable.getColumnLabel(iLabel)
for iRow in range(0,nRows):
dataArray[iRow,iLabel*3] = staticTable.getDependentColumn(currLabel).getElt(0,iRow).get(0)
dataArray[iRow,iLabel*3+1] = staticTable.getDependentColumn(currLabel).getElt(0,iRow).get(1)
dataArray[iRow,iLabel*3+2] = staticTable.getDependentColumn(currLabel).getElt(0,iRow).get(2)
#Convert numpy array to pandas dataframe and add column labels
#Get column labels
colLabels = list()
for iLabel in range(0,nLabels):
colLabels.append(staticTable.getColumnLabel(iLabel)+'_x')
colLabels.append(staticTable.getColumnLabel(iLabel)+'_y')
colLabels.append(staticTable.getColumnLabel(iLabel)+'_z')
#Convert to dataframe
static_df = pd.DataFrame(data = dataArray,columns=colLabels)
#Get the pelvis marker data
#In this step we convert back to the traditional Vicon coordinate system
#and millimetres. It was easier to do this than mess around with the hip
#joint centre calculations
RASIS = np.zeros((nRows,3))
RASIS[:,0] = static_df['RASI_z']*1000
RASIS[:,1] = static_df['RASI_x']*1000
RASIS[:,2] = static_df['RASI_y']*1000
LASIS = np.zeros((nRows,3))
LASIS[:,0] = static_df['LASI_z']*1000
LASIS[:,1] = static_df['LASI_x']*1000
LASIS[:,2] = static_df['LASI_y']*1000
RPSIS = np.zeros((nRows,3))
RPSIS[:,0] = static_df['RPSI_z']*1000
RPSIS[:,1] = static_df['RPSI_x']*1000
RPSIS[:,2] = static_df['RPSI_y']*1000
LPSIS = np.zeros((nRows,3))
LPSIS[:,0] = static_df['LPSI_z']*1000
LPSIS[:,1] = static_df['LPSI_x']*1000
LPSIS[:,2] = static_df['LPSI_y']*1000
#Calculate hip joint centre at each time step
#Pre-allocate size
RHJC = np.zeros((nRows,3))
LHJC = np.zeros((nRows,3))
#Loop through sample points
for t in range(0,nRows):
#Right handed pelvis reference system definition
SACRUM = (RPSIS[t,:] + LPSIS[t,:]) / 2
#Global Pelvis Center position
OP = (LASIS[t,:] + RASIS[t,:]) / 2
PROVV = (RASIS[t,:] - SACRUM) / np.linalg.norm(RASIS[t,:] - SACRUM,2)
IB = (RASIS[t,:] - LASIS[t,:]) / np.linalg.norm(RASIS[t,:] - LASIS[t,:],2)
KB = np.cross(IB,PROVV)
KB = KB / np.linalg.norm(KB,2)
JB = np.cross(KB,IB)
JB = JB / np.linalg.norm(JB,2)
OB = OP
#rotation+ traslation in homogeneous coordinates (4x4)
addPelvis = np.array([0,0,0,1])
pelvis = np.hstack((IB.reshape(3,1),
JB.reshape(3,1),
KB.reshape(3,1),
OB.reshape(3,1)))
pelvis = np.vstack((pelvis,addPelvis))
#Trasformation into pelvis coordinate system (CS)
OPB = np.linalg.inv(pelvis) @ np.vstack((OB.reshape(3,1),np.array([1])))
PW = np.linalg.norm(RASIS[t,:] - LASIS[t,:])
PD = np.linalg.norm(SACRUM - OP)
#Harrington formulae (starting from pelvis center)
diff_ap = -0.24 * PD - 9.9
diff_v = -0.30 * PW - 10.9
diff_ml = 0.33 * PW + 7.3
#vector that must be subtract to OP to obtain hjc in pelvis CS
vett_diff_pelvis_sx = np.array([-diff_ml,diff_ap,diff_v,1])
vett_diff_pelvis_dx = np.array([diff_ml,diff_ap,diff_v,1])
#hjc in pelvis CS (4x4)
rhjc_pelvis = OPB[:,0] + vett_diff_pelvis_dx
lhjc_pelvis = OPB[:,0] + vett_diff_pelvis_sx
#Transformation Local to Global
RHJC[t,:] = pelvis[0:3,0:3] @ rhjc_pelvis[0:3] + OB
LHJC[t,:] = pelvis[0:3,0:3] @ lhjc_pelvis[0:3] + OB
#Add to data frame
#Convert back to appropriate units and coordinate system here too
static_df['RHJC_x'] = RHJC[:,1] / 1000
static_df['RHJC_y'] = RHJC[:,2] / 1000
static_df['RHJC_z'] = RHJC[:,0] / 1000
static_df['LHJC_x'] = LHJC[:,1] / 1000
static_df['LHJC_y'] = LHJC[:,2] / 1000
static_df['LHJC_z'] = LHJC[:,0] / 1000
#Create mid point markers
#Mid epicondyles
static_df['RKJC_x'] = (static_df['RLFC_x'] + static_df['RMFC_x']) / 2
static_df['RKJC_y'] = (static_df['RLFC_y'] + static_df['RMFC_y']) / 2
static_df['RKJC_z'] = (static_df['RLFC_z'] + static_df['RMFC_z']) / 2
static_df['LKJC_x'] = (static_df['LLFC_x'] + static_df['LMFC_x']) / 2
static_df['LKJC_y'] = (static_df['LLFC_y'] + static_df['LMFC_y']) / 2
static_df['LKJC_z'] = (static_df['LLFC_z'] + static_df['LMFC_z']) / 2
#Mid malleoli
static_df['RAJC_x'] = (static_df['RLMAL_x'] + static_df['RMMAL_x']) / 2
static_df['RAJC_y'] = (static_df['RLMAL_y'] + static_df['RMMAL_y']) / 2
static_df['RAJC_z'] = (static_df['RLMAL_z'] + static_df['RMMAL_z']) / 2
static_df['LAJC_x'] = (static_df['LLMAL_x'] + static_df['LMMAL_x']) / 2
static_df['LAJC_y'] = (static_df['LLMAL_y'] + static_df['LMMAL_y']) / 2
static_df['LAJC_z'] = (static_df['LLMAL_z'] + static_df['LMMAL_z']) / 2
#Mid metatarsal
static_df['RMT3_x'] = (static_df['RMT1_x'] + static_df['RMT5_x']) / 2
static_df['RMT3_y'] = (static_df['RMT1_y'] + static_df['RMT5_y']) / 2
static_df['RMT3_z'] = (static_df['RMT1_z'] + static_df['RMT5_z']) / 2
static_df['LMT3_x'] = (static_df['LMT1_x'] + static_df['LMT5_x']) / 2
static_df['LMT3_y'] = (static_df['LMT1_y'] + static_df['LMT5_y']) / 2
static_df['LMT3_z'] = (static_df['LMT1_z'] + static_df['LMT5_z']) / 2
#Create projections of foot and ankle markers to floor (i.e. y = 0)
#Heel markers
static_df['fRCAL_x'] = static_df['RCAL_x']
static_df['fRCAL_y'] = np.zeros((len(static_df),1))
static_df['fRCAL_z'] = static_df['RCAL_z']
static_df['fLCAL_x'] = static_df['LCAL_x']
static_df['fLCAL_y'] = np.zeros((len(static_df),1))
static_df['fLCAL_z'] = static_df['LCAL_z']
#Ankle joint centres
static_df['fRAJC_x'] = static_df['RAJC_x']
static_df['fRAJC_y'] = np.zeros((len(static_df),1))
static_df['fRAJC_z'] = static_df['RAJC_z']
static_df['fLAJC_x'] = static_df['LAJC_x']
static_df['fLAJC_y'] = np.zeros((len(static_df),1))
static_df['fLAJC_z'] = static_df['LAJC_z']
#Toe markers
static_df['fRMT1_x'] = static_df['RMT1_x']
static_df['fRMT1_y'] = np.zeros((len(static_df),1))
static_df['fRMT1_z'] = static_df['RMT1_z']
static_df['fLMT1_x'] = static_df['LMT1_x']
static_df['fLMT1_y'] = np.zeros((len(static_df),1))
static_df['fLMT1_z'] = static_df['LMT1_z']
static_df['fRMT5_x'] = static_df['RMT5_x']
static_df['fRMT5_y'] = np.zeros((len(static_df),1))
static_df['fRMT5_z'] = static_df['RMT5_z']
static_df['fLMT5_x'] = static_df['LMT5_x']
static_df['fLMT5_y'] = np.zeros((len(static_df),1))
static_df['fLMT5_z'] = static_df['LMT5_z']
static_df['fRMT3_x'] = static_df['RMT3_x']
static_df['fRMT3_y'] = np.zeros((len(static_df),1))
static_df['fRMT3_z'] = static_df['RMT3_z']
static_df['fLMT3_x'] = static_df['LMT3_x']
static_df['fLMT3_y'] = np.zeros((len(static_df),1))
static_df['fLMT3_z'] = static_df['LMT3_z']
#Extra mid points for torso and pelvis markers
#Mid torso
static_df['MTOR_x'] = (((static_df['RACR_x'] + static_df['LACR_x']) / 2) + ((static_df['C7_x'] + static_df['CLAV_x']) / 2)) / 2
static_df['MTOR_y'] = (((static_df['RACR_y'] + static_df['LACR_y']) / 2) + ((static_df['C7_y'] + static_df['CLAV_y']) / 2)) / 2
static_df['MTOR_z'] = (((static_df['RACR_z'] + static_df['LACR_z']) / 2) + ((static_df['C7_z'] + static_df['CLAV_z']) / 2)) / 2
#Mid pelvis
static_df['MPEL_x'] = (((static_df['RASI_x'] + static_df['LASI_x']) / 2) + ((static_df['RPSI_x'] + static_df['LPSI_x']) / 2)) / 2
static_df['MPEL_y'] = (((static_df['RASI_y'] + static_df['LASI_y']) / 2) + ((static_df['RPSI_y'] + static_df['LPSI_y']) / 2)) / 2
static_df['MPEL_z'] = (((static_df['RASI_z'] + static_df['LASI_z']) / 2) + ((static_df['RPSI_z'] + static_df['LPSI_z']) / 2)) / 2
#Build the new time series Vec3 table
#Get the time array
time = staticTable.getIndependentColumn()
#Set the label names
#Get the original labels
markerLabels = list(staticTable.getColumnLabels())
#Append new marker labels
markerLabels.append('RHJC')
markerLabels.append('LHJC')
markerLabels.append('RKJC')
markerLabels.append('LKJC')
markerLabels.append('RAJC')
markerLabels.append('LAJC')
markerLabels.append('RMT3')
markerLabels.append('LMT3')
markerLabels.append('fRCAL')
markerLabels.append('fLCAL')
markerLabels.append('fRAJC')
markerLabels.append('fLAJC')
markerLabels.append('fRMT1')
markerLabels.append('fLMT1')
markerLabels.append('fRMT5')
markerLabels.append('fLMT5')
markerLabels.append('fRMT3')
markerLabels.append('fLMT3')
markerLabels.append('MTOR')
markerLabels.append('MPEL')
#Initialise time series table of vec3 format
newTable = osim.TimeSeriesTableVec3()
#Set labels in table
newLabels = osim.StdVectorString()
for ii in range(0,len(markerLabels)):
newLabels.append(markerLabels[ii])
newTable.setColumnLabels(newLabels)
#Build the time series table
#Loop through rows and allocate data
for iRow in range(0,nRows):
#Create a blank opensim row vector
row = osim.RowVectorVec3(len(markerLabels))
#Create and fill row data
for iCol in range(0,len(markerLabels)):
#Identify the dataframe labels for the current marker label
xLabel = markerLabels[iCol]+'_x'
yLabel = markerLabels[iCol]+'_y'
zLabel = markerLabels[iCol]+'_z'
#Set data in row
row.getElt(0,iCol).set(0,static_df[xLabel][iRow])
row.getElt(0,iCol).set(1,static_df[yLabel][iRow])
row.getElt(0,iCol).set(2,static_df[zLabel][iRow])
#Append row to table
newTable.appendRow(iRow,row)
#Set time value
newTable.setIndependentValueAtIndex(iRow,time[iRow])
#Update the meta data in the new table based on the original
#Get meta data keys
metaKeys = list(staticTable.getTableMetaDataKeys())
#Loop through keys and append to new table
for ii in range(0,len(metaKeys)):
#Get current meta data string
currMeta = metaKeys[ii]
#Append with relevant value
newTable.addTableMetaDataString(currMeta,staticTable.getTableMetaDataAsString(currMeta))
#Write the new table to file
osim.TRCFileAdapter().write(newTable,outputTRC)
#Print confirm message
print('Virtual markers added to new .trc file: '+outputTRC)
def addVirtualMarkersDynamic(staticTRC = None, dynamicTRC = None,
outputTRC = 'dynamic_withVirtualMarkers.trc'):
# Convenience function for adding virtual markers to dynamic trial.
#
# Input: staticTRC - .trc filename for static trial with virtual markers added
# dynamicTRC - .trc filename for dynamic trial to add markers to
# outputTRC - optional input for filename for output .trc file
#
# Hip joint centres are placed according to the method outlined by
# Harrington et al. (2007), J Biomech, 40: 595-602.
#
# A 3d trilateration function from https://github.com/akshayb6/trilateration-in-3d
# is used to create virtual markers at positions on the scaled model based
# on distances and positions of four other markers.
#
# Gaps in marker data come across as zeros in the .trc file, so any calculations
# over where missing markers are present will be wrong. These areas should
# not be used in the actual trial assessment without appropriate filling.
#Check for input
if staticTRC is None:
raise ValueError('Input of staticTRC is required')
if dynamicTRC is None:
raise ValueError('Input of dynamicTRC is required')
#Use the Vec3 TimeSeriesTable to read the Vec3 type data file.
dynamicTable = osim.TimeSeriesTableVec3(dynamicTRC)
#Convert to more easily readable object for easier manipulation
#Get number of column labels and data rows
nLabels = dynamicTable.getNumColumns()
nRows = dynamicTable.getNumRows()
#Pre-allocate numpy array based on data size
dataArray = np.zeros((nRows,nLabels*3))
#Loop through labels and rows and get data
for iLabel in range(0,nLabels):
#Get current column label
currLabel = dynamicTable.getColumnLabel(iLabel)
for iRow in range(0,nRows):
dataArray[iRow,iLabel*3] = dynamicTable.getDependentColumn(currLabel).getElt(0,iRow).get(0)
dataArray[iRow,iLabel*3+1] = dynamicTable.getDependentColumn(currLabel).getElt(0,iRow).get(1)
dataArray[iRow,iLabel*3+2] = dynamicTable.getDependentColumn(currLabel).getElt(0,iRow).get(2)
#Convert numpy array to pandas dataframe and add column labels
#Get column labels
colLabels = list()
for iLabel in range(0,nLabels):
colLabels.append(dynamicTable.getColumnLabel(iLabel)+'_x')
colLabels.append(dynamicTable.getColumnLabel(iLabel)+'_y')
colLabels.append(dynamicTable.getColumnLabel(iLabel)+'_z')
#Convert to dataframe
dynamic_df = pd.DataFrame(data = dataArray,columns=colLabels)
#Get the pelvis marker data
#In this step we convert back to the traditional Vicon coordinate system
#and millimetres. It was easier to do this than mess around with the hip
#joint centre calculations
RASIS = np.zeros((nRows,3))
RASIS[:,0] = dynamic_df['RASI_z']*1000
RASIS[:,1] = dynamic_df['RASI_x']*1000
RASIS[:,2] = dynamic_df['RASI_y']*1000
LASIS = np.zeros((nRows,3))
LASIS[:,0] = dynamic_df['LASI_z']*1000
LASIS[:,1] = dynamic_df['LASI_x']*1000
LASIS[:,2] = dynamic_df['LASI_y']*1000
RPSIS = np.zeros((nRows,3))
RPSIS[:,0] = dynamic_df['RPSI_z']*1000
RPSIS[:,1] = dynamic_df['RPSI_x']*1000
RPSIS[:,2] = dynamic_df['RPSI_y']*1000
LPSIS = np.zeros((nRows,3))
LPSIS[:,0] = dynamic_df['LPSI_z']*1000
LPSIS[:,1] = dynamic_df['LPSI_x']*1000
LPSIS[:,2] = dynamic_df['LPSI_y']*1000
#Calculate hip joint centre at each time step
#Pre-allocate size
RHJC = np.zeros((nRows,3))
LHJC = np.zeros((nRows,3))
#Loop through sample points
for t in range(0,nRows):
#Right handed pelvis reference system definition
SACRUM = (RPSIS[t,:] + LPSIS[t,:]) / 2
#Global Pelvis Center position
OP = (LASIS[t,:] + RASIS[t,:]) / 2
PROVV = (RASIS[t,:] - SACRUM) / np.linalg.norm(RASIS[t,:] - SACRUM,2)
IB = (RASIS[t,:] - LASIS[t,:]) / np.linalg.norm(RASIS[t,:] - LASIS[t,:],2)
KB = np.cross(IB,PROVV)
KB = KB / np.linalg.norm(KB,2)
JB = np.cross(KB,IB)
JB = JB / np.linalg.norm(JB,2)
OB = OP
#rotation+ traslation in homogeneous coordinates (4x4)
addPelvis = np.array([0,0,0,1])
pelvis = np.hstack((IB.reshape(3,1),
JB.reshape(3,1),
KB.reshape(3,1),
OB.reshape(3,1)))
pelvis = np.vstack((pelvis,addPelvis))
#Trasformation into pelvis coordinate system (CS)
OPB = np.linalg.inv(pelvis) @ np.vstack((OB.reshape(3,1),np.array([1])))
PW = np.linalg.norm(RASIS[t,:] - LASIS[t,:])
PD = np.linalg.norm(SACRUM - OP)
#Harrington formulae (starting from pelvis center)
diff_ap = -0.24 * PD - 9.9
diff_v = -0.30 * PW - 10.9
diff_ml = 0.33 * PW + 7.3
#vector that must be subtract to OP to obtain hjc in pelvis CS
vett_diff_pelvis_sx = np.array([-diff_ml,diff_ap,diff_v,1])
vett_diff_pelvis_dx = np.array([diff_ml,diff_ap,diff_v,1])
#hjc in pelvis CS (4x4)
rhjc_pelvis = OPB[:,0] + vett_diff_pelvis_dx
lhjc_pelvis = OPB[:,0] + vett_diff_pelvis_sx
#Transformation Local to Global
RHJC[t,:] = pelvis[0:3,0:3] @ rhjc_pelvis[0:3] + OB
LHJC[t,:] = pelvis[0:3,0:3] @ lhjc_pelvis[0:3] + OB
#Add to data frame
#Convert back to appropriate units and coordinate system here too
dynamic_df['RHJC_x'] = RHJC[:,1] / 1000
dynamic_df['RHJC_y'] = RHJC[:,2] / 1000
dynamic_df['RHJC_z'] = RHJC[:,0] / 1000
dynamic_df['LHJC_x'] = LHJC[:,1] / 1000
dynamic_df['LHJC_y'] = LHJC[:,2] / 1000
dynamic_df['LHJC_z'] = LHJC[:,0] / 1000
#Create mid point markers
#Mid torso
dynamic_df['MTOR_x'] = (((dynamic_df['RACR_x'] + dynamic_df['LACR_x']) / 2) + ((dynamic_df['C7_x'] + dynamic_df['CLAV_x']) / 2)) / 2
dynamic_df['MTOR_y'] = (((dynamic_df['RACR_y'] + dynamic_df['LACR_y']) / 2) + ((dynamic_df['C7_y'] + dynamic_df['CLAV_y']) / 2)) / 2
dynamic_df['MTOR_z'] = (((dynamic_df['RACR_z'] + dynamic_df['LACR_z']) / 2) + ((dynamic_df['C7_z'] + dynamic_df['CLAV_z']) / 2)) / 2
#Mid pelvis
dynamic_df['MPEL_x'] = (((dynamic_df['RASI_x'] + dynamic_df['LASI_x']) / 2) + ((dynamic_df['RPSI_x'] + dynamic_df['LPSI_x']) / 2)) / 2
dynamic_df['MPEL_y'] = (((dynamic_df['RASI_y'] + dynamic_df['LASI_y']) / 2) + ((dynamic_df['RPSI_y'] + dynamic_df['LPSI_y']) / 2)) / 2
dynamic_df['MPEL_z'] = (((dynamic_df['RASI_z'] + dynamic_df['LASI_z']) / 2) + ((dynamic_df['RPSI_z'] + dynamic_df['LPSI_z']) / 2)) / 2
#Create virtual markers using 3d trilateration
#Define the 3d trilateration function
#Trilateration calculations (from: https://github.com/akshayb6/trilateration-in-3d)
def trilateration3D(p1 = None, p2 = None, p3 = None, p4 = None,
d1 = None, d2 = None, d3 = None, d4 = None):
# Inputs
# p1, p2, p3 and p4 - 3 column x nrow array of the 3d marker points
# d1, d2, d3 and d4 - distances to the virtual marker in the static trial from the corresponding points
#Check inputs
if p1 is None or p2 is None or p3 is None or p4 is None:
raise ValueError('All 4 points need to be specified as nd array')
if d1 is None or d2 is None or d3 is None or d4 is None:
raise ValueError('All 4 distances need to be specified as floats')
#Allocate numpy array for results
virtualMarker = np.zeros((len(p1),3))
#Run calculations
#Loop through numpy arrays
for pp in range(0,len(p1)):
e_x = (p2[pp]-p1[pp])/np.linalg.norm(p2[pp]-p1[pp])
i = np.dot(e_x,(p3[pp]-p1[pp]))
e_y = (p3[pp]-p1[pp]-(i*e_x))/(np.linalg.norm(p3[pp]-p1[pp]-(i*e_x)))
e_z = np.cross(e_x,e_y)
d = np.linalg.norm(p2[pp]-p1[pp])
j = np.dot(e_y,(p3[pp]-p1[pp]))
x = ((d1**2)-(d2**2)+(d**2))/(2*d)
y = (((d1**2)-(d3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(x))
z1 = np.sqrt(d1**2-x**2-y**2)
z2 = np.sqrt(d1**2-x**2-y**2)*(-1)
ans1 = p1[pp]+(x*e_x)+(y*e_y)+(z1*e_z)
ans2 = p1[pp]+(x*e_x)+(y*e_y)+(z2*e_z)
dist1 = np.linalg.norm(p4[pp]-ans1)
dist2 = np.linalg.norm(p4[pp]-ans2)
if np.abs(d4-dist1) < np.abs(d4-dist2):
virtualMarker[pp,:] = ans1
else:
virtualMarker[pp,:] = ans2
#Return calculated virtual marker from function
return virtualMarker
#Load in the static marker file to identify average marker distances
#Use the Vec3 TimeSeriesTable to read the Vec3 type data file.
staticTable = osim.TimeSeriesTableVec3(staticTRC)
#Convert to more easily readable object for easier manipulation
#Get number of column labels and data rows
nLabelsStatic = staticTable.getNumColumns()
nRowsStatic = staticTable.getNumRows()
#Pre-allocate numpy array based on data size
dataArrayStatic = np.zeros((nRowsStatic,nLabelsStatic*3))
#Loop through labels and rows and get data
for iLabel in range(0,nLabelsStatic):
#Get current column label
currLabel = staticTable.getColumnLabel(iLabel)
for iRow in range(0,nRowsStatic):
dataArrayStatic[iRow,iLabel*3] = staticTable.getDependentColumn(currLabel).getElt(0,iRow).get(0)
dataArrayStatic[iRow,iLabel*3+1] = staticTable.getDependentColumn(currLabel).getElt(0,iRow).get(1)
dataArrayStatic[iRow,iLabel*3+2] = staticTable.getDependentColumn(currLabel).getElt(0,iRow).get(2)
#Convert numpy array to pandas dataframe and add column labels
#Get column labels
colLabelsStatic = list()
for iLabel in range(0,nLabelsStatic):
colLabelsStatic.append(staticTable.getColumnLabel(iLabel)+'_x')
colLabelsStatic.append(staticTable.getColumnLabel(iLabel)+'_y')
colLabelsStatic.append(staticTable.getColumnLabel(iLabel)+'_z')
#Convert to dataframe
static_df = pd.DataFrame(data = dataArrayStatic,columns=colLabelsStatic)
#Right and left knee joint centres
#This will use the location and distances of the hip joint centre, and thigh
#cluster markers
#Calculate the mean distances for the four markers to the added knee joint centre
# #Right limb
# #Calculate distances from relevant markers
# RHJC_RKJC_d = np.sqrt(np.square((np.mean(static_df['RKJC_x']) - np.mean(static_df['RHJC_x']))) +
# np.square((np.mean(static_df['RKJC_y']) - np.mean(static_df['RHJC_y']))) +
# np.square((np.mean(static_df['RKJC_z']) - np.mean(static_df['RHJC_z']))))
# RTH1_RKJC_d = np.sqrt(np.square((np.mean(static_df['RKJC_x']) - np.mean(static_df['RTH1_x']))) +
# np.square((np.mean(static_df['RKJC_y']) - np.mean(static_df['RTH1_y']))) +
# np.square((np.mean(static_df['RKJC_z']) - np.mean(static_df['RTH1_z']))))
# RTH2_RKJC_d = np.sqrt(np.square((np.mean(static_df['RKJC_x']) - np.mean(static_df['RTH2_x']))) +
# np.square((np.mean(static_df['RKJC_y']) - np.mean(static_df['RTH2_y']))) +
# np.square((np.mean(static_df['RKJC_z']) - np.mean(static_df['RTH2_z']))))
# RTH3_RKJC_d = np.sqrt(np.square((np.mean(static_df['RKJC_x']) - np.mean(static_df['RTH3_x']))) +
# np.square((np.mean(static_df['RKJC_y']) - np.mean(static_df['RTH3_y']))) +
# np.square((np.mean(static_df['RKJC_z']) - np.mean(static_df['RTH3_z']))))
# #Run trilateration function
# RKJC = trilateration3D(p1 = dynamic_df[['RHJC_x','RHJC_y','RHJC_z']].to_numpy(),
# p2 = dynamic_df[['RTH1_x','RTH1_y','RTH1_z']].to_numpy(),
# p3 = dynamic_df[['RTH2_x','RTH2_y','RTH2_z']].to_numpy(),
# p4 = dynamic_df[['RTH3_x','RTH3_y','RTH3_z']].to_numpy(),
# d1 = RHJC_RKJC_d, d2 = RTH1_RKJC_d, d3 = RTH2_RKJC_d, d4 = RTH3_RKJC_d)
# #Allocate to dataframe
# dynamic_df['RKJC_x'] = RKJC[:,0]
# dynamic_df['RKJC_y'] = RKJC[:,1]
# dynamic_df['RKJC_z'] = RKJC[:,2]
# #Left limb
# #Calculate distances from relevant markers
# LHJC_LKJC_d = np.sqrt(np.square((np.mean(static_df['LKJC_x']) - np.mean(static_df['LHJC_x']))) +
# np.square((np.mean(static_df['LKJC_y']) - np.mean(static_df['LHJC_y']))) +
# np.square((np.mean(static_df['LKJC_z']) - np.mean(static_df['LHJC_z']))))
# LTH1_LKJC_d = np.sqrt(np.square((np.mean(static_df['LKJC_x']) - np.mean(static_df['LTH1_x']))) +
# np.square((np.mean(static_df['LKJC_y']) - np.mean(static_df['LTH1_y']))) +
# np.square((np.mean(static_df['LKJC_z']) - np.mean(static_df['LTH1_z']))))
# LTH2_LKJC_d = np.sqrt(np.square((np.mean(static_df['LKJC_x']) - np.mean(static_df['LTH2_x']))) +
# np.square((np.mean(static_df['LKJC_y']) - np.mean(static_df['LTH2_y']))) +
# np.square((np.mean(static_df['LKJC_z']) - np.mean(static_df['LTH2_z']))))
# LTH3_LKJC_d = np.sqrt(np.square((np.mean(static_df['LKJC_x']) - np.mean(static_df['LTH3_x']))) +
# np.square((np.mean(static_df['LKJC_y']) - np.mean(static_df['LTH3_y']))) +
# np.square((np.mean(static_df['LKJC_z']) - np.mean(static_df['LTH3_z']))))
# #Run trilateration function
# LKJC = trilateration3D(p1 = dynamic_df[['LHJC_x','LHJC_y','LHJC_z']].to_numpy(),
# p2 = dynamic_df[['LTH1_x','LTH1_y','LTH1_z']].to_numpy(),
# p3 = dynamic_df[['LTH2_x','LTH2_y','LTH2_z']].to_numpy(),
# p4 = dynamic_df[['LTH3_x','LTH3_y','LTH3_z']].to_numpy(),
# d1 = LHJC_LKJC_d, d2 = LTH1_LKJC_d, d3 = LTH2_LKJC_d, d4 = LTH3_LKJC_d)
# #Allocate to dataframe
# dynamic_df['LKJC_x'] = LKJC[:,0]
# dynamic_df['LKJC_y'] = LKJC[:,1]
# dynamic_df['LKJC_z'] = LKJC[:,2]
##### NOTE: trialteration doesn't work that great, markers move around...
#Build the new time series Vec3 table
#Get the time array
time = dynamicTable.getIndependentColumn()
#Set the label names
#Get the original labels
markerLabels = list(dynamicTable.getColumnLabels())
#Append new marker labels
markerLabels.append('RHJC')
markerLabels.append('LHJC')
# markerLabels.append('RKJC')
# markerLabels.append('LKJC')
markerLabels.append('MTOR')
markerLabels.append('MPEL')
#Initialise time series table of vec3 format
newTable = osim.TimeSeriesTableVec3()
#Set labels in table
newLabels = osim.StdVectorString()
for ii in range(0,len(markerLabels)):
newLabels.append(markerLabels[ii])
newTable.setColumnLabels(newLabels)
#Build the time series table
#Loop through rows and allocate data
for iRow in range(0,nRows):
#Create a blank opensim row vector
row = osim.RowVectorVec3(len(markerLabels))
#Create and fill row data
for iCol in range(0,len(markerLabels)):
#Identify the dataframe labels for the current marker label
xLabel = markerLabels[iCol]+'_x'
yLabel = markerLabels[iCol]+'_y'
zLabel = markerLabels[iCol]+'_z'
#Set data in row
row.getElt(0,iCol).set(0,dynamic_df[xLabel][iRow])
row.getElt(0,iCol).set(1,dynamic_df[yLabel][iRow])
row.getElt(0,iCol).set(2,dynamic_df[zLabel][iRow])
#Append row to table
newTable.appendRow(iRow,row)
#Set time value
newTable.setIndependentValueAtIndex(iRow,time[iRow])
#Update the meta data in the new table based on the original
#Get meta data keys
metaKeys = list(dynamicTable.getTableMetaDataKeys())
#Loop through keys and append to new table
for ii in range(0,len(metaKeys)):
#Get current meta data string
currMeta = metaKeys[ii]
#Append with relevant value
newTable.addTableMetaDataString(currMeta,dynamicTable.getTableMetaDataAsString(currMeta))
#Write the new table to file
osim.TRCFileAdapter().write(newTable,outputTRC)
#Print confirm message
print('Virtual markers added to new .trc file: '+outputTRC)
'ground_force_vx', 'ground_force_vy', 'ground_force_vz', 'ground_force_px',
'ground_force_py', 'ground_force_pz', 'ground_torque_x', 'ground_torque_y',
'ground_torque_z'
]
# !! Here we assume that force plate #1 is the left leg and force plate #2 is
# right. One can swap the prefix below after inspecting the c3d file with Mokka.
print('Warning: it is assumed that force plate #1 records left leg '
'and force plate #2 right. Please change accordingly (swap prefix).')
labels_force = ['left_' + label for label in labels_wrench] +\
['right_' + label for label in labels_wrench]
# OpenSim data adapters
adapter = opensim.C3DFileAdapter()
adapter.setLocationForForceExpression(
opensim.C3DFileAdapter.ForceLocation_CenterOfPressure)
trc_adapter = opensim.TRCFileAdapter()
##
# extract data for static trial
# get markers
static = adapter.read(os.path.join(c3d_dir, static_file))
markers_static = adapter.getMarkersTable(static)
# process markers and save to .trc file
rotate_data_table(markers_static, [1, 0, 0], -90)
trc_adapter.write(markers_static, os.path.join(output_dir, 'static.trc'))
##
# extract data for task (e.g., walk, run)
for file in glob.glob('*static.c3d'):
staticFile.append(file)
#Get subject mass based on static file name and participant info database
#Need check in place as files are labelled differently for certain participants
if ii >= 21:
newStaticFile = staticFile[0].split('0')[0]+'00'+staticFile[0].split('0')[1]
mass = df_subInfo.loc[df_subInfo['FileName'] == newStaticFile,['Mass']].values[0][0]
else:
mass = df_subInfo.loc[df_subInfo['FileName'] == staticFile[0],['Mass']].values[0][0]
#Convert c3d markers to trc file
#Set-up data adapters
c3d = osim.C3DFileAdapter()
trc = osim.TRCFileAdapter()
#Get markers
static = c3d.read(staticFile[0])
markersStatic = c3d.getMarkersTable(static)
#Write static data to file
trc.write(markersStatic, staticFile[0].split('.')[0]+'.trc')
#Set variable for static trial trc
staticTrial_trc = staticFile[0].split('.')[0]+'.trc'
#Set scale time range
scaleTimeRange = osim.ArrayDouble()
scaleTimeRange.set(0,osim.Storage(staticTrial_trc).getFirstTime())
scaleTimeRange.set(1,osim.Storage(staticTrial_trc).getFirstTime()+0.5)