C++ PrescribedController类代码示例

void addExtensionController(Model& model)
{
	PrescribedController* controller = new PrescribedController();
	controller->setName( "extension_controller");
	controller->setActuators( model.updActuators());
	
	double control_time[2] = {0.01, 0.02}; // time nodes for linear function
	double control_acts[2] = {0.0, 1.0}; // force values at t1 and t2
	//control_func->setName( "constant_control_func");

	string muscle_name;
	for (int i=0; i<model.getActuators().getSize(); i++)
	{
		muscle_name = model.getActuators().get(i).getName();
		// activate quadriceps
		if (muscle_name == "rect_fem_r" || muscle_name == "vas_med_r" || muscle_name == "vas_int_r" || muscle_name == "vas_lat_r" )
		{
			Constant* ccf = new Constant(0.8);
			//PiecewiseLinearFunction *ccf = new PiecewiseLinearFunction( 2, control_time, control_acts);
			controller->prescribeControlForActuator( i, ccf);
		}
		else 
		{
			Constant* zccf = new Constant(0);
			controller->prescribeControlForActuator( i, zccf);
		}
	}
	model.addController( controller);
}

void addFlexionController(Model& model)
{
	PrescribedController* controller = new PrescribedController();
	controller->setName( "flexion_controller");
	controller->setActuators( model.updActuators());
	
	double control_time[2] = {0, 0.05}; // time nodes for linear function
	double control_acts[2] = {1.0, 0}; // force values at t1 and t2

	string muscle_name;
	for (int i=0; i<model.getActuators().getSize(); i++)
	{
		muscle_name = model.getActuators().get(i).getName();
		// hamstrings: bi*, semi*
		if ( muscle_name == "bifemlh_r" || muscle_name == "bifemsh_r" || muscle_name == "grac_r" \
			|| muscle_name == "lat_gas_r" || muscle_name == "med_gas_r" || muscle_name == "sar_r" \
			|| muscle_name == "semimem_r" || muscle_name == "semiten_r")
		{
			Constant* ccf = new Constant(1.0);
			//PiecewiseLinearFunction *ccf = new PiecewiseLinearFunction( 2, control_time, control_acts);
			controller->prescribeControlForActuator( i, ccf);
		}
		else 
		{
			Constant* zccf = new Constant(0);
			controller->prescribeControlForActuator( i, zccf);
		}
	}
	model.addController( controller);
}

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

		// Add PistonActuator between the first linkage and the block
		Vec3 pointOnBodies(0);
		PistonActuator *piston = new PistonActuator();
		piston->setName("piston");
		piston->setBodyA(linkage1);
		piston->setBodyB(block);
		piston->setPointA(pointOnBodies);
		piston->setPointB(pointOnBodies);
		piston->setOptimalForce(200.0);
		piston->setPointsAreGlobal(false);

		osimModel.addForce(piston);
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		// Added ControllableSpring between the first linkage and the second block
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		ControllableSpring *spring = new ControllableSpring;
		spring->setName("spring");
		spring->setBodyA(block);
		spring->setBodyB(linkage1);
		spring->setPointA(pointOnBodies);
		spring->setPointB(pointOnBodies);
		spring->setOptimalForce(2000.0);
		spring->setPointsAreGlobal(false);
		spring->setRestLength(0.8);

		osimModel.addForce(spring);

		// define the simulation times
		double t0(0.0), tf(15);

		// create a controller to control the piston and spring actuators
		// the prescribed controller sets the controls as functions of time
		PrescribedController *legController = new PrescribedController();
		// give the legController control over all (two) model actuators
		legController->setActuators(osimModel.updActuators());

		// specify some control nodes for spring stiffness control
		double t[] = {0.0, 4.0, 7.0,  10.0, 15.0};
        double x[] = {1.0, 1.0, 0.25,  0.25, 5.0};

		// specify the control function for each actuator
		legController->prescribeControlForActuator("piston", new Constant(0.1));
		legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));

		// add the controller to the model
		osimModel.addController(legController);		
		
		// define the acceration due to gravity
		osimModel.setGravity(Vec3(0, -9.80665, 0));

		// enable the model visualizer see the model in action, which can be
		// useful for debugging
		osimModel.setUseVisualizer(true);

		// Initialize system
		SimTK::State& si = osimModel.initSystem();
		
		// Pin joint initial states
		double q1_i = -Pi/4;
		double q2_i = - 2*q1_i;
		CoordinateSet &coordinates = osimModel.updCoordinateSet();
		coordinates[0].setValue(si, q1_i, true);
		coordinates[1].setValue(si,q2_i, true);

		// Setup integrator and manager

