示例#1
0
/**
* This test verifies the use of BodyActuator for applying spatial forces to a selected
* body. It checks if using a BodyActuator generates equivalent acceleration compared 
* to when applying the forces via mobilityForce.
*
* @author Soha Pouya
*/
void testBodyActuator()
{
    using namespace SimTK;
    // start timing
    std::clock_t startTime = std::clock();

    // Setup OpenSim model
    Model *model = new Model;

    // turn off gravity
    model->setGravity(Vec3(0));

    //OpenSim body 1: Ground
    const Ground& ground = model->getGround();

    // OpenSim body 2: A Block
    // Geometrical/Inertial properties for the block
    double blockMass = 1.0, blockSideLength = 1;
    Vec3 blockMassCenter(0);
    Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
        blockSideLength/2, blockSideLength/2); // for the halves see doxygen for brick 

    OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
                                             blockMassCenter, blockInertia);

    // Add display geometry to the block to visualize in the GUI
    block->attachGeometry(new Brick(Vec3(blockSideLength/2,
                                     blockSideLength/2, 
                                     blockSideLength/2)));

    Vec3 locationInParent(0, blockSideLength / 2, 0), orientationInParent(0), 
        locationInBody(0), orientationInBody(0);
    FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundBall", 
        ground, locationInParent, orientationInParent, 
        *block, locationInBody, orientationInBody);
    
    model->addBody(block);
    model->addJoint(blockToGroundFree);
    
    // specify magnitude and direction of applied force and torque
    double forceMag = 1.0;
    Vec3 forceAxis(1, 1, 1);
    Vec3 forceInG = forceMag * forceAxis;

    double torqueMag = 1.0;
    Vec3 torqueAxis(1, 1, 1);
    Vec3 torqueInG = torqueMag*torqueAxis;

    // ---------------------------------------------------------------------------
    // Use MobilityForces to Apply the given Torques and Forces to the body
    // ---------------------------------------------------------------------------
    State& state = model->initSystem();

    model->getMultibodySystem().realize(state, Stage::Dynamics);
    Vector_<SpatialVec>& bodyForces =
        model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
    bodyForces.dump("Body Forces before applying 6D spatial force:");

    model->getMatterSubsystem().addInBodyTorque(state, block->getMobilizedBodyIndex(),
        torqueInG, bodyForces);
    model->getMatterSubsystem().addInStationForce(state, block->getMobilizedBodyIndex(),
        Vec3(0), forceInG, bodyForces);

    bodyForces.dump("Body Forces after applying 6D spatial force to the block");

    model->getMultibodySystem().realize(state, Stage::Acceleration);
    Vector udotBody = state.getUDot();
    udotBody.dump("Accelerations due to body forces");

    // clear body forces
    bodyForces *= 0;

    // 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[i] = torqueInG[i];
        mobilityForces[i+3] = forceInG[i];
    }
    mobilityForces.dump("Mobility Forces after applying 6D spatial force to the block");


    model->getMultibodySystem().realize(state, Stage::Acceleration);
    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], SimTK::Eps);
    }

    // clear the mobility forces
    mobilityForces = 0;

    // ---------------------------------------------------------------------------
    // Use a BodyActuator to Apply the same given Torques and Forces to the body
    // ---------------------------------------------------------------------------

    // Create and add the body actuator to the model
    BodyActuator* actuator = new BodyActuator(*block);
    actuator->setName("BodyAct");
    model->addForce(actuator);
    model->setUseVisualizer(false);

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

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

    // -------------- Provide control signals for bodyActuator ----------
    // Get the default control vector of the model
    Vector modelControls = model->getDefaultControls();
    
    // Specify a vector of control signals to include desired torques and forces
    Vector fixedControls(6);
    for (int i = 0; i < 3; i++){
        fixedControls(i) = torqueInG(i);
        fixedControls(i + 3) = forceInG(i);
    }
    fixedControls.dump("Spatial forces applied by body Actuator:");

    // Add control values and set their values
    actuator->addInControls(fixedControls, modelControls);
    model->setDefaultControls(modelControls);

    // ------------------- Compute Acc and Compare -------------
    // Compute the acc due to spatial forces applied by body actuator
    model->computeStateVariableDerivatives(state1);

    Vector udotBodyActuator = state1.getUDot();
    udotBodyActuator.dump("Accelerations due to body actuator");

    // First make sure that accelerations are not zero accidentally
    ASSERT(udotMobility.norm() != 0.0 || udotBodyActuator.norm() != 0.0);
    // Then verify that the BodyActuator also generates the same acceleration
    // as the equivalent applied mobility force
    for (int i = 0; i<udotBodyActuator.size(); ++i){
        ASSERT_EQUAL(udotMobility[i], udotBodyActuator[i], SimTK::Eps);
    }

    // -------------- Setup integrator and manager -------------------
    RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
    integrator.setAccuracy(integ_accuracy);
    Manager manager(*model, integrator);

    manager.setInitialTime(0.0);
    double final_t = 1.00;
    manager.setFinalTime(final_t);
    manager.integrate(state1);

    // ----------------- Test Copying the model -------------------
    // Before exiting lets see if copying the actuator works
    BodyActuator* copyOfActuator = actuator->clone();
    ASSERT(*copyOfActuator == *actuator);

    // Check that de/serialization works
    Model modelFromFile("TestBodyActuatorModel.osim");
    ASSERT(modelFromFile == *model, __FILE__, __LINE__,
        "Model from file FAILED to match model in memory.");

    std::cout << " ********** Test BodyActuator time = ********** " <<
        1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
