C++ PrescribedController::set_isDisabled方法代码示例

void ExploreCoactivation(Model* model, State& initial_state, double ti, double tf, double slope, double activation, string dir = "")
{



    Coordinate& rx = model->updCoordinateSet().get("platform_rx");
    rx.set_locked(false);
    rx.setDefaultValue(Pi*slope / 180.0);
    rx.set_locked(true);

    ControllerSet& controllers = model->updControllerSet();

    // set all invertor excitations to the predetermined value
    // invertors are stronger than evertors, so must scale down invertor activity to produce co-activation with no net moment
    double invertorActivation = activation*invertorScaleFactor;
    PrescribedController* invControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("inverter_controls_r"));
    invControls->set_isDisabled(false);
    for (int i = 0; i<invControls->getActuatorSet().getSize(); i++)
    {

        OpenSim::Constant currentAct(invertorActivation);
        invControls->prescribeControlForActuator(i, currentAct.clone());

        OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&invControls->updActuators().get(i));
        if (muscle)
        {
            muscle->setDefaultActivation(invertorActivation);
        }
    }

    // set all evertor excitations to the predetermined value
    PrescribedController* evControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("everter_controls_r"));
    evControls->set_isDisabled(false);
    for (int i = 0; i<evControls->getActuatorSet().getSize(); i++)
    {
        OpenSim::Constant currentAct(activation);
        evControls->prescribeControlForActuator(i, currentAct.clone());

        OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&evControls->updActuators().get(i));
        if (muscle)
        {
            muscle->setDefaultActivation(activation);
        }
    }

    // turn off other everter and inverter controllers
    //model->updControllerSet().get("AnkleEverterDelayedReflexes").set_isDisabled(true);
    //model->updControllerSet().get("AnkleInverterDelayedReflexes").set_isDisabled(true);

    // Create a force reporter
    ForceReporter* reporter = new ForceReporter(model);
    model->addAnalysis(reporter);

    SimTK::State& osim_state = model->initSystem();
    osim_state.setQ(initial_state.getQ());
    osim_state.setU(initial_state.getU());

    rx.set_locked(false);
    rx.setValue(osim_state, Pi*slope / 180.0);
    rx.set_locked(true);
    model->equilibrateMuscles(osim_state);

    // Create the integrator for integrating system dynamics
    SimTK::RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
    integrator.setAccuracy(integratorTolerance);
    integrator.setMaximumStepSize(1);
    integrator.setMinimumStepSize(1e-8);

    // Create the manager managing the forward integration and its outputs
    Manager manager(*model, integrator);
    // Integrate from initial time to final time

    manager.setInitialTime(ti);
    manager.setFinalTime(tf);
    cout << "\nIntegrating from " << ti << " to " << tf << endl;
    manager.integrate(osim_state);

    //////////////////////////////
    // SAVE THE RESULTS TO FILE //
    //////////////////////////////
    // Save the model states from forward integration
    Storage statesDegrees(manager.getStateStorage());

    char trial_name[100];
    sprintf(trial_name, "%sincline_%.1f_activation_%.1f", dir, slope, activation);

    statesDegrees.print(string(trial_name) + string("_states.sto"));

    // Save the forces
    reporter->getForceStorage().print(string(trial_name) + string("_forces.sto"));

    // Save the controls
    model->printControlStorage(trial_name + string("_controls.sto"));

    // save the model 
    model->print(string(trial_name) + string("_model.osim"));

}

