コード例 #1
0
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"));

}
コード例 #2
0
int main(int argc, char* argv[])
{

    try {
        std::clock_t startTime = std::clock();	

        string modelFile = "bestModel.osim";
        string ctrlFile = "reflexControllers.xml";
        string kinematicsFile = "initialState.sto";
        double ti = initialTime;
        double tf = finalTime; 

        // Options
        for (int i = 0; i < argc; i++) {
        
            if (strcmp(argv[i], "-m") == 0) {
                modelFile = argv[i+1];
            }

            if (strcmp(argv[i], "-csfile") == 0) {
                ctrlFile = argv[i+1];
            }

            if (strcmp(argv[i], "-kfile") == 0) {
                kinematicsFile = argv[i + 1];
            }

            if (strcmp(argv[i], "-ti") == 0) {
                ti = atof(argv[i + 1]);
            }

            if (strcmp(argv[i], "-tf") == 0) {
                tf = atof(argv[i + 1]);
            }

        }

        

        OpenSim::Model osimModel(modelFile);
        Vec3 platform_origin(0.0, 0.015, 0.45);
        //Vec3 twist_y(0.0, 0.20943951023931953, 0.0);
        osimModel.updJointSet().get("ground_platform").setLocationInParent(platform_origin);
        //osimModel.updJointSet().get("ground_platform").setOrientationInParent(twist_y);

        //osimModel.updForceSet().get("foot_floor_l").set_isDisabled(true);
        CoordinateSet& coords = osimModel.updCoordinateSet();
        //coords.get("platform_rx").set_locked(false);
        //coords.get("platform_rx").setDefaultValue(platformAngle*Pi/180);
        /*
        coords.get("platform_rx").setDefaultValue(0.0);
        coords.get("platform_rx").set_locked(true);
        coords.get("platform_ry").set_locked(false);
        coords.get("platform_ry").setDefaultValue(0.0);
        coords.get("platform_ry").set_locked(true);
        coords.get("platform_rz").set_locked(false);
        coords.get("platform_rz").setDefaultValue(0.0);
        coords.get("platform_rz").set_locked(true);
        coords.get("platform_ty").set_locked(false);
        coords.get("platform_ty").setDefaultValue(0.0);
        coords.get("platform_ty").set_locked(true);
        */


        Array<double> actLevels;

        actLevels.append(0.0);
        actLevels.append(0.1);
        actLevels.append(0.2);
        actLevels.append(0.3);
        actLevels.append(0.4);
        actLevels.append(0.5);
        actLevels.append(0.6);
        actLevels.append(0.7);
        actLevels.append(0.8);
        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);
        }
        cout << "All desired coordinates are in radians." << endl;

        if (u_desired->isInDegrees())
        {
            cout << "Converting velocities from degrees to radians." << endl;
            osimModel.getSimbodyEngine().convertDegreesToRadians(*u_desired);
        }
        cout << "All desired velocities are in radians." << endl;
        Vector qi(osimState.getNQ(), 0.0), ui(osimState.getNU(),0.0);
        // get and set initial position
        q_desired->getDataAtTime(ti, osimState.getNQ(), qi);
        //cout << "Setting q's." << endl;
        //qi.dump();

        osimState.setQ(qi);
        // get and set initial velocity
        u_desired->getDataAtTime(ti, osimState.getNU(), ui);
        //cout << "Setting u's" << endl;
        //ui.dump();
        osimState.setU(ui);
        cout << "Initial position and velocity set." << endl;
        State initial_state = osimState;
        
        for (int j = 0; j<actLevels.getSize(); j++)
        {
            cout << "\tCo-contraction level " << actLevels[j]
                << " (" << j + 1 << "/" << actLevels.getSize() << "):" << endl;

            Model* modelCopy = osimModel.clone();
            ExploreCoactivation(modelCopy, initial_state, ti, tf, platformAngle, actLevels[j], resultDir);
        }
        

        for (int j = 0; j<reflexGains.getSize(); j++)
        {
            cout << "\tReflex Gain " << reflexGains[j]
                << " (" << j + 1 << "/" << reflexGains.getSize() << "):" << endl;
            Model* modelCopy = osimModel.clone();
            ExploreReflexes(modelCopy, initial_state, ti, tf, platformAngle, reflexGains[j], reflexDelay,resultDir);
            //simulateDropHeightAndReflexes(dropModel, dropHeights[i], platformAngle, reflexGains[j], reflexDelay);
        }

        clock_t endTime = std::clock();
        fwdLog << "computeTime: " << endTime - startTime << "ms" << endl;
    }
    catch (const std::exception& ex)
    {
        std::cout << ex.what() << std::endl;
        return 1;
    }

    catch (...)
    {
        std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
        return 1;
    }

    std::cout << "\nSimulations Completed\n";
    return 0;
    // End of main() routine.
    
}
コード例 #3
0
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"));

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

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