示例#2
0
/**
* This test verifies if using a BodyActuator generates equivalent result in the body
* acceleration compared to when using a combination of PointActuaor, TorqueActuaor 
* and BodyActuator. 
* It therefore also verifies model consistency when user defines and uses a 
* combination of these 3 actuators. 
* 
* @author Soha Pouya
*/
void testActuatorsCombination()
{
    using namespace SimTK;
    // start timing
    std::clock_t startTime = std::clock();

    // Setup OpenSim model
    Model *model = new Model;

    // turn off gravity
    model->setGravity(Vec3(0));

    // OpenSim bodies: 1) The ground
    const Ground& ground = model->getGround();
    //ground.addDisplayGeometry("block.vtp");

    // OpenSim bodies: 2) A Block
    // Geometrical/Inertial properties for the block
    double blockMass = 1.0, blockSideLength = 1.0;
    Vec3 blockMassCenter(0);
    // Brick Inertia: for the halves see doxygen  
    Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2, 
                                    blockSideLength/2, blockSideLength/2);
    std::cout << "blockInertia: " << blockInertia << std::endl;

    OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
                                    blockMassCenter, blockInertia);

    // Add display geometry to the block to visualize in the GUI
    block->attachGeometry(new Brick(Vec3(blockSideLength/2, 
                                     blockSideLength/2, 
                                     blockSideLength/2)));

    // Make a FreeJoint from block to ground
    Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), //locationInParent(0, blockSideLength, 0)
         locationInBody(0), orientationInBody(0);
    FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundFreeJoint", 
                ground, locationInParent, orientationInParent, 
                *block, locationInBody, orientationInBody);

    // Add the body and joint to the model
    model->addBody(block);
    model->addJoint(blockToGroundFree);

    // specify magnitude and direction of desired force and torque vectors to apply
    double forceMag = 1.0;
    Vec3 forceAxis(1, 1, 1);
    SimTK::UnitVec3 forceUnitAxis(forceAxis); // to normalize
    Vec3 forceInG = forceMag * forceUnitAxis;

    double torqueMag = 1.0;
    Vec3 torqueAxis(1, 2, 1);
    SimTK::UnitVec3 torqueUnitAxis(torqueAxis); // to normalize
    Vec3 torqueInG = torqueMag*torqueUnitAxis;

    // needed to be called here once to build controller for body actuator
    /*State& state = */model->initSystem();
    
    // ---------------------------------------------------------------------------
    // Add a set of PointActuator, TorqueActuator and BodyActuator to the model
    // ---------------------------------------------------------------------------
    // Create and add a body actuator to the model
    BodyActuator* bodyActuator1 = new BodyActuator(*block);
    bodyActuator1->setName("BodyAct1");
    bodyActuator1->set_point(Vec3(0, blockSideLength/2, 0));
    model->addForce(bodyActuator1);
    
    // Create and add a torque actuator to the model
    TorqueActuator* torqueActuator =
        new TorqueActuator(*block, ground, torqueUnitAxis, true);
    torqueActuator->setName("torqueAct");
    model->addForce(torqueActuator);

    // Create and add a point actuator to the model
    PointActuator* pointActuator =
        new PointActuator("block");
    pointActuator->setName("pointAct");
    pointActuator->set_direction(forceUnitAxis);
    pointActuator->set_point(Vec3(0, blockSideLength/2,0));
    model->addForce(pointActuator);

    // ------ build the model -----
    model->print("TestActuatorCombinationModel.osim");
    model->setUseVisualizer(false);

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

    // ------------------- Provide control signals for bodyActuator ----------------
    // Get the default control vector of the model
    Vector modelControls = model->getDefaultControls();

    // Specify a vector of control signals for desired torques and forces
    Vector bodyActuator1Controls(6,0.0); 
    for (int i=0; i<3; i++) bodyActuator1Controls(i) = torqueInG(i); // torque in 3 axes
    for (int i=0; i<3; i++) bodyActuator1Controls(i+3) = forceInG(i); // force along 3 axes
    
    
    bodyActuator1Controls.dump("Spatial forces applied by first Body Actuator:");

    // Add control values and set their values
    bodyActuator1->addInControls(bodyActuator1Controls, modelControls);
    model->setDefaultControls(modelControls);

    // ---------------- Provide control signals for torqueActuator -----------------
    Vector torqueActuatorControls(1); // input to addInControl should be a Vector
    torqueActuatorControls(0) = torqueMag; // axis already defined when initializing 

    Vector torqueActuatorVector(3); // to print out the 3D vector of applied torque
    for (int i = 0; i < 3; i++){
        torqueActuatorVector(i) = torqueInG(i);
    }
    torqueActuatorVector.dump("Torques applied by the Torque Actuator:");

    // Add control values and set their values
    torqueActuator->addInControls(torqueActuatorControls, modelControls);
    model->setDefaultControls(modelControls);

    // ------------------ Provide control signals for pointActuator ----------------
    Vector pointActuatorControls(1); // input to addInControl should be a Vector
    pointActuatorControls(0) = forceMag; // axis already defined when initializing

    Vector pointActuatorVector(3); // to print out the whole force vector
    for (int i = 0; i < 3; i++){
        pointActuatorVector(i) = forceInG(i);
    }
    pointActuatorVector.dump("Forces applied by the point Actuator:");

    // Add control values and set their values
    pointActuator->addInControls(pointActuatorControls, modelControls);
    model->setDefaultControls(modelControls);

    
    // ----------------------- Compute the acc to Compare later --------------------
    // compare the acc due to forces/torques applied by all actuator
    model->computeStateVariableDerivatives(state1);

    Vector udotActuatorsCombination = state1.getUDot();
    udotActuatorsCombination.dump("Accelerations due to all 3 actuators");


    // -----------------------------------------------------------------------------
    // Add a BodyActuator to enclose all of the above spatial forces in one Actuator 
    // -----------------------------------------------------------------------------
    // Create and add a body actuator to the model
    BodyActuator* bodyActuator_sum = new BodyActuator(*block);
    bodyActuator_sum->setName("BodyAct_Sum");
    model->addForce(bodyActuator_sum);
    bodyActuator_sum->set_point(Vec3(0, blockSideLength / 2, 0));


    State& state2 = model->initSystem();
    model->setUseVisualizer(true);

    // Get the default control vector of the model
    Vector modelControls_2 = model->getDefaultControls();

    // Specify a vector of control signals for desired torques and forces
    Vector bodyActuatorSum_Controls(6,0.0);

    // make the torque component as the sum of body, torque and point actuators used 
    // in previous test case
    for (int i = 0; i < 3; i++){
        bodyActuatorSum_Controls(i)   = 2*torqueInG(i);
        bodyActuatorSum_Controls(i+3) = 2*forceInG(i);
    }   

    bodyActuatorSum_Controls.dump("Spatial forces applied by 2nd Body Actuator:");
    std::cout <<"(encloses sum of the above spatial forces in one BodyActuator)"<< std::endl;

    // Add control values and set their values
    bodyActuator_sum->addInControls(bodyActuatorSum_Controls, modelControls_2);
    model->setDefaultControls(modelControls_2);

    // --------------------------- Compute Acc and Compare -------------------------
    // now compare the acc due to forces/torques applied by this body actuator
    model->computeStateVariableDerivatives(state2);

    Vector udotOnlyBodyActuator = state2.getUDot();
    udotOnlyBodyActuator.dump("Accelerations due to only-one body actuator");

    // Verify that the bodyActuator_sum also generates the same acceleration
    // as the equivalent applied by 3 Actuators in previous test case
    // Also make sure that accelerations are not zero accidentally
    ASSERT(udotOnlyBodyActuator.norm() != 0.0 || udotActuatorsCombination.norm() != 0.0);
    for (int i = 0; i<udotActuatorsCombination.size(); ++i){
        ASSERT_EQUAL(udotOnlyBodyActuator[i], udotActuatorsCombination[i], 1.0e-12);
    }
    
    // ------------------------ Setup integrator and manager -----------------------
    RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
    integrator.setAccuracy(integ_accuracy);
    Manager manager(*model, integrator);

    manager.setInitialTime(0.0);
    double final_t = 1.00;
    manager.setFinalTime(final_t);
    manager.integrate(state2);


    std::cout << " ********** Test Actuator Combination time = ********** " <<
        1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