void testMcKibbenActuator()
{

    double pressure = 5 * 10e5; // 5 bars
    double num_turns = 1.5;     // 1.5 turns
    double B = 277.1 * 10e-4;  // 277.1 mm

    using namespace SimTK;
    std::clock_t startTime = std::clock();

    double mass = 1;
    double ball_radius = 10e-6;

    Model *model = new Model;
    model->setGravity(Vec3(0));

    Ground& ground = model->updGround();

    McKibbenActuator *actuator = new McKibbenActuator("mckibben", num_turns, B);
    
    OpenSim::Body* ball = new OpenSim::Body("ball", mass ,Vec3(0),  mass*SimTK::Inertia::sphere(0.1));
    ball->scale(Vec3(ball_radius), false);

    actuator->addNewPathPoint("mck_ground", ground, Vec3(0));
    actuator->addNewPathPoint("mck_ball", *ball, Vec3(ball_radius));

    Vec3 locationInParent(0, ball_radius, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
    SliderJoint *ballToGround = new SliderJoint("ballToGround", ground, locationInParent, orientationInParent, *ball, locationInBody, orientationInBody);

    ballToGround->updCoordinate().setName("ball_d");
    ballToGround->updCoordinate().setPrescribedFunction(LinearFunction(20 * 10e-4, 0.5 * 264.1 * 10e-4));
    ballToGround->updCoordinate().set_prescribed(true);

    model->addBody(ball);
    model->addJoint(ballToGround);
    model->addForce(actuator);

    PrescribedController* controller =  new PrescribedController();
    controller->addActuator(*actuator);
    controller->prescribeControlForActuator("mckibben", new Constant(pressure));

    model->addController(controller);

    ForceReporter* reporter = new ForceReporter(model);
    model->addAnalysis(reporter);
    
    SimTK::State& si = model->initSystem();

    model->getMultibodySystem().realize(si, Stage::Position);

    double final_t = 10.0;
    double nsteps = 10;
    double dt = final_t / nsteps;

    RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
    integrator.setAccuracy(1e-7);
    Manager manager(*model, integrator);
    manager.setInitialTime(0.0);

    for (int i = 1; i <= nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(si);
        model->getMultibodySystem().realize(si, Stage::Velocity);
        Vec3 pos;
        model->updSimbodyEngine().getPosition(si, *ball, Vec3(0), pos);

        double applied = actuator->computeActuation(si);;

        double theoretical = (pressure / (4* pow(num_turns,2) * SimTK::Pi)) * (3*pow(pos(0), 2) - pow(B, 2));

        ASSERT_EQUAL(applied, theoretical, 10.0);

        manager.setInitialTime(dt*i);
    }


    std::cout << " ******** Test McKibbenActuator time = ********" <<
        1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}

void testClutchedPathSpring()
{
    using namespace SimTK;

    // start timing
    std::clock_t startTime = std::clock();

    double mass = 1;
    double stiffness = 100;
    double dissipation = 0.3;
    double start_h = 0.5;
    //double ball_radius = 0.25;

    //double omega = sqrt(stiffness/mass);

    // Setup OpenSim model
    Model* model = new Model;
    model->setName("ClutchedPathSpringModel");
    model->setGravity(gravity_vec);

    //OpenSim bodies
    const Ground* ground = &model->getGround();
    
    // body that acts as the pulley that the path wraps over
    OpenSim::Body* pulleyBody =
        new OpenSim::Body("PulleyBody", mass ,Vec3(0),  mass*Inertia::brick(0.1, 0.1, 0.1));
    
    // body the path spring is connected to at both ends
    OpenSim::Body* block =
        new OpenSim::Body("block", mass ,Vec3(0),  mass*Inertia::brick(0.2, 0.1, 0.1));
    block->attachGeometry(new Brick(Vec3(0.2, 0.1, 0.1)));
    block->scale(Vec3(0.2, 0.1, 0.1), false);

    //double dh = mass*gravity_vec(1)/stiffness;
    
    WrapCylinder* pulley = new WrapCylinder();
    pulley->set_radius(0.1);
    pulley->set_length(0.05);

    // Add the wrap object to the body, which takes ownership of it
    pulleyBody->addWrapObject(pulley);

    // Add joints
    WeldJoint* weld = 
        new WeldJoint("weld", *ground, Vec3(0, 1.0, 0), Vec3(0), *pulleyBody, Vec3(0), Vec3(0));
    
    SliderJoint* slider =
        new SliderJoint("slider", *ground, Vec3(0), Vec3(0,0,Pi/2),*block, Vec3(0), Vec3(0,0,Pi/2));

    double positionRange[2] = {-10, 10};
    // Rename coordinates for a slider joint
    slider->updCoordinate().setName("block_h");
    slider->updCoordinate().setRange(positionRange);

    model->addBody(pulleyBody);
    model->addJoint(weld);

    model->addBody(block);
    model->addJoint(slider);

    ClutchedPathSpring* spring = 
        new ClutchedPathSpring("clutch_spring", stiffness, dissipation, 0.01);

    spring->updGeometryPath().appendNewPathPoint("origin", *block, Vec3(-0.1, 0.0 ,0.0));
    
    int N = 10;
    for(int i=1; i<N; ++i){
        double angle = i*Pi/N;
        double x = 0.1*cos(angle);
        double y = 0.1*sin(angle);
        spring->updGeometryPath().appendNewPathPoint("", *pulleyBody, Vec3(-x, y ,0.0));
    }

    spring->updGeometryPath().appendNewPathPoint("insertion", *block, Vec3(0.1, 0.0 ,0.0));

    // BUG in defining wrapping API requires that the Force containing the GeometryPath be
    // connected to the model before the wrap can be added
    model->addForce(spring);

    PrescribedController* controller = new PrescribedController();
    controller->addActuator(*spring);
    
    // Control greater than 1 or less than 0 should be treated as 1 and 0 respectively.
    double     timePts[4] = {0.0,  5.0, 6.0, 10.0};
    double clutchOnPts[4] = {1.5, -2.0, 0.5,  0.5};

    PiecewiseConstantFunction* controlfunc = 
        new PiecewiseConstantFunction(4, timePts, clutchOnPts);

    controller->prescribeControlForActuator("clutch_spring", controlfunc);
    model->addController(controller);

    model->print("ClutchedPathSpringModel.osim");

    //Test deserialization
    delete model;
    model = new Model("ClutchedPathSpringModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(model);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    // update mobility forces
    Vector& mobilityForces = model->getMultibodySystem()
        .updMobilityForces(state, Stage::Dynamics);

    // Apply torques as mobility forces of the ball joint
    for(int i=0; i<3; ++i){
        mobilityForces[6+i] = torqueInG[i]; 
    }

    model->getMultibodySystem().realize(state, Stage::Acceleration);
    const Vector& udotMobility = state.getUDot();
    udotMobility.dump("Accelerations due to mobility forces");

    // First make sure that accelerations are not zero accidentally
    ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
    // Then check if they are equal
    for(int i=0; i<udotMobility.size(); ++i){
        ASSERT_EQUAL(udotMobility[i], udotBody[i], 1.0e-12);
    }

    // clear the mobility forces
    mobilityForces = 0;

    //Now add the actuator to the model and control it to generate the same
    //torque as applied directly to the multibody system (above)

    // Create and add the torque actuator to the model
    TorqueActuator* actuator =
        new TorqueActuator(*bodyA, *bodyB, torqueAxis, true);
    actuator->setName("torque");
    model->addForce(actuator);

    // Create and add a controller to control the actuator
    PrescribedController* controller =  new PrescribedController();
    controller->addActuator(*actuator);
    // Apply torque about torqueAxis
    controller->prescribeControlForActuator("torque", new Constant(torqueMag));

    model->addController(controller);

    /*
    ActuatorPowerProbe* powerProbe = new ActuatorPowerProbe(Array<string>("torque",1),false, 1); 
    powerProbe->setOperation("integrate");
    powerProbe->setInitialConditions(Vector(1, 0.0));
    */

    //model->addProbe(powerProbe);

    model->print("TestTorqueActuatorModel.osim");
    model->setUseVisualizer(false);

    // get a new system and state to reflect additions to the model
    state = model->initSystem();

    model->computeStateVariableDerivatives(state);

    const Vector &udotTorqueActuator = state.getUDot();

    // First make sure that accelerations are not zero accidentally
    ASSERT(udotMobility.norm() != 0.0 || udotTorqueActuator.norm() != 0.0);

    // Then verify that the TorqueActuator also generates the same acceleration
    // as the equivalent applied mobility force
    for(int i=0; i<udotMobility.size(); ++i){
        ASSERT_EQUAL(udotMobility[i], udotTorqueActuator[i], 1.0e-12);
    }

//.........这里部分代码省略.........
            new Millard2012EquilibriumMuscle("original",
                maxIsometricForce, optimalFiberLength, tendonSlackLength,
                pennationAngle);

        // Define the path of the muscles
        fatigable->addNewPathPoint("fatigable-point1", ground, 
            Vec3(0.0, halfLength, -0.35));
        fatigable->addNewPathPoint("fatigable-point2", *block, 
            Vec3(0.0, halfLength, -halfLength));

        original->addNewPathPoint("original-point1", ground, 
            Vec3(0.0, halfLength, 0.35));
        original->addNewPathPoint("original-point2", *block, 
            Vec3(0.0, halfLength, halfLength));

        // Define the default states for the two muscles
        // Activation
        fatigable->setDefaultActivation(0.01);
        original->setDefaultActivation(0.01);
        // Fiber length
        fatigable->setDefaultFiberLength(optimalFiberLength);
        original->setDefaultFiberLength(optimalFiberLength);

        // Add the two muscles (as forces) to the model
        osimModel.addForce(fatigable);
        osimModel.addForce(original);

        ///////////////////////////////////
        // DEFINE CONTROLS FOR THE MODEL //
        ///////////////////////////////////
        // Create a prescribed controller that simply supplies controls as 
        // a function of time.
        // For muscles, controls are normalized stoor-neuron excitations
        PrescribedController *muscleController = new PrescribedController();
        muscleController->setActuators(osimModel.updActuators());
    
        // Set the prescribed muscle controller to use the same muscle control function for each muscle
        muscleController->prescribeControlForActuator("fatigable", new Constant(1.0));
        muscleController->prescribeControlForActuator("original", new Constant(1.0));

        // Add the muscle controller to the model
        osimModel.addController(muscleController);

        // Add a Muscle analysis
        MuscleAnalysis* muscAnalysis = new MuscleAnalysis(&osimModel);
        Array<std::string> coords(blockToGround->getCoordinate(FreeJoint::Coord::TranslationZ).getName(),1);
        muscAnalysis->setCoordinates(coords);
        muscAnalysis->setComputeMoments(false);
        osimModel.addAnalysis(muscAnalysis);

        // Turn on the visualizer to view the simulation run live.
        osimModel.setUseVisualizer(false);

        //////////////////////////
        // PERFORM A SIMULATION //
        //////////////////////////

        // Initialize the system and get the state
        SimTK::State& si = osimModel.initSystem();

        // Init coords to 0 and lock the rotational degrees of freedom so the block doesn't twist
        CoordinateSet& coordinates = osimModel.updCoordinateSet();
        coordinates[0].setValue(si, 0);
        coordinates[1].setValue(si, 0);
        coordinates[2].setValue(si, 0);
        coordinates[3].setValue(si, 0);

//.........这里部分代码省略.........
    kneeExtensorLeft->set_ignore_tendon_compliance(true);
    model.addForce(kneeExtensorLeft);
    
    // add a back extensor to controll the upper 4-bar linkage
    Millard2012EquilibriumMuscle* backExtensorRight = new Millard2012EquilibriumMuscle(
                                            "back_extensor_right",
                                            back_extensor_F0, back_extensor_lm0,
                                            back_extensor_lts, pennationAngle);
    
    backExtensorRight->addNewPathPoint("back_extensor_right_origin", *chest,
                                      back_extensor_origin);
    backExtensorRight->addNewPathPoint("back_extensor_right_insertion", *back,
                                      back_extensor_insertion);
    backExtensorRight->set_ignore_tendon_compliance(true);
    model.addForce(backExtensorRight);
    
    // copy right back extensor and use to make left extensor
    Millard2012EquilibriumMuscle* backExtensorLeft =
            new Millard2012EquilibriumMuscle(*backExtensorRight);
    
    backExtensorLeft->setName("back_extensor_left");
    
    PathPointSet& pointsLeft = backExtensorLeft->updGeometryPath()
        .updPathPointSet();
    for (int i=0; i<points.getSize(); ++i) {
        pointsLeft[i].setLocationCoord(2, -1*pointsLeft[i].getLocationCoord(2));
    }
    backExtensorLeft->set_ignore_tendon_compliance(true);
    model.addForce(backExtensorLeft);
    
    
    
    // MUSCLE CONTROLLERS
    //________________________________________________________________________
    
    // specify a piecwise linear function for the muscle excitations
    PiecewiseConstantFunction* x_of_t = new PiecewiseConstantFunction(3, times,
                                                                  excitations);
    
    
    PrescribedController* kneeController = new PrescribedController();
    kneeController->addActuator(*kneeExtensorLeft);
    kneeController->addActuator(*kneeExtensorRight);
    kneeController->prescribeControlForActuator(0, x_of_t);
    kneeController->prescribeControlForActuator(1, x_of_t->clone());
    
    model.addController(kneeController);
    
    PrescribedController* backController = new PrescribedController();
    backController->addActuator(*backExtensorLeft);
    backController->addActuator(*backExtensorRight);
    backController->prescribeControlForActuator(0, x_of_t->clone());
    backController->prescribeControlForActuator(1, x_of_t->clone());
    
    model.addController(backController);

    
    /* You'll find that these muscles can make Luxo Myo stand, but not jump.
     * Jumping will require an assistive device. We'll add two frames for
     * attaching a point to point assistive actuator.
     */
    
    
    // add frames for connecting a back assitance device between the chest
    // and pelvis
    PhysicalOffsetFrame* back_assist_origin_frame = new
        PhysicalOffsetFrame("back_assist_origin",
                            *chest,
                            back_assist_origin_transform);
    
    PhysicalOffsetFrame* back_assist_insertion_frame = new
        PhysicalOffsetFrame("back_assist_insertion",
                            *pelvisBracket,
                            back_assist_insertion_transform);
    
    model.addFrame(back_assist_origin_frame);
    model.addFrame(back_assist_insertion_frame);
    
    // add frames for connecting a knee assistance device between the posterior
    // leg and bottom bracket.
    PhysicalOffsetFrame* knee_assist_origin_frame = new
    PhysicalOffsetFrame("knee_assist_origin",
                        *posteriorLegBar,
                        knee_assist_origin_transform);
    
    PhysicalOffsetFrame* knee_assist_insertion_frame = new
    PhysicalOffsetFrame("knee_assist_insertion",
                        *bottom_bracket,
                        knee_assist_insertion_transform);
    
    model.addFrame(knee_assist_origin_frame);
    model.addFrame(knee_assist_insertion_frame);

    // Temporary: make the frame geometry disappear.
    for (auto& c : model.getComponentList<OpenSim::FrameGeometry>()) {
        const_cast<OpenSim::FrameGeometry*>(&c)->set_scale_factors(
                SimTK::Vec3(0.001, 0.001, 0.001));
    }
    
}

//.........这里部分代码省略.........
        muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05));
        // Path for muscle 2
        muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35));
        muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05));

        // Add the two muscles (as forces) to the model
        osimModel.addForce(muscle1);
        osimModel.addForce(muscle2);


        // PRESCRIBED FORCE
        // Create a new prescribed force to be applied to the block
        PrescribedForce *prescribedForce = new PrescribedForce(block);
        prescribedForce->setName("prescribedForce");

        // Specify properties of the force function to be applied to the block
        double time[2] = {0, finalTime};                    // time nodes for linear function
        double fXofT[2] = {0,  -blockMass*gravity[1]*3.0};  // force values at t1 and t2

        // Create linear function for the force components
        PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);
        // Set the force and point functions for the new prescribed force
        prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
        prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));

        // Add the new prescribed force to the model
        osimModel.addForce(prescribedForce);

        ///////////////////////////////////
        // DEFINE CONTROLS FOR THE MODEL //
        ///////////////////////////////////
        // Create a prescribed controller that simply applies controls as function of time
        // For muscles, controls are normalized motor-neuron excitations
        PrescribedController *muscleController = new PrescribedController();
        muscleController->setActuators(osimModel.updActuators());
        // Define linear functions for the control values for the two muscles
        Array<double> slopeAndIntercept1(0.0, 2);  // array of 2 doubles
        Array<double> slopeAndIntercept2(0.0, 2);
        // muscle1 control has slope of -1 starting 1 at t = 0
        slopeAndIntercept1[0] = -1.0/(finalTime-initialTime);
        slopeAndIntercept1[1] = 1.0;
        // muscle2 control has slope of 0.95 starting 0.05 at t = 0
        slopeAndIntercept2[0] = 0.95/(finalTime-initialTime);
        slopeAndIntercept2[1] = 0.05;

        // Set the indiviudal muscle control functions for the prescribed muscle controller
        muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1));
        muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));

        // Add the muscle controller to the model
        osimModel.addController(muscleController);

        ///////////////////////////////////
        // SPECIFY MODEL DEFAULT STATES  //
        ///////////////////////////////////
        // Define the default states for the two muscles
        // Activation
        muscle1->setDefaultActivation(slopeAndIntercept1[1]);
        muscle2->setDefaultActivation(slopeAndIntercept2[1]);
        // Fiber length
        muscle2->setDefaultFiberLength(optimalFiberLength);
        muscle1->setDefaultFiberLength(optimalFiberLength);

        // Save the model to a file
        osimModel.print("tugOfWar_model.osim");

