Esempio n. 1
0
    def calibration_GarchesFlatFoot(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "Datasets Tests\\didier\\08_02_18_Vincent Pere\\"
        staticFilename = "08_02_18_Vincent_Pere_Statique_000_MOKKA.c3d"

        markerDiameter = 14
        mp = {
            'Bodymass': 70.0,
            'LeftLegLength': 890.0,
            'RightLegLength': 890.0,
            'LeftKneeWidth': 150.0,
            'RightKneeWidth': 150.0,
            'LeftAnkleWidth': 88.0,
            'RightAnkleWidth': 99.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            "LeftToeOffset": 0,
            "RightToeOffset": 0,
        }

        # --- Calibration ---
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        model = cgm2.CGM2_4()
        model.configure()

        model.addAnthropoInputParameters(mp)

        # ---- Calibration ----

        scp = modelFilters.StaticCalibrationProcedure(model)
        modelFilters.ModelCalibrationFilter(scp, acqStatic, model).compute()

        # cgm decorator
        modelDecorator.HipJointCenterDecorator(model).hara()
        modelDecorator.KneeCalibrationDecorator(model).midCondyles(
            acqStatic, markerDiameter=markerDiameter, side="both")
        modelDecorator.AnkleCalibrationDecorator(model).midMaleolus(
            acqStatic, markerDiameter=markerDiameter, side="both")

        # final
        modelFilters.ModelCalibrationFilter(scp,
                                            acqStatic,
                                            model,
                                            markerDiameter=markerDiameter,
                                            leftFlatFoot=True,
                                            rightFlatFoot=True).compute()

        # display CS
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqStatic)
        csf.setStatic(True)
        csf.display()

        btkTools.smartWriter(acqStatic, "cgm2.4_GarchesFlatFoot.c3d")
Esempio n. 2
0
    def calibration_FlatFoot(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM2\\cgm2.4\\medial\\"
        staticFilename = "static.c3d"

        markerDiameter = 14
        mp = {
            'Bodymass': 69.0,
            'LeftLegLength': 930.0,
            'RightLegLength': 930.0,
            'LeftKneeWidth': 94.0,
            'RightKneeWidth': 64.0,
            'LeftAnkleWidth': 67.0,
            'RightAnkleWidth': 62.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            "LeftToeOffset": 0,
            "RightToeOffset": 0,
        }

        # --- Calibration ---
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        model = cgm2.CGM2_4()
        model.configure()

        model.addAnthropoInputParameters(mp)

        # ---- Calibration ----

        scp = modelFilters.StaticCalibrationProcedure(model)
        modelFilters.ModelCalibrationFilter(scp, acqStatic, model).compute()

        # cgm decorator
        modelDecorator.HipJointCenterDecorator(model).hara()
        modelDecorator.KneeCalibrationDecorator(model).midCondyles(
            acqStatic, markerDiameter=markerDiameter, side="both")
        modelDecorator.AnkleCalibrationDecorator(model).midMaleolus(
            acqStatic, markerDiameter=markerDiameter, side="both")

        # final
        modelFilters.ModelCalibrationFilter(scp,
                                            acqStatic,
                                            model,
                                            markerDiameter=markerDiameter,
                                            leftFlatFoot=True,
                                            rightFlatFoot=True).compute()

        # display CS
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqStatic)
        csf.setStatic(True)
        csf.display()

        btkTools.smartWriter(acqStatic, "cgm2.4_FlatFoot.c3d")
Esempio n. 3
0
    def CGM1_UpperLimb(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\full-PiG\\"
        staticFilename = "PN01NORMSTAT.c3d"

        acqStatic = btkTools.smartReader(str(MAIN_PATH +  staticFilename))

        model=cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.UpperLimb)


        markerDiameter=14
        mp={
        'LeftShoulderOffset'   : 50,
        'LeftElbowWidth' : 91,
        'LeftWristWidth' : 56 ,
        'LeftHandThickness' : 28 ,
        'RightShoulderOffset'   : 45,
        'RightElbowWidth' : 90,
        'RightWristWidth' : 55 ,
        'RightHandThickness' : 30         }
        model.addAnthropoInputParameters(mp)

         # -----------CGM STATIC CALIBRATION--------------------
        scp=modelFilters.StaticCalibrationProcedure(model)

        modelFilters.ModelCalibrationFilter(scp,acqStatic,model,headFlat= True).compute()
        csp = modelFilters.ModelCoordinateSystemProcedure(model)


        # --- motion ----
        gaitFilename="PN01NORMSS01.c3d"
        acqGait = btkTools.smartReader(str(MAIN_PATH +  gaitFilename))


        modMotion=modelFilters.ModelMotionFilter(scp,acqGait,model,enums.motionMethod.Determinist)
        modMotion.compute()

        csdf = modelFilters.CoordinateSystemDisplayFilter(csp,model,acqGait)
        csdf.setStatic(False)
        csdf.display()

        #   thorax
        R_thorax= model.getSegment("Thorax").anatomicalFrame.motion[10].getRotation()
        R_thorax_vicon = getViconRmatrix(10, acqGait, "TRXO", "TRXA", "TRXL", "XZY")
        np.testing.assert_almost_equal( R_thorax,
                                R_thorax_vicon, decimal =3)

        #   head
        R_head= model.getSegment("Head").anatomicalFrame.motion[10].getRotation()
        R_head_vicon = getViconRmatrix(10, acqGait, "HEDO", "HEDA", "HEDL", "XZY")

        np.testing.assert_almost_equal( R_head,
                                R_head_vicon, decimal =2)
Esempio n. 4
0
    def CGM1_upperLimb(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\full-PiG\\"
        staticFilename = "PN01NORMSTAT.c3d"

        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        model = cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.UpperLimb)

        markerDiameter = 14
        mp = {
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }
        model.addAnthropoInputParameters(mp)

        # -----------CGM STATIC CALIBRATION--------------------
        scp = modelFilters.StaticCalibrationProcedure(model)

        modelFilters.ModelCalibrationFilter(scp,
                                            acqStatic,
                                            model,
                                            headHorizontal=False).compute()

        # --- motion ----
        gaitFilename = "PN01NORMSS01.c3d"
        acqGait = btkTools.smartReader(str(MAIN_PATH + gaitFilename))

        modMotion = modelFilters.ModelMotionFilter(
            scp, acqGait, model, enums.motionMethod.Determinist)
        modMotion.compute()

        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqGait)
        csdf.setStatic(False)
        csdf.display()

        modelFilters.ModelJCSFilter(model, acqGait).compute(
            description="vectoriel", pointLabelSuffix="cgm1_6dof")

        #plot("LNeckAngles",acqGait,"cgm1_6dof")

        np.testing.assert_almost_equal(
            acqGait.GetPoint("LShoulderAngles").GetValues(),
            acqGait.GetPoint("LShoulderAngles_cgm1_6dof").GetValues(),
            decimal=3)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("RShoulderAngles").GetValues(),
            acqGait.GetPoint("RShoulderAngles_cgm1_6dof").GetValues(),
            decimal=3)

        np.testing.assert_almost_equal(
            acqGait.GetPoint("RElbowAngles").GetValues(),
            acqGait.GetPoint("RElbowAngles_cgm1_6dof").GetValues(),
            decimal=3)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LElbowAngles").GetValues(),
            acqGait.GetPoint("LElbowAngles_cgm1_6dof").GetValues(),
            decimal=3)

        np.testing.assert_almost_equal(
            acqGait.GetPoint("RWristAngles").GetValues(),
            acqGait.GetPoint("RWristAngles_cgm1_6dof").GetValues(),
            decimal=3)  # fail on transverse
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LWristAngles").GetValues(),
            acqGait.GetPoint("LWristAngles_cgm1_6dof").GetValues(),
            decimal=3)  # fail on transverse

        np.testing.assert_almost_equal(
            acqGait.GetPoint("RNeckAngles").GetValues(),
            acqGait.GetPoint("RNeckAngles_cgm1_6dof").GetValues(),
            decimal=1)  # fail on coronal
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LNeckAngles").GetValues(),
            acqGait.GetPoint("LNeckAngles_cgm1_6dof").GetValues(),
            decimal=1)  # fail on coronal

        btkTools.smartWriter(acqGait, "upperLimb_angle.c3d")
Esempio n. 5
0
    def CGM1_fullbody_static(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\full-PiG\\"
        staticFilename = "PN01NORMSTAT.c3d"

        # CALIBRATION ###############################
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        markerDiameter = 14

        # Lower Limb
        mp = {
            'Bodymass': 83,
            'LeftLegLength': 874,
            'RightLegLength': 876.0,
            'LeftKneeWidth': 106.0,
            'RightKneeWidth': 103.0,
            'LeftAnkleWidth': 74.0,
            'RightAnkleWidth': 72.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }

        model = cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.FullBody)
        model.addAnthropoInputParameters(mp)

        scp = modelFilters.StaticCalibrationProcedure(
            model)  # load calibration procedure

        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=True,
            rightFlatFoot=True,
            markerDiameter=14,
            viconCGM1compatible=True).compute()

        # MOTION ###############################
        gaitFilename = "PN01NORMSTAT.c3d"
        acqGait = btkTools.smartReader(str(MAIN_PATH + gaitFilename))

        modMotion = modelFilters.ModelMotionFilter(
            scp,
            acqGait,
            model,
            enums.motionMethod.Determinist,
            markerDiameter=14,
            viconCGM1compatible=False)
        modMotion.compute()

        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model,
                                                          acqGait).display()

        modelFilters.ModelJCSFilter(model, acqGait).compute(
            description="vectoriel", pointLabelSuffix="cgm1_6dof")

        plot("RSpineAngles", acqGait, "cgm1_6dof")
        plot("LSpineAngles", acqGait, "cgm1_6dof")

        np.testing.assert_almost_equal(
            acqGait.GetPoint("RSpineAngles").GetValues(),
            acqGait.GetPoint("RSpineAngles_cgm1_6dof").GetValues(),
            decimal=2)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LSpineAngles").GetValues(),
            acqGait.GetPoint("LSpineAngles_cgm1_6dof").GetValues(),
            decimal=2)

        btkTools.smartWriter(acqGait, "fullbody.c3d")
Esempio n. 6
0
    def KadMed_TrueEquinus_leftSkinMarkers(cls):
        DATA_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\kad-med-TrueEquinus-leftSkinMarkers\\"
        staticFilename = "static.c3d"


        markerDiameter=14
        required_mp={
        'Bodymass'   : 36.9,
        'LeftLegLength' : 665.0,
        'RightLegLength' : 655.0 ,
        'LeftKneeWidth' : 102.7,
        'RightKneeWidth' : 100.2,
        'LeftAnkleWidth' : 64.5,
        'RightAnkleWidth' : 63.0,
        'LeftSoleDelta' : 0,
        'RightSoleDelta' : 0,
        }

        optional_mp={
        'InterAsisDistance'   : 0,
        'LeftAsisTrocanterDistance' : 0,
        'LeftThighRotation' : 0,
        'LeftShankRotation' : 0 ,
        'LeftTibialTorsion' : 0,
        'RightAsisTrocanterDistance' : 0,
        'RightThighRotation' : 0,
        'RightShankRotation' : 0,
        'RightTibialTorsion' : 0
        }


        
        settings = files.openJson(pyCGM2.PYCGM2_APPDATA_PATH,"CGM1_1-pyCGM2.settings")
        translators = settings["Translators"]

        pointSuffix = "cgm1_6dof"

        model,acqStatic = cgm1.calibrate(DATA_PATH,staticFilename,translators,
                                   required_mp,optional_mp,
                                   False,False,markerDiameter,
                                   pointSuffix)

        mcsp = modelFilters.ModelCoordinateSystemProcedure(model)
        mcsf = modelFilters.CoordinateSystemDisplayFilter(mcsp,model,acqStatic)
        mcsf.setStatic(False)
        mcsf.display()
        btkTools.smartWriter(acqStatic,"CGM1-KadMed-TrueEquinus-leftSkin-static.c3d")


        #motion
        gaitFilename="gait trial 01.c3d"
        mfpa  = None
        momentProjection=enums.MomentProjection.Distal

        acqGait = cgm1.fitting(model,DATA_PATH, gaitFilename,
                 translators,
                 markerDiameter,
                 pointSuffix,
                 mfpa,
                 momentProjection)


        mcsp = modelFilters.ModelCoordinateSystemProcedure(model)
        mcsf = modelFilters.CoordinateSystemDisplayFilter(mcsp,model,acqGait)
        mcsf.setStatic(False)
        mcsf.display()

        #mcsp = modelFilters.ModelCoordinateSystemProcedure(model)
        #modelFilters.CoordinateSystemDisplayFilter(mcsp,model,acqGait).display()


        btkTools.smartWriter(acqGait, "CGM1-KadMed-TrueEquinus-leftSkin-gait.c3d")


        # testings
        offsetTesting(acqStatic,model,display = True, unitTesting=True)
        testJointCentres(acqGait)
        testRelativesAngles(acqGait,pointSuffix)
Esempio n. 7
0
    def CGM1_fullbody(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\Full PIG - StephenL5_C7\\"
        staticFilename = "PN01NORMSTAT.c3d"

        # CALIBRATION ###############################
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        markerDiameter = 14

        # Lower Limb
        mp = {
            'Bodymass': 83,
            'LeftLegLength': 874,
            'RightLegLength': 876.0,
            'LeftKneeWidth': 106.0,
            'RightKneeWidth': 103.0,
            'LeftAnkleWidth': 74.0,
            'RightAnkleWidth': 72.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }

        model = cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.FullBody)
        model.addAnthropoInputParameters(mp)

        scp = modelFilters.StaticCalibrationProcedure(
            model)  # load calibration procedure
        csp = modelFilters.ModelCoordinateSystemProcedure(model)

        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=False,
            rightFlatFoot=False,
            markerDiameter=14,
            viconCGM1compatible=True,
        ).compute()
        #headHorizontal=True

        # --- motion ----
        gaitFilename = "PN01NORMSS01_stephen.c3d"
        acqGait = btkTools.smartReader(str(MAIN_PATH + gaitFilename))

        modMotion = modelFilters.ModelMotionFilter(
            scp, acqGait, model, enums.motionMethod.Determinist)
        modMotion.compute()

        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqGait)
        csdf.setStatic(False)
        csdf.display()

        # TESTING
        #   thorax
        R_thorax = model.getSegment(
            "Thorax").anatomicalFrame.motion[10].getRotation()
        R_thorax_vicon = getViconRmatrix(10, acqGait, "TRXO", "TRXA", "TRXL",
                                         "XZY")
        np.testing.assert_almost_equal(R_thorax, R_thorax_vicon, decimal=3)

        #   head
        R_head = model.getSegment(
            "Head").anatomicalFrame.motion[10].getRotation()
        R_head_vicon = getViconRmatrix(10, acqGait, "HEDO", "HEDA", "HEDL",
                                       "XZY")

        np.testing.assert_almost_equal(R_head, R_head_vicon, decimal=2)

        btkTools.smartWriter(acqStatic, "FullBody_motion_CGM1_fullbody.c3d")