示例#3
0
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);
    model->addAnalysis(reporter);

    model->setUseVisualizer(false);
    SimTK::State& state = model->initSystem();

    CoordinateSet& coords = model->updCoordinateSet();
    coords[0].setValue(state, start_h);
    model->getMultibodySystem().realize(state, Stage::Position );

    //==========================================================================
    // Compute the force and torque at the specified times.

    RungeKuttaMersonIntegrator integrator(model->getMultibodySystem() );
    integrator.setAccuracy(integ_accuracy);
    Manager manager(*model,  integrator);
    manager.setWriteToStorage(true);

    manager.setInitialTime(0.0);

    double final_t = 4.99999;

    manager.setFinalTime(final_t);
    manager.integrate(state);

    // tension is dynamics dependent because controls must be computed
    model->getMultibodySystem().realize(state, Stage::Dynamics);

    spring = dynamic_cast<ClutchedPathSpring*>(
                &model->updForceSet().get("clutch_spring"));
    // Now check that the force reported by spring
    double model_force = spring->getTension(state);
    double stretch0 = spring->getStretch(state);

    // the tension should be half the weight of the block
    double analytical_force = -0.5*(gravity_vec(1))*mass;

    cout << "Tension is: " << model_force << " and should be: " << analytical_force << endl;

    // error if the block does not reach equilibrium since spring is clamped
    ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);

    // unclamp and continue integrating
    manager.setInitialTime(final_t);
    final_t = 5.99999;
    manager.setFinalTime(final_t);
    manager.integrate(state);

    // tension is dynamics dependent because controls must be computed
    model->getMultibodySystem().realize(state, Stage::Dynamics);

    // tension should go to zero quickly
    model_force = spring->getTension(state);

    cout << "Tension is: " << model_force << " and should be: 0.0" << endl;
    // is unclamped and block should be in free-fall
    ASSERT_EQUAL(model_force, 0.0, 10*integ_accuracy);

    // spring is reclamped at 7s so keep integrating
    manager.setInitialTime(final_t);
    final_t = 10.0;
    manager.setFinalTime(final_t);
    manager.integrate(state);

    // tension is dynamics dependent because controls must be computed
    model->getMultibodySystem().realize(state, Stage::Dynamics);

    // tension should build to support the block again
    model_force = spring->getTension(state);
    double stretch1 = spring->getStretch(state);

    cout << "Tension is: " << model_force << " and should be: "<< analytical_force << endl;

    // is unclamped and block should be in free-fall
    ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);

    cout << "Steady stretch at control = 1.0 is " << stretch0 << " m." << endl;
    cout << "Steady stretch at control = 0.5 is " << stretch1 << " m." << endl;

    ASSERT_EQUAL(2*stretch0, stretch1, 10*integ_accuracy);

    manager.getStateStorage().print("clutched_path_spring_states.sto");
    model->getControllerSet().printControlStorage("clutched_path_spring_controls.sto");

    // Save the forces
    reporter->getForceStorage().print("clutched_path_spring_forces.mot"); 

    model->disownAllComponents();

    cout << " ********** Test clutched spring time = ********** " << 
        1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
示例#4
0
/**
 * Run a simulation of a sliding block being pulled by two muscle 
 */
