def test_TimeSeriesTableVec3(self):
table = osim.TimeSeriesTableVec3()
# Set columns labels.
table.setColumnLabels(['0', '1', '2'])
assert table.getColumnLabels() == ('0', '1', '2')
# Append a row to the table.
row = osim.RowVectorVec3(
[osim.Vec3(1, 2, 3),
osim.Vec3(4, 5, 6),
osim.Vec3(7, 8, 9)])
table.appendRow(0.1, row)
assert table.getNumRows() == 1
assert table.getNumColumns() == 3
row0 = table.getRowAtIndex(0)
assert (str(row0[0]) == str(row[0]) and str(row0[1]) == str(row[1])
and str(row0[2]) == str(row[2]))
print(table)
# Append another row to the table.
row = osim.RowVectorVec3(
[osim.Vec3(2, 4, 6),
osim.Vec3(8, 10, 12),
osim.Vec3(14, 16, 18)])
table.appendRow(0.2, row)
assert table.getNumRows() == 2
assert table.getNumColumns() == 3
row1 = table.getRow(0.2)
assert (str(row1[0]) == str(row[0]) and str(row1[1]) == str(row[1])
and str(row1[2]) == str(row[2]))
print(table)
# Append another row to the table with a timestamp
# less than the previous one. Exception expected.
try:
table.appendRow(0.15, row)
assert False
except RuntimeError:
pass
tableDouble = table.flatten()
assert tableDouble.getNumRows() == 2
assert tableDouble.getNumColumns() == 9
assert len(tableDouble.getColumnLabels()) == 9
assert tableDouble.getRowAtIndex(0)[0] == 1
assert tableDouble.getRowAtIndex(1)[0] == 2
assert tableDouble.getRowAtIndex(0)[8] == 9
assert tableDouble.getRowAtIndex(1)[8] == 18
print(tableDouble)
tableDouble = table.flatten(['_x', '_y', '_z'])
assert tableDouble.getColumnLabels() == ('0_x', '0_y', '0_z', '1_x',
'1_y', '1_z', '2_x', '2_y',
'2_z')
print(tableDouble)
def computeMarkersReference(predictedSolution):
model = createDoublePendulumModel()
model.initSystem()
states = predictedSolution.exportToStatesStorage()
# Workaround for a bug in Storage::operator=().
columnLabels = model.getStateVariableNames()
columnLabels.insert(0, "time")
states.setColumnLabels(columnLabels)
statesTraj = osim.StatesTrajectory.createFromStatesStorage(model, states)
markerTrajectories = osim.TimeSeriesTableVec3()
markerTrajectories.setColumnLabels(["/markerset/m0", "/markerset/m1"])
for state in statesTraj:
model.realizePosition(state)
m0 = model.getComponent("markerset/m0")
m1 = model.getComponent("markerset/m1")
markerTrajectories.appendRow(
state.getTime(),
osim.RowVectorVec3(
[m0.getLocationInGround(state),
m1.getLocationInGround(state)]))
# Assign a weight to each marker.
markerWeights = osim.SetMarkerWeights()
markerWeights.cloneAndAppend(osim.MarkerWeight("/markerset/m0", 1))
markerWeights.cloneAndAppend(osim.MarkerWeight("/markerset/m1", 5))
return osim.MarkersReference(markerTrajectories, markerWeights)
def test_vector_rowvector(self):
print()
print('Test transpose RowVector to Vector.')
row = osim.RowVector([1, 2, 3, 4])
col = row.transpose()
assert (col[0] == row[0] and col[1] == row[1] and col[2] == row[2]
and col[3] == row[3])
print('Test transpose Vector to RowVector.')
row_copy = col.transpose()
assert (row_copy[0] == row[0] and row_copy[1] == row[1]
and row_copy[2] == row[2] and row_copy[3] == row[3])
print('Test transpose RowVectorVec3 to VectorVec3.')
row = osim.RowVectorVec3(
[osim.Vec3(1, 2, 3),
osim.Vec3(4, 5, 6),
osim.Vec3(7, 8, 9)])
col = row.transpose()
assert (str(col[0]) == str(row[0]) and str(col[1]) == str(row[1])
and str(col[2]) == str(row[2]))
print('Test transpose VectorOVec3 to RowVectorVec3.')
row_copy = col.transpose()
assert (str(row_copy[0]) == str(row[0])
and str(row_copy[1]) == str(row[1])
and str(row_copy[2]) == str(row[2]))
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)
def test_DataTableVec3(self):
table = osim.DataTableVec3()
# Set columns labels.
table.setColumnLabels(['0', '1', '2'])
assert table.getColumnLabels() == ('0', '1', '2')
# Append a row to the table.
row = osim.RowVectorVec3(
[osim.Vec3(1, 2, 3),
osim.Vec3(4, 5, 6),
osim.Vec3(7, 8, 9)])
table.appendRow(0.1, row)
assert table.getNumRows() == 1
assert table.getNumColumns() == 3
row0 = table.getRowAtIndex(0)
assert (str(row0[0]) == str(row[0]) and str(row0[1]) == str(row[1])
and str(row0[2]) == str(row[2]))
print(table)
# Append another row to the table.
row = osim.RowVectorVec3(
[osim.Vec3(2, 4, 6),
osim.Vec3(8, 10, 12),
osim.Vec3(14, 16, 18)])
table.appendRow(0.2, row)
assert table.getNumRows() == 2
assert table.getNumColumns() == 3
row1 = table.getRow(0.2)
assert (str(row1[0]) == str(row[0]) and str(row1[1]) == str(row[1])
and str(row1[2]) == str(row[2]))
print(table)
# Append another row to the table.
row = osim.RowVectorVec3([
osim.Vec3(4, 8, 12),
osim.Vec3(16, 20, 24),
osim.Vec3(28, 32, 36)
])
table.appendRow(0.3, row)
assert table.getNumRows() == 3
assert table.getNumColumns() == 3
row2 = table.getRow(0.3)
assert (str(row2[0]) == str(row[0]) and str(row2[1]) == str(row[1])
and str(row2[2]) == str(row[2]))
print(table)
# Retrieve independent column.
assert table.getIndependentColumn() == (0.1, 0.2, 0.3)
# Retrieve dependent columns.
col1 = table.getDependentColumnAtIndex(1)
assert (str(col1[0]) == str(osim.Vec3(4, 5, 6))
and str(col1[1]) == str(osim.Vec3(8, 10, 12))
and str(col1[2]) == str(osim.Vec3(16, 20, 24)))
col2 = table.getDependentColumn('2')
assert (str(col2[0]) == str(osim.Vec3(7, 8, 9))
and str(col2[1]) == str(osim.Vec3(14, 16, 18))
and str(col2[2]) == str(osim.Vec3(28, 32, 36)))
# Flatten the table into table of doubles.
tableDouble = table.flatten()
assert tableDouble.getNumRows() == 3
assert tableDouble.getNumColumns() == 9
assert len(tableDouble.getColumnLabels()) == 9
assert tableDouble.getColumnLabels() == ('0_1', '0_2', '0_3', '1_1',
'1_2', '1_3', '2_1', '2_2',
'2_3')
assert tableDouble.getRowAtIndex(0)[0] == 1
assert tableDouble.getRowAtIndex(1)[0] == 2
assert tableDouble.getRowAtIndex(2)[0] == 4
assert tableDouble.getRowAtIndex(0)[8] == 9
assert tableDouble.getRowAtIndex(1)[8] == 18
assert tableDouble.getRowAtIndex(2)[8] == 36
print(tableDouble)
tableDouble = table.flatten(['_x', '_y', '_z'])
assert tableDouble.getColumnLabels() == ('0_x', '0_y', '0_z', '1_x',
'1_y', '1_z', '2_x', '2_y',
'2_z')
print(tableDouble)
# Edit rows of the table.
row0 = table.getRowAtIndex(0)
row0[0] = osim.Vec3(10, 10, 10)
row0[1] = osim.Vec3(10, 10, 10)
row0[2] = osim.Vec3(10, 10, 10)
row0 = table.getRowAtIndex(0)
assert (str(row0[0]) == str(osim.Vec3(10, 10, 10))
and str(row0[1]) == str(osim.Vec3(10, 10, 10))
and str(row0[2]) == str(osim.Vec3(10, 10, 10)))
row2 = table.getRow(0.3)
row2[0] = osim.Vec3(20, 20, 20)
row2[1] = osim.Vec3(20, 20, 20)
row2[2] = osim.Vec3(20, 20, 20)
row2 = table.getRow(0.3)
assert (str(row2[0]) == str(osim.Vec3(20, 20, 20))
and str(row2[1]) == str(osim.Vec3(20, 20, 20))
and str(row2[2]) == str(osim.Vec3(20, 20, 20)))
print(table)
# Edit columns of the table.
col1 = table.getDependentColumnAtIndex(1)
col1[0] = osim.Vec3(30, 30, 30)
col1[1] = osim.Vec3(30, 30, 30)
col1[2] = osim.Vec3(30, 30, 30)
col1 = table.getDependentColumnAtIndex(1)
assert (str(col1[0]) == str(osim.Vec3(30, 30, 30))
and str(col1[1]) == str(osim.Vec3(30, 30, 30))
and str(col1[2]) == str(osim.Vec3(30, 30, 30)))
col2 = table.getDependentColumn('2')
col2[0] = osim.Vec3(40, 40, 40)
col2[1] = osim.Vec3(40, 40, 40)
col2[2] = osim.Vec3(40, 40, 40)
col2 = table.getDependentColumn('2')
assert (str(col2[0]) == str(osim.Vec3(40, 40, 40))
and str(col2[1]) == str(osim.Vec3(40, 40, 40))
and str(col2[2]) == str(osim.Vec3(40, 40, 40)))
print(table)