Esempio n. 8
0
def calibrate(DATA_PATH,calibrateFilenameLabelled,translators,weights,
              required_mp,optional_mp,
              ik_flag,leftFlatFoot,rightFlatFoot,headFlat,
              markerDiameter,hjcMethod,
              pointSuffix,**kwargs):
    """
    Calibration of the CGM2.3

    :param DATA_PATH [str]: path to your data
    :param calibrateFilenameLabelled [str]: c3d file
    :param translators [dict]:  translators to apply
    :param required_mp [dict]: required anthropometric data
    :param optional_mp [dict]: optional anthropometric data (ex: LThighOffset,...)
    :param ik_flag [bool]: enable the inverse kinematic solver
    :param leftFlatFoot [bool]: enable of the flat foot option for the left foot
    :param rightFlatFoot [bool]: enable of the flat foot option for the right foot
    :param headFlat [bool]: enable of the head flat  option
    :param markerDiameter [double]: marker diameter (mm)
    :param hjcMethod [str or list of 3 float]: method for locating the hip joint centre
    :param pointSuffix [str]: suffix to add to model outputs

    """
    # --------------------------STATIC FILE WITH TRANSLATORS --------------------------------------

    if "Fitting" in weights.keys():
        weights  = weights["Fitting"]["Weight"]


    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqStatic = kwargs["forceBtkAcq"]
    else:
        acqStatic = btkTools.smartReader((DATA_PATH+calibrateFilenameLabelled))

    btkTools.checkMultipleSubject(acqStatic)
    if btkTools.isPointExist(acqStatic,"SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        logging.info("[pyCGM2] Sacrum marker detected")

    acqStatic =  btkTools.applyTranslators(acqStatic,translators)

    # ---check marker set used----
    dcm = cgm.CGM.detectCalibrationMethods(acqStatic)

    # --------------------------MODEL--------------------------------------
    # ---definition---
    model=cgm2.CGM2_3()
    model.configure(acq=acqStatic,detectedCalibrationMethods=dcm)
    model.addAnthropoInputParameters(required_mp,optional=optional_mp)

    # --store calibration parameters--
    model.setStaticFilename(calibrateFilenameLabelled)
    model.setCalibrationProperty("leftFlatFoot",leftFlatFoot)
    model.setCalibrationProperty("rightFlatFoot",rightFlatFoot)
    model.setCalibrationProperty("headFlat",headFlat)
    model.setCalibrationProperty("markerDiameter",markerDiameter)




    # --------------------------STATIC CALBRATION--------------------------
    scp=modelFilters.StaticCalibrationProcedure(model) # load calibration procedure

    # ---initial calibration filter----
    # use if all optional mp are zero
    modelFilters.ModelCalibrationFilter(scp,acqStatic,model,
                                        leftFlatFoot = leftFlatFoot, rightFlatFoot = rightFlatFoot,
                                        headFlat= headFlat,
                                        markerDiameter=markerDiameter,
                                        ).compute()

    # ---- Decorators -----
    decorators.applyBasicDecorators(dcm, model,acqStatic,optional_mp,markerDiameter)
    decorators.applyHJCDecorators(model,hjcMethod)


    # ----Final Calibration filter if model previously decorated -----
    if model.decoratedModel:
        # initial static filter
        modelFilters.ModelCalibrationFilter(scp,acqStatic,model,
                           leftFlatFoot = leftFlatFoot, rightFlatFoot = rightFlatFoot,
                           headFlat= headFlat,
                           markerDiameter=markerDiameter).compute()


    # ----------------------CGM MODELLING----------------------------------
    # ----motion filter----
    modMotion=modelFilters.ModelMotionFilter(scp,acqStatic,model,enums.motionMethod.Sodervisk,
                                              markerDiameter=markerDiameter)

    modMotion.compute()

    if "noKinematicsCalculation" in kwargs.keys() and kwargs["noKinematicsCalculation"]:
        logging.warning("[pyCGM2] No Kinematic calculation done for the static file")
        return model, acqStatic
    else:
        if model.getBodyPart() == enums.BodyPart.UpperLimb:
            ik_flag = False
            logging.warning("[pyCGM2] Fitting only applied for the upper limb")

        if ik_flag:
            #                        ---OPENSIM IK---

            # --- opensim calibration Filter ---
            osimfile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\osim\\lowerLimb_ballsJoints.osim"    # osimfile
            markersetFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_3\\cgm2_3-markerset.xml" # markerset
            cgmCalibrationprocedure = opensimFilters.CgmOpensimCalibrationProcedures(model) # procedure

            oscf = opensimFilters.opensimCalibrationFilter(osimfile,
                                                    model,
                                                    cgmCalibrationprocedure,
                                                    (DATA_PATH))
            oscf.addMarkerSet(markersetFile)
            scalingOsim = oscf.build()


            # --- opensim Fitting Filter ---
            iksetupFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_3\\cgm2_3-ikSetUp_template.xml" # ik tool file

            cgmFittingProcedure = opensimFilters.CgmOpensimFittingProcedure(model) # procedure
            cgmFittingProcedure.updateMarkerWeight("LASI",weights["LASI"])
            cgmFittingProcedure.updateMarkerWeight("RASI",weights["RASI"])
            cgmFittingProcedure.updateMarkerWeight("LPSI",weights["LPSI"])
            cgmFittingProcedure.updateMarkerWeight("RPSI",weights["RPSI"])
            cgmFittingProcedure.updateMarkerWeight("RTHI",weights["RTHI"])
            cgmFittingProcedure.updateMarkerWeight("RKNE",weights["RKNE"])
            cgmFittingProcedure.updateMarkerWeight("RTIB",weights["RTIB"])
            cgmFittingProcedure.updateMarkerWeight("RANK",weights["RANK"])
            cgmFittingProcedure.updateMarkerWeight("RHEE",weights["RHEE"])
            cgmFittingProcedure.updateMarkerWeight("RTOE",weights["RTOE"])
            cgmFittingProcedure.updateMarkerWeight("LTHI",weights["LTHI"])
            cgmFittingProcedure.updateMarkerWeight("LKNE",weights["LKNE"])
            cgmFittingProcedure.updateMarkerWeight("LTIB",weights["LTIB"])
            cgmFittingProcedure.updateMarkerWeight("LANK",weights["LANK"])
            cgmFittingProcedure.updateMarkerWeight("LHEE",weights["LHEE"])
            cgmFittingProcedure.updateMarkerWeight("LTOE",weights["LTOE"])

            cgmFittingProcedure.updateMarkerWeight("LTHAP",weights["LTHAP"])
            cgmFittingProcedure.updateMarkerWeight("LTHAD",weights["LTHAD"])
            cgmFittingProcedure.updateMarkerWeight("LTIAP",weights["LTIAP"])
            cgmFittingProcedure.updateMarkerWeight("LTIAD",weights["LTIAD"])
            cgmFittingProcedure.updateMarkerWeight("RTHAP",weights["RTHAP"])
            cgmFittingProcedure.updateMarkerWeight("RTHAD",weights["RTHAD"])
            cgmFittingProcedure.updateMarkerWeight("RTIAP",weights["RTIAP"])
            cgmFittingProcedure.updateMarkerWeight("RTIAD",weights["RTIAD"])

            osrf = opensimFilters.opensimFittingFilter(iksetupFile,
                                                              scalingOsim,
                                                              cgmFittingProcedure,
                                                              (DATA_PATH) )
            acqStaticIK = osrf.run(acqStatic,(DATA_PATH + calibrateFilenameLabelled ))



        # eventual static acquisition to consider for joint kinematics
        finalAcqStatic = acqStaticIK if ik_flag else acqStatic

        # --- final pyCGM2 model motion Filter ---
        # use fitted markers
        modMotionFitted=modelFilters.ModelMotionFilter(scp,finalAcqStatic,model,enums.motionMethod.Sodervisk)
        modMotionFitted.compute()

        if "displayCoordinateSystem" in kwargs.keys() and kwargs["displayCoordinateSystem"]:
            csp = modelFilters.ModelCoordinateSystemProcedure(model)
            csdf = modelFilters.CoordinateSystemDisplayFilter(csp,model,finalAcqStatic)
            csdf.setStatic(False)
            csdf.display()

        #---- Joint kinematics----
        # relative angles
        modelFilters.ModelJCSFilter(model,finalAcqStatic).compute(description="vectoriel", pointLabelSuffix=pointSuffix)

        # detection of traveling axis + absolute angle
        if model.m_bodypart != enums.BodyPart.UpperLimb:
            pfp = progressionFrame.PelvisProgressionFrameProcedure()
        else:
            pfp = progressionFrame.ThoraxProgressionFrameProcedure()

        pff = progressionFrame.ProgressionFrameFilter(finalAcqStatic,pfp)
        pff.compute()
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]


        if model.m_bodypart != enums.BodyPart.UpperLimb:
                modelFilters.ModelAbsoluteAnglesFilter(model,finalAcqStatic,
                                                       segmentLabels=["Left Foot","Right Foot","Pelvis"],
                                                        angleLabels=["LFootProgress", "RFootProgress","Pelvis"],
                                                        eulerSequences=["TOR","TOR", "ROT"],
                                                        globalFrameOrientation = globalFrame,
                                                        forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

        if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
                modelFilters.ModelAbsoluteAnglesFilter(model,finalAcqStatic,
                                              segmentLabels=["Thorax"],
                                              angleLabels=["Thorax"],
                                              eulerSequences=["YXZ"],
                                              globalFrameOrientation = globalFrame,
                                              forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

        if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

                modelFilters.ModelAbsoluteAnglesFilter(model,finalAcqStatic,
                                              segmentLabels=["Thorax","Head"],
                                              angleLabels=["Thorax", "Head"],
                                              eulerSequences=["YXZ","TOR"],
                                              globalFrameOrientation = globalFrame,
                                              forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)
        # BSP model
        bspModel = bodySegmentParameters.Bsp(model)
        bspModel.compute()

        if  model.m_bodypart == enums.BodyPart.FullBody:
            modelFilters.CentreOfMassFilter(model,finalAcqStatic).compute(pointLabelSuffix=pointSuffix)



        return model, finalAcqStatic
Esempio n. 9
0
def fitting(model, DATA_PATH, reconstructFilenameLabelled, translators,
            markerDiameter, pointSuffix, mfpa, momentProjection, **kwargs):
    """
    Fitting of the CGM1.1

    :param model [str]: pyCGM2 model previously calibrated
    :param DATA_PATH [str]: path to your data
    :param reconstructFilenameLabelled [string list]: c3d files
    :param translators [dict]:  translators to apply
    :param mfpa [str]: manual force plate assignement
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs
    :param momentProjection [str]: Coordinate system in which joint moment is expressed

    """
    # --------------------------ACQUISITION ------------------------------------

    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqGait = kwargs["forceBtkAcq"]
    else:
        acqGait = btkTools.smartReader(
            (DATA_PATH + reconstructFilenameLabelled))

    btkTools.checkMultipleSubject(acqGait)
    if btkTools.isPointExist(acqGait, "SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        logging.info("[pyCGM2] Sacrum marker detected")

    acqGait = btkTools.applyTranslators(acqGait, translators)
    trackingMarkers = model.getTrackingMarkers(acqGait)
    validFrames, vff, vlf = btkTools.findValidFrames(acqGait, trackingMarkers)

    # filtering
    # -----------------------
    if "fc_lowPass_marker" in kwargs.keys(
    ) and kwargs["fc_lowPass_marker"] != 0:
        fc = kwargs["fc_lowPass_marker"]
        order = 4
        if "order_lowPass_marker" in kwargs.keys():
            order = kwargs["order_lowPass_marker"]
        signal_processing.markerFiltering(acqGait,
                                          trackingMarkers,
                                          order=order,
                                          fc=fc)

    if "fc_lowPass_forcePlate" in kwargs.keys(
    ) and kwargs["fc_lowPass_forcePlate"] != 0:
        fc = kwargs["fc_lowPass_forcePlate"]
        order = 4
        if "order_lowPass_forcePlate" in kwargs.keys():
            order = kwargs["order_lowPass_forcePlate"]
        signal_processing.forcePlateFiltering(acqGait, order=order, fc=fc)

    scp = modelFilters.StaticCalibrationProcedure(model)  # procedure

    # ---Motion filter----
    modMotion = modelFilters.ModelMotionFilter(scp,
                                               acqGait,
                                               model,
                                               enums.motionMethod.Determinist,
                                               markerDiameter=markerDiameter)

    modMotion.compute()

    if "displayCoordinateSystem" in kwargs.keys(
    ) and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqGait)
        csdf.setStatic(False)
        csdf.display()

    if "NaimKneeCorrection" in kwargs.keys() and kwargs["NaimKneeCorrection"]:

        # Apply Naim 2019 method
        if type(kwargs["NaimKneeCorrection"]) is float:
            nmacp = modelFilters.Naim2019ThighMisaligmentCorrectionProcedure(
                model, "Both", threshold=(kwargs["NaimKneeCorrection"]))
        else:
            nmacp = modelFilters.Naim2019ThighMisaligmentCorrectionProcedure(
                model, "Both")
        mmcf = modelFilters.ModelMotionCorrectionFilter(nmacp)
        mmcf.correct()

        # btkTools.smartAppendPoint(acqGait,"LNaim",mmcf.m_procedure.m_virtual["Left"])
        # btkTools.smartAppendPoint(acqGait,"RNaim",mmcf.m_procedure.m_virtual["Right"])

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model,
                                acqGait).compute(description="vectoriel",
                                                 pointLabelSuffix=pointSuffix)

    # detection of traveling axis + absolute angle
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        pfp = progressionFrame.PelvisProgressionFrameProcedure()
    else:
        pfp = progressionFrame.ThoraxProgressionFrameProcedure()

    pff = progressionFrame.ProgressionFrameFilter(acqGait, pfp)
    pff.compute()
    globalFrame = pff.outputs["globalFrame"]
    forwardProgression = pff.outputs["forwardProgression"]

    if model.m_bodypart != enums.BodyPart.UpperLimb:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Left Foot", "Right Foot", "Pelvis"],
            angleLabels=["LFootProgress", "RFootProgress", "Pelvis"],
            eulerSequences=["TOR", "TOR", "ROT"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Thorax"],
            angleLabels=["Thorax"],
            eulerSequences=["YXZ"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Thorax", "Head"],
            angleLabels=["Thorax", "Head"],
            eulerSequences=["YXZ", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    #---- Body segment parameters----
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    if model.m_bodypart == enums.BodyPart.FullBody:
        modelFilters.CentreOfMassFilter(
            model, acqGait).compute(pointLabelSuffix=pointSuffix)

    # Inverse dynamics
    if btkTools.checkForcePlateExist(acqGait):
        if model.m_bodypart != enums.BodyPart.UpperLimb:
            # --- force plate handling----
            # find foot  in contact
            mappedForcePlate = forceplates.matchingFootSideOnForceplate(
                acqGait, mfpa=mfpa)
            forceplates.addForcePlateGeneralEvents(acqGait, mappedForcePlate)
            logging.warning("Manual Force plate assignment : %s" %
                            mappedForcePlate)

            # assembly foot and force plate
            modelFilters.ForcePlateAssemblyFilter(
                model,
                acqGait,
                mappedForcePlate,
                leftSegmentLabel="Left Foot",
                rightSegmentLabel="Right Foot").compute(
                    pointLabelSuffix=pointSuffix)

            #---- Joint kinetics----
            idp = modelFilters.CGMLowerlimbInverseDynamicProcedure()
            modelFilters.InverseDynamicFilter(
                model,
                acqGait,
                procedure=idp,
                projection=momentProjection,
                globalFrameOrientation=globalFrame,
                forwardProgression=forwardProgression).compute(
                    pointLabelSuffix=pointSuffix)

            #---- Joint energetics----
            modelFilters.JointPowerFilter(
                model, acqGait).compute(pointLabelSuffix=pointSuffix)

    #---- zero unvalid frames ---
    btkTools.applyValidFramesOnOutput(acqGait, validFrames)

    return acqGait
Esempio n. 10
0
def fitting(model, DATA_PATH, reconstructFilenameLabelled, translators,
            weights, ik_flag, markerDiameter, pointSuffix, mfpa,
            momentProjection, **kwargs):
    """
    Fitting of the CGM2.5

    :param model [str]: pyCGM2 model previously calibrated
    :param DATA_PATH [str]: path to your data
    :param reconstructFilenameLabelled [string list]: c3d files
    :param translators [dict]:  translators to apply
    :param ik_flag [bool]: enable the inverse kinematic solver
    :param mfpa [str]: manual force plate assignement
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs
    :param momentProjection [str]: Coordinate system in which joint moment is expressed
    """

    detectAnomaly = False

    if "anomalyException" in kwargs.keys():
        anomalyException = kwargs["anomalyException"]
    else:
        anomalyException = False

    if "forceFoot6DoF" in kwargs.keys() and kwargs["forceFoot6DoF"]:
        forceFoot6DoF_flag = True
    else:
        forceFoot6DoF_flag = False

    if "Fitting" in weights.keys():
        weights = weights["Fitting"]["Weight"]

    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqGait = kwargs["forceBtkAcq"]
    else:
        acqGait = btkTools.smartReader(
            (DATA_PATH + reconstructFilenameLabelled))

    btkTools.checkMultipleSubject(acqGait)
    if btkTools.isPointExist(acqGait, "SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        LOGGER.logger.info("[pyCGM2] Sacrum marker detected")

    acqGait = btkTools.applyTranslators(acqGait, translators)
    trackingMarkers = cgm2.CGM2_5.LOWERLIMB_TRACKING_MARKERS + cgm2.CGM2_5.THORAX_TRACKING_MARKERS + cgm2.CGM2_5.UPPERLIMB_TRACKING_MARKERS
    actual_trackingMarkers, phatoms_trackingMarkers = btkTools.createPhantoms(
        acqGait, trackingMarkers)
    vff, vlf = btkTools.getFrameBoundaries(acqGait, actual_trackingMarkers)
    if "frameInit" in kwargs.keys() and kwargs["frameInit"] is not None:
        vff = kwargs["frameInit"]
        LOGGER.logger.info("[pyCGM2]  first frame forced to frame [%s]" %
                           (vff))
    if "frameEnd" in kwargs.keys() and kwargs["frameEnd"] is not None:
        vlf = kwargs["frameEnd"]
        LOGGER.logger.info("[pyCGM2]  end frame forced to frame [%s]" % (vlf))
    flag = btkTools.getValidFrames(acqGait,
                                   actual_trackingMarkers,
                                   frameBounds=[vff, vlf])

    LOGGER.logger.info("[pyCGM2]  Computation from frame [%s] to frame [%s]" %
                       (vff, vlf))
    # --------------------ANOMALY------------------------------
    for marker in actual_trackingMarkers:
        if marker not in model.getStaticTrackingMarkers():
            LOGGER.logger.warning(
                "[pyCGM2-Anomaly]  marker [%s] - not used during static calibration - wrong kinematic for the segment attached to this marker. "
                % (marker))

    # --marker presence
    markersets = [
        cgm2.CGM2_5.LOWERLIMB_TRACKING_MARKERS,
        cgm2.CGM2_5.THORAX_TRACKING_MARKERS,
        cgm2.CGM2_5.UPPERLIMB_TRACKING_MARKERS
    ]
    for markerset in markersets:
        ipdp = InspectorProcedure.MarkerPresenceDetectionProcedure(markerset)
        idf = InspectorFilter.InspectorFilter(acqGait,
                                              reconstructFilenameLabelled,
                                              ipdp)
        inspector = idf.run()

        # --marker outliers
        if inspector["In"] != []:
            madp = AnomalyDetectionProcedure.MarkerAnomalyDetectionRollingProcedure(
                inspector["In"], plot=False, window=5, threshold=3)
            adf = AnomalyFilter.AnomalyDetectionFilter(
                acqGait,
                reconstructFilenameLabelled,
                madp,
                frameRange=[vff, vlf])
            anomaly = adf.run()
            anomalyIndexes = anomaly["Output"]
            if anomaly["ErrorState"]: detectAnomaly = True

    if btkTools.checkForcePlateExist(acqGait):
        afpp = AnomalyDetectionProcedure.ForcePlateAnomalyProcedure()
        adf = AnomalyFilter.AnomalyDetectionFilter(acqGait,
                                                   reconstructFilenameLabelled,
                                                   afpp,
                                                   frameRange=[vff, vlf])
        anomaly = adf.run()
        if anomaly["ErrorState"]: detectAnomaly = True

    if detectAnomaly and anomalyException:
        raise Exception(
            "Anomalies has been detected - Check Warning message of the log file"
        )

    # --------------------MODELLING------------------------------

    # filtering
    # -----------------------
    if "fc_lowPass_marker" in kwargs.keys(
    ) and kwargs["fc_lowPass_marker"] != 0:
        fc = kwargs["fc_lowPass_marker"]
        order = 4
        if "order_lowPass_marker" in kwargs.keys():
            order = kwargs["order_lowPass_marker"]
        signal_processing.markerFiltering(acqGait,
                                          trackingMarkers,
                                          order=order,
                                          fc=fc)

    if "fc_lowPass_forcePlate" in kwargs.keys(
    ) and kwargs["fc_lowPass_forcePlate"] != 0:
        fc = kwargs["fc_lowPass_forcePlate"]
        order = 4
        if "order_lowPass_forcePlate" in kwargs.keys():
            order = kwargs["order_lowPass_forcePlate"]
        signal_processing.forcePlateFiltering(acqGait, order=order, fc=fc)

    # --- initial motion Filter ---
    scp = modelFilters.StaticCalibrationProcedure(model)
    modMotion = modelFilters.ModelMotionFilter(scp, acqGait, model,
                                               enums.motionMethod.Sodervisk)
    modMotion.compute()

    progressionFlag = False
    if btkTools.isPointExist(acqGait, 'LHEE',
                             ignorePhantom=False) or btkTools.isPointExist(
                                 acqGait, 'RHEE', ignorePhantom=False):

        pfp = progressionFrame.PointProgressionFrameProcedure(marker="LHEE") \
            if btkTools.isPointExist(acqGait, 'LHEE',ignorePhantom=False) \
            else  progressionFrame.PointProgressionFrameProcedure(marker="RHEE")

        pff = progressionFrame.ProgressionFrameFilter(acqGait, pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]
        progressionFlag = True

    elif btkTools.isPointsExist(acqGait, ['LASI', 'RASI', 'RPSI', 'LPSI'],
                                ignorePhantom=False) and not progressionFlag:
        LOGGER.logger.info(
            "[pyCGM2] - progression axis detected from Pelvic markers ")
        pfp = progressionFrame.PelvisProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqGait, pfp)
        pff.compute()
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

        progressionFlag = True
    elif btkTools.isPointsExist(acqGait, ['C7', 'T10', 'CLAV', 'STRN'],
                                ignorePhantom=False) and not progressionFlag:
        LOGGER.logger.info(
            "[pyCGM2] - progression axis detected from Thoracic markers ")
        pfp = progressionFrame.ThoraxProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqGait, pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

    else:
        globalFrame = "XYZ"
        progressionAxis = "X"
        forwardProgression = True
        LOGGER.logger.error(
            "[pyCGM2] - impossible to detect progression axis - neither pelvic nor thoracic markers are present. Progression set to +X by default "
        )

    for target in weights.keys():
        if target not in actual_trackingMarkers or target not in model.getStaticIkTargets(
        ):
            weights[target] = 0
            LOGGER.logger.warning(
                "[pyCGM2] - the IK targeted marker [%s] is not labelled in the acquisition [%s]"
                % (target, reconstructFilenameLabelled))

    if ik_flag:
        #                        ---OPENSIM IK---

        # --- opensim calibration Filter ---
        osimfile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\osim\\lowerLimb_ballsJoints.osim"  # osimfile
        markersetFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_4\\cgm2_4-markerset.xml"  # markerset
        cgmCalibrationprocedure = opensimFilters.CgmOpensimCalibrationProcedures(
            model)  # procedure

        oscf = opensimFilters.opensimCalibrationFilter(
            osimfile, model, cgmCalibrationprocedure, DATA_PATH)
        oscf.addMarkerSet(markersetFile)
        scalingOsim = oscf.build()

        # --- opensim Fitting Filter ---
        iksetupFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_4\\cgm2_4-ikSetUp_template.xml"  # ik tl file

        cgmFittingProcedure = opensimFilters.CgmOpensimFittingProcedure(
            model)  # procedure
        cgmFittingProcedure.updateMarkerWeight("LASI", weights["LASI"])
        cgmFittingProcedure.updateMarkerWeight("RASI", weights["RASI"])
        cgmFittingProcedure.updateMarkerWeight("LPSI", weights["LPSI"])
        cgmFittingProcedure.updateMarkerWeight("RPSI", weights["RPSI"])
        cgmFittingProcedure.updateMarkerWeight("RTHI", weights["RTHI"])
        cgmFittingProcedure.updateMarkerWeight("RKNE", weights["RKNE"])
        cgmFittingProcedure.updateMarkerWeight("RTIB", weights["RTIB"])
        cgmFittingProcedure.updateMarkerWeight("RANK", weights["RANK"])
        cgmFittingProcedure.updateMarkerWeight("RHEE", weights["RHEE"])
        cgmFittingProcedure.updateMarkerWeight("RTOE", weights["RTOE"])

        cgmFittingProcedure.updateMarkerWeight("LTHI", weights["LTHI"])
        cgmFittingProcedure.updateMarkerWeight("LKNE", weights["LKNE"])
        cgmFittingProcedure.updateMarkerWeight("LTIB", weights["LTIB"])
        cgmFittingProcedure.updateMarkerWeight("LANK", weights["LANK"])
        cgmFittingProcedure.updateMarkerWeight("LHEE", weights["LHEE"])
        cgmFittingProcedure.updateMarkerWeight("LTOE", weights["LTOE"])

        cgmFittingProcedure.updateMarkerWeight("LTHAP", weights["LTHAP"])
        cgmFittingProcedure.updateMarkerWeight("LTHAD", weights["LTHAD"])
        cgmFittingProcedure.updateMarkerWeight("LTIAP", weights["LTIAP"])
        cgmFittingProcedure.updateMarkerWeight("LTIAD", weights["LTIAD"])
        cgmFittingProcedure.updateMarkerWeight("RTHAP", weights["RTHAP"])
        cgmFittingProcedure.updateMarkerWeight("RTHAD", weights["RTHAD"])
        cgmFittingProcedure.updateMarkerWeight("RTIAP", weights["RTIAP"])
        cgmFittingProcedure.updateMarkerWeight("RTIAD", weights["RTIAD"])

        cgmFittingProcedure.updateMarkerWeight("LSMH", weights["LSMH"])
        cgmFittingProcedure.updateMarkerWeight("LFMH", weights["LFMH"])
        cgmFittingProcedure.updateMarkerWeight("LVMH", weights["LVMH"])

        cgmFittingProcedure.updateMarkerWeight("RSMH", weights["RSMH"])
        cgmFittingProcedure.updateMarkerWeight("RFMH", weights["RFMH"])
        cgmFittingProcedure.updateMarkerWeight("RVMH", weights["RVMH"])

        #       cgmFittingProcedure.updateMarkerWeight("LTHL",weights["LTHL"])
        #       cgmFittingProcedure.updateMarkerWeight("LTHLD",weights["LTHLD"])
        #       cgmFittingProcedure.updateMarkerWeight("LPAT",weights["LPAT"])
        #       cgmFittingProcedure.updateMarkerWeight("LTIBL",weights["LTIBL"])
        #       cgmFittingProcedure.updateMarkerWeight("RTHL",weights["RTHL"])
        #       cgmFittingProcedure.updateMarkerWeight("RTHLD",weights["RTHLD"])
        #       cgmFittingProcedure.updateMarkerWeight("RPAT",weights["RPAT"])
        #       cgmFittingProcedure.updateMarkerWeight("RTIBL",weights["RTIBL"])

        osrf = opensimFilters.opensimFittingFilter(iksetupFile, scalingOsim,
                                                   cgmFittingProcedure,
                                                   DATA_PATH, acqGait)
        osrf.setTimeRange(acqGait, beginFrame=vff, lastFrame=vlf)
        if "ikAccuracy" in kwargs.keys():
            osrf.setAccuracy(kwargs["ikAccuracy"])

        LOGGER.logger.info("-------INVERSE KINEMATICS IN PROGRESS----------")
        try:
            acqIK = osrf.run(DATA_PATH + reconstructFilenameLabelled,
                             progressionAxis=progressionAxis,
                             forwardProgression=forwardProgression)
            LOGGER.logger.info("[pyCGM2] - IK solver complete")
        except:
            LOGGER.logger.error("[pyCGM2] - IK solver fails")
            acqIK = acqGait
            detectAnomaly = True
        LOGGER.logger.info(
            "---------------------------------------------------")

    # eventual gait acquisition to consider for joint kinematics
    finalAcqGait = acqIK if ik_flag else acqGait

    if "displayCoordinateSystem" in kwargs.keys(
    ) and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(
            csp, model, finalAcqGait)
        csdf.setStatic(False)
        csdf.display()

    # --- final pyCGM2 model motion Filter ---
    # use fitted markers
    modMotionFitted = modelFilters.ModelMotionFilter(
        scp,
        finalAcqGait,
        model,
        enums.motionMethod.Sodervisk,
        markerDiameter=markerDiameter,
        forceFoot6DoF=forceFoot6DoF_flag)

    modMotionFitted.compute()

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model, finalAcqGait).compute(
        description="vectoriel", pointLabelSuffix=pointSuffix)

    modelFilters.ModelAbsoluteAnglesFilter(
        model,
        finalAcqGait,
        segmentLabels=["Left Foot", "Right Foot", "Pelvis", "Thorax", "Head"],
        angleLabels=[
            "LFootProgress", "RFootProgress", "Pelvis", "Thorax", "Head"
        ],
        eulerSequences=["TOR", "TOR", "ROT", "YXZ", "TOR"],
        globalFrameOrientation=globalFrame,
        forwardProgression=forwardProgression).compute(
            pointLabelSuffix=pointSuffix)

    #---- Body segment parameters----
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    modelFilters.CentreOfMassFilter(
        model, finalAcqGait).compute(pointLabelSuffix=pointSuffix)

    # Inverse dynamics
    if btkTools.checkForcePlateExist(acqGait):
        # --- force plate handling----
        # find foot  in contact
        mappedForcePlate = forceplates.matchingFootSideOnForceplate(
            finalAcqGait, mfpa=mfpa)
        forceplates.addForcePlateGeneralEvents(finalAcqGait, mappedForcePlate)
        LOGGER.logger.warning("Manual Force plate assignment : %s" %
                              mappedForcePlate)

        # assembly foot and force plate
        modelFilters.ForcePlateAssemblyFilter(
            model,
            finalAcqGait,
            mappedForcePlate,
            leftSegmentLabel="Left Foot",
            rightSegmentLabel="Right Foot").compute(
                pointLabelSuffix=pointSuffix)

        #---- Joint kinetics----
        idp = modelFilters.CGMLowerlimbInverseDynamicProcedure()
        modelFilters.InverseDynamicFilter(
            model,
            finalAcqGait,
            procedure=idp,
            projection=momentProjection,
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

        #---- Joint energetics----
        modelFilters.JointPowerFilter(
            model, finalAcqGait).compute(pointLabelSuffix=pointSuffix)

    #---- zero unvalid frames ---
    btkTools.cleanAcq(finalAcqGait)
    btkTools.applyOnValidFrames(finalAcqGait, flag)

    if detectAnomaly and not anomalyException:
        LOGGER.logger.error(
            "Anomalies has been detected - Check Warning messages of the log file"
        )

    return finalAcqGait, detectAnomaly
Esempio n. 11
0
def calibrate(DATA_PATH, calibrateFilenameLabelled, translators, weights,
              required_mp, optional_mp, ik_flag, leftFlatFoot, rightFlatFoot,
              headFlat, markerDiameter, hjcMethod, pointSuffix, **kwargs):
    """
    Calibration of the CGM2.5

    :param DATA_PATH [str]: path to your data
    :param calibrateFilenameLabelled [str]: c3d file
    :param translators [dict]:  translators to apply
    :param required_mp [dict]: required anthropometric data
    :param optional_mp [dict]: optional anthropometric data (ex: LThighOffset,...)
    :param ik_flag [bool]: enable the inverse kinematic solver
    :param leftFlatFoot [bool]: enable of the flat foot option for the left foot
    :param rightFlatFoot [bool]: enable of the flat foot option for the right foot
    :param headFlat [bool]: enable of the head flat  option
    :param markerDiameter [double]: marker diameter (mm)
    :param hjcMethod [str or list of 3 float]: method for locating the hip joint centre
    :param pointSuffix [str]: suffix to add to model outputs

    """
    detectAnomaly = False

    if "anomalyException" in kwargs.keys():
        anomalyException = kwargs["anomalyException"]
    else:
        anomalyException = False

    if "Fitting" in weights.keys():
        weights = weights["Fitting"]["Weight"]

    # ---btk acquisition---
    if "forceBtkAcq" in kwargs.keys():
        acqStatic = kwargs["forceBtkAcq"]
    else:
        acqStatic = btkTools.smartReader(
            (DATA_PATH + calibrateFilenameLabelled))

    btkTools.checkMultipleSubject(acqStatic)
    if btkTools.isPointExist(acqStatic, "SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        LOGGER.logger.info("[pyCGM2] Sacrum marker detected")

    acqStatic = btkTools.applyTranslators(acqStatic, translators)

    trackingMarkers = cgm2.CGM2_5.LOWERLIMB_TRACKING_MARKERS + cgm2.CGM2_5.THORAX_TRACKING_MARKERS + cgm2.CGM2_5.UPPERLIMB_TRACKING_MARKERS
    actual_trackingMarkers, phatoms_trackingMarkers = btkTools.createPhantoms(
        acqStatic, trackingMarkers)

    vff = acqStatic.GetFirstFrame()
    vlf = acqStatic.GetLastFrame()
    # vff,vlf = btkTools.getFrameBoundaries(acqStatic,actual_trackingMarkers)
    flag = btkTools.getValidFrames(acqStatic,
                                   actual_trackingMarkers,
                                   frameBounds=[vff, vlf])

    gapFlag = btkTools.checkGap(acqStatic,
                                actual_trackingMarkers,
                                frameBounds=[vff, vlf])
    if gapFlag:
        raise Exception(
            "[pyCGM2] Calibration aborted. Gap find during interval [%i-%i]. Crop your c3d "
            % (vff, vlf))

    # --------------------ANOMALY------------------------------
    # --Check MP
    adap = AnomalyDetectionProcedure.AnthropoDataAnomalyProcedure(required_mp)
    adf = AnomalyFilter.AnomalyDetectionFilter(None, None, adap)
    mp_anomaly = adf.run()
    if mp_anomaly["ErrorState"]: detectAnomaly = True

    # --marker presence
    markersets = [
        cgm2.CGM2_5.LOWERLIMB_TRACKING_MARKERS,
        cgm2.CGM2_5.THORAX_TRACKING_MARKERS,
        cgm2.CGM2_5.UPPERLIMB_TRACKING_MARKERS
    ]
    for markerset in markersets:
        ipdp = InspectorProcedure.MarkerPresenceDetectionProcedure(markerset)
        idf = InspectorFilter.InspectorFilter(acqStatic,
                                              calibrateFilenameLabelled, ipdp)
        inspector = idf.run()

        # # --marker outliers
        if inspector["In"] != []:
            madp = AnomalyDetectionProcedure.MarkerAnomalyDetectionRollingProcedure(
                inspector["In"], plot=False, window=4, threshold=3)
            adf = AnomalyFilter.AnomalyDetectionFilter(
                acqStatic, calibrateFilenameLabelled, madp)
            anomaly = adf.run()
            anomalyIndexes = anomaly["Output"]
            if anomaly["ErrorState"]: detectAnomaly = True

    if detectAnomaly and anomalyException:
        raise Exception(
            "Anomalies has been detected - Check Warning message of the log file"
        )

    # --------------------MODELLING------------------------------

    # ---check marker set used----
    dcm = cgm.CGM.detectCalibrationMethods(acqStatic)

    # --------------------------MODEL--------------------------------------
    # ---definition---
    model = cgm2.CGM2_5()
    model.configure(detectedCalibrationMethods=dcm)
    model.addAnthropoInputParameters(required_mp, optional=optional_mp)

    if dcm["Left Knee"] == enums.JointCalibrationMethod.KAD:
        actual_trackingMarkers.append("LKNE")
    if dcm["Right Knee"] == enums.JointCalibrationMethod.KAD:
        actual_trackingMarkers.append("RKNE")
    model.setStaticTrackingMarkers(actual_trackingMarkers)

    # --store calibration parameters--
    model.setStaticFilename(calibrateFilenameLabelled)
    model.setCalibrationProperty("leftFlatFoot", leftFlatFoot)
    model.setCalibrationProperty("rightFlatFoot", rightFlatFoot)
    model.setCalibrationProperty("headFlat", headFlat)
    model.setCalibrationProperty("markerDiameter", markerDiameter)

    # --------------------------STATIC CALBRATION--------------------------
    scp = modelFilters.StaticCalibrationProcedure(
        model)  # load calibration procedure

    # ---initial calibration filter----
    # use if all optional mp are zero
    modelFilters.ModelCalibrationFilter(
        scp,
        acqStatic,
        model,
        leftFlatFoot=leftFlatFoot,
        rightFlatFoot=rightFlatFoot,
        headFlat=headFlat,
        markerDiameter=markerDiameter,
    ).compute()

    # ---- Decorators -----
    decorators.applyBasicDecorators(dcm, model, acqStatic, optional_mp,
                                    markerDiameter)
    decorators.applyHJCDecorators(model, hjcMethod)

    # ----Final Calibration filter if model previously decorated -----
    if model.decoratedModel:
        # initial static filter
        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=leftFlatFoot,
            rightFlatFoot=rightFlatFoot,
            headFlat=headFlat,
            markerDiameter=markerDiameter).compute()

    # ----------------------CGM MODELLING----------------------------------
    # ----motion filter----
    modMotion = modelFilters.ModelMotionFilter(scp,
                                               acqStatic,
                                               model,
                                               enums.motionMethod.Sodervisk,
                                               markerDiameter=markerDiameter)

    modMotion.compute()

    # ----progression Frame----
    progressionFlag = False
    if btkTools.isPointsExist(acqStatic, ['LASI', 'RASI', 'RPSI', 'LPSI'],
                              ignorePhantom=False):
        LOGGER.logger.info(
            "[pyCGM2] - progression axis detected from Pelvic markers ")
        pfp = progressionFrame.PelvisProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqStatic, pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]
        progressionFlag = True
    elif btkTools.isPointsExist(acqStatic, ['C7', 'T10', 'CLAV', 'STRN'],
                                ignorePhantom=False) and not progressionFlag:
        LOGGER.logger.info(
            "[pyCGM2] - progression axis detected from Thoracic markers ")
        pfp = progressionFrame.ThoraxProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqStatic, pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

    else:
        globalFrame = "XYZ"
        progressionAxis = "X"
        forwardProgression = True
        LOGGER.logger.error(
            "[pyCGM2] - impossible to detect progression axis - neither pelvic nor thoracic markers are present. Progression set to +X by default "
        )

    # ----manage IK Targets----
    ikTargets = list()
    for target in weights.keys():
        if target not in actual_trackingMarkers:
            weights[target] = 0
            LOGGER.logger.warning(
                "[pyCGM2] - the IK targeted marker [%s] is not labelled in the acquisition [%s]"
                % (target, calibrateFilenameLabelled))
        else:
            ikTargets.append(target)
    model.setStaticIkTargets(ikTargets)

    if "noKinematicsCalculation" in kwargs.keys(
    ) and kwargs["noKinematicsCalculation"]:
        LOGGER.logger.warning(
            "[pyCGM2] No Kinematic calculation done for the static file")
        return model, acqStatic, detectAnomaly
    else:

        if ik_flag:
            #                        ---OPENSIM IK---

            # --- opensim calibration Filter ---
            osimfile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\osim\\lowerLimb_ballsJoints.osim"  # osimfile
            markersetFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_4\\cgm2_4-markerset.xml"  # markerset
            cgmCalibrationprocedure = opensimFilters.CgmOpensimCalibrationProcedures(
                model)  # procedure

            oscf = opensimFilters.opensimCalibrationFilter(
                osimfile, model, cgmCalibrationprocedure, DATA_PATH)
            oscf.addMarkerSet(markersetFile)
            scalingOsim = oscf.build()

            # --- opensim Fitting Filter ---
            iksetupFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_4\\cgm2_4-ikSetUp_template.xml"  # ik tool file

            cgmFittingProcedure = opensimFilters.CgmOpensimFittingProcedure(
                model)  # procedure
            cgmFittingProcedure.updateMarkerWeight("LASI", weights["LASI"])
            cgmFittingProcedure.updateMarkerWeight("RASI", weights["RASI"])
            cgmFittingProcedure.updateMarkerWeight("LPSI", weights["LPSI"])
            cgmFittingProcedure.updateMarkerWeight("RPSI", weights["RPSI"])
            cgmFittingProcedure.updateMarkerWeight("RTHI", weights["RTHI"])
            cgmFittingProcedure.updateMarkerWeight("RKNE", weights["RKNE"])
            cgmFittingProcedure.updateMarkerWeight("RTIB", weights["RTIB"])
            cgmFittingProcedure.updateMarkerWeight("RANK", weights["RANK"])
            cgmFittingProcedure.updateMarkerWeight("RHEE", weights["RHEE"])
            cgmFittingProcedure.updateMarkerWeight("RTOE", weights["RTOE"])

            cgmFittingProcedure.updateMarkerWeight("LTHI", weights["LTHI"])
            cgmFittingProcedure.updateMarkerWeight("LKNE", weights["LKNE"])
            cgmFittingProcedure.updateMarkerWeight("LTIB", weights["LTIB"])
            cgmFittingProcedure.updateMarkerWeight("LANK", weights["LANK"])
            cgmFittingProcedure.updateMarkerWeight("LHEE", weights["LHEE"])
            cgmFittingProcedure.updateMarkerWeight("LTOE", weights["LTOE"])

            cgmFittingProcedure.updateMarkerWeight("LTHAP", weights["LTHAP"])
            cgmFittingProcedure.updateMarkerWeight("LTHAD", weights["LTHAD"])
            cgmFittingProcedure.updateMarkerWeight("LTIAP", weights["LTIAP"])
            cgmFittingProcedure.updateMarkerWeight("LTIAD", weights["LTIAD"])
            cgmFittingProcedure.updateMarkerWeight("RTHAP", weights["RTHAP"])
            cgmFittingProcedure.updateMarkerWeight("RTHAD", weights["RTHAD"])
            cgmFittingProcedure.updateMarkerWeight("RTIAP", weights["RTIAP"])
            cgmFittingProcedure.updateMarkerWeight("RTIAD", weights["RTIAD"])

            cgmFittingProcedure.updateMarkerWeight("LSMH", weights["LSMH"])
            cgmFittingProcedure.updateMarkerWeight("LFMH", weights["LFMH"])
            cgmFittingProcedure.updateMarkerWeight("LVMH", weights["LVMH"])

            cgmFittingProcedure.updateMarkerWeight("RSMH", weights["RSMH"])
            cgmFittingProcedure.updateMarkerWeight("RFMH", weights["RFMH"])
            cgmFittingProcedure.updateMarkerWeight("RVMH", weights["RVMH"])

            #            cgmFittingProcedure.updateMarkerWeight("LTHL",weights["LTHL"])
            #            cgmFittingProcedure.updateMarkerWeight("LTHLD",weights["LTHLD"])
            #            cgmFittingProcedure.updateMarkerWeight("LPAT",weights["LPAT"])
            #            cgmFittingProcedure.updateMarkerWeight("LTIBL",weights["LTIBL"])
            #            cgmFittingProcedure.updateMarkerWeight("RTHL",weights["RTHL"])
            #            cgmFittingProcedure.updateMarkerWeight("RTHLD",weights["RTHLD"])
            #            cgmFittingProcedure.updateMarkerWeight("RPAT",weights["RPAT"])
            #            cgmFittingProcedure.updateMarkerWeight("RTIBL",weights["RTIBL"])

            osrf = opensimFilters.opensimFittingFilter(iksetupFile,
                                                       scalingOsim,
                                                       cgmFittingProcedure,
                                                       DATA_PATH,
                                                       acqStatic,
                                                       accuracy=1e-5)

            LOGGER.logger.info(
                "-------INVERSE KINEMATICS IN PROGRESS----------")
            try:
                acqStaticIK = osrf.run(DATA_PATH + calibrateFilenameLabelled,
                                       progressionAxis=progressionAxis,
                                       forwardProgression=forwardProgression)
                LOGGER.logger.info("[pyCGM2] - IK solver complete")
            except:
                LOGGER.logger.error("[pyCGM2] - IK solver fails")
                acqStaticIK = acqStatic
                detectAnomaly = True
            LOGGER.logger.info(
                "-----------------------------------------------")

        # eventual static acquisition to consider for joint kinematics
        finalAcqStatic = acqStaticIK if ik_flag else acqStatic

        # --- final pyCGM2 model motion Filter ---
        # use fitted markers
        modMotionFitted = modelFilters.ModelMotionFilter(
            scp, finalAcqStatic, model, enums.motionMethod.Sodervisk)
        modMotionFitted.compute()

        if "displayCoordinateSystem" in kwargs.keys(
        ) and kwargs["displayCoordinateSystem"]:
            csp = modelFilters.ModelCoordinateSystemProcedure(model)
            csdf = modelFilters.CoordinateSystemDisplayFilter(
                csp, model, finalAcqStatic)
            csdf.setStatic(False)
            csdf.display()

        #---- Joint kinematics----
        # relative angles
        modelFilters.ModelJCSFilter(model, finalAcqStatic).compute(
            description="vectoriel", pointLabelSuffix=pointSuffix)

        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            finalAcqStatic,
            segmentLabels=[
                "Left Foot", "Right Foot", "Pelvis", "Thorax", "Head"
            ],
            angleLabels=[
                "LFootProgress", "RFootProgress", "Pelvis", "Thorax", "Head"
            ],
            eulerSequences=["TOR", "TOR", "ROT", "YXZ", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)
        # BSP model
        bspModel = bodySegmentParameters.Bsp(model)
        bspModel.compute()

        modelFilters.CentreOfMassFilter(
            model, finalAcqStatic).compute(pointLabelSuffix=pointSuffix)

        btkTools.cleanAcq(finalAcqStatic)
        if detectAnomaly and not anomalyException:
            LOGGER.logger.error(
                "Anomalies has been detected - Check Warning messages of the log file"
            )

        return model, finalAcqStatic, detectAnomaly
Esempio n. 12
0
def fitting(model, DATA_PATH, reconstructFilenameLabelled, translators,
            settings, markerDiameter, pointSuffix, mfpa, momentProjection,
            **kwargs):
    """
    Fitting of the CGM2.2

    :param model [str]: pyCGM2 model previously calibrated
    :param DATA_PATH [str]: path to your data
    :param reconstructFilenameLabelled [string list]: c3d files
    :param translators [dict]:  translators to apply
    :param mfpa [str]: manual force plate assignement
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs
    :param momentProjection [str]: Coordinate system in which joint moment is expressed
    """

    # --------------------------ACQUISITION ------------------------------------

    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqGait = kwargs["forceBtkAcq"]
    else:
        acqGait = btkTools.smartReader(
            str(DATA_PATH + reconstructFilenameLabelled))

    btkTools.checkMultipleSubject(acqGait)
    acqGait = btkTools.applyTranslators(acqGait, translators)
    trackingMarkers = model.getTrackingMarkers()
    validFrames, vff, vlf = btkTools.findValidFrames(acqGait, trackingMarkers)

    # --- initial motion Filter ---
    scp = modelFilters.StaticCalibrationProcedure(model)
    modMotion = modelFilters.ModelMotionFilter(scp, acqGait, model,
                                               enums.motionMethod.Determinist)
    modMotion.compute()

    #                        ---OPENSIM IK---

    # --- opensim calibration Filter ---
    osimfile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\osim\\lowerLimb_ballsJoints.osim"  # osimfile
    markersetFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm1\\cgm1-markerset.xml"  # markerset
    cgmCalibrationprocedure = opensimFilters.CgmOpensimCalibrationProcedures(
        model)  # procedure

    oscf = opensimFilters.opensimCalibrationFilter(osimfile, model,
                                                   cgmCalibrationprocedure,
                                                   str(DATA_PATH))
    oscf.addMarkerSet(markersetFile)
    scalingOsim = oscf.build()

    # --- opensim Fitting Filter ---
    iksetupFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm1\\cgm1-ikSetUp_template.xml"  # ik tool file

    cgmFittingProcedure = opensimFilters.CgmOpensimFittingProcedure(
        model)  # procedure
    cgmFittingProcedure.updateMarkerWeight(
        "LASI", settings["Fitting"]["Weight"]["LASI"])
    cgmFittingProcedure.updateMarkerWeight(
        "RASI", settings["Fitting"]["Weight"]["RASI"])
    cgmFittingProcedure.updateMarkerWeight(
        "LPSI", settings["Fitting"]["Weight"]["LPSI"])
    cgmFittingProcedure.updateMarkerWeight(
        "RPSI", settings["Fitting"]["Weight"]["RPSI"])
    cgmFittingProcedure.updateMarkerWeight(
        "RTHI", settings["Fitting"]["Weight"]["RTHI"])
    cgmFittingProcedure.updateMarkerWeight(
        "RKNE", settings["Fitting"]["Weight"]["RKNE"])
    cgmFittingProcedure.updateMarkerWeight(
        "RTIB", settings["Fitting"]["Weight"]["RTIB"])
    cgmFittingProcedure.updateMarkerWeight(
        "RANK", settings["Fitting"]["Weight"]["RANK"])
    cgmFittingProcedure.updateMarkerWeight(
        "RHEE", settings["Fitting"]["Weight"]["RHEE"])
    cgmFittingProcedure.updateMarkerWeight(
        "RTOE", settings["Fitting"]["Weight"]["RTOE"])
    cgmFittingProcedure.updateMarkerWeight(
        "LTHI", settings["Fitting"]["Weight"]["LTHI"])
    cgmFittingProcedure.updateMarkerWeight(
        "LKNE", settings["Fitting"]["Weight"]["LKNE"])
    cgmFittingProcedure.updateMarkerWeight(
        "LTIB", settings["Fitting"]["Weight"]["LTIB"])
    cgmFittingProcedure.updateMarkerWeight(
        "LANK", settings["Fitting"]["Weight"]["LANK"])
    cgmFittingProcedure.updateMarkerWeight(
        "LHEE", settings["Fitting"]["Weight"]["LHEE"])
    cgmFittingProcedure.updateMarkerWeight(
        "LTOE", settings["Fitting"]["Weight"]["LTOE"])

    osrf = opensimFilters.opensimFittingFilter(iksetupFile, scalingOsim,
                                               cgmFittingProcedure,
                                               str(DATA_PATH))

    logging.info("-------INVERSE KINEMATICS IN PROGRESS----------")
    acqIK = osrf.run(acqGait, str(DATA_PATH + reconstructFilenameLabelled))
    logging.info("-------INVERSE KINEMATICS DONE-----------------")

    # --- final pyCGM2 model motion Filter ---
    # use fitted markers
    modMotionFitted = modelFilters.ModelMotionFilter(
        scp,
        acqIK,
        model,
        enums.motionMethod.Determinist,
        markerDiameter=markerDiameter)

    modMotionFitted.compute()

    if "displayCoordinateSystem" in kwargs.keys(
    ) and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqIK)
        csdf.setStatic(False)
        csdf.display()

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model,
                                acqIK).compute(description="vectoriel",
                                               pointLabelSuffix=pointSuffix)

    # detection of traveling axis + absolute angle
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromPelvicMarkers(
            acqIK, ["LASI", "LPSI", "RASI", "RPSI"])
    else:
        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromLongAxis(
            acqIK, "C7", "CLAV")

    if model.m_bodypart != enums.BodyPart.UpperLimb:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqIK,
            segmentLabels=["Left Foot", "Right Foot", "Pelvis"],
            angleLabels=["LFootProgress", "RFootProgress", "Pelvis"],
            eulerSequences=["TOR", "TOR", "ROT"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqIK,
            segmentLabels=["Thorax"],
            angleLabels=["Thorax"],
            eulerSequences=["YXZ"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqIK,
            segmentLabels=["Thorax", "Head"],
            angleLabels=["Thorax", "Head"],
            eulerSequences=["YXZ", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    #---- Body segment parameters----
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    if model.m_bodypart == enums.BodyPart.FullBody:
        modelFilters.CentreOfMassFilter(
            model, acqIK).compute(pointLabelSuffix=pointSuffix)

    # Inverse dynamics
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        # --- force plate handling----
        # find foot  in contact
        mappedForcePlate = forceplates.matchingFootSideOnForceplate(acqIK,
                                                                    mfpa=mfpa)
        forceplates.addForcePlateGeneralEvents(acqIK, mappedForcePlate)
        logging.warning("Manual Force plate assignment : %s" %
                        mappedForcePlate)

        # assembly foot and force plate
        modelFilters.ForcePlateAssemblyFilter(
            model,
            acqIK,
            mappedForcePlate,
            leftSegmentLabel="Left Foot",
            rightSegmentLabel="Right Foot").compute()

        #---- Joint kinetics----
        idp = modelFilters.CGMLowerlimbInverseDynamicProcedure()
        modelFilters.InverseDynamicFilter(
            model,
            acqIK,
            procedure=idp,
            projection=momentProjection,
        ).compute(pointLabelSuffix=pointSuffix)

        #---- Joint energetics----
        modelFilters.JointPowerFilter(
            model, acqIK).compute(pointLabelSuffix=pointSuffix)

    #---- zero unvalid frames ---
    btkTools.applyValidFramesOnOutput(acqIK, validFrames)

    return acqIK
Esempio n. 13
0
    def CGM1_fullbody_onStatic(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\Full PIG - StephenL5_C7\\"
        staticFilename = "PN01NORMSTAT.c3d"

        # CALIBRATION ###############################
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        markerDiameter = 14

        # Lower Limb
        mp = {
            'Bodymass': 83,
            'LeftLegLength': 874,
            'RightLegLength': 876.0,
            'LeftKneeWidth': 106.0,
            'RightKneeWidth': 103.0,
            'LeftAnkleWidth': 74.0,
            'RightAnkleWidth': 72.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }

        model = cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.FullBody)
        model.addAnthropoInputParameters(mp)

        scp = modelFilters.StaticCalibrationProcedure(
            model)  # load calibration procedure
        csp = modelFilters.ModelCoordinateSystemProcedure(model)

        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=False,
            rightFlatFoot=False,
            markerDiameter=14,
            viconCGM1compatible=True,
        ).compute()
        #headHorizontal=True

        # --- motion ----
        gaitFilename = "PN01NORMSTAT_stephen.c3d"
        acqGait = btkTools.smartReader(str(MAIN_PATH + gaitFilename))

        modMotion = modelFilters.ModelMotionFilter(
            scp, acqGait, model, enums.motionMethod.Determinist)
        modMotion.compute()

        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqGait)
        csdf.setStatic(False)
        csdf.display()

        # angles
        modelFilters.ModelJCSFilter(model, acqGait).compute(
            description="vectoriel", pointLabelSuffix="cgm1_6dof")

        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromLongAxis(
            acqGait, "C7", "CLAV")
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Thorax"],
            angleLabels=[
                "Thorax",
            ],
            eulerSequences=["YXZ"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix="cgm1_6dof")

        # testing
        np.testing.assert_equal(longitudinalAxis, "X")
        np.testing.assert_equal(forwardProgression, True)
        np.testing.assert_equal(globalFrame, "XYZ")

        # BSP model
        bspModel = bodySegmentParameters.Bsp(model)
        bspModel.compute()

        btkTools.smartAppendPoint(
            acqGait, "pelvisCOM_py",
            model.getSegment("Pelvis").getComTrajectory())
        btkTools.smartAppendPoint(acqGait, "headCOM_py",
                                  model.getSegment("Head").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "ThoraxCOM_py",
            model.getSegment("Thorax").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "LhumCOM_py",
            model.getSegment("Left UpperArm").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "LforeCom_py",
            model.getSegment("Left ForeArm").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "LhandCom_py",
            model.getSegment("Left Hand").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "RhumCOM_py",
            model.getSegment("Right UpperArm").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "RforeCom_py",
            model.getSegment("Right ForeArm").getComTrajectory())
        btkTools.smartAppendPoint(
            acqGait, "RhandCom_py",
            model.getSegment("Right Hand").getComTrajectory())

        modelFilters.CentreOfMassFilter(
            model, acqGait).compute(pointLabelSuffix="py2")

        TL5_pelvis = model.getSegment(
            "Pelvis").anatomicalFrame.getNodeTrajectory("TL5")
        TL5_thorax = model.getSegment(
            "Thorax").anatomicalFrame.getNodeTrajectory("TL5")
        C7o_thorax = model.getSegment(
            "Thorax").anatomicalFrame.getNodeTrajectory("C7o")
        C7_thorax = model.getSegment(
            "Thorax").anatomicalFrame.getNodeTrajectory("C7")
        C7_head = model.getSegment("Head").anatomicalFrame.getNodeTrajectory(
            "C7")
        btkTools.smartAppendPoint(acqGait, "TL5_pelvis", TL5_pelvis, desc="")
        btkTools.smartAppendPoint(acqGait, "TL5_thorax", TL5_thorax, desc="")
        btkTools.smartAppendPoint(acqGait, "C7o_thorax", C7o_thorax, desc="")
        btkTools.smartAppendPoint(acqGait, "C7_thorax", C7_thorax, desc="")
        btkTools.smartAppendPoint(acqGait, "C7_head", C7_thorax, desc="")

        btkTools.smartWriter(acqGait,
                             "FullBody_COM_CGM1_fullbody_onStatic.c3d")
Esempio n. 14
0
    def CGM1_fullbody(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\Full PIG - StephenL5_C7\\"
        staticFilename = "PN01NORMSTAT.c3d"

        # CALIBRATION ###############################
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        markerDiameter = 14

        # Lower Limb
        mp = {
            'Bodymass': 83,
            'LeftLegLength': 874,
            'RightLegLength': 876.0,
            'LeftKneeWidth': 106.0,
            'RightKneeWidth': 103.0,
            'LeftAnkleWidth': 74.0,
            'RightAnkleWidth': 72.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }

        model = cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.FullBody)
        model.addAnthropoInputParameters(mp)

        scp = modelFilters.StaticCalibrationProcedure(
            model)  # load calibration procedure

        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=False,
            rightFlatFoot=False,
            markerDiameter=14,
            viconCGM1compatible=True,
        ).compute()
        #headHorizontal=True

        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(
            csp, model, acqStatic)
        csdf.setStatic(True)
        csdf.display()

        # joint centres
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("TRXO").GetValues().mean(axis=0),
            acqStatic.GetPoint("OT").GetValues().mean(axis=0),
            decimal=3)

        np.testing.assert_almost_equal(
            acqStatic.GetPoint("LCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("LSJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("LHUO").GetValues().mean(axis=0),
            acqStatic.GetPoint("LEJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("LCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("LSJC").GetValues().mean(axis=0),
            decimal=3)

        np.testing.assert_almost_equal(
            acqStatic.GetPoint("RCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("RSJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("RHUO").GetValues().mean(axis=0),
            acqStatic.GetPoint("REJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("RCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("RSJC").GetValues().mean(axis=0),
            decimal=3)

        btkTools.smartWriter(acqStatic, "FullBody_Static_CGM1_fullbody.c3d")
Esempio n. 15
0
    def CGM1_fullbody(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\Full PIG - StephenL5_C7\\"
        staticFilename = "PN01NORMSTAT.c3d"

        # CALIBRATION ###############################
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        markerDiameter = 14

        # Lower Limb
        mp = {
            'Bodymass': 83,
            'LeftLegLength': 874,
            'RightLegLength': 876.0,
            'LeftKneeWidth': 106.0,
            'RightKneeWidth': 103.0,
            'LeftAnkleWidth': 74.0,
            'RightAnkleWidth': 72.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }

        model = cgm.CGM1()
        model.configure(bodyPart=enums.BodyPart.FullBody)
        model.addAnthropoInputParameters(mp)

        scp = modelFilters.StaticCalibrationProcedure(
            model)  # load calibration procedure
        csp = modelFilters.ModelCoordinateSystemProcedure(model)

        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=False,
            rightFlatFoot=False,
            markerDiameter=14,
            viconCGM1compatible=True,
        ).compute()
        #headHorizontal=True

        # --- motion ----
        gaitFilename = "PN01NORMSS01_stephen.c3d"
        acqGait = btkTools.smartReader(str(MAIN_PATH + gaitFilename))

        modMotion = modelFilters.ModelMotionFilter(
            scp, acqGait, model, enums.motionMethod.Determinist)
        modMotion.compute()

        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqGait)
        csdf.setStatic(False)
        csdf.display()

        # angles
        modelFilters.ModelJCSFilter(model, acqGait).compute(
            description="vectoriel", pointLabelSuffix="cgm1_6dof")

        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromLongAxis(
            acqGait, "C7", "CLAV")
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Thorax"],
            angleLabels=[
                "Thorax",
            ],
            eulerSequences=["YXZ"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix="cgm1_6dof")

        # testing
        np.testing.assert_equal(longitudinalAxis, "X")
        np.testing.assert_equal(forwardProgression, False)
        np.testing.assert_equal(globalFrame, "XYZ")

        #plot("LNeckAngles",acqGait,"cgm1_6dof")

        # relative angles
        np.testing.assert_almost_equal(
            acqGait.GetPoint("RSpineAngles").GetValues(),
            acqGait.GetPoint("RSpineAngles_cgm1_6dof").GetValues(),
            decimal=2)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LSpineAngles").GetValues(),
            acqGait.GetPoint("LSpineAngles_cgm1_6dof").GetValues(),
            decimal=2)

        np.testing.assert_almost_equal(
            acqGait.GetPoint("LShoulderAngles").GetValues(),
            acqGait.GetPoint("LShoulderAngles_cgm1_6dof").GetValues(),
            decimal=3)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("RShoulderAngles").GetValues(),
            acqGait.GetPoint("RShoulderAngles_cgm1_6dof").GetValues(),
            decimal=3)

        np.testing.assert_almost_equal(
            acqGait.GetPoint("RElbowAngles").GetValues(),
            acqGait.GetPoint("RElbowAngles_cgm1_6dof").GetValues(),
            decimal=3)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LElbowAngles").GetValues(),
            acqGait.GetPoint("LElbowAngles_cgm1_6dof").GetValues(),
            decimal=3)

        # np.testing.assert_almost_equal( acqGait.GetPoint("RWristAngles").GetValues(),
        #                             acqGait.GetPoint("RWristAngles_cgm1_6dof").GetValues(), decimal =3) # fail on transverse
        # np.testing.assert_almost_equal( acqGait.GetPoint("LWristAngles").GetValues(),
        #                             acqGait.GetPoint("LWristAngles_cgm1_6dof").GetValues(), decimal =3)# fail on transverse
        #
        # np.testing.assert_almost_equal( acqGait.GetPoint("RNeckAngles").GetValues(),
        #                             acqGait.GetPoint("RNeckAngles_cgm1_6dof").GetValues(), decimal =1) # fail on coronal
        # np.testing.assert_almost_equal( acqGait.GetPoint("LNeckAngles").GetValues(),
        #                             acqGait.GetPoint("LNeckAngles_cgm1_6dof").GetValues(), decimal =1) # fail on coronal

        # absolute angles
        np.testing.assert_almost_equal(
            acqGait.GetPoint("LThoraxAngles").GetValues(),
            acqGait.GetPoint("LThoraxAngles_cgm1_6dof").GetValues(),
            decimal=3)
        np.testing.assert_almost_equal(
            acqGait.GetPoint("RThoraxAngles").GetValues(),
            acqGait.GetPoint("RThoraxAngles_cgm1_6dof").GetValues(),
            decimal=3)

        btkTools.smartWriter(acqStatic, "FullBody_Angles_CGM1_fullbody.c3d")