int main()
{
    std::clock_t startTime = std::clock();

    try {
        ///////////////////////////////////////////////
        // DEFINE THE SIMULATION START AND END TIMES //
        ///////////////////////////////////////////////
        // Define the initial and final simulation times
        double initialTime = 0.0;
        double finalTime = 10.0;

        ///////////////////////////////////////////
        // DEFINE BODIES AND JOINTS OF THE MODEL //
        ///////////////////////////////////////////
        // Create an OpenSim model and set its name
        Model osimModel;
        osimModel.setName("tugOfWar");

        // GROUND FRAME

        // Get a reference to the model's ground body
        Ground& ground = osimModel.updGround();

        // Add display geometry to the ground to visualize in the GUI
        ground.attachGeometry(new Mesh("ground.vtp"));
        ground.attachGeometry(new Mesh("anchor1.vtp"));
        ground.attachGeometry(new Mesh("anchor2.vtp"));

        // BLOCK BODY

        // Specify properties of a 20 kg, 10cm length block body
        double blockMass = 20.0, blockSideLength = 0.1;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, 
            blockSideLength, blockSideLength);

        // Create a new block body with the specified properties
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
            blockMassCenter, blockInertia);

        // Add display geometry to the block to visualize in the GUI
        block->attachGeometry(new Mesh("block.vtp"));

        // FREE JOINT

        // Create a new free joint with 6 degrees-of-freedom (coordinates) 
        // between the block and ground bodies
        double halfLength = blockSideLength/2.0;
        Vec3 locationInParent(0, halfLength, 0), orientationInParent(0);
        Vec3 locationInBody(0, halfLength, 0), orientationInBody(0);
        FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, 
            locationInParent, orientationInParent, 
            *block, locationInBody, orientationInBody);

        // Set the angle and position ranges for the free (6-degree-of-freedom)
        // joint between the block and ground frames.
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation1X).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation2Y).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation3Z).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationX).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationY).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationZ).setRange(positionRange);

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

        ///////////////////////////////////////
        // DEFINE FORCES ACTING ON THE MODEL //
        ///////////////////////////////////////
        // MUSCLE FORCES

        // Create two new muscles
        double maxIsometricForce = 1000.0, optimalFiberLength = 0.2, 
               tendonSlackLength = 0.1,    pennationAngle = 0.0,  
               fatigueFactor = 0.30, recoveryFactor = 0.20;

        // fatigable muscle (Millard2012EquilibriumMuscle with fatigue)
        FatigableMuscle* fatigable = new FatigableMuscle("fatigable",
            maxIsometricForce, optimalFiberLength, tendonSlackLength, 
            pennationAngle, fatigueFactor, recoveryFactor);

        // original muscle model (muscle without fatigue)
        Millard2012EquilibriumMuscle* original = 
            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);
        coordinates[4].setValue(si, 0); 
        coordinates[5].setValue(si, 0);
        coordinates[0].setLocked(si, true);
        coordinates[1].setLocked(si, true);
        coordinates[2].setLocked(si, true);
        // Last coordinate (index 5) is the Z translation of the block
        coordinates[4].setLocked(si, true); 

        // Compute initial conditions for muscles
        osimModel.equilibrateMuscles(si);

        // Create the integrator, force reporter, and manager for the simulation.
        // Create the integrator
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-6);
        
        // Create the force reporter
        ForceReporter* reporter = new ForceReporter(&osimModel);
        osimModel.updAnalysisSet().adoptAndAppend(reporter);
        // Create the manager
        Manager manager(osimModel, integrator);

        // Print out details of the model
        osimModel.printDetailedInfo(si, std::cout);

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

        //////////////////////////////
        // SAVE THE RESULTS TO FILE //
        //////////////////////////////

        // Save the simulation results
        // Save the states
        auto statesTable = manager.getStatesTable();
        STOFileAdapter_<double>::write(statesTable, 
                                      "tugOfWar_fatigue_states.sto");

        auto forcesTable = reporter->getForcesTable();
        STOFileAdapter_<double>::write(forcesTable, 
                                      "tugOfWar_fatigue_forces.sto");

        // Save the muscle analysis results
        IO::makeDir("MuscleAnalysisResults");
        muscAnalysis->printResults("fatigue", "MuscleAnalysisResults");

        // Save the OpenSim model to a file
        osimModel.print("tugOfWar_fatigue_model.osim");
    }
    catch (const std::exception& ex)
    {
        std::cout << ex.what() << std::endl;
        return 1;
    }
    catch (...)
    {
        std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
        return 1;
    }

    std::cout << "main() routine time = " << 1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n";

    std::cout << "OpenSim example completed successfully.\n";
    return 0;
}
/**
 * Method for building the Luxo Jr articulating model. It sets up the system of
 * rigid bodies and joint articulations to define Luxo Jr lamp geometry.
 */
