def saveInC3d(cls):
MAIN_PATH = pyCGM2.TEST_DATA_PATH + "operations\\forceplates\\detectFoot\\"
# --- Motion 1
gaitFilename = "walking_oppositeX_2pf.c3d"
acqGait0 = btkTools.smartReader(str(MAIN_PATH + gaitFilename))
acqGait = btk.btkAcquisition.Clone(acqGait0)
signal_processing.forcePlateFiltering(acqGait)
pfe0 = btk.btkForcePlatformsExtractor()
pfe0.SetInput(acqGait0)
pfc0 = pfe0.GetOutput()
pfc0.Update()
pfe1 = btk.btkForcePlatformsExtractor()
pfe1.SetInput(acqGait)
pfc1 = pfe1.GetOutput()
pfc1.Update()
plt.figure()
plt.plot(acqGait0.GetAnalog("Force.Fx1").GetValues()[:, 0], "-r")
plt.plot(acqGait.GetAnalog("Force.Fx1").GetValues()[:, 0], "ob")
plt.figure()
plt.plot(pfc0.GetItem(0).GetChannel(0).GetValues()[:, 0], "-r")
plt.plot(pfc1.GetItem(0).GetChannel(0).GetValues()[:, 0], "-b")
plt.show()
def Kistler4_Noraxon1_Xsens1_wrenchOuputs(cls):
NEXUS = ViconNexus.ViconNexus()
NEXUS_PYTHON_CONNECTED = NEXUS.Client.IsConnected()
DATA_PATH = "C:\\Users\\HLS501\\Documents\\VICON DATA\\pyCGM2-Data\\NexusAPI\\BtkAcquisitionCreator\\sample0\\"
filenameNoExt = "gait_cropped"
NEXUS.OpenTrial(str(DATA_PATH + filenameNoExt), 30)
subject = NEXUS.GetSubjectNames()[0]
acqConstructorFilter = nexusFilters.NexusConstructAcquisitionFilter(
filenameNoExt, subject)
acq = acqConstructorFilter.build()
#btkTools.smartWriter(acq,"NEWC3D.c3d")
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(acq)
pfc = pfe.GetOutput()
grwf = btk.btkGroundReactionWrenchFilter()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
# --- reference values ---
acq0 = btkTools.smartReader(str(DATA_PATH + filenameNoExt + ".c3d"))
pfe0 = btk.btkForcePlatformsExtractor()
pfe0.SetInput(acq0)
pfc0 = pfe0.GetOutput()
grwf0 = btk.btkGroundReactionWrenchFilter()
grwf0.SetInput(pfc0)
grwc0 = grwf0.GetOutput()
grwc0.Update()
np.testing.assert_array_almost_equal(
grwc.GetItem(0).GetForce().GetValues(),
grwc0.GetItem(0).GetForce().GetValues(),
decimal=1)
np.testing.assert_array_almost_equal(
grwc.GetItem(0).GetMoment().GetValues(),
grwc0.GetItem(0).GetMoment().GetValues(),
decimal=1)
np.testing.assert_array_almost_equal(
grwc.GetItem(0).GetPosition().GetValues(),
grwc0.GetItem(0).GetPosition().GetValues(),
decimal=1)
def test_Kistler4_Noraxon1_Xsens1_wrenchOuputs(self):
NEXUS = ViconNexus.ViconNexus()
DATA_PATH = pyCGM2.TEST_DATA_PATH + "NexusAPI\\BtkAcquisitionCreator\\sample0\\"
filenameNoExt = "gait_cropped"
NEXUS.OpenTrial(str(DATA_PATH + filenameNoExt), 30)
subject = nexusTools.getActiveSubject(NEXUS)
# btkAcq builder
nacf = nexusFilters.NexusConstructAcquisitionFilter(
DATA_PATH, filenameNoExt, subject)
acq = nacf.build()
#btkTools.smartWriter(acq,"NEWC3D.c3d")
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(acq)
pfc = pfe.GetOutput()
grwf = btk.btkGroundReactionWrenchFilter()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
# --- reference values ---
acq0 = btkTools.smartReader(DATA_PATH + filenameNoExt + ".c3d")
pfe0 = btk.btkForcePlatformsExtractor()
pfe0.SetInput(acq0)
pfc0 = pfe0.GetOutput()
grwf0 = btk.btkGroundReactionWrenchFilter()
grwf0.SetInput(pfc0)
grwc0 = grwf0.GetOutput()
grwc0.Update()
np.testing.assert_array_almost_equal(
grwc.GetItem(0).GetForce().GetValues(),
grwc0.GetItem(0).GetForce().GetValues(),
decimal=1)
np.testing.assert_array_almost_equal(
grwc.GetItem(0).GetMoment().GetValues(),
grwc0.GetItem(0).GetMoment().GetValues(),
decimal=1)
np.testing.assert_array_almost_equal(
grwc.GetItem(0).GetPosition().GetValues(),
grwc0.GetItem(0).GetPosition().GetValues(),
decimal=1)
def forcePlateFiltering(btkAcq, order=4, fc=5):
"""
Low-pass filtering of all points in an acquisition
:Parameters:
- `btkAcq` (btkAcquisition) - btk acquisition instance
- `fc` (double) - cut-off frequency
- `order` (double) - order of the low-pass filter
"""
fp = btkAcq.GetAnalogFrequency()
bPoint, aPoint = signal.butter(order, fc / (fp * 0.5), btype='lowpass')
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
pfc.Update()
for i in range(0, pfc.GetItemNumber()):
for j in range(0, pfc.GetItem(i).GetChannelNumber()):
values = pfc.GetItem(i).GetChannel(j).GetValues()[:, 0]
values_filt = signal.filtfilt(bPoint, aPoint, values, axis=0)
label = pfc.GetItem(i).GetChannel(
j).GetLabel() # SetValues on channel not store new values
btkAcq.GetAnalog(label).SetValues(values_filt)
def getNumberOfForcePlate(btkAcq):
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
pfc.Update()
return pfc.GetItemNumber()
def appendForcePlateCornerAsMarker(btkAcq):
"""
Add a marker at each force plate corners
:Parameters:
- `btkAcq` (btkAcquisition) : Btk acquisition instance from a c3d
"""
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
pfc.Update()
for i in range(0, pfc.GetItemNumber()):
val_corner0 = pfc.GetItem(i).GetCorner(0).T * np.ones(
(btkAcq.GetPointFrameNumber(), 3))
btkTools.smartAppendPoint(btkAcq,
"fp" + str(i) + "corner0",
val_corner0,
desc="forcePlate")
val_corner1 = pfc.GetItem(i).GetCorner(1).T * np.ones(
(btkAcq.GetPointFrameNumber(), 3))
btkTools.smartAppendPoint(btkAcq,
"fp" + str(i) + "corner1",
val_corner1,
desc="forcePlate")
val_corner2 = pfc.GetItem(i).GetCorner(2).T * np.ones(
(btkAcq.GetPointFrameNumber(), 3))
btkTools.smartAppendPoint(btkAcq,
"fp" + str(i) + "corner2",
val_corner2,
desc="forcePlate")
val_corner3 = pfc.GetItem(i).GetCorner(3).T * np.ones(
(btkAcq.GetPointFrameNumber(), 3))
btkTools.smartAppendPoint(btkAcq,
"fp" + str(i) + "corner3",
val_corner3,
desc="forcePlate")
val_origin = (
pfc.GetItem(i).GetCorner(0) + pfc.GetItem(i).GetCorner(1) +
pfc.GetItem(i).GetCorner(2) + pfc.GetItem(i).GetCorner(3)) / 4.0
val_origin2 = val_origin.T * np.ones((btkAcq.GetPointFrameNumber(), 3))
btkTools.smartAppendPoint(btkAcq,
"fp" + str(i) + "origin",
val_origin2,
desc="forcePlate")
def getForcePlateWrench(btkAcq, fpIndex=None):
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
if fpIndex is not None:
return grwc.GetItem(fpIndex - 1)
else:
return grwc
def addForcePlateGeneralEvents(btkAcq, mappedForcePlate):
"""
Add General events from force plate assignmenet
"""
ff = btkAcq.GetFirstFrame()
lf = btkAcq.GetLastFrame()
pf = btkAcq.GetPointFrequency()
appf = btkAcq.GetNumberAnalogSamplePerFrame()
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
# remove force plates events
btkTools.clearEvents(btkAcq, ["Left-FP", "Right-FP"])
# add general events
indexFP = 0
for letter in mappedForcePlate:
force = grwc.GetItem(indexFP).GetForce().GetValues()
force_downsample = force[0:(lf - ff + 1) * appf:appf] # downsample
Rz = np.abs(force_downsample[:, 2])
frameMax = ff + np.argmax(Rz)
if letter == "L":
ev = btk.btkEvent('Left-FP', (frameMax - 1) / pf, 'General',
btk.btkEvent.Automatic, '',
'event from Force plate assignment')
btkAcq.AppendEvent(ev)
elif letter == "R":
ev = btk.btkEvent('Right-FP', (frameMax - 1) / pf, 'General',
btk.btkEvent.Automatic, '',
'event from Force plate assignment')
btkAcq.AppendEvent(ev)
indexFP += 1
def test_forcePlateTest(self):
NEXUS = ViconNexus.ViconNexus()
DATA_PATH = pyCGM2.TEST_DATA_PATH + "NexusAPI\\c3d_x2d\\"
filenameNoExt = "gait_GAP"
NEXUS.OpenTrial(str(DATA_PATH + filenameNoExt), 30)
#----Btk----
acq0 = btkTools.smartReader(str(DATA_PATH + filenameNoExt + ".c3d"))
analogFx = acq0.GetAnalog("Force.Fx1").GetValues()
analogFy = acq0.GetAnalog("Force.Fy1").GetValues()
analogFz = acq0.GetAnalog("Force.Fz1").GetValues()
analogMx = acq0.GetAnalog("Moment.Mx1").GetValues()
analogMy = acq0.GetAnalog("Moment.My1").GetValues()
analogMz = acq0.GetAnalog("Moment.Mz1").GetValues()
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(acq0)
pfc = pfe.GetOutput()
pfc.Update()
btkfp0 = pfc.GetItem(0)
ch0_Fx = btkfp0.GetChannel(0).GetValues()
ch1_Fy = btkfp0.GetChannel(1).GetValues()
ch2_Fz = btkfp0.GetChannel(2).GetValues()
ch3_Mx = btkfp0.GetChannel(3).GetValues()
ch4_My = btkfp0.GetChannel(4).GetValues()
ch5_Mz = btkfp0.GetChannel(5).GetValues()
# --- ground reaction force wrench ---
grwf = btk.btkGroundReactionWrenchFilter()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
grw0_force = grwc.GetItem(0).GetForce().GetValues()
#----Nexus----
nexusForcePlate = Devices.ForcePlate(1)
forceLocal = nexusForcePlate.getLocalReactionForce()
momentLocal = nexusForcePlate.getLocalReactionMoment()
def run(self, acq, filename, options):
ff = acq.GetFirstFrame()
lf = acq.GetLastFrame()
if "frameRange" in options.keys():
frameInit = options["frameRange"][0] - ff
frameEnd = options["frameRange"][1] - ff + 1
else:
frameInit = ff - ff
frameEnd = lf - ff + 1
errorState = False
indexes = []
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(acq)
pfc = pfe.GetOutput()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
fp_counter = 1
for fpIt in btk.Iterate(grwc):
force_Z = fpIt.GetForce().GetValues()[frameInit:frameEnd, 2]
max = np.max(force_Z)
if max != 0:
indexes = np.where(force_Z == max)
if indexes[0].shape[0] > 1:
LOGGER.logger.warning(
"[pyCGM2-Anomaly] - check Force plate (%s) of file [%s] - signal Fz seems saturated"
% (str(fp_counter), filename))
errorState = True
fp_counter += 1
self.anomaly["Output"] = indexes
self.anomaly["ErrorState"] = errorState
def matchingFootSideOnForceplate(btkAcq,
enableRefine=True,
forceThreshold=50,
left_markerLabelToe="LTOE",
left_markerLabelHeel="LHEE",
right_markerLabelToe="RTOE",
right_markerLabelHeel="RHEE",
display=False,
mfpa=None):
"""
Convenient function detecting foot in contact with a force plate
**synopsis**
This function firsly assign foot side to FP from minimal distance with the application point of reaction force.
A refinement is done subsequently, it confirm if foot side is valid. A foot is invalided if :
- FP output no data superior to the set threshold
- Foot markers are not contain in the polygon defined by force plate corner
:Parameters:
- `btkAcq` (btkAcquisition) - Btk acquisition instance from a c3d
- `left_markerLabelToe` (str) - label of the left toe marker
- `left_markerLabelHeel` (str) - label of the left heel marker
- `right_markerLabelToe` (str) - label of the right toe marker
- `right_markerLabelHeel` (str) - label of the right heel marker
- `display` (bool) - display n figures ( n depend on force plate number) presenting relative distance between mid foot and the orgin of the force plate
- `mfpa` (string or dict) - manual force plate assigmenment from another method. Can be a string (XLRA, A stand for automatic) or a dict returing assigned foot to a Force plate ID.
"""
appendForcePlateCornerAsMarker(btkAcq)
ff = btkAcq.GetFirstFrame()
lf = btkAcq.GetLastFrame()
appf = btkAcq.GetNumberAnalogSamplePerFrame()
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
midfoot_L = (btkAcq.GetPoint(left_markerLabelToe).GetValues() +
btkAcq.GetPoint(left_markerLabelHeel).GetValues()) / 2.0
midfoot_R = (btkAcq.GetPoint(right_markerLabelToe).GetValues() +
btkAcq.GetPoint(right_markerLabelHeel).GetValues()) / 2.0
suffix = str()
if mfpa is not None:
try:
pfIDS = []
for i in range(0, pfc.GetItemNumber()):
pfIDS.append(
re.findall(
"\[(.*?)\]",
pfc.GetItem(i).GetChannel(0).GetDescription())[0])
except Exception:
logging.info("[pyCGM2]: Id of Force plate not detected")
pass
for i in range(0, grwc.GetItemNumber()):
pos = grwc.GetItem(i).GetPosition().GetValues()
pos_downsample = pos[0:(lf - ff + 1) * appf:appf] # downsample
diffL = np.linalg.norm(midfoot_L - pos_downsample, axis=1)
diffR = np.linalg.norm(midfoot_R - pos_downsample, axis=1)
if display:
plt.figure()
ax = plt.subplot(1, 1, 1)
plt.title("Force plate " + str(i + 1))
ax.plot(diffL, '-r')
ax.plot(diffR, '-b')
if np.nanmin(diffL) < np.nanmin(diffR):
logging.debug(" Force plate " + str(i) + " : left foot")
suffix = suffix + "L"
else:
logging.debug(" Force plate " + str(i) + " : right foot")
suffix = suffix + "R"
logging.debug("Matched Force plate ===> %s", (suffix))
if enableRefine:
# refinement of suffix
indexFP = 0
for letter in suffix:
force = grwc.GetItem(indexFP).GetForce().GetValues()
force_downsample = force[0:(lf - ff + 1) * appf:appf] # downsample
Rz = np.abs(force_downsample[:, 2])
boolLst = Rz > forceThreshold
enableDataFlag = False
for it in boolLst.tolist():
if it == True:
enableDataFlag = True
break
if not enableDataFlag:
logging.debug(
"PF #%s not activated. It provides no data superior to threshold"
% (str(indexFP)))
li = list(suffix)
li[indexFP] = "X"
suffix = "".join(li)
else:
if letter == "L":
hee = btkAcq.GetPoint(left_markerLabelHeel).GetValues()
toe = btkAcq.GetPoint(left_markerLabelToe).GetValues()
elif letter == "R":
hee = btkAcq.GetPoint(right_markerLabelHeel).GetValues()
toe = btkAcq.GetPoint(right_markerLabelToe).GetValues()
# polygon builder
corner0 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner0").GetValues()[0, :]
corner1 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner1").GetValues()[0, :]
corner2 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner2").GetValues()[0, :]
corner3 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner3").GetValues()[0, :]
verts = [
corner0[0:2], # left, bottom
corner1[0:2], # left, top
corner2[0:2], # right, top
corner3[0:2], # right, bottom
corner0[0:2], # ignored
]
codes = [
Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path = Path(verts, codes)
# check if contain both toe and hee marker
containFlags = list()
for i in range(0, Rz.shape[0]):
if boolLst[i]:
if path.contains_point(
hee[i, 0:2]) and path.contains_point(toe[i,
0:2]):
containFlags.append(True)
else:
containFlags.append(False)
if not all(containFlags) == True:
logging.debug(
"PF #%s not activated. While Rz superior to threshold, foot markers are not contained in force plate geometry "
% (str(indexFP)))
# replace only one character
li = list(suffix)
li[indexFP] = "X"
suffix = "".join(li)
indexFP += 1
# correction with manual assignement
if mfpa is not None:
correctedSuffix = ""
if type(mfpa) == dict:
logging.warning(
"[pyCGM2] : automatic force plate assigment corrected with context associated with the device Id "
)
i = 0
for id in pfIDS:
fpa = mfpa[id]
if fpa != "A":
correctedSuffix = correctedSuffix + fpa
else:
correctedSuffix = correctedSuffix + suffix[i]
i += 1
else:
logging.warning(
"[pyCGM2] : automatic force plate assigment corrected ")
if len(mfpa) < len(suffix):
raise Exception(
"[pyCGM2] number of assigned force plate inferior to the number of force plate number. Your assignment should have %s letters at least"
% (str(len(suffix))))
else:
if len(mfpa) > len(suffix):
logging.warning(
"[pyCGM2]: Your manual force plate assignement mentions more force plates than the number of force plates stored in the c3d"
)
for i in range(0, len(suffix)):
if mfpa[i] != "A":
correctedSuffix = correctedSuffix + mfpa[i]
else:
correctedSuffix = correctedSuffix + suffix[i]
return correctedSuffix
else:
return suffix
def correctForcePlateType5(btkAcq):
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
pfc.Update()
md_force_platform = btkAcq.GetMetaData().FindChild(
str("FORCE_PLATFORM")).value()
md_force_platform_channels = btkAcq.GetMetaData().FindChild(
str("FORCE_PLATFORM")).value().FindChild(str("CHANNEL")).value()
md_force_platform.RemoveChild("CAL_MATRIX")
md_force_platform.RemoveChild("MATRIX_STORE")
md_channels = list()
for i in btkTools.smartGetMetadata(btkAcq, 'FORCE_PLATFORM', "CHANNEL"):
md_channels.append(i)
pfds = [pfc.GetItem(0), pfc.GetItem(1)]
forcePlateNumber = len(pfds)
channel_number_byFp = list()
for i in range(0, len(pfds)):
if pfds[i].GetType() in [1, 2, 4]:
channel_number_byFp.append(6)
if pfds[i].GetType() in [3, 5]:
channel_number_byFp.append(8)
init = 0
channel_indexes_ofAnalog = list()
for i in channel_number_byFp:
channel_indexes_ofAnalog.append(md_channels[init:init + i])
init = i
newChannelIndexes = []
for i in range(0, len(pfds)):
if pfds[i].GetType() == 5:
numAnalogs = btkAcq.GetAnalogNumber()
analogChannels = np.zeros((8, btkAcq.GetAnalogFrameNumber()))
j = 0
for index in channel_indexes_ofAnalog[i]:
analogChannels[j, :] = btkAcq.GetAnalog(int(index) -
1).GetValues().T
md_channels.remove(index)
j += 1
wrench = np.dot(
pfds[i].GetCalMatrix().T.reshape(8, 6).T, analogChannels
) # warning : storage of cal_matrix of type5 force plate is wrong in btk.
force = wrench[0:3, :].T
moment = wrench[3:6, :].T
origin = pfds[i].GetOrigin()
corners = pfds[i].GetCorners()
btkTools.smartAppendAnalog(btkAcq,
"Force.Fx" + str(i),
force[:, 0],
desc="virtual Force plate")
btkTools.smartAppendAnalog(btkAcq,
"Force.Fy" + str(i),
force[:, 1],
desc="virtual Force plate")
btkTools.smartAppendAnalog(btkAcq,
"Force.Fz" + str(i),
force[:, 2],
desc="virtual Force plate")
btkTools.smartAppendAnalog(btkAcq,
"Moment.Mx" + str(i),
moment[:, 0],
desc="virtual Force plate")
btkTools.smartAppendAnalog(btkAcq,
"Moment.My" + str(i),
moment[:, 1],
desc="virtual Force plate")
btkTools.smartAppendAnalog(btkAcq,
"Moment.Mz" + str(i),
moment[:, 2],
desc="virtual Force plate")
new_channel_indexes_ofAnalog = range(numAnalogs, numAnalogs + 6)
numAnalogs = btkAcq.GetAnalogNumber()
btkTools.smartSetMetadata(btkAcq, 'FORCE_PLATFORM', "TYPE", i,
str(2))
newChannelIndexes = newChannelIndexes + new_channel_indexes_ofAnalog
else:
newChannelIndexes = newChannelIndexes + channel_indexes_ofAnalog[i]
md_newChannelIndexes = map(lambda x: x + 1, newChannelIndexes)
md_force_platform_channels.SetInfo(
btk.btkMetaDataInfo([6, int(forcePlateNumber)], md_newChannelIndexes))
btkAcq.GetMetaData().FindChild(str("FORCE_PLATFORM")).value().FindChild(
str("ZERO")).value().SetInfo(
btk.btkMetaDataInfo(btk.btkDoubleArray(forcePlateNumber, 0)))
return btkAcq
def forcePlateTest(cls):
NEXUS = ViconNexus.ViconNexus()
NEXUS_PYTHON_CONNECTED = NEXUS.Client.IsConnected()
DATA_PATH = "C:\\Users\\HLS501\\Documents\\VICON DATA\\pyCGM2-Data\\operations\\NexusAPI\\c3d_x2d\\"
filenameNoExt = "gait_GAP"
NEXUS.OpenTrial(str(DATA_PATH + filenameNoExt), 30)
#----Btk----
acq0 = btkTools.smartReader(str(DATA_PATH + filenameNoExt + ".c3d"))
analogFx = acq0.GetAnalog("Force.Fx1").GetValues()
analogFy = acq0.GetAnalog("Force.Fy1").GetValues()
analogFz = acq0.GetAnalog("Force.Fz1").GetValues()
analogMx = acq0.GetAnalog("Moment.Mx1").GetValues()
analogMy = acq0.GetAnalog("Moment.My1").GetValues()
analogMz = acq0.GetAnalog("Moment.Mz1").GetValues()
pfe = btk.btkForcePlatformsExtractor()
pfe.SetInput(acq0)
pfc = pfe.GetOutput()
pfc.Update()
btkfp0 = pfc.GetItem(0)
ch0_Fx = btkfp0.GetChannel(0).GetValues()
ch1_Fy = btkfp0.GetChannel(1).GetValues()
ch2_Fz = btkfp0.GetChannel(2).GetValues()
ch3_Mx = btkfp0.GetChannel(3).GetValues()
ch4_My = btkfp0.GetChannel(4).GetValues()
ch5_Mz = btkfp0.GetChannel(5).GetValues()
# --- ground reaction force wrench ---
grwf = btk.btkGroundReactionWrenchFilter()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
grw0_force = grwc.GetItem(0).GetForce().GetValues()
# ch3_Mx = btkfp0.GetChannel(3).GetValues()
# ch4_My = btkfp0.GetChannel(4).GetValues()
# ch5_Mz = btkfp0.GetChannel(5).GetValues()
#----Nexus----
nexusForcePlate = Devices.ForcePlate(1)
forceLocal = nexusForcePlate.getLocalReactionForce()
# plt.figure()
# plt.plot(forceLocal[2310:,0],"-r")
# plt.plot(ch0_Fx,"or")
# plt.plot(analogFx,"oc")
# #plt.plot(grw0_force[:,0],"-g")
# #plt.plot(forceGlobal[2320:,0],"og")
#
#
# plt.figure()
# plt.plot(forceLocal[2310:,1],"-r")
# plt.plot(ch1_Fy,"or")
# plt.plot(analogFy,"oc")
#
#
# plt.figure()
# plt.plot(forceLocal[2310:,2],"-r")
# plt.plot(ch2_Fz,"or")
# plt.plot(analogFz,"oc")
# plt.show()
momentLocal = nexusForcePlate.getLocalReactionMoment()
def matchingFootSideOnForceplate(btkAcq,
enableRefine=True,
forceThreshold=25,
left_markerLabelToe="LTOE",
left_markerLabelHeel="LHEE",
right_markerLabelToe="RTOE",
right_markerLabelHeel="RHEE",
display=False):
"""
Convenient function detecting foot in contact with a force plate
**synopsis**
This function firsly assign foot side to FP from minimal distance with the application point of reaction force.
A refinement is done subsequently, it confirm if foot side is valid. A foot is invalided if :
- FP output no data superior to the set threshold
- Foot markers are not contain in the polygon defined by force plate corner
:Parameters:
- `btkAcq` (btkAcquisition) - Btk acquisition instance from a c3d
- `left_markerLabelToe` (str) - label of the left toe marker
- `left_markerLabelHeel` (str) - label of the left heel marker
- `right_markerLabelToe` (str) - label of the right toe marker
- `right_markerLabelHeel` (str) - label of the right heel marker
- `display` (bool) - display n figures ( n depend on force plate number) presenting relative distance between mid foot and the orgin of the force plate
"""
appendForcePlateCornerAsMarker(btkAcq)
ff = btkAcq.GetFirstFrame()
lf = btkAcq.GetLastFrame()
appf = btkAcq.GetNumberAnalogSamplePerFrame()
# --- ground reaction force wrench ---
pfe = btk.btkForcePlatformsExtractor()
grwf = btk.btkGroundReactionWrenchFilter()
pfe.SetInput(btkAcq)
pfc = pfe.GetOutput()
grwf.SetInput(pfc)
grwc = grwf.GetOutput()
grwc.Update()
midfoot_L = (btkAcq.GetPoint(left_markerLabelToe).GetValues() +
btkAcq.GetPoint(left_markerLabelHeel).GetValues()) / 2.0
midfoot_R = (btkAcq.GetPoint(right_markerLabelToe).GetValues() +
btkAcq.GetPoint(right_markerLabelHeel).GetValues()) / 2.0
suffix = str()
for i in range(0, grwc.GetItemNumber()):
pos = grwc.GetItem(i).GetPosition().GetValues()
pos_downsample = pos[0:(lf - ff + 1) * appf:appf] # downsample
diffL = np.linalg.norm(midfoot_L - pos_downsample, axis=1)
diffR = np.linalg.norm(midfoot_R - pos_downsample, axis=1)
if display:
plt.figure()
ax = plt.subplot(1, 1, 1)
plt.title("Force plate " + str(i + 1))
ax.plot(diffL, '-r')
ax.plot(diffR, '-b')
if np.min(diffL) < np.min(diffR):
logging.debug(" Force plate " + str(i) + " : left foot")
suffix = suffix + "L"
else:
logging.debug(" Force plate " + str(i) + " : right foot")
suffix = suffix + "R"
logging.debug("Matched Force plate ===> %s", (suffix))
if enableRefine:
# refinement of suffix
indexFP = 0
for letter in suffix:
force = grwc.GetItem(indexFP).GetForce().GetValues()
force_downsample = force[0:(lf - ff + 1) * appf:appf] # downsample
Rz = np.abs(force_downsample[:, 2])
boolLst = Rz > forceThreshold
enableDataFlag = False
for it in boolLst.tolist():
if it == True:
enableDataFlag = True
break
if not enableDataFlag:
logging.debug(
"PF #%s not activated. It provides no data superior to threshold"
% (str(indexFP)))
li = list(suffix)
li[indexFP] = "X"
suffix = "".join(li)
else:
if letter == "L":
hee = btkAcq.GetPoint(left_markerLabelHeel).GetValues()
toe = btkAcq.GetPoint(left_markerLabelToe).GetValues()
elif letter == "R":
hee = btkAcq.GetPoint(right_markerLabelHeel).GetValues()
toe = btkAcq.GetPoint(right_markerLabelToe).GetValues()
# polygon builder
corner0 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner0").GetValues()[0, :]
corner1 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner1").GetValues()[0, :]
corner2 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner2").GetValues()[0, :]
corner3 = btkAcq.GetPoint("fp" + str(indexFP) +
"corner3").GetValues()[0, :]
verts = [
corner0[0:2], # left, bottom
corner1[0:2], # left, top
corner2[0:2], # right, top
corner3[0:2], # right, bottom
corner0[0:2], # ignored
]
codes = [
Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path = Path(verts, codes)
# check if contain both toe and hee marker
containFlags = list()
for i in range(0, Rz.shape[0]):
if boolLst[i]:
if path.contains_point(
hee[i, 0:2]) and path.contains_point(toe[i,
0:2]):
containFlags.append(True)
else:
containFlags.append(False)
if not all(containFlags) == True:
logging.debug(
"PF #%s not activated. While Rz superior to threshold, foot markers are not contained in force plate geometry "
% (str(indexFP)))
# replace only one character
li = list(suffix)
li[indexFP] = "X"
suffix = "".join(li)
indexFP += 1
return suffix
