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 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 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 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