void createLuxoJr(OpenSim::Model &model){
    
    // Create base
    //--------------
    OpenSim::Body* base = new OpenSim::Body("base", baseMass, Vec3(0.0),
                Inertia::cylinderAlongY(0.1, baseHeight));
    
    // Add visible geometry
    base->attachGeometry(new Mesh("Base_meters.obj"));
    
    
    // Define base to float relative to ground via free joint
    FreeJoint* base_ground = new FreeJoint("base_ground",
                // parent body, location in parent body, orientation in parent
                model.getGround(), Vec3(0.0), Vec3(0.0),
                // child body, location in child body, orientation in child
                *base, Vec3(0.0,-baseHeight/2.0,0.0),Vec3(0.0));
    
    // add base to model
    model.addBody(base); model.addJoint(base_ground);
    
    /*for (int i = 0; i<base_ground->get_CoordinateSet().getSize(); ++i) {
        base_ground->upd_CoordinateSet()[i].set_locked(true);
    }*/
    
    // Fix a frame to the base axis for attaching the bottom bracket
    SimTK::Transform* shift_and_rotate = new SimTK::Transform();
    //shift_and_rotate->setToZero();
    shift_and_rotate->set(Rotation(-1*SimTK::Pi/2,
                                   SimTK::CoordinateAxis::XCoordinateAxis()),
                          Vec3(0.0, bracket_location, 0.0));

    PhysicalOffsetFrame pivot_frame_on_base("pivot_frame_on_base",
            *base, *shift_and_rotate);

    // Create bottom bracket
    //-----------------------
    OpenSim::Body* bottom_bracket = new OpenSim::Body("bottom_bracket",
                                            bracket_mass, Vec3(0.0),
                                            Inertia::brick(0.03, 0.03, 0.015));
    // add bottom bracket to model
    model.addBody(bottom_bracket);
    
    // Fix a frame to the bracket for attaching joint
    shift_and_rotate->setP(Vec3(0.0));
    PhysicalOffsetFrame pivot_frame_on_bottom_bracket(
        "pivot_frame_on_bottom_bracket", *bottom_bracket, *shift_and_rotate);
    
    // Add visible geometry
    bottom_bracket->attachGeometry(new Mesh("bottom_bracket_meters.obj"));

    // Make bottom bracket to twist on base with vertical pin joint.
    // You can create a joint from any existing physical frames attached to
    // rigid bodies. One way to reference them is by name, like this...
    PinJoint* base_pivot = new PinJoint("base_pivot", pivot_frame_on_base,
                                        pivot_frame_on_bottom_bracket);
    
    base_pivot->append_frames(pivot_frame_on_base);
    base_pivot->append_frames(pivot_frame_on_bottom_bracket);
    // add base pivot joint to the model
    model.addJoint(base_pivot);
    
    // add some damping to the pivot
    // initialized to zero stiffness and damping
    BushingForce* pivotDamper = new BushingForce("pivot_bushing",
                                         "pivot_frame_on_base",
                                         "pivot_frame_on_bottom_bracket");
    
    pivotDamper->set_rotational_damping(pivot_damping);
    
    model.addForce(pivotDamper);
    
    // Create posterior leg
    //-----------------------
    OpenSim::Body* posteriorLegBar = new OpenSim::Body("posterior_leg_bar",
                                    bar_mass,
                                    Vec3(0.0),
                                    Inertia::brick(leg_bar_dimensions/2.0));
    
    posteriorLegBar->attachGeometry(new Mesh("Leg_meters.obj"));

    PhysicalOffsetFrame posterior_knee_on_bottom_bracket(
        "posterior_knee_on_bottom_bracket",
            *bottom_bracket, Transform(posterior_bracket_hinge_location) );

    PhysicalOffsetFrame posterior_knee_on_posterior_bar(
        "posterior_knee_on_posterior_bar",
            *posteriorLegBar, Transform(inferior_bar_hinge_location) );

    // Attach posterior leg to bottom bracket using another pin joint.
    // Another way to reference physical frames in a joint is by creating them
    // in place, like this...
    OpenSim::PinJoint* posteriorKnee = new OpenSim::PinJoint("posterior_knee",
                                    posterior_knee_on_bottom_bracket,
                                    posterior_knee_on_posterior_bar);
    // posteriorKnee will own and serialize the attachment offset frames 
    posteriorKnee->append_frames(posterior_knee_on_bottom_bracket);
    posteriorKnee->append_frames(posterior_knee_on_posterior_bar);

    
    // add posterior leg to model
    model.addBody(posteriorLegBar); model.addJoint(posteriorKnee);
    
    // allow this joint's coordinate to float freely when assembling constraints
    // the joint we create next will drive the pose of the 4-bar linkage
    posteriorKnee->upd_CoordinateSet()[0]
                                    .set_is_free_to_satisfy_constraints(true);
    
    // Create anterior leg Hlink
    //----------------------------
    OpenSim::Body* leg_Hlink = new OpenSim::Body("leg_Hlink",
                                    bar_mass,
                                    Vec3(0.0),
                                    Inertia::brick(leg_Hlink_dimensions/2.0));
    
    leg_Hlink->attachGeometry(new Mesh("H_Piece_meters.obj"));
    
    
    PhysicalOffsetFrame anterior_knee_on_bottom_bracket(
        "anterior_knee_on_bottom_bracket",
            *bottom_bracket, Transform(anterior_bracket_hinge_location));

    
    PhysicalOffsetFrame anterior_knee_on_anterior_bar(
        "anterior_knee_on_anterior_bar",
            *leg_Hlink, Transform(inferior_Hlink_hinge_location));


    // Connect anterior leg to bottom bracket via pin joint
    OpenSim::PinJoint* anterior_knee = new OpenSim::PinJoint("anterior_knee",
                                    anterior_knee_on_bottom_bracket,
                                    anterior_knee_on_anterior_bar);
    anterior_knee->append_frames(anterior_knee_on_bottom_bracket);
    anterior_knee->append_frames(anterior_knee_on_anterior_bar);

    
    // add anterior leg to model
    model.addBody(leg_Hlink); model.addJoint(anterior_knee);
    
    // this anterior knee joint defines the motion of the lower 4-bar linkage
    // set it's default coordinate value to a slightly flexed position.
    anterior_knee->upd_CoordinateSet()[0].set_default_value(SimTK::Pi/6);
    
    // Create pelvis bracket
    //-----------------------
    OpenSim::Body* pelvisBracket = new OpenSim::Body("pelvis_bracket",
                                            bracket_mass,
                                            Vec3(0.0),
                                        Inertia::brick(pelvis_dimensions/2.0));
    
    pelvisBracket->attachGeometry(new Mesh("Pelvis_bracket_meters.obj"));
    
    // Connect pelvis to Hlink via pin joint
    SimTK::Transform pelvis_anterior_shift(
                                        anterior_superior_pelvis_pin_location);

    PhysicalOffsetFrame anterior_hip_on_Hlink(
        "anterior_hip_on_Hlink",
        *leg_Hlink, Transform(superior_Hlink_hinge_location));

    PhysicalOffsetFrame anterior_hip_on_pelvis(
            "anterior_hip_on_pelvis",
        *pelvisBracket, pelvis_anterior_shift);

    OpenSim::PinJoint* anteriorHip = new OpenSim::PinJoint("anterior_hip",
                                                    anterior_hip_on_Hlink,
                                                    anterior_hip_on_pelvis);
    anteriorHip->append_frames(anterior_hip_on_Hlink);
    anteriorHip->append_frames(anterior_hip_on_pelvis);
    
    // add anterior leg to model
    model.addBody(pelvisBracket); model.addJoint(anteriorHip);
    
    // since the previous, anterior knee joint drives the pose of the lower
    // 4-bar linkage, set the anterior hip angle such that it's free to satisfy
    // constraints that couple it to the 4-bar linkage.
    anteriorHip->upd_CoordinateSet()[0]
                                    .set_is_free_to_satisfy_constraints(true);
    
    // Close the loop for the lower, four-bar linkage with a constraint
    //------------------------------------------------------------------
    
    // Create and configure point on line constraint
    OpenSim::PointOnLineConstraint* posteriorHip =
        new OpenSim::PointOnLineConstraint();
    
    posteriorHip->setLineBodyByName(pelvisBracket->getName());
    posteriorHip->setLineDirection(Vec3(0.0,0.0,1.0));
    posteriorHip->setPointOnLine(inferior_pelvis_pin_location);
    posteriorHip->setFollowerBodyByName(posteriorLegBar->getName());
    posteriorHip->setPointOnFollower(superior_bar_hinge_location);
    
    // add constraint to model
    model.addConstraint(posteriorHip);
    
    // Create chest piece
    //-----------------------
    OpenSim::Body* chest = new OpenSim::Body("chest_bar", bar_mass,
                                     Vec3(0.0),
                                     Inertia::brick(torso_bar_dimensions/2.0));
    
    chest->attachGeometry(new Mesh("Anterior_torso_bar.obj"));

    PhysicalOffsetFrame anterior_torso_hinge_on_pelvis(
        "anterior_torso_hinge_on_pelvis",
        *pelvisBracket, Transform(anterior_superior_pelvis_pin_location) );

    PhysicalOffsetFrame anterior_torso_hinge_on_chest(
        "anterior_torso_hinge_on_chest",
        *chest, Transform(inferior_torso_hinge_location) );
    
    // Attach chest piece to pelvice with pin joint
    OpenSim::PinJoint* anteriorTorsoHinge = new OpenSim::PinJoint(
                                              "anterior_torso_hinge",
                                              anterior_torso_hinge_on_pelvis,
                                              anterior_torso_hinge_on_chest);
    anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_pelvis);
    anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_chest);
    
    // add posterior leg to model
    model.addBody(chest); model.addJoint(anteriorTorsoHinge);
    
    // set torso rotation slightly anterior
    anteriorTorsoHinge->upd_CoordinateSet()[0].setDefaultValue(-1*SimTK::Pi/4);
    
    // Create chest piece
    //-----------------------
    OpenSim::Body* back = new OpenSim::Body("back_bar", bar_mass,
                                     Vec3(0.0),
                                     Inertia::brick(torso_bar_dimensions/2.0));
    
    back->attachGeometry(new Mesh("Posterior_torso_bar.obj"));

    PhysicalOffsetFrame posterior_torso_hinge_on_pelvis(
        "posterior_torso_hinge_on_pelvis",
        *pelvisBracket, Transform(posterior_superior_pelvis_pin_location) );

    PhysicalOffsetFrame posterior_torso_hinge_on_back(
        "posterior_torso_hinge_on_back",
        *back, Transform(back_peg_center) );
    
    // Attach chest piece to pelvis with pin joint
    OpenSim::PinJoint* posteriorTorsoHinge = new OpenSim::PinJoint(
                                          "posterior_torso_hinge",
                                          posterior_torso_hinge_on_pelvis,
                                          posterior_torso_hinge_on_back);
    posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_pelvis);
    posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_back);
    
    // add posterior leg to model
    model.addBody(back); model.addJoint(posteriorTorsoHinge);
    
    // set posterior back joint to freely follow anterior joint through 4-bar
    // linkage coupling.
    posteriorTorsoHinge->upd_CoordinateSet()[0]
    .set_is_free_to_satisfy_constraints(true);
    
    // Create shoulder bracket
    //-----------------------
    OpenSim::Body* shoulderBracket = new OpenSim::Body("shoulder_bracket",
                                     bracket_mass,
                                     Vec3(0.0),
                                     Inertia::brick(shoulder_dimensions/2.0));

    shoulderBracket->attachGeometry(new Mesh("Shoulder_meters.obj"));
    // add anterior leg to model
    model.addBody(shoulderBracket);

    PhysicalOffsetFrame anterior_thoracic_joint_on_chest(
        "anterior_thoracic_joint_on_chest",
        *chest, Transform(superior_torso_hinge_location) );

    PhysicalOffsetFrame anterior_thoracic_joint_on_shoulder(
        "anterior_thoracic_joint_on_shoulder",
        *shoulderBracket, Transform(anterior_thoracic_joint_center));
    
    // Connect pelvis to Hlink via pin joint
    OpenSim::PinJoint* anteriorThoracicJoint =
                        new OpenSim::PinJoint("anterior_thoracic_joint",
                                       anterior_thoracic_joint_on_chest,
                                       anterior_thoracic_joint_on_shoulder);
    anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_chest);
    anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_shoulder);
    // add back joint
    model.addJoint(anteriorThoracicJoint);
    
    // since the previous, anterior thoracic joint drives the pose of the lower
    // 4-bar linkage, set the anterior shoulder angle such that it's free to
    // satisfy constraints that couple it to the 4-bar linkage.
    anteriorThoracicJoint->upd_CoordinateSet()[0]
        .set_is_free_to_satisfy_constraints(true);

    // Close the loop for the lower, four-bar linkage with a constraint
    //------------------------------------------------------------------
    // Create and configure point on line constraint
    OpenSim::PointOnLineConstraint* posteriorShoulder =
    new OpenSim::PointOnLineConstraint();
    
    posteriorShoulder->setLineBodyByName(shoulderBracket->getName());
    posteriorShoulder->setLineDirection(Vec3(0.0,0.0,1.0));
    posteriorShoulder->setPointOnLine(posterior_thoracic_joint_center);
    posteriorShoulder->setFollowerBodyByName(back->getName());
    posteriorShoulder->setPointOnFollower(superior_torso_hinge_location);

    // add constraint to model
    model.addConstraint(posteriorShoulder);

    // Create and add luxo head
    OpenSim::Body* head = new OpenSim::Body("head", head_mass, Vec3(0),
            Inertia::cylinderAlongX(0.5*head_dimension[1], head_dimension[1]));

    head->attachGeometry(new Mesh("luxo_head_meters.obj"));
    head->attachGeometry(new Mesh("Bulb_meters.obj"));
    model.addBody(head);

    PhysicalOffsetFrame cervical_joint_on_shoulder("cervical_joint_on_shoulder",
        *shoulderBracket, Transform(superior_shoulder_hinge_location) );

    PhysicalOffsetFrame cervical_joint_on_head("cervical_joint_on_head",
        *head, Transform(cervicle_joint_center));

    // attach to shoulder via pin joint
    OpenSim::PinJoint* cervicalJoint = new OpenSim::PinJoint("cervical_joint",
                                  cervical_joint_on_shoulder,
                                  cervical_joint_on_head);

    cervicalJoint->append_frames(cervical_joint_on_shoulder);
    cervicalJoint->append_frames(cervical_joint_on_head);
    // add a neck joint
     model.addJoint(cervicalJoint);
    
    // lock the kneck coordinate so the head doens't spin without actuators or
    // passive forces
    cervicalJoint->upd_CoordinateSet()[0].set_locked(true);

    // Coordinate Limit forces for restricting leg range of motion.
    //-----------------------------------------------------------------------
    CoordinateLimitForce* kneeLimitForce =
            new CoordinateLimitForce(
                     anterior_knee->get_CoordinateSet()[0].getName(),
                     knee_flexion_max, joint_softstop_stiffness,
                     knee_flexion_min, joint_softstop_stiffness,
                     joint_softstop_damping,
                     transition_region);
    model.addForce(kneeLimitForce);
    
    // Coordinate Limit forces for restricting back range motion.
    //-----------------------------------------------------------------------
    CoordinateLimitForce* backLimitForce =
    new CoordinateLimitForce(
                         anteriorTorsoHinge->get_CoordinateSet()[0].getName(),
                         back_extension_max, joint_softstop_stiffness,
                         back_extension_min, joint_softstop_stiffness,
                         joint_softstop_damping,
                         transition_region);
    model.addForce(backLimitForce);
    
    
    // Contact
    //-----------------------------------------------------------------------
    ContactHalfSpace* floor_surface = new ContactHalfSpace(SimTK::Vec3(0),
                                       SimTK::Vec3(0, 0, -0.5*SimTK::Pi),
                                       model.updGround(), "floor_surface");
    
    OpenSim::ContactMesh* foot_surface = new ContactMesh(
                                         "thin_disc_0.11_by_0.01_meters.obj",
                                          SimTK::Vec3(0),
                                          SimTK::Vec3(0),
                                          *base,
                                          "foot_surface");
    
    // add contact geometry to model
    model.addContactGeometry(floor_surface);
    model.addContactGeometry(foot_surface);
    
    // define contact as an elastic foundation force
    OpenSim::ElasticFoundationForce::ContactParameters* contactParameters =
            new OpenSim::ElasticFoundationForce::ContactParameters(
                                               stiffness,
                                               dissipation,
                                               friction,
                                               friction,
                                               viscosity);
    
    contactParameters->addGeometry("foot_surface");
    contactParameters->addGeometry("floor_surface");
    
    OpenSim::ElasticFoundationForce* contactForce =
                        new OpenSim::ElasticFoundationForce(contactParameters);
    contactForce->setName("contact_force");
    
    model.addForce(contactForce);
    
    
    
    // MUSCLES
    //-----------------------------------------------------------------------
    
    // add a knee extensor to control the lower 4-bar linkage
    Millard2012EquilibriumMuscle* kneeExtensorRight = new Millard2012EquilibriumMuscle(
                                            "knee_extensor_right",
                                            knee_extensor_F0, knee_extensor_lm0,
                                            knee_extensor_lts, pennationAngle);
    kneeExtensorRight->addNewPathPoint("knee_extensor_right_origin", *leg_Hlink,
                                  knee_extensor_origin);
    kneeExtensorRight->addNewPathPoint("knee_extensor_right_insertion",
                                      *bottom_bracket,
                                        knee_extensor_insertion);
    kneeExtensorRight->set_ignore_tendon_compliance(true);
    model.addForce(kneeExtensorRight);
    
    // add a second copy of this knee extensor for the left side
    Millard2012EquilibriumMuscle* kneeExtensorLeft =
                                new Millard2012EquilibriumMuscle(*kneeExtensorRight);
    kneeExtensorLeft->setName("kneeExtensorLeft");
    
    // flip the z coordinates of all path points
    PathPointSet& points = kneeExtensorLeft->updGeometryPath().updPathPointSet();
    for (int i=0; i<points.getSize(); ++i) {
        points[i].setLocationCoord(2, -1*points[i].getLocationCoord(2));
    }

    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));
    }
    
}
/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{
    try {
        // Create a new OpenSim model
        Model osimModel;
        osimModel.setName("osimModel");
        osimModel.setAuthors("Matt DeMers");

        double Pi = SimTK::Pi;
            
        // Get the ground body
        Ground& ground = osimModel.updGround();
        ground.attachGeometry(new Mesh("checkered_floor.vtp"));

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
        
        Vec3 linkageMassCenter(0,linkageLength/2,0);
        Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

        OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
        linkage1->attachGeometry(new Sphere(0.1));

        // Graphical representation
        Cylinder cyl(linkageDiameter/2, linkageLength);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, 
            Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        cyl1Frame->attachGeometry(cyl.clone());
        osimModel.addFrame(cyl1Frame);

        // Create a second linkage body as a clone of the first
        OpenSim::Body* linkage2 = linkage1->clone();
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2,
            Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);

        // Create a block to be the pelvis
        double blockMass = 20.0, blockSideLength = 0.2;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
        block->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));

        // Create 1 degree-of-freedom pin joints between the bodies to create a kinematic chain from ground through the block
        Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

        PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, 
            locationInChild, orientationInChild);

        PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
            locationInChild, orientationInChild);

        PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
            locationInChild, orientationInChild);
        
        double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
        ankle->updCoordinate().setName("q1");
        ankle->updCoordinate().setRange(range);

        knee->updCoordinate().setName("q2");
        knee->updCoordinate().setRange(range);

        hip->updCoordinate().setName("q3");
        hip->updCoordinate().setRange(range);

        // Add the bodies to the model
        osimModel.addBody(linkage1);
        osimModel.addBody(linkage2);
        osimModel.addBody(block);

        // Add the joints to the model
        osimModel.addJoint(ankle);
        osimModel.addJoint(knee);
        osimModel.addJoint(hip);
        // Define constraints on the model
        //  Add a point on line constraint to limit the block to vertical motion

        Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
        PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
        osimModel.addConstraint(lineConstraint);

        // 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 acceleration 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(false);

        // 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
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-3);

        ForceReporter *forces = new ForceReporter(&osimModel);  
        osimModel.updAnalysisSet().adoptAndAppend(forces);
        Manager manager(osimModel, integrator);
    
        //Examine the model
        osimModel.printDetailedInfo(si, std::cout);
        // Save the model
        osimModel.print("toyLeg.osim");
        // Print out the initial position and velocity states
        si.getQ().dump("Initial q's");
        si.getU().dump("Initial u's");
        std::cout << "Initial time: " << si.getTime() << std::endl;

        // Integrate
        manager.setInitialTime(t0);
        manager.setFinalTime(tf);
        std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
        manager.integrate(si);

        // Save results
        auto controlsTable = osimModel.getControlsTable();
        STOFileAdapter_<double>::write(controlsTable, 
                                      "SpringActuatedLeg_controls.sto");

        auto statesTable = manager.getStatesTable();
        osimModel.updSimbodyEngine().convertRadiansToDegrees(statesTable);
        STOFileAdapter_<double>::write(statesTable, 
                                      "SpringActuatedLeg_states_degrees.sto");

        auto forcesTable = forces->getForcesTable();
        STOFileAdapter_<double>::write(forcesTable, "actuator_forces.sto");
    }
    catch (const std::exception& ex)
    {
        std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
        return 1;
    }

    std::cout << "Done." << std::endl;
    return 0;
}
/**
 * Create a model that does nothing. 
 */
