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