Esempio n. 16
0
def calibrate(DATA_PATH,calibrateFilenameLabelled,translators,
              required_mp,optional_mp,
              leftFlatFoot,rightFlatFoot,headFlat,
              markerDiameter,hjcMethod,
              pointSuffix,**kwargs):

    """
    Calibration of the CGM2.1

    :param DATA_PATH [str]: path to your data
    :param calibrateFilenameLabelled [str]: c3d file
    :param translators [dict]:  translators to apply
    :param required_mp [dict]: required anthropometric data
    :param optional_mp [dict]: optional anthropometric data (ex: LThighOffset,...)
    :param leftFlatFoot [bool]: enable of the flat foot option for the left foot
    :param rightFlatFoot [bool]: enable of the flat foot option for the right foot
    :param headFlat [bool]: enable of the head flat  option
    :param markerDiameter [double]: marker diameter (mm)
    :param hjcMethod [str or list of 3 float]: method for locating the hip joint centre
    :param pointSuffix [str]: suffix to add to model outputs

    """
    detectAnomaly = False
    if "anomalyException" in kwargs.keys():
        anomalyException = kwargs["anomalyException"]
    else:
        anomalyException=False
    # --------------------------ACQUISITION ------------------------------------

    # ---btk acquisition---
    if "forceBtkAcq" in kwargs.keys():
        acqStatic = kwargs["forceBtkAcq"]
    else:
        acqStatic = btkTools.smartReader((DATA_PATH+calibrateFilenameLabelled))

    btkTools.checkMultipleSubject(acqStatic)
    if btkTools.isPointExist(acqStatic,"SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        LOGGER.logger.info("[pyCGM2] Sacrum marker detected")

    acqStatic =  btkTools.applyTranslators(acqStatic,translators)

    trackingMarkers = cgm.CGM1.LOWERLIMB_TRACKING_MARKERS + cgm.CGM1.THORAX_TRACKING_MARKERS+ cgm.CGM1.UPPERLIMB_TRACKING_MARKERS
    actual_trackingMarkers,phatoms_trackingMarkers = btkTools.createPhantoms(acqStatic, trackingMarkers)

    vff = acqStatic.GetFirstFrame()
    vlf = acqStatic.GetLastFrame()
    # vff,vlf = btkTools.getFrameBoundaries(acqStatic,actual_trackingMarkers)
    flag = btkTools.getValidFrames(acqStatic,actual_trackingMarkers,frameBounds=[vff,vlf])

    gapFlag = btkTools.checkGap(acqStatic,actual_trackingMarkers,frameBounds=[vff,vlf])
    if gapFlag:
        raise Exception("[pyCGM2] Calibration aborted. Gap find during interval [%i-%i]. Crop your c3d " %(vff,vlf))

    # --------------------ANOMALY------------------------------
    # --Check MP
    adap = AnomalyDetectionProcedure.AnthropoDataAnomalyProcedure( required_mp)
    adf = AnomalyFilter.AnomalyDetectionFilter(None,None,adap)
    mp_anomaly = adf.run()
    if mp_anomaly["ErrorState"]: detectAnomaly = True

    # --marker presence
    markersets = [cgm.CGM1.LOWERLIMB_TRACKING_MARKERS, cgm.CGM1.THORAX_TRACKING_MARKERS, cgm.CGM1.UPPERLIMB_TRACKING_MARKERS]
    for markerset in markersets:
        ipdp = InspectorProcedure.MarkerPresenceDetectionProcedure( markerset)
        idf = InspectorFilter.InspectorFilter(acqStatic,calibrateFilenameLabelled,ipdp)
        inspector = idf.run()

        # # --marker outliers
        if inspector["In"] !=[]:
            madp = AnomalyDetectionProcedure.MarkerAnomalyDetectionRollingProcedure(inspector["In"], plot=False, window=4,threshold = 3)
            adf = AnomalyFilter.AnomalyDetectionFilter(acqStatic,calibrateFilenameLabelled,madp)
            anomaly = adf.run()
            anomalyIndexes = anomaly["Output"]
            if anomaly["ErrorState"]: detectAnomaly = True


    if detectAnomaly and anomalyException:
        raise Exception ("Anomalies has been detected - Check Warning message of the log file")


    # --------------------MODELLING------------------------------

    # ---check marker set used----
    dcm = cgm.CGM.detectCalibrationMethods(acqStatic)

    # ---definition---
    model=cgm2.CGM2_1()
    model.configure(detectedCalibrationMethods=dcm)
    model.addAnthropoInputParameters(required_mp,optional=optional_mp)

    if dcm["Left Knee"] == enums.JointCalibrationMethod.KAD: actual_trackingMarkers.append("LKNE")
    if dcm["Right Knee"] == enums.JointCalibrationMethod.KAD: actual_trackingMarkers.append("RKNE")
    model.setStaticTrackingMarkers(actual_trackingMarkers)


    # --store calibration parameters--
    model.setStaticFilename(calibrateFilenameLabelled)
    model.setCalibrationProperty("leftFlatFoot",leftFlatFoot)
    model.setCalibrationProperty("rightFlatFoot",rightFlatFoot)
    model.setCalibrationProperty("headFlat",headFlat)
    model.setCalibrationProperty("markerDiameter",markerDiameter)


    # --------------------------STATIC CALBRATION--------------------------
    scp=modelFilters.StaticCalibrationProcedure(model) # load calibration procedure

    # ---initial calibration filter----
    # use if all optional mp are zero
    modelFilters.ModelCalibrationFilter(scp,acqStatic,model,
                                        leftFlatFoot = leftFlatFoot, rightFlatFoot = rightFlatFoot,
                                        headFlat= headFlat,
                                        markerDiameter=markerDiameter,
                                        ).compute()

    # ---- Decorators -----
    decorators.applyBasicDecorators(dcm, model,acqStatic,optional_mp,markerDiameter)
    decorators.applyHJCDecorators(model,hjcMethod)


    # ----Final Calibration filter if model previously decorated -----
    if model.decoratedModel:
        # initial static filter
        modelFilters.ModelCalibrationFilter(scp,acqStatic,model,
                                            leftFlatFoot = leftFlatFoot,
                                            rightFlatFoot = rightFlatFoot,
                                            markerDiameter=markerDiameter,
                                            headFlat= headFlat,
                                            ).compute()

    modMotion=modelFilters.ModelMotionFilter(scp,acqStatic,model,enums.motionMethod.Determinist,
                                              markerDiameter=markerDiameter)

    modMotion.compute()

    # ----progression Frame----
    progressionFlag = False
    if btkTools.isPointsExist(acqStatic, ['LASI', 'RASI', 'RPSI', 'LPSI'],ignorePhantom=False):
        LOGGER.logger.info("[pyCGM2] - progression axis detected from Pelvic markers ")
        pfp = progressionFrame.PelvisProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqStatic,pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]
        progressionFlag = True
    elif btkTools.isPointsExist(acqStatic, ['C7', 'T10', 'CLAV', 'STRN'],ignorePhantom=False) and not progressionFlag:
        LOGGER.logger.info("[pyCGM2] - progression axis detected from Thoracic markers ")
        pfp = progressionFrame.ThoraxProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqStatic,pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

    else:
        globalFrame = "XYZ"
        progressionAxis = "X"
        forwardProgression = True
        LOGGER.logger.error("[pyCGM2] - impossible to detect progression axis - neither pelvic nor thoracic markers are present. Progression set to +X by default ")


    if "displayCoordinateSystem" in kwargs.keys() and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp,model,acqStatic)
        csdf.setStatic(False)
        csdf.display()


    # ----------------------CGM MODELLING----------------------------------
    # ----motion filter----
    # notice : viconCGM1compatible option duplicate error on Construction of the foot coordinate system

    if "noKinematicsCalculation" in kwargs.keys() and kwargs["noKinematicsCalculation"]:
        LOGGER.logger.warning("[pyCGM2] No Kinematic calculation done for the static file")
        return model, acqStatic
    else:

        #---- Joint kinematics----
        # relative angles
        modelFilters.ModelJCSFilter(model,acqStatic).compute(description="vectoriel", pointLabelSuffix=pointSuffix)

        modelFilters.ModelAbsoluteAnglesFilter(model,acqStatic,
                                               segmentLabels=["Left Foot","Right Foot","Pelvis","Thorax","Head"],
                                                angleLabels=["LFootProgress", "RFootProgress","Pelvis","Thorax", "Head"],
                                                eulerSequences=["TOR","TOR", "ROT","YXZ","TOR"],
                                                globalFrameOrientation = globalFrame,
                                                forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

        # BSP model
        bspModel = bodySegmentParameters.Bsp(model)
        bspModel.compute()

        modelFilters.CentreOfMassFilter(model,acqStatic).compute(pointLabelSuffix=pointSuffix)

        btkTools.cleanAcq(acqStatic)
        if detectAnomaly and not anomalyException:
            LOGGER.logger.error("Anomalies has been detected - Check Warning messages of the log file")

        return model, acqStatic,detectAnomaly
Esempio n. 17
0
def fitting(model,DATA_PATH, reconstructFilenameLabelled,
    translators,
    markerDiameter,
    pointSuffix,
    mfpa,
    momentProjection,**kwargs):

    """
    Fitting of the CGM2.1

    :param model [str]: pyCGM2 model previously calibrated
    :param DATA_PATH [str]: path to your data
    :param reconstructFilenameLabelled [string list]: c3d files
    :param translators [dict]:  translators to apply
    :param mfpa [str]: manual force plate assignement
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs
    :param momentProjection [str]: Coordinate system in which joint moment is expressed
    """

    detectAnomaly = False
    if "anomalyException" in kwargs.keys():
        anomalyException = kwargs["anomalyException"]
    else:
        anomalyException=False
    # --------------------------ACQUISITION ------------------------------------

    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqGait = kwargs["forceBtkAcq"]
    else:
        acqGait = btkTools.smartReader((DATA_PATH + reconstructFilenameLabelled))

    btkTools.checkMultipleSubject(acqGait)
    if btkTools.isPointExist(acqGait,"SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        LOGGER.logger.info("[pyCGM2] Sacrum marker detected")

    acqGait =  btkTools.applyTranslators(acqGait,translators)

    trackingMarkers = cgm.CGM1.LOWERLIMB_TRACKING_MARKERS + cgm.CGM1.THORAX_TRACKING_MARKERS+ cgm.CGM1.UPPERLIMB_TRACKING_MARKERS
    actual_trackingMarkers,phatoms_trackingMarkers = btkTools.createPhantoms(acqGait, trackingMarkers)

    vff,vlf = btkTools.getFrameBoundaries(acqGait,actual_trackingMarkers)
    if "frameInit" in kwargs.keys() and kwargs["frameInit"] is not None:
        vff = kwargs["frameInit"]
        LOGGER.logger.info("[pyCGM2]  first frame forced to frame [%s]"%(vff))
    if "frameEnd" in kwargs.keys() and kwargs["frameEnd"] is not None:
        vlf = kwargs["frameEnd"]
        LOGGER.logger.info("[pyCGM2]  end frame forced to frame [%s]"%(vlf))

    flag = btkTools.getValidFrames(acqGait,actual_trackingMarkers,frameBounds=[vff,vlf])

    LOGGER.logger.info("[pyCGM2]  Computation from frame [%s] to frame [%s]"%(vff,vlf))

    # --------------------ANOMALY------------------------------
    for marker in actual_trackingMarkers:
        if marker not in model.getStaticTrackingMarkers():
            LOGGER.logger.warning("[pyCGM2-Anomaly]  marker [%s] - not used during static calibration - wrong kinematic for the segment attached to this marker. "%(marker))

    # --marker presence
    markersets = [cgm.CGM1.LOWERLIMB_TRACKING_MARKERS, cgm.CGM1.THORAX_TRACKING_MARKERS, cgm.CGM1.UPPERLIMB_TRACKING_MARKERS]
    for markerset in markersets:
        ipdp = InspectorProcedure.MarkerPresenceDetectionProcedure( markerset)
        idf = InspectorFilter.InspectorFilter(acqGait,reconstructFilenameLabelled,ipdp)
        inspector = idf.run()

        # --marker outliers
        if inspector["In"] !=[]:
            madp = AnomalyDetectionProcedure.MarkerAnomalyDetectionRollingProcedure( inspector["In"], plot=False, window=5,threshold = 3)
            adf = AnomalyFilter.AnomalyDetectionFilter(acqGait,reconstructFilenameLabelled,madp, frameRange=[vff,vlf])
            anomaly = adf.run()
            anomalyIndexes = anomaly["Output"]
            if anomaly["ErrorState"]: detectAnomaly = True


    if btkTools.checkForcePlateExist(acqGait):
        afpp = AnomalyDetectionProcedure.ForcePlateAnomalyProcedure()
        adf = AnomalyFilter.AnomalyDetectionFilter(acqGait,reconstructFilenameLabelled,afpp, frameRange=[vff,vlf])
        anomaly = adf.run()
        if anomaly["ErrorState"]: detectAnomaly = True

    if detectAnomaly and anomalyException:
        raise Exception ("Anomalies has been detected - Check Warning message of the log file")

   # --------------------MODELLING------------------------------

    # filtering
    # -----------------------
    if "fc_lowPass_marker" in kwargs.keys() and kwargs["fc_lowPass_marker"]!=0 :
        fc = kwargs["fc_lowPass_marker"]
        order = 4
        if "order_lowPass_marker" in kwargs.keys():
            order = kwargs["order_lowPass_marker"]
        signal_processing.markerFiltering(acqGait,trackingMarkers,order=order, fc =fc)

    if "fc_lowPass_forcePlate" in kwargs.keys() and kwargs["fc_lowPass_forcePlate"]!=0 :
        fc = kwargs["fc_lowPass_forcePlate"]
        order = 4
        if "order_lowPass_forcePlate" in kwargs.keys():
            order = kwargs["order_lowPass_forcePlate"]
        signal_processing.forcePlateFiltering(acqGait,order=order, fc =fc)



    scp=modelFilters.StaticCalibrationProcedure(model)
    # ---Motion filter----
    modMotion=modelFilters.ModelMotionFilter(scp,acqGait,model,enums.motionMethod.Determinist,
                                              markerDiameter=markerDiameter)

    modMotion.compute()

    progressionFlag = False
    if btkTools.isPointExist(acqGait, 'LHEE',ignorePhantom=False) or btkTools.isPointExist(acqGait, 'RHEE',ignorePhantom=False):

        pfp = progressionFrame.PointProgressionFrameProcedure(marker="LHEE") \
            if btkTools.isPointExist(acqGait, 'LHEE',ignorePhantom=False) \
            else  progressionFrame.PointProgressionFrameProcedure(marker="RHEE")

        pff = progressionFrame.ProgressionFrameFilter(acqGait,pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]
        progressionFlag = True

    elif btkTools.isPointsExist(acqGait, ['LASI', 'RASI', 'RPSI', 'LPSI'],ignorePhantom=False) and not progressionFlag:
        LOGGER.logger.info("[pyCGM2] - progression axis detected from Pelvic markers ")
        pfp = progressionFrame.PelvisProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqGait,pfp)
        pff.compute()
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

        progressionFlag = True
    elif btkTools.isPointsExist(acqGait, ['C7', 'T10', 'CLAV', 'STRN'],ignorePhantom=False) and not progressionFlag:
        LOGGER.logger.info("[pyCGM2] - progression axis detected from Thoracic markers ")
        pfp = progressionFrame.ThoraxProgressionFrameProcedure()
        pff = progressionFrame.ProgressionFrameFilter(acqGait,pfp)
        pff.compute()
        progressionAxis = pff.outputs["progressionAxis"]
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

    else:
        globalFrame = "XYZ"
        progressionAxis = "X"
        forwardProgression = True
        LOGGER.logger.error("[pyCGM2] - impossible to detect progression axis - neither pelvic nor thoracic markers are present. Progression set to +X by default ")

    if "displayCoordinateSystem" in kwargs.keys() and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp,model,acqGait)
        csdf.setStatic(False)
        csdf.display()

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model,acqGait).compute(description="vectoriel", pointLabelSuffix=pointSuffix)

    modelFilters.ModelAbsoluteAnglesFilter(model,acqGait,
                                           segmentLabels=["Left Foot","Right Foot","Pelvis","Thorax","Head"],
                                            angleLabels=["LFootProgress", "RFootProgress","Pelvis","Thorax", "Head"],
                                            eulerSequences=["TOR","TOR", "ROT","YXZ","TOR"],
                                            globalFrameOrientation = globalFrame,
                                            forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

    #---- Body segment parameters----
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    modelFilters.CentreOfMassFilter(model,acqGait).compute(pointLabelSuffix=pointSuffix)

    # Inverse dynamics
    if btkTools.checkForcePlateExist(acqGait):
        if model.m_bodypart != enums.BodyPart.UpperLimb:
            # --- force plate handling----
            # find foot  in contact
            mappedForcePlate = forceplates.matchingFootSideOnForceplate(acqGait,mfpa=mfpa)
            forceplates.addForcePlateGeneralEvents(acqGait,mappedForcePlate)
            LOGGER.logger.info("Manual Force plate assignment : %s" %mappedForcePlate)

            # assembly foot and force plate
            modelFilters.ForcePlateAssemblyFilter(model,acqGait,mappedForcePlate,
                                     leftSegmentLabel="Left Foot",
                                     rightSegmentLabel="Right Foot").compute(pointLabelSuffix=pointSuffix)

            #---- Joint kinetics----
            idp = modelFilters.CGMLowerlimbInverseDynamicProcedure()
            modelFilters.InverseDynamicFilter(model,
                                 acqGait,
                                 procedure = idp,
                                 projection = momentProjection,
                                 globalFrameOrientation = globalFrame,
                                 forwardProgression = forwardProgression
                                 ).compute(pointLabelSuffix=pointSuffix)


            #---- Joint energetics----
            modelFilters.JointPowerFilter(model,acqGait).compute(pointLabelSuffix=pointSuffix)

    btkTools.cleanAcq(acqGait)
    btkTools.applyOnValidFrames(acqGait,flag)

    if detectAnomaly and not anomalyException:
        LOGGER.logger.error("Anomalies has been detected - Check Warning messages of the log file")



    return acqGait,detectAnomaly
Esempio n. 18
0
    def noIK_6dof(cls):
        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM2\\cgm2.3\\fullBody\\"
        staticFilename = "PN01OP01S01STAT.c3d"
        gaitFilename = "PN01OP01S01SS01.c3d"

        markerDiameter = 14
        mp = {
            'Bodymass': 83.0,
            'LeftLegLength': 874.0,
            'RightLegLength': 876.0,
            'LeftKneeWidth': 106.0,
            'RightKneeWidth': 103.0,
            'LeftAnkleWidth': 74.0,
            'RightAnkleWidth': 72.0,
            'LeftSoleDelta': 0,
            'RightSoleDelta': 0,
        }

        # --- Calibration ---
        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        translators = files.getTranslators(MAIN_PATH, "CGM2_3.translators")
        acqStatic = btkTools.applyTranslators(acqStatic, translators)

        model = cgm2.CGM2_3LowerLimbs()
        model.configure()

        model.addAnthropoInputParameters(mp)

        # ---- Calibration ----

        scp = modelFilters.StaticCalibrationProcedure(model)
        modelFilters.ModelCalibrationFilter(scp, acqStatic, model).compute()

        print "----"
        print model.getSegment("Left Shank").getReferential(
            "TF").relativeMatrixAnatomic
        print "----"

        # # cgm decorator
        modelDecorator.HipJointCenterDecorator(model).hara()
        modelDecorator.KneeCalibrationDecorator(model).midCondyles(
            acqStatic, markerDiameter=markerDiameter, side="both")
        modelDecorator.AnkleCalibrationDecorator(model).midMaleolus(
            acqStatic, markerDiameter=markerDiameter, side="both")
        #
        # # final
        modelFilters.ModelCalibrationFilter(
            scp, acqStatic, model, markerDiameter=markerDiameter).compute()

        #import ipdb; ipdb.set_trace()
        # ------ Fitting -------
        acqGait = btkTools.smartReader(str(MAIN_PATH + gaitFilename))
        acqGait = btkTools.applyTranslators(acqGait, translators)

        # Motion FILTER
        modMotion = modelFilters.ModelMotionFilter(
            scp, acqGait, model, enums.motionMethod.Sodervisk)
        modMotion.compute()

        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        modelFilters.CoordinateSystemDisplayFilter(csp, model,
                                                   acqGait).display()

        btkTools.smartWriter(acqGait, "cgm23_noIK6dof_Motion.c3d")
Esempio n. 19
0
def fitting(model, DATA_PATH, reconstructFilenameLabelled, translators,
            markerDiameter, pointSuffix, mfpa, momentProjection, **kwargs):
    """
    Fitting of the CGM1

    :param model [str]: pyCGM2 model previously calibrated
    :param DATA_PATH [str]: path to your data
    :param reconstructFilenameLabelled [string list]: c3d files
    :param translators [dict]:  translators to apply
    :param mfpa [str]: manual force plate assignement
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs
    :param momentProjection [str]: Coordinate system in which joint moment is expressed

    """

    # --------------------------ACQUISITION ------------------------------------

    if "forceBtkAcq" in kwargs.keys():
        acqGait = kwargs["forceBtkAcq"]
    else:
        # --- btk acquisition ----
        acqGait = btkTools.smartReader(
            str(DATA_PATH + reconstructFilenameLabelled))

    btkTools.checkMultipleSubject(acqGait)
    acqGait = btkTools.applyTranslators(acqGait, translators)
    trackingMarkers = model.getTrackingMarkers()
    validFrames, vff, vlf = btkTools.findValidFrames(acqGait, trackingMarkers)

    scp = modelFilters.StaticCalibrationProcedure(model)  # procedure

    # ---Motion filter----
    modMotion = modelFilters.ModelMotionFilter(scp,
                                               acqGait,
                                               model,
                                               enums.motionMethod.Determinist,
                                               markerDiameter=markerDiameter,
                                               viconCGM1compatible=True)

    modMotion.compute()

    if "displayCoordinateSystem" in kwargs.keys(
    ) and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp, model, acqGait)
        csdf.setStatic(False)
        csdf.display()

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model,
                                acqGait).compute(description="vectoriel",
                                                 pointLabelSuffix=pointSuffix)

    # detection of traveling axis + absolute angle
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromPelvicMarkers(
            acqGait, ["LASI", "LPSI", "RASI", "RPSI"])
    else:
        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromLongAxis(
            acqGait, "C7", "CLAV")

    if model.m_bodypart != enums.BodyPart.UpperLimb:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Left Foot", "Right Foot", "Pelvis"],
            angleLabels=["LFootProgress", "RFootProgress", "Pelvis"],
            eulerSequences=["TOR", "TOR", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Thorax"],
            angleLabels=["Thorax"],
            eulerSequences=["YXZ"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqGait,
            segmentLabels=["Thorax", "Head"],
            angleLabels=["Thorax", "Head"],
            eulerSequences=["YXZ", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    #---- Body segment parameters----
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    #---- CentreOfMass----
    if model.m_bodypart == enums.BodyPart.FullBody:
        modelFilters.CentreOfMassFilter(
            model, acqGait).compute(pointLabelSuffix=pointSuffix)

    # Inverse dynamics
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        # --- force plate handling----
        # find foot  in contact
        mappedForcePlate = forceplates.matchingFootSideOnForceplate(acqGait,
                                                                    mfpa=mfpa)
        forceplates.addForcePlateGeneralEvents(acqGait, mappedForcePlate)
        logging.warning("Manual Force plate assignment : %s" %
                        mappedForcePlate)

        # assembly foot and force plate
        modelFilters.ForcePlateAssemblyFilter(
            model,
            acqGait,
            mappedForcePlate,
            leftSegmentLabel="Left Foot",
            rightSegmentLabel="Right Foot").compute()

        #---- Joint kinetics----
        idp = modelFilters.CGMLowerlimbInverseDynamicProcedure()
        modelFilters.InverseDynamicFilter(
            model,
            acqGait,
            procedure=idp,
            projection=momentProjection,
            viconCGM1compatible=True).compute(pointLabelSuffix=pointSuffix)

        #---- Joint energetics----
        modelFilters.JointPowerFilter(
            model, acqGait).compute(pointLabelSuffix=pointSuffix)

    #---- zero unvalid frames ---
    btkTools.applyValidFramesOnOutput(acqGait, validFrames)

    return acqGait
Esempio n. 20
0
def calibrate(DATA_PATH, calibrateFilenameLabelled, translators, required_mp,
              optional_mp, leftFlatFoot, rightFlatFoot, headFlat,
              markerDiameter, pointSuffix, **kwargs):
    """
    Calibration of the CGM1.1

    :param DATA_PATH [str]: path to your data
    :param calibrateFilenameLabelled [str]: c3d file
    :param translators [dict]:  translators to apply
    :param required_mp [dict]: required anthropometric data
    :param optional_mp [dict]: optional anthropometric data (ex: LThighOffset,...)
    :param leftFlatFoot [bool]: enable of the flat foot option for the left foot
    :param rightFlatFoot [bool]: enable of the flat foot option for the right foot
    :param headFlat [bool]: enable of the head flat  option
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs

    """
    # --------------------------ACQUISITION ------------------------------------
    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqStatic = kwargs["forceBtkAcq"]
    else:
        acqStatic = btkTools.smartReader(
            (DATA_PATH + calibrateFilenameLabelled))

    btkTools.checkMultipleSubject(acqStatic)
    if btkTools.isPointExist(acqStatic, "SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        logging.info("[pyCGM2] Sacrum marker detected")

    acqStatic = btkTools.applyTranslators(acqStatic, translators)

    # ---detectedCalibrationMethods----
    dcm = cgm.CGM.detectCalibrationMethods(acqStatic)

    # ---definition---
    model = cgm.CGM1()
    model.setVersion("CGM1.1")
    model.configure(acq=acqStatic, detectedCalibrationMethods=dcm)
    model.addAnthropoInputParameters(required_mp, optional=optional_mp)

    # --store calibration parameters--
    model.setStaticFilename(calibrateFilenameLabelled)
    model.setCalibrationProperty("leftFlatFoot", leftFlatFoot)
    model.setCalibrationProperty("rightFlatFoot", rightFlatFoot)
    model.setCalibrationProperty("headFlat", headFlat)
    model.setCalibrationProperty("markerDiameter", markerDiameter)

    # --------------------------STATIC CALBRATION--------------------------
    scp = modelFilters.StaticCalibrationProcedure(
        model)  # load calibration procedure

    # ---initial calibration filter----
    modelFilters.ModelCalibrationFilter(
        scp,
        acqStatic,
        model,
        leftFlatFoot=leftFlatFoot,
        rightFlatFoot=rightFlatFoot,
        headFlat=headFlat,
        markerDiameter=markerDiameter,
    ).compute()
    # ---- Decorators -----
    decorators.applyBasicDecorators(dcm, model, acqStatic, optional_mp,
                                    markerDiameter)

    # ----Final Calibration filter if model previously decorated -----
    if model.decoratedModel:
        # initial static filter
        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=leftFlatFoot,
            rightFlatFoot=rightFlatFoot,
            headFlat=headFlat,
            markerDiameter=markerDiameter).compute()

    # ----------------------CGM MODELLING----------------------------------
    # ----motion filter----
    # notice : viconCGM1compatible option duplicate error on Construction of the foot coordinate system

    modMotion = modelFilters.ModelMotionFilter(scp,
                                               acqStatic,
                                               model,
                                               enums.motionMethod.Determinist,
                                               markerDiameter=markerDiameter)
    modMotion.compute()

    if "displayCoordinateSystem" in kwargs.keys(
    ) and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(
            csp, model, acqStatic)
        csdf.setStatic(False)
        csdf.display()

    if "noKinematicsCalculation" in kwargs.keys(
    ) and kwargs["noKinematicsCalculation"]:
        logging.warning(
            "[pyCGM2] No Kinematic calculation done for the static file")
        return model, acqStatic
    else:
        #---- Joint kinematics----
        # relative angles
        modelFilters.ModelJCSFilter(model, acqStatic).compute(
            description="vectoriel", pointLabelSuffix=pointSuffix)

        # detection of traveling axis + absolute angle
        if model.m_bodypart != enums.BodyPart.UpperLimb:
            pfp = progressionFrame.PelvisProgressionFrameProcedure()
        else:
            pfp = progressionFrame.ThoraxProgressionFrameProcedure()

        pff = progressionFrame.ProgressionFrameFilter(acqStatic, pfp)
        pff.compute()
        globalFrame = pff.outputs["globalFrame"]
        forwardProgression = pff.outputs["forwardProgression"]

        if model.m_bodypart != enums.BodyPart.UpperLimb:
            modelFilters.ModelAbsoluteAnglesFilter(
                model,
                acqStatic,
                segmentLabels=["Left Foot", "Right Foot", "Pelvis"],
                angleLabels=["LFootProgress", "RFootProgress", "Pelvis"],
                eulerSequences=["TOR", "TOR", "ROT"],
                globalFrameOrientation=globalFrame,
                forwardProgression=forwardProgression).compute(
                    pointLabelSuffix=pointSuffix)

        if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
            modelFilters.ModelAbsoluteAnglesFilter(
                model,
                acqStatic,
                segmentLabels=["Thorax"],
                angleLabels=["Thorax"],
                eulerSequences=["YXZ"],
                globalFrameOrientation=globalFrame,
                forwardProgression=forwardProgression).compute(
                    pointLabelSuffix=pointSuffix)

        if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

            modelFilters.ModelAbsoluteAnglesFilter(
                model,
                acqStatic,
                segmentLabels=["Thorax", "Head"],
                angleLabels=["Thorax", "Head"],
                eulerSequences=["YXZ", "TOR"],
                globalFrameOrientation=globalFrame,
                forwardProgression=forwardProgression).compute(
                    pointLabelSuffix=pointSuffix)
        # BSP model
        bspModel = bodySegmentParameters.Bsp(model)
        bspModel.compute()

        if model.m_bodypart == enums.BodyPart.FullBody:
            modelFilters.CentreOfMassFilter(
                model, acqStatic).compute(pointLabelSuffix=pointSuffix)

        return model, acqStatic
Esempio n. 21
0
def calibrate(DATA_PATH, calibrateFilenameLabelled, translators, required_mp,
              optional_mp, leftFlatFoot, rightFlatFoot, headFlat,
              markerDiameter, pointSuffix, **kwargs):
    """
    Calibration of the CGM1

    :param DATA_PATH [str]: path to your data
    :param calibrateFilenameLabelled [str]: c3d file
    :param translators [dict]:  translators to apply
    :param required_mp [dict]: required anthropometric data
    :param optional_mp [dict]: optional anthropometric data (ex: LThighOffset,...)
    :param leftFlatFoot [bool]: enable of the flat foot option for the left foot
    :param rightFlatFoot [bool]: enable of the flat foot option for the right foot
    :param headFlat [bool]: enable of the head flat  option
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs

    """

    # --------------------------ACQUISITION ------------------------------------

    if "forceBtkAcq" in kwargs.keys():
        acqStatic = kwargs["forceBtkAcq"]
    else:
        # ---btk acquisition---
        acqStatic = btkTools.smartReader(
            str(DATA_PATH + calibrateFilenameLabelled))

    btkTools.checkMultipleSubject(acqStatic)
    acqStatic = btkTools.applyTranslators(acqStatic, translators)

    # ---check marker set used----
    dcm = cgm.CGM.detectCalibrationMethods(acqStatic)

    # ---definition---
    model = cgm.CGM1()
    model.configure(acq=acqStatic, detectedCalibrationMethods=dcm)
    model.addAnthropoInputParameters(required_mp, optional=optional_mp)

    # --store calibration parameters--
    model.setStaticFilename(calibrateFilenameLabelled)
    model.setCalibrationProperty("leftFlatFoot", leftFlatFoot)
    model.setCalibrationProperty("rightFlatFoot", rightFlatFoot)
    model.setCalibrationProperty("headFlat", headFlat)
    model.setCalibrationProperty("markerDiameter", markerDiameter)

    # --------------------------STATIC CALBRATION--------------------------
    scp = modelFilters.StaticCalibrationProcedure(
        model)  # load calibration procedure

    # ---initial calibration filter----
    modelFilters.ModelCalibrationFilter(scp,
                                        acqStatic,
                                        model,
                                        leftFlatFoot=leftFlatFoot,
                                        rightFlatFoot=rightFlatFoot,
                                        markerDiameter=markerDiameter,
                                        headFlat=headFlat,
                                        viconCGM1compatible=True).compute()
    # ---- Decorators -----
    decorators.applyBasicDecorators(dcm,
                                    model,
                                    acqStatic,
                                    optional_mp,
                                    markerDiameter,
                                    cgm1only=True)
    pigStaticMarkers = cgm.CGM.get_markerLabelForPiGStatic(dcm)

    # ----Final Calibration filter if model previously decorated -----
    if model.decoratedModel:
        # initial static filter
        modelFilters.ModelCalibrationFilter(
            scp,
            acqStatic,
            model,
            leftFlatFoot=leftFlatFoot,
            rightFlatFoot=rightFlatFoot,
            headFlat=headFlat,
            markerDiameter=markerDiameter,
            viconCGM1compatible=True).compute()

    # ----------------------CGM MODELLING----------------------------------
    # ----motion filter----
    # notice : viconCGM1compatible option duplicate error on Construction of the foot coordinate system

    modMotion = modelFilters.ModelMotionFilter(
        scp,
        acqStatic,
        model,
        enums.motionMethod.Determinist,
        markerDiameter=markerDiameter,
        viconCGM1compatible=False,
        pigStatic=True,
        useLeftKJCmarker=pigStaticMarkers[0],
        useLeftAJCmarker=pigStaticMarkers[1],
        useRightKJCmarker=pigStaticMarkers[2],
        useRightAJCmarker=pigStaticMarkers[3])
    modMotion.compute()

    if "displayCoordinateSystem" in kwargs.keys(
    ) and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(
            csp, model, acqStatic)
        csdf.setStatic(False)
        csdf.display()

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model, acqStatic).compute(
        description="vectoriel", pointLabelSuffix=pointSuffix)

    # detection of traveling axis + absolute angle
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromPelvicMarkers(
            acqStatic, ["LASI", "LPSI", "RASI", "RPSI"])
    else:
        longitudinalAxis, forwardProgression, globalFrame = btkTools.findProgressionAxisFromLongAxis(
            acqStatic, "C7", "CLAV")

    if model.m_bodypart != enums.BodyPart.UpperLimb:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqStatic,
            segmentLabels=["Left Foot", "Right Foot", "Pelvis"],
            angleLabels=["LFootProgress", "RFootProgress", "Pelvis"],
            eulerSequences=["TOR", "TOR", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqStatic,
            segmentLabels=["Thorax"],
            angleLabels=["Thorax"],
            eulerSequences=["YXZ"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

        modelFilters.ModelAbsoluteAnglesFilter(
            model,
            acqStatic,
            segmentLabels=["Thorax", "Head"],
            angleLabels=["Thorax", "Head"],
            eulerSequences=["YXZ", "TOR"],
            globalFrameOrientation=globalFrame,
            forwardProgression=forwardProgression).compute(
                pointLabelSuffix=pointSuffix)
    # BSP model
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    if model.m_bodypart == enums.BodyPart.FullBody:
        modelFilters.CentreOfMassFilter(
            model, acqStatic).compute(pointLabelSuffix=pointSuffix)

    return model, acqStatic
Esempio n. 22
0
def fitting(model,DATA_PATH, reconstructFilenameLabelled,
    translators,weights,
    ik_flag,markerDiameter,
    pointSuffix,
    mfpa,
    momentProjection,**kwargs):

    """
    Fitting of the CGM2.3

    :param model [str]: pyCGM2 model previously calibrated
    :param DATA_PATH [str]: path to your data
    :param reconstructFilenameLabelled [string list]: c3d files
    :param translators [dict]:  translators to apply
    :param ik_flag [bool]: enable the inverse kinematic solver
    :param mfpa [str]: manual force plate assignement
    :param markerDiameter [double]: marker diameter (mm)
    :param pointSuffix [str]: suffix to add to model outputs
    :param momentProjection [str]: Coordinate system in which joint moment is expressed
    """

    if "Fitting" in weights.keys():
        weights  = weights["Fitting"]["Weight"]

    # --------------------------ACQ WITH TRANSLATORS --------------------------------------

    # --- btk acquisition ----
    if "forceBtkAcq" in kwargs.keys():
        acqGait = kwargs["forceBtkAcq"]
    else:
        acqGait = btkTools.smartReader((DATA_PATH + reconstructFilenameLabelled))

    btkTools.checkMultipleSubject(acqGait)
    if btkTools.isPointExist(acqGait,"SACR"):
        translators["LPSI"] = "SACR"
        translators["RPSI"] = "SACR"
        logging.info("[pyCGM2] Sacrum marker detected")

    acqGait =  btkTools.applyTranslators(acqGait,translators)
    trackingMarkers = model.getTrackingMarkers(acqGait)
    validFrames,vff,vlf = btkTools.findValidFrames(acqGait,trackingMarkers)
    # filtering
    # -----------------------
    if "fc_lowPass_marker" in kwargs.keys() and kwargs["fc_lowPass_marker"]!=0 :
        fc = kwargs["fc_lowPass_marker"]
        order = 4
        if "order_lowPass_marker" in kwargs.keys():
            order = kwargs["order_lowPass_marker"]
        signal_processing.markerFiltering(acqGait,trackingMarkers,order=order, fc =fc)

    if "fc_lowPass_forcePlate" in kwargs.keys() and kwargs["fc_lowPass_forcePlate"]!=0 :
        fc = kwargs["fc_lowPass_forcePlate"]
        order = 4
        if "order_lowPass_forcePlate" in kwargs.keys():
            order = kwargs["order_lowPass_forcePlate"]
        signal_processing.forcePlateFiltering(acqGait,order=order, fc =fc)


    # --- initial motion Filter ---
    scp=modelFilters.StaticCalibrationProcedure(model)
    # section to remove : - copy motion of ProximalShank from Shank with Sodervisk
    modMotion=modelFilters.ModelMotionFilter(scp,acqGait,model,enums.motionMethod.Sodervisk)
    modMotion.compute()
    # /section to remove


    if model.getBodyPart() == enums.BodyPart.UpperLimb:
        ik_flag = False
        logging.warning("[pyCGM2] Fitting only applied for the upper limb")

    if ik_flag:

        #                        ---OPENSIM IK---

        # --- opensim calibration Filter ---
        osimfile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\osim\\lowerLimb_ballsJoints.osim"    # osimfile
        markersetFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_3\\cgm2_3-markerset.xml" # markerset
        cgmCalibrationprocedure = opensimFilters.CgmOpensimCalibrationProcedures(model) # procedure

        oscf = opensimFilters.opensimCalibrationFilter(osimfile,
                                                model,
                                                cgmCalibrationprocedure,
                                                (DATA_PATH))
        oscf.addMarkerSet(markersetFile)
        scalingOsim = oscf.build()


        # --- opensim Fitting Filter ---
        iksetupFile = pyCGM2.OPENSIM_PREBUILD_MODEL_PATH + "models\\settings\\cgm2_3\\cgm2_3-ikSetUp_template.xml" # ik tl file

        cgmFittingProcedure = opensimFilters.CgmOpensimFittingProcedure(model) # procedure
        cgmFittingProcedure.updateMarkerWeight("LASI",weights["LASI"])
        cgmFittingProcedure.updateMarkerWeight("RASI",weights["RASI"])
        cgmFittingProcedure.updateMarkerWeight("LPSI",weights["LPSI"])
        cgmFittingProcedure.updateMarkerWeight("RPSI",weights["RPSI"])
        cgmFittingProcedure.updateMarkerWeight("RTHI",weights["RTHI"])
        cgmFittingProcedure.updateMarkerWeight("RKNE",weights["RKNE"])
        cgmFittingProcedure.updateMarkerWeight("RTIB",weights["RTIB"])
        cgmFittingProcedure.updateMarkerWeight("RANK",weights["RANK"])
        cgmFittingProcedure.updateMarkerWeight("RHEE",weights["RHEE"])
        cgmFittingProcedure.updateMarkerWeight("RTOE",weights["RTOE"])
        cgmFittingProcedure.updateMarkerWeight("LTHI",weights["LTHI"])
        cgmFittingProcedure.updateMarkerWeight("LKNE",weights["LKNE"])
        cgmFittingProcedure.updateMarkerWeight("LTIB",weights["LTIB"])
        cgmFittingProcedure.updateMarkerWeight("LANK",weights["LANK"])
        cgmFittingProcedure.updateMarkerWeight("LHEE",weights["LHEE"])
        cgmFittingProcedure.updateMarkerWeight("LTOE",weights["LTOE"])

        cgmFittingProcedure.updateMarkerWeight("LTHAP",weights["LTHAP"])
        cgmFittingProcedure.updateMarkerWeight("LTHAD",weights["LTHAD"])
        cgmFittingProcedure.updateMarkerWeight("LTIAP",weights["LTIAP"])
        cgmFittingProcedure.updateMarkerWeight("LTIAD",weights["LTIAD"])
        cgmFittingProcedure.updateMarkerWeight("RTHAP",weights["RTHAP"])
        cgmFittingProcedure.updateMarkerWeight("RTHAD",weights["RTHAD"])
        cgmFittingProcedure.updateMarkerWeight("RTIAP",weights["RTIAP"])
        cgmFittingProcedure.updateMarkerWeight("RTIAD",weights["RTIAD"])

        osrf = opensimFilters.opensimFittingFilter(iksetupFile,
                                                          scalingOsim,
                                                          cgmFittingProcedure,
                                                          (DATA_PATH) )

        logging.info("-------INVERSE KINEMATICS IN PROGRESS----------")
        acqIK = osrf.run(acqGait,(DATA_PATH + reconstructFilenameLabelled ))
        logging.info("-------INVERSE KINEMATICS DONE-----------------")

    # eventual gait acquisition to consider for joint kinematics
    finalAcqGait = acqIK if ik_flag else acqGait

    # --- final pyCGM2 model motion Filter ---
    # use fitted markers
    modMotionFitted=modelFilters.ModelMotionFilter(scp,finalAcqGait,model,enums.motionMethod.Sodervisk ,
                                              markerDiameter=markerDiameter)

    modMotionFitted.compute()

    if "displayCoordinateSystem" in kwargs.keys() and kwargs["displayCoordinateSystem"]:
        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(csp,model,finalAcqGait)
        csdf.setStatic(False)
        csdf.display()

    #---- Joint kinematics----
    # relative angles
    modelFilters.ModelJCSFilter(model,finalAcqGait).compute(description="vectoriel", pointLabelSuffix=pointSuffix)

    # detection of traveling axis + absolute angle
    if model.m_bodypart != enums.BodyPart.UpperLimb:
        pfp = progressionFrame.PelvisProgressionFrameProcedure()
    else:
        pfp = progressionFrame.ThoraxProgressionFrameProcedure()

    pff = progressionFrame.ProgressionFrameFilter(finalAcqGait,pfp)
    pff.compute()
    globalFrame = pff.outputs["globalFrame"]
    forwardProgression = pff.outputs["forwardProgression"]

    if model.m_bodypart != enums.BodyPart.UpperLimb:
            modelFilters.ModelAbsoluteAnglesFilter(model,finalAcqGait,
                                                   segmentLabels=["Left Foot","Right Foot","Pelvis"],
                                                    angleLabels=["LFootProgress", "RFootProgress","Pelvis"],
                                                    eulerSequences=["TOR","TOR", "ROT"],
                                                    globalFrameOrientation = globalFrame,
                                                    forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.LowerLimbTrunk:
            modelFilters.ModelAbsoluteAnglesFilter(model,finalAcqGait,
                                          segmentLabels=["Thorax"],
                                          angleLabels=["Thorax"],
                                          eulerSequences=["YXZ"],
                                          globalFrameOrientation = globalFrame,
                                          forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

    if model.m_bodypart == enums.BodyPart.UpperLimb or model.m_bodypart == enums.BodyPart.FullBody:

            modelFilters.ModelAbsoluteAnglesFilter(model,finalAcqGait,
                                          segmentLabels=["Thorax","Head"],
                                          angleLabels=["Thorax", "Head"],
                                          eulerSequences=["YXZ","TOR"],
                                          globalFrameOrientation = globalFrame,
                                          forwardProgression = forwardProgression).compute(pointLabelSuffix=pointSuffix)

    #---- Body segment parameters----
    bspModel = bodySegmentParameters.Bsp(model)
    bspModel.compute()

    if  model.m_bodypart == enums.BodyPart.FullBody:
        modelFilters.CentreOfMassFilter(model,finalAcqGait).compute(pointLabelSuffix=pointSuffix)

    # Inverse dynamics
    if btkTools.checkForcePlateExist(acqGait):
        if model.m_bodypart != enums.BodyPart.UpperLimb:
            # --- force plate handling----
            # find foot  in contact
            mappedForcePlate = forceplates.matchingFootSideOnForceplate(finalAcqGait,mfpa=mfpa)
            forceplates.addForcePlateGeneralEvents(finalAcqGait,mappedForcePlate)
            logging.warning("Manual Force plate assignment : %s" %mappedForcePlate)


            # assembly foot and force plate
            modelFilters.ForcePlateAssemblyFilter(model,finalAcqGait,mappedForcePlate,
                                     leftSegmentLabel="Left Foot",
                                     rightSegmentLabel="Right Foot").compute(pointLabelSuffix=pointSuffix)

            #---- Joint kinetics----
            idp = modelFilters.CGMLowerlimbInverseDynamicProcedure()
            modelFilters.InverseDynamicFilter(model,
                                 finalAcqGait,
                                 procedure = idp,
                                 projection = momentProjection,
                                 globalFrameOrientation = globalFrame,
                                 forwardProgression = forwardProgression
                                 ).compute(pointLabelSuffix=pointSuffix)


            #---- Joint energetics----
            modelFilters.JointPowerFilter(model,finalAcqGait).compute(pointLabelSuffix=pointSuffix)

    #---- zero unvalid frames ---
    btkTools.applyValidFramesOnOutput(finalAcqGait,validFrames)


    return finalAcqGait
Esempio n. 23
0
    def CGM1_UpperLimb(cls):

        MAIN_PATH = pyCGM2.TEST_DATA_PATH + "CGM1\\CGM1-TESTS\\full-PiG\\"
        staticFilename = "PN01NORMSTAT.c3d"

        acqStatic = btkTools.smartReader(str(MAIN_PATH + staticFilename))

        model = cgm.CGM1
        model.configure(bodyPart=enums.BodyPart.UpperLimb)

        markerDiameter = 14
        mp = {
            'LeftShoulderOffset': 50,
            'LeftElbowWidth': 91,
            'LeftWristWidth': 56,
            'LeftHandThickness': 28,
            'RightShoulderOffset': 45,
            'RightElbowWidth': 90,
            'RightWristWidth': 55,
            'RightHandThickness': 30
        }
        model.addAnthropoInputParameters(mp)

        # -----------CGM STATIC CALIBRATION--------------------
        scp = modelFilters.StaticCalibrationProcedure(model)
        modelFilters.ModelCalibrationFilter(scp, acqStatic, model).compute()

        csp = modelFilters.ModelCoordinateSystemProcedure(model)
        csdf = modelFilters.CoordinateSystemDisplayFilter(
            csp, model, acqStatic)
        csdf.setStatic(True)
        csdf.display()

        btkTools.smartWriter(acqStatic, "upperLimb_calib.c3d")

        # joint centres
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("TRXO").GetValues().mean(axis=0),
            acqStatic.GetPoint("OT").GetValues().mean(axis=0),
            decimal=3)

        np.testing.assert_almost_equal(
            acqStatic.GetPoint("LCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("LSJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("LHUO").GetValues().mean(axis=0),
            acqStatic.GetPoint("LEJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("LCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("LSJC").GetValues().mean(axis=0),
            decimal=3)

        np.testing.assert_almost_equal(
            acqStatic.GetPoint("RCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("RSJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("RHUO").GetValues().mean(axis=0),
            acqStatic.GetPoint("REJC").GetValues().mean(axis=0),
            decimal=3)
        np.testing.assert_almost_equal(
            acqStatic.GetPoint("RCLO").GetValues().mean(axis=0),
            acqStatic.GetPoint("RSJC").GetValues().mean(axis=0),
            decimal=3)