int main()
{
    try {

        ///////////////////////////////////////////
        // DEFINE BODIES AND JOINTS OF THE MODEL //
        ///////////////////////////////////////////

        // Create an OpenSim model and set its name
        Model osimModel;
        osimModel.setName("tugOfWar");

        // GROUND FRAME

        // Get a reference to the model's ground frame
        Ground& ground = osimModel.updGround();

        // Attach geometry to the ground to visualize in the GUI
        ground.attachGeometry(new Mesh("ground.vtp"));
        ground.attachGeometry(new Mesh("anchor1.vtp"));
        ground.attachGeometry(new Mesh("anchor2.vtp"));

        // BLOCK BODY

        // Specify properties of a 20 kg, 0.1 m^3 block body
        double blockMass = 20.0, blockSideLength = 0.1;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

        // Create a new block body with the specified properties
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);

        // Add display geometry to the block to visualize in the GUI
        block->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));

        // FREE JOINT

        // Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground frames
        Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
        FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
        
        // Set the angle and position ranges for the free (6-degree-of-freedom)
        // joint between the block and ground frames.
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation1X).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation2Y).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation3Z).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationX).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationY).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationZ).setRange(positionRange);

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

        ///////////////////////////////////////////////
        // DEFINE THE SIMULATION START AND END TIMES //
        ///////////////////////////////////////////////

        // Define the initial and final simulation times
        double initialTime = 0.0;
        double finalTime = 3.00;

        /////////////////////////////////////////////
        // DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
        /////////////////////////////////////////////

        // Specify properties of a constant distance constraint to limit the block's motion
        double distance = 0.2;
        Vec3 pointOnGround(0, blockSideLength/2 ,0);
        Vec3 pointOnBlock(0, 0, 0);

        // Create a new constant distance constraint
        ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground, 
                                        pointOnGround, *block, pointOnBlock, distance);

        // Add the new point on a line constraint to the model
        osimModel.addConstraint(constDist);

        ///////////////////////////////////////
        // DEFINE FORCES ACTING ON THE MODEL //
        ///////////////////////////////////////

        // GRAVITY
        // Obtain the default acceleration due to gravity
        Vec3 gravity = osimModel.getGravity();
    

        // MUSCLE FORCES
        // Create two new muscles with identical properties
        double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0; 
        Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
        Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);

        // Specify the paths for the two muscles
        // Path for muscle 1
        muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35));
        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("prescribedForce", *block);

        // 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 individual 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 //
        /////////////////////////
        
        //osimModel.setUseVisualizer(true);

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

        // Define non-zero (defaults are 0) states for the free joint
        CoordinateSet& modelCoordinateSet = osimModel.updCoordinateSet();
        modelCoordinateSet[3].setValue(si, distance); // set x-translation value
        modelCoordinateSet[5].setValue(si, 0.0); // set z-translation value
        modelCoordinateSet[3].setSpeedValue(si, 0.0); // set x-speed value
        double h_start = 0.5;
        modelCoordinateSet[4].setValue(si, h_start); // set y-translation which is height
        
        std::cout << "Start height = "<< h_start << std::endl;

        osimModel.getMultibodySystem().realize(si, Stage::Velocity);

        // Compute initial conditions for muscles
        osimModel.equilibrateMuscles(si);

        // double mfv1 = muscle1->getFiberVelocity(si);
        // double mfv2 = muscle2->getFiberVelocity(si);

        // Create the force reporter for obtaining the forces applied to the model
        // during a forward simulation
        ForceReporter* reporter = new ForceReporter(&osimModel);
        osimModel.addAnalysis(reporter);

        // Create the manager managing the forward integration and its outputs
        Manager manager(osimModel);
        manager.setIntegratorAccuracy(1.0e-6);

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

    }
    catch (const std::exception& ex)
    {
        std::cerr << ex.what() << std::endl;
        return 1;
    }
    catch (...)
    {
        std::cerr << "UNRECOGNIZED EXCEPTION" << std::endl;
        return 1;
    }

    std::cout << "OpenSim environment test completed successfully. You should see a block attached to two muscles visualized in a separate window." << std::endl;

    return 0;
}