//.........这里部分代码省略.........
//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================

    //Attach the muscle
    const string &actuatorType = aMuscle->getConcreteClassName();
    aMuscle->setName("muscle");
    aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0));
    aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0));
    
    ActivationFiberLengthMuscle_Deprecated *aflMuscle 
        = dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle);
    if(aflMuscle){
        // Define the default states for the muscle that has 
        //activation and fiber-length states
        aflMuscle->setDefaultActivation(act0);
        aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength());
    }else{
        ActivationFiberLengthMuscle *aflMuscle2 
            = dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle);
        if(aflMuscle2){
            // Define the default states for the muscle 
            //that has activation and fiber-length states
            aflMuscle2->setDefaultActivation(act0);
            aflMuscle2->setDefaultFiberLength(aflMuscle2
                ->getOptimalFiberLength());
        }
    }

    model.addForce(aMuscle);

    // Create a prescribed controller that simply 
    //applies controls as function of time
    PrescribedController * muscleController = new PrescribedController();
    if(control != NULL){
        muscleController->setActuators(model.updActuators());
        // Set the indiviudal muscle control functions 
        //for the prescribed muscle controller
        muscleController->prescribeControlForActuator("muscle",control->clone());

        // Add the control set controller to the model
        model.addController(muscleController);
    }

    // Set names for muscles / joints.
    Array<string> muscNames;
    muscNames.append(aMuscle->getName());
    Array<string> jointNames;
    jointNames.append("slider");

