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

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

//==========================================================================================================
// Test Cases
//==========================================================================================================
void testPrescribedForce(OpenSim::Function* forceX, OpenSim::Function* forceY, OpenSim::Function* forceZ,
                 OpenSim::Function* pointX, OpenSim::Function* pointY, OpenSim::Function* pointZ,
                 OpenSim::Function* torqueX, OpenSim::Function* torqueY, OpenSim::Function* torqueZ,
                 vector<SimTK::Real>& times, vector<SimTK::Vec3>& accelerations, vector<SimTK::Vec3>& angularAccelerations)
{
	using namespace SimTK;

	//==========================================================================================================
	// Setup OpenSim model
	Model *osimModel = new Model;
	//OpenSim bodies
    OpenSim::Body& ground = osimModel->getGroundBody();
	OpenSim::Body ball;
	ball.setName("ball");

	// Add joints
	FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0), false);

	// Rename coordinates for a free joint
	CoordinateSet free_coords = free.getCoordinateSet();
	for(int i=0; i<free_coords.getSize(); i++){
		std::stringstream coord_name;
		coord_name << "free_q" << i;
		free_coords.get(i).setName(coord_name.str());
		free_coords.get(i).setMotionType(i > 2 ? Coordinate::Translational : Coordinate::Rotational);
	}

	osimModel->addBody(&ball);
	osimModel->addJoint(&free);

	// Add a PrescribedForce.
	PrescribedForce force(&ball);
    if (forceX != NULL)
        force.setForceFunctions(forceX, forceY, forceZ);
    if (pointX != NULL)
        force.setPointFunctions(pointX, pointY, pointZ);
    if (torqueX != NULL)
        force.setTorqueFunctions(torqueX, torqueY, torqueZ);

	counter++;
	osimModel->updForceSet().append(&force);

	// BAD: have to set memoryOwner to false or program will crash when this test is complete.
	osimModel->disownAllComponents();

    //Set mass
	ball.setMass(ballMass.getMass());
	ball.setMassCenter(ballMass.getMassCenter());
	ball.setInertia(ballMass.getInertia());

	osimModel->setGravity(gravity_vec);
	osimModel->print("TestPrescribedForceModel.osim");

	delete osimModel;
	// Check that serialization/deserialization is working correctly as well
	osimModel = new Model("TestPrescribedForceModel.osim");
    SimTK::State& osim_state = osimModel->initSystem();
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position );

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

	const OpenSim::Body& body = osimModel->getBodySet().get("ball");

    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
    Manager manager(*osimModel,  integrator);
    manager.setInitialTime(0.0);
    for (unsigned int i = 0; i < times.size(); ++i)
    {
        manager.setFinalTime(times[i]);
        manager.integrate(osim_state);
        osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 accel, angularAccel;
        osimModel->updSimbodyEngine().getAcceleration(osim_state, body, Vec3(0), accel);
        osimModel->updSimbodyEngine().getAngularAcceleration(osim_state, body, angularAccel);
        ASSERT_EQUAL(accelerations[i][0], accel[0], 1e-10);
        ASSERT_EQUAL(accelerations[i][1], accel[1], 1e-10);
        ASSERT_EQUAL(accelerations[i][2], accel[2], 1e-10);
        ASSERT_EQUAL(angularAccelerations[i][0], angularAccel[0], 1e-10);
        ASSERT_EQUAL(angularAccelerations[i][1], angularAccel[1], 1e-10);
        ASSERT_EQUAL(angularAccelerations[i][2], angularAccel[2], 1e-10);
    }
}

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(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->setRadius(0.1);
    pulley->setLength(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
    CoordinateSet &slider_coords = slider->upd_CoordinateSet();
    slider_coords[0].setName("block_h");
    slider_coords[0].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
//.........这里部分代码省略.........

const SimTK::MobilizedBodyIndex Joint::
    getMobilizedBodyIndex(const OpenSim::Body& body) const
{
        return body.getMobilizedBodyIndex();
} 

/**
 * 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 BODY

        // 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.addMeshGeometry("ground.vtp");
        ground.addMeshGeometry("anchor1.vtp");
        ground.addMeshGeometry("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
        Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
        block->addGeometry(brick);

        // FREE JOINT

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

        // Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
        CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();

        // Set the angle and position ranges for the coordinate set
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        jointCoordinateSet[0].setRange(angleRange);
        jointCoordinateSet[1].setRange(angleRange);
        jointCoordinateSet[2].setRange(angleRange);
        jointCoordinateSet[3].setRange(positionRange);
        jointCoordinateSet[4].setRange(positionRange);
        jointCoordinateSet[5].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
        // Obtaine 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);
//.........这里部分代码省略.........

/**
 * Add a body to the SIMM model.
 *
 * @param aBody The Simbody body that the SimbodySimmBody is based on.
 */
void SimbodySimmModel::addBody(const OpenSim::Body& aBody)
{
   SimbodySimmBody* b = new SimbodySimmBody(&aBody, aBody.getName());
   _simmBody.append(b);
}

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

//-----------------------------------------------------------------------------
// METHODS TO APPLY FORCES AND TORQUES
//-----------------------------------------------------------------------------
void Force::applyForceToPoint(const SimTK::State &s, const OpenSim::Body &aBody, const Vec3& aPoint,
                              const Vec3& aForce, Vector_<SpatialVec> &bodyForces) const
{
    _model->getMatterSubsystem().addInStationForce(s, SimTK::MobilizedBodyIndex(aBody.getIndex()),
            aPoint, aForce, bodyForces);
}

void Force::applyTorque(const SimTK::State &s, const OpenSim::Body &aBody,
                        const Vec3& aTorque, Vector_<SpatialVec> &bodyForces) const
{
    _model->getMatterSubsystem().addInBodyTorque(s, SimTK::MobilizedBodyIndex(aBody.getIndex()),
            aTorque, bodyForces);
}

/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{
    clock_t startTime = clock();

	try {
		//////////////////////
		// MODEL PARAMETERS //
		//////////////////////

		// Specify body mass of a 20 kg, 0.1m sides of cubed block body
		double blockMass = 20.0, blockSideLength = 0.1;

		// Constant distance of constraint to limit the block's motion
		double constantDistance = 0.2;

		// Contact parameters
		double stiffness = 1.0e7, dissipation = 0.1, friction = 0.2, viscosity=0.01;

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

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

		// GROUND BODY
		// Get a reference to the model's ground body
		OpenSim::Body& ground = osimModel.getGroundBody();

		// Add display geometry to the ground to visualize in the Visualizer and GUI
		// add a checkered floor
		ground.addDisplayGeometry("checkered_floor.vtp");
		// add anchors for the muscles to be fixed too
		ground.addDisplayGeometry("block.vtp");
		ground.addDisplayGeometry("block.vtp");

		// block is 0.1 by 0.1 by 0.1m cube and centered at origin. 
		// transform anchors to be placed at the two extremes of the sliding block (to come)
		GeometrySet& geometry = ground.updDisplayer()->updGeometrySet();
		DisplayGeometry& anchor1 = geometry[1];
		DisplayGeometry& anchor2 = geometry[2];
		// scale the anchors
		anchor1.setScaleFactors(Vec3(5, 1, 1));
		anchor2.setScaleFactors(Vec3(5, 1, 1));
		// reposition the anchors
		anchor1.setTransform(Transform(Vec3(0, 0.05, 0.35)));
		anchor2.setTransform(Transform(Vec3(0, 0.05, -0.35)));

		// BLOCK BODY
		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->addDisplayGeometry("block.vtp");

		// FREE JOINT

		// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
		Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
		FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
		
		// Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
		CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();

		// Set the angle and position ranges for the coordinate set
		double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
		double positionRange[2] = {-1, 1};
		jointCoordinateSet[0].setRange(angleRange);
		jointCoordinateSet[1].setRange(angleRange);
		jointCoordinateSet[2].setRange(angleRange);
		jointCoordinateSet[3].setRange(positionRange);
		jointCoordinateSet[4].setRange(positionRange);
		jointCoordinateSet[5].setRange(positionRange);

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

		// Define non-zero default states for the free joint
		jointCoordinateSet[3].setDefaultValue(constantDistance); // set x-translation value
		double h_start = blockMass*gravity[1]/(stiffness*blockSideLength*blockSideLength);
		jointCoordinateSet[4].setDefaultValue(h_start); // set y-translation which is height

		// Add the block and joint 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;
//.........这里部分代码省略.........

//==========================================================================================================
// Test Cases
//==========================================================================================================
int testBouncingBall(bool useMesh)
{
    // Setup OpenSim model
    Model *osimModel = new Model;

    //OpenSim bodies
    OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity,
        Vec3(0), Inertia());
    osimModel->addBody(&ground);

    OpenSim::Body ball;
    ball.setName("ball");
    ball.set_mass(mass);
    ball.set_mass_center(Vec3(0));
    ball.setInertia(Inertia(1.0));

    // Add joints
    FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0));
    osimModel->addBody(&ball);
    osimModel->addJoint(&free);

    // Create ContactGeometry.
    ContactHalfSpace *floor = new ContactHalfSpace(Vec3(0), Vec3(0, 0, -0.5*SimTK_PI), ground, "ground");
    osimModel->addContactGeometry(floor);
    OpenSim::ContactGeometry* geometry;
    if (useMesh)
        geometry = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball");
    else
        geometry = new ContactSphere(radius, Vec3(0), ball, "ball");
    osimModel->addContactGeometry(geometry);

    OpenSim::Force* force;
    if (useMesh)
    {
        // Add an ElasticFoundationForce.
        OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/radius, 1e-5, 0.0, 0.0, 0.0);
        contactParams->addGeometry("ball");
        contactParams->addGeometry("ground");
        force = new OpenSim::ElasticFoundationForce(contactParams);
        osimModel->addForce(force);
    }
    else
    {
        // Add a HuntCrossleyForce.
        OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 1e-5, 0.0, 0.0, 0.0);
        contactParams->addGeometry("ball");
        contactParams->addGeometry("ground");
        force = new OpenSim::HuntCrossleyForce(contactParams);
        osimModel->addForce(force);
    }

    osimModel->setGravity(gravity_vec);

    osimModel->setName("TestContactGeomtery_Ball");
    osimModel->clone()->print("TestContactGeomtery_Ball.osim");

    Kinematics* kin = new Kinematics(osimModel);
    osimModel->addAnalysis(kin);

    SimTK::State& osim_state = osimModel->initSystem();
    osim_state.updQ()[4] = height;
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position );

    //Initial system energy is all potential
    double Etot_orig = mass*(-gravity_vec[1])*height;

    //==========================================================================================================
    // Simulate it and see if it bounces correctly.
    cout << "stateY=" << osim_state.getY() << std::endl;

    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
    integrator.setAccuracy(integ_accuracy);
    Manager manager(*osimModel, integrator);

    for (unsigned int i = 0; i < duration/interval; ++i)
    {
        manager.setInitialTime(i*interval);
        manager.setFinalTime((i+1)*interval);
        manager.integrate(osim_state);
        double time = osim_state.getTime();

        osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos, vel;

        osimModel->updSimbodyEngine().getPosition(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), pos);
        osimModel->updSimbodyEngine().getVelocity(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), vel);

        double Etot = mass*((-gravity_vec[1])*pos[1] + 0.5*vel[1]*vel[1]);

        //cout << "starting system energy = " << Etot_orig << " versus current energy = " << Etot << endl;
        // contact absorbs and returns energy so make sure not in contact
        if (pos[1] > 2*radius)
        {
            ASSERT_EQUAL(Etot_orig, Etot, 1e-2, __FILE__, __LINE__, "Bouncing ball on plane Failed: energy was not conserved.");
        }
        else
        {
//.........这里部分代码省略.........

// test sphere to sphere contact using elastic foundation with and without 
// meshes and their combination
int testBallToBallContact(bool useElasticFoundation, bool useMesh1, bool useMesh2)
{
    // Setup OpenSim model
    Model *osimModel = new Model;

    //OpenSim bodies
    OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity,
        Vec3(0), Inertia());
    osimModel->addBody(&ground);

    OpenSim::Body ball;
    ball.setName("ball");
    ball.setMass(mass);
    ball.setMassCenter(Vec3(0));
    ball.setInertia(Inertia(1.0));

    // Add joints
    FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0));

    osimModel->addBody(&ball);
    osimModel->addJoint(&free);

    // Create ContactGeometry.
    OpenSim::ContactGeometry *ball1, *ball2;

    if (useElasticFoundation && useMesh1)
        ball1 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ground, "ball1");
    else
        ball1 = new ContactSphere(radius, Vec3(0), ground, "ball1");

    if (useElasticFoundation && useMesh2)
        ball2 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball2");
    else
        ball2 = new ContactSphere(radius, Vec3(0), ball, "ball2");
    
    osimModel->addContactGeometry(ball1);
    osimModel->addContactGeometry(ball2);

    OpenSim::Force* force;

    std::string prefix;
    if (useElasticFoundation){
        
    }
    else{
        
    }
    if (useElasticFoundation)
    {
        // Add an ElasticFoundationForce.
        OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/(2*radius), 0.001, 0.0, 0.0, 0.0);
        contactParams->addGeometry("ball1");
        contactParams->addGeometry("ball2");
        force = new OpenSim::ElasticFoundationForce(contactParams);
        prefix = "EF_";
        prefix += useMesh1 ?"Mesh":"noMesh";
        prefix += useMesh2 ? "_to_Mesh":"_to_noMesh";
        
    }
    else
    {
        // Add a Hertz HuntCrossleyForce.
        OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 0.001, 0.0, 0.0, 0.0);
        contactParams->addGeometry("ball1");
        contactParams->addGeometry("ball2");
        force = new OpenSim::HuntCrossleyForce(contactParams);
        prefix = "Hertz";
        
    }

    force->setName("contact");
    osimModel->addForce(force);
    osimModel->setGravity(gravity_vec);

    osimModel->setName(prefix);
    osimModel->clone()->print(prefix+".osim");

    Kinematics* kin = new Kinematics(osimModel);
    osimModel->addAnalysis(kin);

    ForceReporter* reporter = new ForceReporter(osimModel);
    osimModel->addAnalysis(reporter);

    SimTK::State& osim_state = osimModel->initSystem();
    osim_state.updQ()[4] = height;
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================================================
    // Simulate it and see if it bounces correctly.
    cout << "stateY=" << osim_state.getY() << std::endl;

    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
    integrator.setAccuracy(integ_accuracy);
    integrator.setMaximumStepSize(100*integ_accuracy);
    Manager manager(*osimModel, integrator);
    manager.setInitialTime(0.0);
    manager.setFinalTime(duration);
    manager.integrate(osim_state);