//.........这里部分代码省略.........
        actLevels.append(0.9);
        actLevels.append(1.0);


        Array<double> reflexGains;

        reflexGains.append(0.0);
        reflexGains.append(0.5);
        reflexGains.append(1.0);
        reflexGains.append(2.0);
        reflexGains.append(5.0);
        reflexGains.append(10.0);
        //reflexGains.append(100.0);
        //reflexGains.append(1000.0);

        

        ControllerSet inputControllerSet(osimModel,ctrlFile);
        cout << "Found " << inputControllerSet.getSize() << " controllers." << endl;
        
        for (int i = 0; i < inputControllerSet.getSize(); i++)
        {
            
            DelayedPathReflexController* reflex = dynamic_cast<DelayedPathReflexController*>(&inputControllerSet.get(i));
            if (reflex) {
                for (int i = 0; i<reflex->getActuatorSet().getSize(); i++)
                {
                    OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&reflex->updActuators().get(i));
                    if (muscle)
                    {
                        muscle->setDefaultActivation(0.0);
                    }
                }
                reflex->set_isDisabled(true);
                osimModel.addController(reflex->clone());
            }
        }
        
        for (int i = 0; i < inputControllerSet.getSize(); i++)
        {

            PrescribedController* prescribed = dynamic_cast<PrescribedController*>(&inputControllerSet.get(i));
            if (prescribed) {
                prescribed->set_isDisabled(true);
                osimModel.addController(prescribed->clone());
            }
        }
        ControllerSet& controllers = osimModel.updControllerSet();
        cout << "Model has " << controllers.getSize() << " controllers." << endl;
        int numControllers, numParameters;
        numControllers = controllers.getSize();
        //optLog << "numControllers = " << numControllers << endl;
        
        
        State& osimState = osimModel.initSystem();

        /* Read desired kinematics*/
        Storage k_desired = Storage(kinematicsFile);
        
        Storage *q_desired, *u_desired;
        osimModel.getSimbodyEngine().formCompleteStorages(osimState, k_desired, q_desired, u_desired);
        cout << "Joint velocities estimated." << endl;
        if (q_desired->isInDegrees())
        {
            cout << "Converting coordinates from degrees to radians." << endl;
            osimModel.getSimbodyEngine().convertDegreesToRadians(*q_desired);

void simulateDropHeightAndCocontraction(Model* model, double height, double slope, double activation, string dir = "")
{
    // get the component of gravity in the y direction
    double g = model->getGravity()[1];

    // use gravity and height to compute the velocity after falling a distance equal to the input height.

    double landingVelocity = g*sqrt(-2 * height / g);

    Coordinate& ty = model->updCoordinateSet().get("pelvis_ty");
    ty.set_locked(false);
    ty.setDefaultSpeedValue(landingVelocity);
    /*ty.set_locked(false);
    ty.setDefaultValue(-1*height);
    ty.set_locked(true);*/

    Coordinate& rx = model->updCoordinateSet().get("platform_rx");
    rx.set_locked(false);
    rx.setDefaultValue(Pi*slope / 180.0);
    rx.set_locked(true);


    // set all invertor excitations to the predetermined value
    PrescribedController* invControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("inverter_controls_r"));
    invControls->set_isDisabled(false);
    for (int i = 0; i<invControls->getActuatorSet().getSize(); i++)
    {

        OpenSim::Constant currentAct(activation);
        invControls->prescribeControlForActuator(i, currentAct.clone());

        OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&invControls->updActuators().get(i));
        if (muscle)
        {
            muscle->setDefaultActivation(activation);
        }
    }

    // set all evertor excitations to the predetermined value
    PrescribedController* evControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("everter_controls_r"));
    evControls->set_isDisabled(false);
    for (int i = 0; i<evControls->getActuatorSet().getSize(); i++)
    {
        OpenSim::Constant currentAct(activation);
        evControls->prescribeControlForActuator(i, currentAct.clone());

        OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&evControls->updActuators().get(i));
        if (muscle)
        {
            muscle->setDefaultActivation(activation);
        }
    }

    // turn off other everter and inverter controllers
    model->updControllerSet().get("AnkleEverterReflexes").set_isDisabled(true);
    model->updControllerSet().get("AnkleInverterReflexes").set_isDisabled(true);
    model->updControllerSet().get("AnkleEverterDelayedReflexes").set_isDisabled(true);
    model->updControllerSet().get("AnkleInverterDelayedReflexes").set_isDisabled(true);


    // Create a force reporter
    ForceReporter* reporter = new ForceReporter(model);
    model->addAnalysis(reporter);

    SimTK::State& osim_state = model->initSystem();
    //ty.setValue(osim_state, -1*height);
    ty.setSpeedValue(osim_state, landingVelocity);
    model->getMultibodySystem().realize(osim_state, Stage::Velocity);
    model->equilibrateMuscles(osim_state);

    // Create the integrator for integrating system dynamics
    SimTK::RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
    integrator.setAccuracy(1.0e-6);
    integrator.setMaximumStepSize(1);
    integrator.setMinimumStepSize(1e-8);

    // Create the manager managing the forward integration and its outputs
    Manager manager(*model, integrator);
    // Integrate from initial time to final time
    double ti = 0;
    double tf = 0.3; //300 ms after landing

    manager.setInitialTime(ti);
    manager.setFinalTime(tf);
    cout << "\nIntegrating from " << ti << " to " << tf << endl;
    manager.integrate(osim_state);

    //////////////////////////////
    // SAVE THE RESULTS TO FILE //
    //////////////////////////////
    // Save the model states from forward integration
    Storage statesDegrees(manager.getStateStorage());

    char trial_name[100];
    sprintf(trial_name, "%sheight_%.1f_incline_%.1f_activation_%.1f", dir, height, slope, activation);
    statesDegrees.print(string(trial_name) + string("_states.sto"));

    // Save the forces
    reporter->getForceStorage().print(string(trial_name) + string("_forces.sto"));

//.........这里部分代码省略.........