//==========================================================================
// 2. SIMULATION SETUP: Instrument the test with probes
//==========================================================================

    Array<string> muscNamesTwice = muscNames;
    muscNamesTwice.append(muscNames.get(0));
    cout << "------------\nPROBES\n------------" << endl;
    int probeCounter = 1;

    // Add ActuatorPowerProbe to measure work done by the muscle 
    ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1);
    //muscWorkProbe->setName("ActuatorWork");
    muscWorkProbe->setOperation("integrate");
    SimTK::Vector ic1(1);
    ic1 = 9.0;      // some arbitary initial condition.
    muscWorkProbe->setInitialConditions(ic1);

//==========================================================================================================
void testPrescribedControllerOnBlock(bool disabled)
{
    using namespace SimTK;

    // Create a new OpenSim model
    Model osimModel;
    osimModel.setName("osimModel");

    // Get the ground body
    OpenSim::Body& ground = osimModel.getGroundBody();

    // Create a 20 kg, 0.1 m^3 block body
    double blockMass = 20.0, blockSideLength = 0.1;
    Vec3 blockMassCenter(0), groundOrigin(0), blockInGround(0, blockSideLength/2, 0);
    Inertia blockIntertia = Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

    OpenSim::Body block("block", blockMass, blockMassCenter, blockMass*blockIntertia);

    //Create a free joint with 6 degrees-of-freedom
    SimTK::Vec3 noRotation(0);
    SliderJoint blockToGround("",ground, blockInGround, noRotation, block, blockMassCenter, noRotation);
    // Create 6 coordinates (degrees-of-freedom) between the ground and block
    CoordinateSet& jointCoordinateSet = blockToGround.upd_CoordinateSet();
    double posRange[2] = {-1, 1};
    jointCoordinateSet[0].setName("xTranslation");
    jointCoordinateSet[0].setMotionType(Coordinate::Translational);
    jointCoordinateSet[0].setRange(posRange);

    // Add the block body to the model
    osimModel.addBody(&block);

    // Define a single coordinate actuator.
    CoordinateActuator actuator(jointCoordinateSet[0].getName());
    actuator.setName("actuator");

    // Add the actuator to the model
    osimModel.addForce(&actuator);

    double initialTime = 0;
    double finalTime = 1.0;

    // Define the initial and final control values
    double controlForce = 100;

    // Create a prescribed controller that simply applies a function of the force
    PrescribedController actuatorController;
    actuatorController.setName("testPrescribedController");
    actuatorController.setActuators(osimModel.updActuators());
    actuatorController.prescribeControlForActuator(0, new Constant(controlForce));
    actuatorController.setDisabled(disabled);

    // add the controller to the model
    osimModel.addController(&actuatorController);

    osimModel.print("blockWithPrescribedController.osim");
    Model modelfileFromFile("blockWithPrescribedController.osim");

    // Verify that serialization and then deserialization of the disable flag is correct
    ASSERT(modelfileFromFile.getControllerSet().get("testPrescribedController").isDisabled() == disabled);

    // Initialize the system and get the state representing the state system
    SimTK::State& si = osimModel.initSystem();

    // Specify zero slider joint kinematic states
    CoordinateSet &coordinates = osimModel.updCoordinateSet();
    coordinates[0].setValue(si, 0.0);    // x translation
    coordinates[0].setSpeedValue(si, 0.0);			 // x speed

    // Create the integrator and manager for the simulation.
    double accuracy = 1.0e-3;
    SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
    integrator.setAccuracy(accuracy);
    Manager manager(osimModel, integrator);

    // Integrate from initial time to final time
    manager.setInitialTime(initialTime);
    manager.setFinalTime(finalTime);
    std::cout<<"\n\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
    manager.integrate(si);

    si.getQ().dump("Final position:");

    double expected = disabled ? 0 : 0.5*(controlForce/blockMass)*finalTime*finalTime;
    ASSERT_EQUAL(expected, coordinates[0].getValue(si), accuracy, __FILE__, __LINE__, "PrescribedController failed to produce the expected motion of block.");

    // Save the simulation results
    Storage states(manager.getStateStorage());
    states.print("block_push.sto");

    osimModel.disownAllComponents();
}// end of testPrescribedControllerOnBlock()

