def getForceData(self, acq, frame, noFrames):
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(acq)
pfc = pfe.GetOutput()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
for filename in self.fileNames:
forceData = []
acq = qtools.fileOpen(filename)
for forceNumber in range(grwc.GetItemNumber()):
forceData.append({
"id": forceNumber + 1,
"forceCount": 1,
"forcenumber": 2951,
"data": {
"force":
list(
self.resampleForceData(grwc, forceNumber, 0,
noFrames)[frame, :]),
"moment":
list(
self.resampleForceData(grwc, forceNumber, 1,
noFrames)[frame, :]),
"position":
list(
self.resampleForceData(grwc, forceNumber, 2,
noFrames)[frame, :])
}
})
return forceData
def test_FileSample10Type2(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN + "sample10/TYPE-2.C3D")
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(reader.GetOutput())
pfc = pfe.GetOutput()
pfe.Update()
self.assertEqual(pfc.GetItemNumber(), 1)
pf1 = pfc.GetItem(0)
self.assertEqual(pf1.GetType(), 2)
self.assertEqual(pf1.GetChannelNumber(), 6)
self.assertEqual(pf1.GetChannel(0).GetLabel(), "FX1")
self.assertEqual(pf1.GetChannel(1).GetLabel(), "FY1")
self.assertEqual(pf1.GetChannel(2).GetLabel(), "FZ1")
self.assertEqual(pf1.GetChannel(3).GetLabel(), "MX1")
self.assertEqual(pf1.GetChannel(4).GetLabel(), "MY1")
self.assertEqual(pf1.GetChannel(5).GetLabel(), "MZ1")
o1 = numpy.array([[-1.6] , [0.7], [37.5]])
numpy.testing.assert_array_almost_equal(pf1.GetOrigin(), o1, 4)
self.assertAlmostEqual(pf1.GetChannel(0).GetValues()[1], 0.08843, 5)
self.assertAlmostEqual(pf1.GetChannel(0).GetValues()[3], -0.08843, 5)
self.assertAlmostEqual(pf1.GetChannel(0).GetValues()[1020], -69.59441, 5)
self.assertAlmostEqual(pf1.GetChannel(0).GetValues()[1021], -72.24731, 5)
self.assertAlmostEqual(pf1.GetChannel(1).GetValues()[0], 0.17762, 5)
self.assertAlmostEqual(pf1.GetChannel(3).GetValues()[2], 36.83129, 4)
self.assertAlmostEqual(pf1.GetChannel(3).GetValues()[560], 73.66259, 4)
self.assertAlmostEqual(pf1.GetChannel(3).GetValues()[561], 0.0, 5)
def get_fp_info(acq):
fpe = btk.btkForcePlatformsExtractor()
fpe.SetInput(acq)
fpe.Update()
fpc = fpe.GetOutput()
n_fp = fpc.GetItemNumber()
dict_fp = {}
for i in range(n_fp):
dict_fp.update({i:{}})
fp = fpc.GetItem(i)
type = fp.GetType()
dict_fp[i].update({'TYPE': type})
n_chs = fp.GetChannelNumber()
dict_fp[i].update({'VALUES':{}})
labels = []
for j in range(n_chs):
ch = fp.GetChannel(j)
ch_name = ch.GetLabel()
labels.append(ch_name)
# dict_fp[i]['channel'].update({ch_name:{}})
ch_val = ch.GetValues()
dict_fp[i]['VALUES'].update({ch_name: np.asarray(np.squeeze(ch_val), dtype=np.float32)})
dict_fp[i].update({'LABELS': labels})
corners = fp.GetCorners()
dict_fp[i].update({'CORNERS': np.asarray(corners.T, dtype=np.float32)})
origin = fp.GetOrigin()
dict_fp[i].update({'ORIGIN': np.asarray(np.squeeze(origin.T), dtype=np.float32)})
cal_matrix = fp.GetCalMatrix()
dict_fp[i].update({'CAL_MATRIX': np.asarray(cal_matrix.T, dtype=np.float32)})
return dict_fp
def test_FileSample10Type4(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN + "sample10/TYPE-4.C3D")
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(reader.GetOutput())
pfc = pfe.GetOutput()
pfe.Update()
self.assertEqual(pfc.GetItemNumber(), 1)
pf1 = pfc.GetItem(0)
self.assertEqual(pf1.GetType(), 4)
self.assertEqual(pf1.GetChannelNumber(), 6)
self.assertEqual(pf1.GetChannel(0).GetFrameNumber(), 3980)
self.assertEqual(pf1.GetChannel(0).GetLabel(), "FX1")
self.assertEqual(pf1.GetChannel(1).GetLabel(), "FY1")
self.assertEqual(pf1.GetChannel(2).GetLabel(), "FZ1")
self.assertEqual(pf1.GetChannel(3).GetLabel(), "MX1")
self.assertEqual(pf1.GetChannel(4).GetLabel(), "MY1")
self.assertEqual(pf1.GetChannel(5).GetLabel(), "MZ1")
cal = numpy.array([[ 1.5270, 0.0140, 0.0460,-4.2840, -5.4940, 2.8560 ],
[-0.0050, 1.5390,-0.0190, 17.1600, 5.6680, -9.6390 ],
[ 0.0000,-0.0090, 5.9880, 13.2330, 13.2500, 4.6090 ],
[ 0.0000, 0.0020, 0.0000,741.8710, -0.5390, -0.5970 ],
[ 0.0020,-0.0010, 0.000, -1.6820,739.6310, 2.0420 ],
[-0.0020,-0.0050,-0.0020, -3.2500, -0.5940,391.8790 ]])
numpy.testing.assert_array_almost_equal(pf1.GetCalMatrix(), cal.transpose(), 4)
o1 = numpy.array([[-1.6] , [0.7], [37.5]])
numpy.testing.assert_array_almost_equal(pf1.GetOrigin(), o1)
def test_PluginC3D_Threshold50(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN +
"sample09/PlugInC3D.c3d")
reader.Update()
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
dswc = btk.btkWrenchCollectionDownsampleFilter()
dswc.SetUpDownRatio(reader.GetOutput().GetNumberAnalogSamplePerFrame())
pfe.SetInput(reader.GetOutput())
grwf.SetInput(pfe.GetOutput())
dswc.SetInput(grwf.GetOutput())
vgrfged = btk.btkVerticalGroundReactionForceGaitEventDetector()
vgrfged.SetInput(dswc.GetOutput())
vgrfged.SetThresholdValue(50)
vgrfged.SetForceplateContextMapping(["Right", "Left"])
vgrfged.SetAcquisitionInformation(
reader.GetOutput().GetFirstFrame(),
reader.GetOutput().GetPointFrequency(), "")
output = vgrfged.GetOutput()
output.Update()
self.assertEqual(output.GetItemNumber(), 4)
ev = output.GetItem(0)
self.assertEqual(ev.GetLabel(), "Foot Strike")
self.assertEqual(ev.GetContext(), "Right")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 1)
self.assertEqual(ev.GetFrame(), 67)
self.assertEqual(ev.GetTime(), 67.0 / 60.0)
ev = output.GetItem(1)
self.assertEqual(ev.GetLabel(), "Foot Off")
self.assertEqual(ev.GetContext(), "Right")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 2)
self.assertEqual(ev.GetFrame(), 114)
self.assertEqual(ev.GetTime(), 114.0 / 60.0)
ev = output.GetItem(2)
self.assertEqual(ev.GetLabel(), "Foot Strike")
self.assertEqual(ev.GetContext(), "Left")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 1)
self.assertEqual(ev.GetFrame(), 109)
self.assertEqual(ev.GetTime(), 109.0 / 60.0)
ev = output.GetItem(3)
self.assertEqual(ev.GetLabel(), "Foot Off")
self.assertEqual(ev.GetContext(), "Left")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 2)
self.assertEqual(ev.GetFrame(), 148)
self.assertEqual(ev.GetTime(), 148.0 / 60.0)
def test_Gait_NoMapping(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN + "others/Gait.c3d")
reader.Update()
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
dswc = btk.btkWrenchCollectionDownsampleFilter()
dswc.SetUpDownRatio(reader.GetOutput().GetNumberAnalogSamplePerFrame())
pfe.SetInput(reader.GetOutput())
grwf.SetInput(pfe.GetOutput())
dswc.SetInput(grwf.GetOutput())
vgrfged = btk.btkVerticalGroundReactionForceGaitEventDetector()
vgrfged.SetInput(dswc.GetOutput())
vgrfged.SetAcquisitionInformation(
reader.GetOutput().GetFirstFrame(),
reader.GetOutput().GetPointFrequency(), "")
output = vgrfged.GetOutput()
output.Update()
self.assertEqual(output.GetItemNumber(), 4)
ev = output.GetItem(0)
self.assertEqual(ev.GetLabel(), "Foot Strike")
self.assertEqual(ev.GetContext(), "General")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 1)
self.assertEqual(ev.GetFrame(), 257)
self.assertEqual(ev.GetTime(), 2.57)
ev = output.GetItem(1)
self.assertEqual(ev.GetLabel(), "Foot Off")
self.assertEqual(ev.GetContext(), "General")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 2)
self.assertEqual(ev.GetFrame(), 316)
self.assertEqual(ev.GetTime(), 3.16)
ev = output.GetItem(2)
self.assertEqual(ev.GetLabel(), "Foot Strike")
self.assertEqual(ev.GetContext(), "General")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 1)
self.assertEqual(ev.GetFrame(), 209)
self.assertEqual(ev.GetTime(), 2.09)
ev = output.GetItem(3)
self.assertEqual(ev.GetLabel(), "Foot Off")
self.assertEqual(ev.GetContext(), "General")
self.assertEqual(
ev.GetDetectionFlags(),
btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform)
self.assertEqual(ev.GetId(), 2)
self.assertEqual(ev.GetFrame(), 267)
self.assertEqual(ev.GetTime(), 2.67)
def test_FileSample10Type4a(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN + "sample10/type-4a.c3d")
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(reader.GetOutput())
grwf.SetInput(pfe.GetOutput())
grwc = grwf.GetOutput()
grwc.Update()
self.assertEqual(grwc.GetItemNumber(), 2)
grw1 = grwc.GetItem(0)
self.assertEqual(grw1.GetPosition().GetFrameNumber(), 5760)
def test_FileSample01Eb015pi(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN + "sample01/Eb015pi.c3d")
acq = reader.GetOutput()
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(reader.GetOutput())
pfc = pfe.GetOutput()
pfe.Update()
self.assertEqual(pfc.GetItemNumber(), 2)
pf1 = pfc.GetItem(0)
self.assertEqual(pf1.GetType(), 2)
self.assertEqual(pf1.GetChannelNumber(), 6)
self.assertEqual(pf1.GetChannel(0).GetLabel(), "FX1")
self.assertEqual(pf1.GetChannel(1).GetLabel(), "FY1")
self.assertEqual(pf1.GetChannel(2).GetLabel(), "FZ1")
self.assertEqual(pf1.GetChannel(3).GetLabel(), "MX1")
self.assertEqual(pf1.GetChannel(4).GetLabel(), "MY1")
self.assertEqual(pf1.GetChannel(5).GetLabel(), "MZ1")
o1 = numpy.array([[-4.4], [1.9], [-21.6]])
numpy.testing.assert_array_almost_equal(pf1.GetOrigin(), o1)
c11 = numpy.array([[520.0451], [1242.1694], [0.6219]])
c21 = numpy.array([[57.0463], [1243.1996], [0.6211]])
c31 = numpy.array([[58.1765], [1751.1963], [2.0812]])
c41 = numpy.array([[521.1754], [1750.1661], [2.0820]])
numpy.testing.assert_array_almost_equal(pf1.GetCorner(0), c11, 4)
numpy.testing.assert_array_almost_equal(pf1.GetCorner(1), c21, 4)
numpy.testing.assert_array_almost_equal(pf1.GetCorner(2), c31, 4)
numpy.testing.assert_array_almost_equal(pf1.GetCorner(3), c41, 4)
cs1 = numpy.concatenate((c11,c21,c31,c41), axis=1)
numpy.testing.assert_array_almost_equal(pf1.GetCorners(), cs1, 4)
pf2 = pfc.GetItem(1)
self.assertEqual(pf2.GetType(), 2)
self.assertEqual(pf2.GetChannelNumber(), 6)
self.assertEqual(pf2.GetChannel(0).GetLabel(), "FX2")
self.assertEqual(pf2.GetChannel(1).GetLabel(), "FY2")
self.assertEqual(pf2.GetChannel(2).GetLabel(), "FZ2")
self.assertEqual(pf2.GetChannel(3).GetLabel(), "MX2")
self.assertEqual(pf2.GetChannel(4).GetLabel(), "MY2")
self.assertEqual(pf2.GetChannel(5).GetLabel(), "MZ2")
o2 = numpy.array([[-4.06] , [3.81], [-20.06]])
numpy.testing.assert_array_almost_equal(pf2.GetOrigin(), o2)
c12 = numpy.array([[53.6554] , [1139.9977], [1.9204]])
c22 = numpy.array([[516.6432] , [1143.3159], [1.2880]])
c32 = numpy.array([[520.2825] , [635.3301], [0.1814]])
c42 = numpy.array([[57.2948] , [632.0118], [0.8138]])
numpy.testing.assert_array_almost_equal(pf2.GetCorner(0), c12, 4)
numpy.testing.assert_array_almost_equal(pf2.GetCorner(1), c22, 4)
numpy.testing.assert_array_almost_equal(pf2.GetCorner(2), c32, 4)
numpy.testing.assert_array_almost_equal(pf2.GetCorner(3), c42, 4)
cs2 = numpy.concatenate((c12,c22,c32,c42), axis=1)
numpy.testing.assert_array_almost_equal(pf2.GetCorners(), cs2, 4)
def test_PluginC3D_Threshold50(self): reader = btk.btkAcquisitionFileReader() reader.SetFilename(_TDDConfigure.C3DFilePathIN + "sample09/PlugInC3D.c3d") reader.Update() pfe = btk.btkForcePlatformsExtractor() grwf = btk.btkGroundReactionWrenchFilter() dswc = btk.btkWrenchCollectionDownsampleFilter() dswc.SetUpDownRatio(reader.GetOutput().GetNumberAnalogSamplePerFrame()) pfe.SetInput(reader.GetOutput()) grwf.SetInput(pfe.GetOutput()) dswc.SetInput(grwf.GetOutput()) vgrfged = btk.btkVerticalGroundReactionForceGaitEventDetector() vgrfged.SetInput(dswc.GetOutput()) vgrfged.SetThresholdValue(50) vgrfged.SetForceplateContextMapping(["Right","Left"]) vgrfged.SetAcquisitionInformation(reader.GetOutput().GetFirstFrame(), reader.GetOutput().GetPointFrequency(), "") output = vgrfged.GetOutput() output.Update() self.assertEqual(output.GetItemNumber(), 4) ev = output.GetItem(0) self.assertEqual(ev.GetLabel(), "Foot Strike") self.assertEqual(ev.GetContext(), "Right") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 1) self.assertEqual(ev.GetFrame(), 67) self.assertEqual(ev.GetTime(), 67.0/60.0) ev = output.GetItem(1) self.assertEqual(ev.GetLabel(), "Foot Off") self.assertEqual(ev.GetContext(), "Right") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 2) self.assertEqual(ev.GetFrame(), 114) self.assertEqual(ev.GetTime(), 114.0/60.0) ev = output.GetItem(2) self.assertEqual(ev.GetLabel(), "Foot Strike") self.assertEqual(ev.GetContext(), "Left") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 1) self.assertEqual(ev.GetFrame(), 109) self.assertEqual(ev.GetTime(), 109.0/60.0) ev = output.GetItem(3) self.assertEqual(ev.GetLabel(), "Foot Off") self.assertEqual(ev.GetContext(), "Left") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 2) self.assertEqual(ev.GetFrame(), 148) self.assertEqual(ev.GetTime(), 148.0/60.0)
def test_Gait_NoMapping(self): reader = btk.btkAcquisitionFileReader() reader.SetFilename(_TDDConfigure.C3DFilePathIN + "others/Gait.c3d") reader.Update() pfe = btk.btkForcePlatformsExtractor() grwf = btk.btkGroundReactionWrenchFilter() dswc = btk.btkWrenchCollectionDownsampleFilter() dswc.SetUpDownRatio(reader.GetOutput().GetNumberAnalogSamplePerFrame()) pfe.SetInput(reader.GetOutput()) grwf.SetInput(pfe.GetOutput()) dswc.SetInput(grwf.GetOutput()) vgrfged = btk.btkVerticalGroundReactionForceGaitEventDetector() vgrfged.SetInput(dswc.GetOutput()) vgrfged.SetAcquisitionInformation(reader.GetOutput().GetFirstFrame(), reader.GetOutput().GetPointFrequency(), "") output = vgrfged.GetOutput() output.Update() self.assertEqual(output.GetItemNumber(), 4) ev = output.GetItem(0) self.assertEqual(ev.GetLabel(), "Foot Strike") self.assertEqual(ev.GetContext(), "General") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 1) self.assertEqual(ev.GetFrame(), 257) self.assertEqual(ev.GetTime(), 2.57) ev = output.GetItem(1) self.assertEqual(ev.GetLabel(), "Foot Off") self.assertEqual(ev.GetContext(), "General") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 2) self.assertEqual(ev.GetFrame(), 316) self.assertEqual(ev.GetTime(), 3.16) ev = output.GetItem(2) self.assertEqual(ev.GetLabel(), "Foot Strike") self.assertEqual(ev.GetContext(), "General") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 1) self.assertEqual(ev.GetFrame(), 209) self.assertEqual(ev.GetTime(), 2.09) ev = output.GetItem(3) self.assertEqual(ev.GetLabel(), "Foot Off") self.assertEqual(ev.GetContext(), "General") self.assertEqual(ev.GetDetectionFlags(), btk.btkEvent.Automatic | btk.btkEvent.FromForcePlatform) self.assertEqual(ev.GetId(), 2) self.assertEqual(ev.GetFrame(), 267) self.assertEqual(ev.GetTime(), 2.67)
def test_FileSample19Sample19(self):
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(_TDDConfigure.C3DFilePathIN + "sample19/sample19.c3d")
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(reader.GetOutput())
pfc = pfe.GetOutput()
pfe.Update()
self.assertEqual(pfc.GetItemNumber(), 4)
pf1 = pfc.GetItem(0)
self.assertEqual(pf1.GetChannelNumber(), 6)
self.assertEqual(pf1.GetChannel(0).GetFrameNumber(), 34672 * 18)
self.assertEqual(pf1.GetChannel(0).GetLabel(), "FP1C1")
self.assertEqual(pf1.GetChannel(1).GetLabel(), "FP1C2")
self.assertEqual(pf1.GetChannel(2).GetLabel(), "FP1C3")
self.assertEqual(pf1.GetChannel(3).GetLabel(), "FP1C4")
self.assertEqual(pf1.GetChannel(4).GetLabel(), "FP1C5")
self.assertEqual(pf1.GetChannel(5).GetLabel(), "FP1C6")
self.assertAlmostEqual(pf1.GetChannel(3).GetValues()[561], 0.0, 5)
def _get_forceplate_data(c3dfile):
"""Read data of all forceplates from c3d file.
See read_data.get_forceplate_data() for details.
"""
logger.debug(f'reading forceplate data from {c3dfile}')
acq = _get_c3dacq(c3dfile)
fpe = btk.btkForcePlatformsExtractor()
fpe.SetInput(acq)
fpe.Update()
fpdata = list()
for eclipse_ind, plate in enumerate(btk.Iterate(fpe.GetOutput()), 1):
logger.debug(f'reading from plate {eclipse_ind}')
data = _get_1_forceplate_data(plate)
if data is not None:
# generate the Eclipse key
data['eclipse_key'] = f'FP{eclipse_ind}'
fpdata.append(data)
return fpdata
def ComputeForceplateDisplacements(self):
'''
Computes the transformation from the world frame to the frame of
the force platforms
'''
# Initialize the BTK force platforms extractor
fpe = btk.btkForcePlatformsExtractor()
fpe.SetInput(self.reader.GetOutput())
fpe.Update()
# Extract from the c3d file the corner positions of force platforms,
# from which we can obtain the center of the force platforms
fp_collection = fpe.GetOutput()
num_fp = fp_collection.GetItemNumber()
if num_fp != 4:
raise ValueError("Should never happen!")
forceplate_centers = []
for k in range(num_fp):
fp = fp_collection.GetItem(k)
corners = fp.GetCorners() # 3 x 4 (xyz, four corners)
center = np.mean(corners.astype(float), axis=1)
center /= 1000. # convert mm to meters
forceplate_centers.append(np.matrix(center).T)
# Force platforms: orientations
oR1 = np.matrix([[0, -1, 0], [-1, 0, 0], [0, 0, -1]]).astype(float)
oR2 = oR1.copy() # is equal to oR1
oR3 = np.matrix([[0, 0, -1], [0, 1, 0], [1, 0, 0]]).astype(float)
oR4 = oR3.copy() # is equal to oR3
oM_forceplates = [
se3.SE3(oR1, forceplate_centers[0]),
se3.SE3(oR2, forceplate_centers[1]),
se3.SE3(oR3, forceplate_centers[2]),
se3.SE3(oR4, forceplate_centers[3])
]
return oM_forceplates
def get_fp_wrench(acq, threshold=0.0):
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(acq)
pfe.Update()
pfc = pfe.GetOutput()
grwf = btk.btkGroundReactionWrenchFilter()
grwf.SetInput(pfc)
grwf.SetTransformToGlobalFrame(True)
grwf.SetThresholdState(True)
grwf.SetThresholdValue(threshold)
grwf.Update()
grwc = grwf.GetOutput()
grwc.Update()
wc_output = {}
for i in range(grwc.GetItemNumber()):
wc_data = {}
pos = grwc.GetItem(i).GetPosition().GetValues()
force = grwc.GetItem(i).GetForce().GetValues()
moment = grwc.GetItem(i).GetMoment().GetValues()
wc_data.update({'POS': pos})
wc_data.update({'FORCE': force})
wc_data.update({'MOMENT': moment})
wc_output.update({i: wc_data})
return wc_output
def get_metadata(c3dfile):
""" Read trial and subject metadata """
trialname = os.path.basename(os.path.splitext(c3dfile)[0])
sessionpath = os.path.dirname(c3dfile)
acq = _get_c3dacq(c3dfile)
# frame offset (start of trial data in frames)
offset = acq.GetFirstFrame()
lastfr = acq.GetLastFrame()
length = lastfr - offset + 1 # or acq.GetPointFrameNumber()
framerate = acq.GetPointFrequency()
analograte = acq.GetAnalogFrequency()
samplesperframe = acq.GetNumberAnalogSamplePerFrame()
# count forceplates
fpe = btk.btkForcePlatformsExtractor()
fpe.SetInput(acq)
fpe.Update()
n_forceplates = len(list(btk.Iterate(fpe.GetOutput())))
# get markers
try:
markers = _get_c3d_metadata_field(acq, 'POINT', 'LABELS')
except RuntimeError:
markers = list()
# not sure what the '*xx' markers are, but delete them for now
markers = [m for m in markers if m[0] != '*']
# get events
rstrikes, lstrikes, rtoeoffs, ltoeoffs = [], [], [], []
for i in btk.Iterate(acq.GetEvents()):
if i.GetLabel() == "Foot Strike":
if i.GetContext() == "Right":
rstrikes.append(i.GetFrame())
elif i.GetContext() == "Left":
lstrikes.append(i.GetFrame())
else:
raise GaitDataError("Unknown context on foot strike event")
elif i.GetLabel() == "Foot Off":
if i.GetContext() == "Right":
rtoeoffs.append(i.GetFrame())
elif i.GetContext() == "Left":
ltoeoffs.append(i.GetFrame())
else:
raise GaitDataError("Unknown context on foot strike event")
# get subject info
try:
name = _get_c3d_metadata_field(acq, 'SUBJECTS', 'NAMES')[0]
except RuntimeError:
logger.warning('Cannot get subject name')
name = u'Unknown'
try:
par_names = _get_c3d_metadata_subfields(acq, 'PROCESSING')
except RuntimeError:
raise GaitDataError('cannot read metadata from %s' % c3dfile)
subj_params = defaultdict(lambda: None)
subj_params.update({par: _get_c3d_subject_param(acq, par) for par in par_names})
# sort events (may be in wrong temporal order, at least in c3d files)
for li in [lstrikes, rstrikes, ltoeoffs, rtoeoffs]:
li.sort()
return {
'trialname': trialname,
'sessionpath': sessionpath,
'offset': offset,
'framerate': framerate,
'analograte': analograte,
'name': name,
'subj_params': subj_params,
'lstrikes': lstrikes,
'rstrikes': rstrikes,
'ltoeoffs': ltoeoffs,
'rtoeoffs': rtoeoffs,
'length': length,
'samplesperframe': samplesperframe,
'n_forceplates': n_forceplates,
'markers': markers,
}
def _get_metadata(c3dfile):
"""Read trial and subject metadata from c3d file.
See read_data.get_metadata() for details.
"""
c3dfile = Path(c3dfile)
trialname = c3dfile.stem
sessionpath = c3dfile.parent
acq = _get_c3dacq(c3dfile)
# frame offset (start of trial data in frames)
offset = acq.GetFirstFrame()
lastfr = acq.GetLastFrame()
length = lastfr - offset + 1 # or acq.GetPointFrameNumber()
framerate = acq.GetPointFrequency()
analograte = acq.GetAnalogFrequency()
samplesperframe = acq.GetNumberAnalogSamplePerFrame()
# count forceplates
fpe = btk.btkForcePlatformsExtractor()
fpe.SetInput(acq)
fpe.Update()
n_forceplates = len(list(btk.Iterate(fpe.GetOutput())))
# get markers
try:
markers = _get_c3d_metadata_field(acq, 'POINT', 'LABELS')
except RuntimeError:
markers = list()
# XXX: not sure what the '*xx' markers are, but delete them for now
markers = [m for m in markers if m[0] != '*']
# get events
events = GaitEvents()
for i in btk.Iterate(acq.GetEvents()):
fr = i.GetFrame()
context = i.GetContext()[0]
ev_type = i.GetLabel()
if ev_type == 'Foot Strike':
ev_type_ours = 'strike'
elif ev_type == 'Foot Off':
ev_type_ours = 'toeoff'
else:
ev_type_ours = ev_type
fr_offset = fr - offset
ev = GaitEvent(fr_offset, ev_type_ours, context)
events.append(ev)
# get subject info
try:
subj_name = _get_c3d_metadata_field(acq, 'SUBJECTS', 'NAMES')[0]
except RuntimeError:
logger.warning('Cannot get subject name')
subj_name = 'Unknown'
subj_params = defaultdict(lambda: None)
try:
par_names = _get_c3d_metadata_subfields(acq, 'PROCESSING')
except RuntimeError:
logger.warning(
f'{c3dfile} is missing the PROCESSING section that contains '
'subject info (bodyweight etc.)')
else:
subj_params.update(
{par: _get_c3d_subject_param(acq, par)
for par in par_names})
return {
'trialname': trialname,
'sessionpath': sessionpath,
'offset': offset,
'framerate': framerate,
'analograte': analograte,
'subject_name': subj_name,
'subj_params': subj_params,
'events': events,
'length': length,
'samplesperframe': samplesperframe,
'n_forceplates': n_forceplates,
'markers': markers,
}
def get_fp_output(acq, threshold=0.0, filt_fc=None, filt_order=2, cop_nan_to_num=True):
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(acq)
pfe.Update()
pfc = pfe.GetOutput()
if pfc.IsEmpty():
return None
point_unit = acq.GetPointUnit()
point_scale = 1.0 if point_unit=='m' else 0.001
analog_fps = acq.GetAnalogFrequency()
rgx_fp = re.compile(r'\S*(\d*[FMP]\d*[XxYyZz]\d*)')
fp_output = {}
fp_idx = 0
for fp in btk.Iterate(pfc):
fp_type = fp.GetType()
# force plate location info
fp_org_raw = np.squeeze(fp.GetOrigin())*point_scale
fp_z_check = -1.0 if fp_org_raw[2]>0 else 1.0
if fp_type == 1:
o_x = 0.0
o_y = 0.0
o_z = (-1.0)*fp_org_raw[2]*fp_z_check
elif fp_type in [2, 4]:
o_x = (-1.0)*fp_org_raw[0]*fp_z_check
o_y = (-1.0)*fp_org_raw[1]*fp_z_check
o_z = (-1.0)*fp_org_raw[2]*fp_z_check
elif fp_type == 3:
o_x = 0.0
o_y = 0.0
o_z = (-1.0)*fp_org_raw[2]*fp_z_check
fp_len_a = np.abs(fp_org_raw[0])
fp_len_b = np.abs(fp_org_raw[1])
fp_corners = fp.GetCorners().T*point_scale
# fp_corners[0] #(+x, +y)
# fp_corners[1] #(-x, +y)
# fp_corners[2] #(-x, -y)
# fp_corners[3] #(+x, -y)
fp_cen = np.mean(fp_corners, axis=0)
fp_len_x = (np.linalg.norm(fp_corners[0]-fp_corners[1])+np.linalg.norm(fp_corners[3]-fp_corners[2]))*0.5
fp_len_y = (np.linalg.norm(fp_corners[0]-fp_corners[3])+np.linalg.norm(fp_corners[1]-fp_corners[2]))*0.5
fp_p0 = fp_cen
fp_p1 = 0.5*(fp_corners[0]+fp_corners[3])
fp_p2 = 0.5*(fp_corners[0]+fp_corners[1])
fp_v0 = fp_p1-fp_p0
fp_v1 = fp_p2-fp_p0
fp_v0_u = fp_v0/np.linalg.norm(fp_v0)
fp_v1_u = fp_v1/np.linalg.norm(fp_v1)
fp_v2 = np.cross(fp_v0_u, fp_v1_u)
fp_v2_u = fp_v2/np.linalg.norm(fp_v2)
fp_v_z = fp_v2_u
fp_v_x = fp_v0_u
fp_v_y = np.cross(fp_v_z, fp_v_x)
fp_rot_mat = np.column_stack([fp_v_x, fp_v_y, fp_v_z])
# force plate force/moment info
fp_data = {}
ch_data = {}
ch_scale = {}
# for ch in btk.Iterate(fp.GetChannels()):
fp_cnt_chs = fp.GetChannelNumber()
if filt_fc is None:
filt_fcs = [None]*fp_cnt_chs
elif type(filt_fc) in [int, float]:
filt_fcs = [float(filt_fc)]*fp_cnt_chs
elif type(filt_fc) in [list, tuple]:
filt_fcs = list(filt_fc)
elif type(filt_fc)==np.ndarray:
if len(filt_fc)==1:
filt_fcs = [filt_fc.item()]*fp_cnt_chs
else:
filt_fcs = filt_fc.tolist()
for ch_idx in range(fp_cnt_chs):
ch_name = fp.GetChannel(ch_idx).GetLabel()
ch = acq.GetAnalog(ch_name)
fm_name = str.upper(rgx_fp.findall(ch.GetLabel())[0])
# assign channel names
if fp_type == 1:
# assume that the order of input analog channels are as follows:
# 'FX', 'FY', 'FZ', 'PX', 'PY', 'TZ'
label = ['FX', 'FY', 'FZ', 'PX', 'PY', 'TZ'][ch_idx]
elif fp_type in [2, 4]:
label = re.sub(r'\d', r'', fm_name)
elif fp_type == 3:
# assume that the order of input analog channels are as follows:
# 'FX12', 'FX34', 'FY14', 'FY23', 'FZ1', 'FZ2', 'FZ3', 'FZ4'
label = ['FX12', 'FX34', 'FY14', 'FY23', 'FZ1', 'FZ2', 'FZ3', 'FZ4'][ch_idx]
# assign channel scale factors
if fp_type == 1:
if label.startswith('F'):
# assume that the force unit is 'N'
ch_scale[label] = 1.0
elif label.startswith('T'):
# assume that the torque unit is 'Nmm'
ch_scale[label] = 0.001
if ch.GetUnit()=='Nm': ch_scale[label] = 1.0
elif label.startswith('P'):
# assume that the position unit is 'mm'
ch_scale[label] = 0.001
if ch.GetUnit()=='m': ch_scale[label] = 1.0
elif fp_type in [2, 3, 4]:
if label.startswith('F'):
# assume that the force unit is 'N'
ch_scale[label] = 1.0
elif label.startswith('M'):
# assume taht the torque unit is 'Nmm'
ch_scale[label] = 0.001
if ch.GetUnit()=='Nm': ch_scale[label] = 1.0
# assign channel values
lp_fc = filt_fcs[ch_idx]
if lp_fc is None:
ch_data[label] = np.squeeze(ch.GetData().GetValues())
else:
ch_data[label] = filt_bw_lp(np.squeeze(ch.GetData().GetValues()), lp_fc, analog_fps, order=filt_order)
if fp_type == 1:
cop_l_x_in = ch_data['PX']*ch_scale['PX']
cop_l_y_in = ch_data['PY']*ch_scale['PY']
t_z_in = ch_data['TZ']*ch_scale['TZ']
fx = ch_data['FX']*ch_scale['FX']
fy = ch_data['FY']*ch_scale['FY']
fz = ch_data['FZ']*ch_scale['FZ']
mx = (cop_l_y_in-o_y)*fz+o_z*fy
my = -o_z*fx-(cop_l_x_in-o_x)*fz
mz = (cop_l_x_in-o_x)*fy-(cop_l_y_in-o_y)*fx+t_z_in
f_raw = np.stack([fx, fy, fz], axis=1)
m_raw = np.stack([mx, my, mz], axis=1)
elif fp_type == 2:
f_raw = np.stack([ch_data['FX']*ch_scale['FX'], ch_data['FY']*ch_scale['FY'], ch_data['FZ']*ch_scale['FZ']], axis=1)
m_raw = np.stack([ch_data['MX']*ch_scale['MX'], ch_data['MY']*ch_scale['MY'], ch_data['MZ']*ch_scale['MZ']], axis=1)
elif fp_type == 4:
fp_cal_mat = fp.GetCalMatrix()
fm_local = np.stack([ch_data['FX'], ch_data['FY'], ch_data['FZ'], ch_data['MX'], ch_data['MY'], ch_data['MZ']], axis=1)
fm_calib = np.dot(fp_cal_mat, fm_local.T).T
f_raw = np.stack([fm_calib[:,0]*ch_scale['FX'], fm_calib[:,1]*ch_scale['FY'], fm_calib[:,2]*ch_scale['FZ']], axis=1)
m_raw = np.stack([fm_calib[:,3]*ch_scale['MX'], fm_calib[:,4]*ch_scale['MY'], fm_calib[:,5]*ch_scale['MZ']], axis=1)
elif fp_type == 3:
fx12 = ch_data['FX12']*ch_scale['FX12']
fx34 = ch_data['FX34']*ch_scale['FX34']
fy14 = ch_data['FY14']*ch_scale['FY14']
fy23 = ch_data['FY23']*ch_scale['FY23']
fz1 = ch_data['FZ1']*ch_scale['FZ1']
fz2 = ch_data['FZ2']*ch_scale['FZ2']
fz3 = ch_data['FZ3']*ch_scale['FZ3']
fz4 = ch_data['FZ4']*ch_scale['FZ4']
fx = fx12+fx34
fy = fy14+fy23
fz = fz1+fz2+fz3+fz4
mx = fp_len_b*(fz1+fz2-fz3-fz4)
my = fp_len_a*(-fz1+fz2+fz3-fz4)
mz = fp_len_b*(-fx12+fx34)+fp_len_a*(fy14-fy23)
f_raw = np.stack([fx, fy, fz], axis=1)
m_raw = np.stack([mx, my, mz], axis=1)
zero_vals = np.zeros((f_raw.shape[0]), dtype=np.float32)
fm_skip_mask = np.abs(f_raw[:,2])<=threshold
f_sensor_local = f_raw.copy()
m_sensor_local = m_raw.copy()
# filter local values by threshold
f_sensor_local[fm_skip_mask,:] = 0.0
m_sensor_local[fm_skip_mask,:] = 0.0
f_x = f_sensor_local[:,0]
f_y = f_sensor_local[:,1]
f_z = f_sensor_local[:,2]
m_x = m_sensor_local[:,0]
m_y = m_sensor_local[:,1]
m_z = m_sensor_local[:,2]
with np.errstate(invalid='ignore'):
f_z_adj = np.where(fm_skip_mask, np.inf, f_z)
cop_l_x = np.where(fm_skip_mask, np.nan, np.clip((-m_y+(-o_z)*f_x)/f_z_adj+o_x, -fp_len_x*0.5, fp_len_x*0.5))
cop_l_y = np.where(fm_skip_mask, np.nan, np.clip((m_x+(-o_z)*f_y)/f_z_adj+o_y, -fp_len_y*0.5, fp_len_y*0.5))
cop_l_z = np.where(fm_skip_mask, np.nan, zero_vals)
if cop_nan_to_num:
cop_l_x = np.nan_to_num(cop_l_x)
cop_l_y = np.nan_to_num(cop_l_y)
cop_l_z = np.nan_to_num(cop_l_z)
t_z = m_z-(cop_l_x-o_x)*f_y+(cop_l_y-o_y)*f_x
# values for the force plate local output
m_cop_local = np.stack([zero_vals, zero_vals, t_z], axis=1)
cop_surf_local = np.stack([cop_l_x, cop_l_y, cop_l_z], axis=1)
f_surf_local = f_sensor_local
m_surf_local = np.cross(np.array([o_x, o_y, o_z], dtype=np.float32), f_sensor_local)+m_sensor_local
# values for the force plate global output
m_cop_global = np.dot(fp_rot_mat, m_cop_local.T).T
cop_surf_global = np.dot(fp_rot_mat, cop_surf_local.T).T
f_surf_global = np.dot(fp_rot_mat, f_surf_local.T).T
m_surf_global = np.dot(fp_rot_mat, m_surf_local.T).T
# values for the lab output
m_cop_lab = m_cop_global
cop_lab = fp_cen+cop_surf_global
f_cop_lab = f_surf_global
# prepare return values
fp_data.update({'F_SURF_LOCAL': f_surf_local})
fp_data.update({'M_SURF_LOCAL': m_surf_local})
fp_data.update({'COP_SURF_LOCAL': cop_surf_local})
fp_data.update({'F_SURF_GLOBAL': f_surf_global})
fp_data.update({'M_SURF_GLOBAL': m_surf_global})
fp_data.update({'COP_SURF_GLOBAL': cop_surf_global})
fp_data.update({'F_COP_LAB': f_cop_lab})
fp_data.update({'M_COP_LAB': m_cop_lab})
fp_data.update({'COP_LAB': cop_lab})
if fp_type == 1:
fp_data.update({'COP_LOCAL_INPUT': np.stack([cop_l_x_in, cop_l_y_in, zero_vals], axis=1)})
fp_output.update({fp_idx: fp_data})
fp_idx += 1
return fp_output
#Write data to file (unfiltered)
trc.write(filtTRC, dynamicFiles[tt].split('.')[0]+'_filtered.trc')
#Set variable for dynamic trial trc
dynamicTrial_trc = dynamicFiles[tt].split('.')[0]+'_filtered.trc'
#Convert c3d force data to file
#Load in the c3d data via btk
dynamicC3D = btk.btkAcquisitionFileReader()
dynamicC3D.SetFilename(dynamicFiles[tt])
dynamicC3D.Update()
dynamicAcq = dynamicC3D.GetOutput()
#Extract the force platforms data
forcePlatforms = btk.btkForcePlatformsExtractor()
forcePlatforms.SetInput(dynamicAcq)
forcePlatforms.Update()
#Get the wrenchs for position and force data
groundReactionWrenches = btk.btkGroundReactionWrenchFilter()
groundReactionWrenches.SetInput(forcePlatforms.GetOutput())
groundReactionWrenches.Update()
#Grab the data from the singular force platform
grf = groundReactionWrenches.GetOutput().GetItem(0).GetForce().GetValues()
grm = groundReactionWrenches.GetOutput().GetItem(0).GetMoment().GetValues()
cop = groundReactionWrenches.GetOutput().GetItem(0).GetPosition().GetValues()
#Convert mm units to m
grm = grm / 1000
def get_forceplate_data(c3dfile):
logger.debug('reading forceplate data from %s' % c3dfile)
read_chs = ['Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz']
acq = _get_c3dacq(c3dfile)
fpe = btk.btkForcePlatformsExtractor()
fpe.SetInput(acq)
fpe.Update()
fpdata = list()
nplate = 0
for plate in btk.Iterate(fpe.GetOutput()):
logger.debug('reading from plate %d' % nplate)
nplate += 1
if plate.GetType() != 2:
# Nexus should always write forceplates as type 2
raise GaitDataError('Only type 2 forceplates are supported for now')
rawdata = dict()
data = dict()
for ch in btk.Iterate(plate.GetChannels()):
label = ch.GetLabel()[-3:-1] # strip descriptor and plate number
rawdata[label] = np.squeeze(ch.GetData().GetValues())
if not all([ch in rawdata for ch in read_chs]):
logger.warning('could not read force/moment data for plate %d' % nplate)
continue
F = np.stack([rawdata['Fx'], rawdata['Fy'], rawdata['Fz']], axis=1)
M = np.stack([rawdata['Mx'], rawdata['My'], rawdata['Mz']], axis=1)
# this should be the plate thickness (from moment origin to physical
# origin) needed for center of pressure calculations
dz = np.abs(plate.GetOrigin()[2])
cop = center_of_pressure(F, M, dz) # in plate local coords
Ftot = np.linalg.norm(F, axis=1)
# locations of +x+y, -x+y, -x-y, +x-y plate corners in world coords
# (in that order)
cor = plate.GetCorners()
wT = np.mean(cor, axis=1) # translation vector, plate -> world
# upper and lower bounds of forceplate
ub = np.max(cor, axis=1)
lb = np.min(cor, axis=1)
# plate unit vectors in world system
px = cor[:, 0] - cor[:, 1]
py = cor[:, 0] - cor[:, 3]
pz = np.array([0, 0, -1])
P = np.stack([px, py, pz], axis=1)
wR = P / np.linalg.norm(P, axis=0) # rotation matrix, plate -> world
# check whether cop stays inside forceplate area and clip if necessary
cop_w = change_coords(cop, wR, wT)
cop_wx = np.clip(cop_w[:, 0], lb[0], ub[0])
cop_wy = np.clip(cop_w[:, 1], lb[1], ub[1])
if not (cop_wx == cop_w[:, 0]).all() and (cop_wy == cop_w[:, 1]).all():
logger.warning(
'center of pressure outside forceplate bounds, clipping to plate'
)
cop[:, 0] = cop_wx
cop[:, 1] = cop_wy
# XXX moment and force transformations may still be wrong
data['F'] = change_coords(-F, wR, 0) # not sure why sign flip needed
data['Ftot'] = Ftot
data['M'] = change_coords(-M, wR, 0) # not sure why sign flip needed
data['CoP'] = cop_w
data['upperbounds'] = ub
data['lowerbounds'] = lb
data['wR'] = wR
data['wT'] = wT
data['cor_full'] = cor.T
fpdata.append(data)
return fpdata