//.........这里部分代码省略.........

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

    auto& coords = ballToGround->upd_CoordinateSet();
    coords[0].setName("ball_d");
    coords[0].setPrescribedFunction(LinearFunction(20 * 10e-4, 0.5 * 264.1 * 10e-4));
    coords[0].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;
}

/*==============================================================================  
    Main test driver to be used on any muscle model (derived from Muscle) so new 
    cases should be easy to add currently, the test only verifies that the work 
    done by the muscle corresponds to the change in system energy.

    TODO: Test will fail wih prescribe motion until the work done by this 
    constraint is accounted for.
================================================================================
*/
void simulateMuscle(
        const Muscle &aMuscModel, 
        double startX, 
        double act0, 
        const Function *motion,  // prescribe motion of free end of muscle
        const Function *control, // prescribed excitation signal to the muscle
        double integrationAccuracy,
        int testType,
        double testTolerance,
        bool printResults)
{
    string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";

    cout << "\n******************************************************" << endl;
    cout << "Test " << aMuscModel.getConcreteClassName() 
         << " Model" << prescribed << endl;
    cout << "******************************************************" << endl;
    using SimTK::Vec3;

//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================

    // Define the initial and final simulation times
    double initialTime = 0.0;
    double finalTime = 4.0;
    
    //Physical properties of the model
    double ballMass = 10;
    double ballRadius = 0.05;
    double anchorWidth = 0.1;

    // Create an OpenSim model
    Model model;

    double optimalFiberLength = aMuscModel.getOptimalFiberLength();
    double pennationAngle     = aMuscModel.getPennationAngleAtOptimalFiberLength();
    double tendonSlackLength  = aMuscModel.getTen