//.........这里部分代码省略.........
        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);
        }

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"));

}

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"));

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

//.........这里部分代码省略.........
		contactParams->addGeometry("cube");
		contactParams->addGeometry("floor");
		
		// Create a new elastic foundation (contact) force between the floor and cube.
		OpenSim::ElasticFoundationForce *contactForce = new OpenSim::ElasticFoundationForce(contactParams);
		contactForce->setName("contactForce");

		// Add the new elastic foundation force to the model
		osimModel.addForce(contactForce);

		// PRESCRIBED FORCE
		// Create a new prescribed force to be applied to the block
		PrescribedForce *prescribedForce = new PrescribedForce(block);
		prescribedForce->setName("prescribedForce");

		// Specify properties of the force function to be applied to the block
		double time[2] = {0, finalTime};					// time nodes for linear function
		double fXofT[2] = {0,  -blockMass*gravity[1]*3.0};	// force values at t1 and t2

		// Create linear function for the force components
		PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);
		// Set the force and point functions for the new prescribed force
		prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
		prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));

		// Add the new prescribed force to the model
		osimModel.addForce(prescribedForce);

		///////////////////////////////////
		// DEFINE CONTROLS FOR THE MODEL //
		///////////////////////////////////
		// Create a prescribed controller that simply applies controls as function of time
		// For muscles, controls are normalized motor-neuron excitations
		PrescribedController *muscleController = new PrescribedController();
		muscleController->setActuators(osimModel.updActuators());
		// Define linear functions for the control values for the two muscles
		Array<double> slopeAndIntercept1(0.0, 2);  // array of 2 doubles
		Array<double> slopeAndIntercept2(0.0, 2);
		// muscle1 control has slope of -1 starting 1 at t = 0
		slopeAndIntercept1[0] = -1.0/(finalTime-initialTime);  slopeAndIntercept1[1] = 1.0;
		// muscle2 control has slope of 0.95 starting 0.05 at t = 0
		slopeAndIntercept2[0] = 0.95/(finalTime-initialTime);  slopeAndIntercept2[1] = 0.05;
		
		// Set the indiviudal muscle control functions for the prescribed muscle controller
		muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1));
		muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));

		// Add the muscle controller to the model
		osimModel.addController(muscleController);

		///////////////////////////////////
		// SPECIFY MODEL DEFAULT STATES  //
		///////////////////////////////////
		// Define the default states for the two muscles
		// Activation
		muscle1->setDefaultActivation(slopeAndIntercept1[1]);
		muscle2->setDefaultActivation(slopeAndIntercept2[1]);
		// Fiber length
		muscle2->setDefaultFiberLength(optimalFiberLength);
		muscle1->setDefaultFiberLength(optimalFiberLength);

		// Save the model to a file
		osimModel.print("tugOfWar_model.osim");

		//////////////////////////
		// PERFORM A SIMULATION //