/**
* 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);
    }

    // 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;
}
/**
* 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();

    // 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;
}
//==========================================================================================================
// 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
    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;
}
示例#5
0
const SimTK::MobilizedBodyIndex Joint::
    getMobilizedBodyIndex(const OpenSim::Body& body) const
{
        return body.getMobilizedBodyIndex();
} 
示例#6
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 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);

        // 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(block);
        prescribedForce->setName("prescribedForce");

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

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

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

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

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

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

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

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

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

        //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 integrator for integrating system dynamics
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-6);

        // Create the manager managing the forward integration and its outputs
        Manager manager(osimModel,  integrator);

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

    }
    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;
}
/**
 * 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 
    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));
    }
    
}
示例#9
0
//-----------------------------------------------------------------------------
// 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);
}
示例#10
0
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);
}
示例#11
0
/**
 * 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;
		double finalTime = 3.00;

		/////////////////////////////////////////////
		// DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
		/////////////////////////////////////////////
		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, constantDistance);

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

		///////////////////////////////////////
		// DEFINE FORCES ACTING ON THE MODEL //
		///////////////////////////////////////
	
		// 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);

		// CONTACT FORCE
		// Define contact geometry
		// Create new floor contact halfspace
		ContactHalfSpace *floor = new ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK_PI), ground, "floor");
		// Create new cube contact mesh
		OpenSim::ContactMesh *cube = new OpenSim::ContactMesh("blockMesh.obj", SimTK::Vec3(0), SimTK::Vec3(0), *block, "cube");

		// Add contact geometry to the model
		osimModel.addContactGeometry(floor);
		osimModel.addContactGeometry(cube);

		// Define contact parameters for elastic foundation force
		OpenSim::ElasticFoundationForce::ContactParameters *contactParams = 
			new OpenSim::ElasticFoundationForce::ContactParameters(stiffness, dissipation, friction, friction, viscosity);
		contactParams->addGeometry("cube");
		contactParams->addGeometry("floor");
		
		// Create a new elastic foundation (contact) force between the floor and cube.
		OpenSim::ElasticFoundationForce *contactForce = new OpenSim::ElasticFoundationForce(contactParams);
		contactForce->setName("contactForce");

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

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

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

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

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

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

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

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

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

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

		// set use visualizer to true to visualize the simulation live
		osimModel.setUseVisualizer(false);

		// Initialize the system and get the default state
		SimTK::State& si = osimModel.initSystem();
		
		// Enable constraint consistent with current configuration of the model
		constDist->setDisabled(si, false);

		cout << "Start height = "<< h_start << 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 integrator for integrating system dynamics
		SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
		integrator.setAccuracy(1.0e-6);
		
		// Create the manager managing the forward integration and its outputs
		Manager manager(osimModel,  integrator);

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

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

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

		// Save the forces
		reporter->getForceStorage().print("tugOfWar_forces.mot");
	}
	catch (const std::exception& ex)
    {
        cerr << ex.what() << endl;
        return 1;
    }
    catch (...)
    {
        cerr << "UNRECOGNIZED EXCEPTION" << endl;
        return 1;
    }

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

    cout << "OpenSim example completed successfully." << endl;

	return 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
        {
            cout << "In contact at time = " << time << endl; 
            ASSERT(pos[1] < 5.0 && pos[1] > 0);
        }
        ASSERT_EQUAL(0.0, pos[0], 1e-4);
        ASSERT_EQUAL(0.0, pos[2], 1e-4);
        ASSERT_EQUAL(0.0, vel[0], 1e-3);
        ASSERT_EQUAL(0.0, vel[2], 1e-3);
    }

    std::string prefix = useMesh?"Kinematics_Mesh":"Kinematics_NoMesh";
    kin->printResults(prefix);

    osimModel->disownAllComponents();
    // model takes ownership of components unless container set is told otherwise
    delete osimModel;

    return 0;
}
// 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);

    kin->printResults(prefix);
    reporter->printResults(prefix);

    osimModel->disownAllComponents();
    // model takes ownership of components unless container set is told otherwise
    delete osimModel;

    return 0;
}
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;
}
示例#15
0
/*==============================================================================  
    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.getTendonSlackLength();

    // Use a copy of the muscle model passed in to add path points later
    PathActuator *aMuscle = aMuscModel.clone();

    // Get a reference to the model's ground body
    Body& ground = model.getGroundBody();
    ground.addDisplayGeometry("box.vtp");
    ground.updDisplayer()
        ->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth));

    OpenSim::Body * ball = new OpenSim::Body("ball", 
                        ballMass , 
                        Vec3(0),  
                        ballMass*SimTK::Inertia::sphere(ballRadius));
    
    ball->addDisplayGeometry("sphere.vtp");
    ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius));
    // ball connected  to ground via a slider along X
    double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength;

    SliderJoint* slider = new SliderJoint( "slider", 
                        ground, 
                        Vec3(anchorWidth/2+xSinG, 0, 0), 
                        Vec3(0), 
                        *ball, 
                        Vec3(0), 
                        Vec3(0));

    CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
        jointCoordinateSet[0].setName("tx");
        jointCoordinateSet[0].setDefaultValue(1.0);
        jointCoordinateSet[0].setRangeMin(0); 
        jointCoordinateSet[0].setRangeMax(1.0);
    
    if(motion != NULL){
        jointCoordinateSet[0].setPrescribedFunction(*motion);
        jointCoordinateSet[0].setDefaultIsPrescribed(true);
    }
    // add ball to model
    model.addBody(ball);
	model.addJoint(slider);


//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================

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

    model.addForce(aMuscle);

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

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

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

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

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

    // Add ActuatorPowerProbe to measure work done by the muscle 
    ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1);
    //muscWorkProbe->setName("ActuatorWork");
    muscWorkProbe->setOperation("integrate");
    SimTK::Vector ic1(1);
    ic1 = 9.0;      // some arbitary initial condition.
    muscWorkProbe->setInitialConditions(ic1);
    model.addProbe(muscWorkProbe);
	model.setup();
    cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl; 
    if (muscWorkProbe->getName() != "UnnamedProbe") {
        string errorMessage = "Incorrect default name for unnamed probe: " + muscWorkProbe->getName(); 
        throw (OpenSim::Exception(errorMessage.c_str()));
    }

    // Add ActuatorPowerProbe to measure power generated by the muscle 
    ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe);	// use copy constructor
    muscPowerProbe->setName("ActuatorPower");
    muscPowerProbe->setOperation("value");
    model.addProbe(muscPowerProbe);
    cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl; 

    // Add ActuatorPowerProbe to report the muscle power MINIMUM
    ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe);			// use copy constructor
    powerProbeMinimum->setName("ActuatorPowerMinimum");
    powerProbeMinimum->setOperation("minimum");
    model.addProbe(powerProbeMinimum);
    cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl;

    // Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM
    ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe);			// use copy constructor
    powerProbeMinAbs->setName("ActuatorPowerMinAbs");
    powerProbeMinAbs->setOperation("minabs");
    model.addProbe(powerProbeMinAbs);
    cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl;

    // Add ActuatorPowerProbe to report the muscle power MAXIMUM
    ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe);			// use copy constructor
    powerProbeMaximum->setName("ActuatorPowerMaximum");
    powerProbeMaximum->setOperation("maximum");
    model.addProbe(powerProbeMaximum);
    cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl;

    // Add ActuatorPowerProbe to report the muscle power MAXABS
    ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe);			// use copy constructor
    powerProbeMaxAbs->setName("ActuatorPowerMaxAbs");
    powerProbeMaxAbs->setOperation("maxabs");
    model.addProbe(powerProbeMaxAbs);
    cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl;


    // Add ActuatorPowerProbe to measure the square of the power generated by the muscle 
    ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe);	// use copy constructor
    muscPowerSquaredProbe->setName("ActuatorPowerSquared");
    muscPowerSquaredProbe->setExponent(2.0);
    model.addProbe(muscPowerSquaredProbe);
    cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl; 

    // Add JointInternalPowerProbe to measure work done by the joint 
    JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1);
    jointWorkProbe->setName("JointWork");
    jointWorkProbe->setOperation("integrate");
    jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0));
    model.addProbe(jointWorkProbe);
    cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl;

    // Add JointPowerProbe to measure power generated by the joint 
    JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe);	// use copy constructor
    jointPowerProbe->setName("JointPower");
    jointPowerProbe->setOperation("value");
    model.addProbe(jointPowerProbe);
    cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl;

    // Add ActuatorForceProbe to measure the impulse of the muscle force 
    ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1);
    impulseProbe->setName("ActuatorImpulse");
    impulseProbe->setOperation("integrate");
    impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0));
    model.addProbe(impulseProbe);
    cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl;

    // Add ActuatorForceProbe to report the muscle force 
    ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe);			// use copy constructor
    forceProbe->setName("ActuatorForce");
    forceProbe->setOperation("value");
    model.addProbe(forceProbe);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl;

    // Add ActuatorForceProbe to report the square of the muscle force 
    ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe);			// use copy constructor
    forceSquaredProbe->setName("ActuatorForceSquared");
    forceSquaredProbe->setExponent(2.0);
    model.addProbe(forceSquaredProbe);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl;

    // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice
    ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe);			// use copy constructor
    forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice");
    forceSquaredProbeTwice->setSumForcesTogether(true);
    forceSquaredProbeTwice->setActuatorNames(muscNamesTwice);
    model.addProbe(forceSquaredProbeTwice);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl;

    // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5
    ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice);			// use copy constructor
    forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved");
    double gain1 = 0.5;
    forceSquaredProbeTwiceScaled->setGain(gain1);
    model.addProbe(forceSquaredProbeTwiceScaled);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl;

    // Add ActuatorForceProbe to report -3.5X the muscle force 
    double gain2 = -3.50;
    ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe);		// use copy constructor
    forceProbeScale->setName("ScaleActuatorForce");
    forceProbeScale->setOperation("value");
    forceProbeScale->setGain(gain2);
    model.addProbe(forceProbeScale);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl;

    // Add ActuatorForceProbe to report the differentiated muscle force 
    ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe);		// use copy constructor
    forceProbeDiff->setName("DifferentiateActuatorForce");
    forceProbeDiff->setOperation("differentiate");
    model.addProbe(forceProbeDiff);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl;

    // Add SystemEnergyProbe to measure the system KE+PE
    SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true);
    sysEnergyProbe->setName("SystemEnergy");
    sysEnergyProbe->setOperation("value");
    sysEnergyProbe->setComputeKineticEnergy(true);
    sysEnergyProbe->setComputePotentialEnergy(true);
    model.addProbe(sysEnergyProbe);
    cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl;

    // Add SystemEnergyProbe to measure system power (d/dt system KE+PE)
    SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe);	// use copy constructor
    sysPowerProbe->setName("SystemPower");
    sysPowerProbe->setDisabled(false);
    sysPowerProbe->setOperation("differentiate");
    model.addProbe(sysPowerProbe);
    cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl;

    // Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
    ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe);			// use copy constructor
    forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR");
    forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false);    // report individually
    forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice);
    //cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl;
    forceSquaredProbeTwiceReportedIndividually1->setOperation("value");
    model.addProbe(forceSquaredProbeTwiceReportedIndividually1);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl;

    // Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
    ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1);			// use copy constructor
    forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR");
    forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false);    // report individually
    forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate");
    model.addProbe(forceSquaredProbeTwiceReportedIndividually2);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;

    // Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
    ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1);			// use copy constructor
    forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR");
    forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false);    // report individually
    forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate");
    SimTK::Vector initCondVec(2);
    initCondVec(0) = 0;
    initCondVec(1) = 10;
    forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec);
    model.addProbe(forceSquaredProbeTwiceReportedIndividually3);
    cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
    cout << "initCondVec = " << initCondVec << endl;

    /* Since all components are allocated on the stack don't have model 
       own them (and try to free)*/
//	model.disownAllComponents();
    model.setName("testProbesModel");
    cout << "Saving model... " << endl;
    model.print("testProbesModel.osim");
    cout << "Re-loading model... " << endl;
    Model reloadedModel = Model("testProbesModel.osim");

    /* Setup analyses and reporters. */
    ProbeReporter* probeReporter = new ProbeReporter(&model);
    model.addAnalysis(probeReporter);
    ForceReporter* forceReporter = new ForceReporter(&model);
    model.addAnalysis(forceReporter);
    MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model);
    model.addAnalysis(muscleReporter);
    model.print("testProbesModel.osim");
    model.printBasicInfo(cout);
   


//==========================================================================
// 3. SIMULATION Initialization
//==========================================================================

    // Initialize the system and get the default state    
    SimTK::State& si = model.initSystem();
    SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si);

    model.getMultibodySystem().realize(si,SimTK::Stage::Dynamics);
    model.equilibrateMuscles(si);

    CoordinateSet& modelCoordinateSet = model.updCoordinateSet();

    // Define non-zero (defaults are 0) states for the free joint
    // set x-translation value
    modelCoordinateSet[0].setValue(si, startX, true); 

    //Copy the initial state
    SimTK::State initialState(si);

    // Check muscle is setup correctly 
    const PathActuator &muscle 
        = dynamic_cast<const PathActuator&>(model.updActuators().get("muscle"));
    double length = muscle.getLength(si);
    double trueLength = startX + xSinG - anchorWidth/2;
    
    ASSERT_EQUAL(length/trueLength, 1.0, testTolerance, __FILE__, __LINE__, 
        "testMuscles: path failed to initialize to correct length." );

    model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);

    double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0);
    //cout << "Muscle initial energy = " << Emuscle0 << endl;
    double Esys0 = model.getMultibodySystem().calcEnergy(si);
    Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0));
    double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si);
    //cout << "Total initial system energy = " << Esys0 << endl; 

//==========================================================================
// 4. SIMULATION Integration
//==========================================================================

    // Create the integrator
    SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
    integrator.setAccuracy(integrationAccuracy);

    // Create the manager
    Manager manager(model, integrator);

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

    // Start timing the simulation
    const clock_t start = clock();
    // simulate
    manager.integrate(si);

    // how long did it take?
    double comp_time = (double)(clock()-start)/CLOCKS_PER_SEC;



//==========================================================================
// 5. SIMULATION Reporting
//==========================================================================

    double realTimeMultiplier = ((finalTime-initialTime)/comp_time);
    printf("testMuscles: Realtime Multiplier: %f\n"
           "           :  simulation duration / clock duration\n"
           "              > 1 : faster than real time\n"
           "              = 1 : real time\n"
           "              < 1 : slower than real time\n",
            realTimeMultiplier );
    
    /*
    ASSERT(comp_time <= (finalTime-initialTime));
    printf("testMuscles: PASSED Realtime test\n"
           "             %s simulation time: %f with accuracy %f\n\n",
                         actuatorType.c_str(), comp_time , accuracy);
    */

    //An analysis only writes to a dir that exists, so create here.
    if(printResults == true){
        Storage states(manager.getStateStorage());
        states.print("testProbes_states.sto");
        probeReporter->getProbeStorage().print("testProbes_probes.sto");
        forceReporter->getForceStorage().print("testProbes_forces.sto");
        muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto");
        cout << "\nDone with printing results..." << endl;
    }

    double muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
    cout << "Muscle work = " << muscleWork << endl;


    // Test the resetting of probes
    cout << "Resetting muscle work probe..." << endl;
    muscWorkProbe->reset(si);
    muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
    cout << "Muscle work = " << muscleWork << endl;
    ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe.");





//==========================================================================
// 6. SIMULATION Tests
//==========================================================================
    model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
    ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
    ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
    ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe.");
    ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe.");
    ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2*pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe.");
    for (int i=0; i<initCondVec.size(); ++i)  {
        stringstream myError;
        //myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl;
        //ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str());
        //if (testRealInitConditions(i) != initCondVec(i))
        //    cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl;
    }


}
示例#16
0
/**
 * 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.addMeshGeometry("checkered_floor.vtp");

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
        
        Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
        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);
        
        // Graphical representation
        Cylinder cyl;
        cyl.set_scale_factors(linkageDimensions);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        osimModel.addFrame(cyl1Frame);
        cyl.setFrameName("Cyl1_frame");
        linkage1->addGeometry(cyl);

        Sphere sphere(0.1);
        linkage1->addGeometry(sphere);
         
        // Creat a second linkage body
        OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);
        (linkage2->upd_geometry(0)).setFrameName("Cyl2_frame");
        // Creat 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);
        Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
        block->addGeometry(brick);

        // Create 1 degree-of-freedom pin joints between the bodies to creat 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};
        CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
        ankleCoordinateSet[0].setName("q1");
        ankleCoordinateSet[0].setRange(range);

        CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
        kneeCoordinateSet[0].setName("q2");
        kneeCoordinateSet[0].setRange(range);

        CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
        hipCoordinateSet[0].setName("q3");
        hipCoordinateSet[0].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 contraints 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 acceration due to gravity
        osimModel.setGravity(Vec3(0, -9.80665, 0));

        // enable the model visualizer see the model in action, which can be
        // useful for debugging
        osimModel.setUseVisualizer(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;

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

        // Save results
        osimModel.printControlStorage("SpringActuatedLeg_controls.sto");
        Storage statesDegrees(manager.getStateStorage());
        osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
        //statesDegrees.print("PistonActuatedLeg_states_degrees.mot");
        statesDegrees.print("SpringActuatedLeg_states_degrees.mot");

        forces->getForceStorage().print("actuator_forces.mot");
        
    }
    catch (const std::exception& ex)
    {
        std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
        return 1;
    }

    std::cout << "Done." << std::endl;
    return 0;
}
int main(int argc, char **argv) {
  cout << "--------------------------------------------------------------------------------" << endl;
  cout << " Multi-Body System Benchmark in OpenSim" << endl;
  cout << " Benchmark reference url: http://lim.ii.udc.es/mbsbenchmark/" << endl;
  cout << " Problem A03: Andrew's Mechanism Model Creator" << endl;
  cout << " Copyright (C) 2013-2015 Luca Tagliapietra, Michele Vivian, Elena Ceseracciu, and Monica Reggiani" << endl;
  cout << "--------------------------------------------------------------------------------" << endl;

  if (argc != 2){
    cout << " ******************************************************************************" << endl;
    cout << " Multi-Body System Benchmark in OpenSim: Creator for Model A03" << endl;
    cout << " Usage: ./AndrewsMechanismCreateModel dataDirectory" << endl;
    cout << "       dataDirectory must contain a vtpFiles folder" << endl;
    cout << " ******************************************************************************" << endl;
    exit(EXIT_FAILURE);
  }

  const std::string dataDir = argv[1];
  cout << "Data directory: " + dataDir << endl;

  OpenSim::Model andrewsMechanism;
  andrewsMechanism.setName("Andrew's Mechanism");
  andrewsMechanism.setAuthors("L.Tagliapietra, M. Vivian, M.Reggiani");

  // Get a reference to the model's ground body
  OpenSim::Body& ground = andrewsMechanism.getGroundBody();
  andrewsMechanism.setGravity(gravityVector);

  //******************************
  // Create OF element
  //******************************
  SimTK::Inertia OFbarInertia(0.1,0.1,OFinertia);
  OpenSim::Body *OF = new OpenSim::Body("OF", OFmass, OFMassCenter, OFbarInertia);

  //Set transformation for visualization pourpose
  SimTK::Rotation rot(SimTK::Pi/2, SimTK::UnitVec3(0,0,1));
  SimTK::Transform trans = SimTK::Transform(rot);

  //Set visualization properties
  OF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visOF = OF->updDisplayer();
  visOF -> updTransform() =  trans;
  visOF -> setScaleFactors(SimTK::Vec3(0.001,OFlength, 0.001));
  visOF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  SimTK::Vec3 orientationInParent(0), orientationInBody(0);
  OpenSim::PinJoint *OJoint = new OpenSim::PinJoint("joint_O", ground, SimTK::Vec3(0), orientationInParent, *OF, SimTK::Vec3(-OFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& OCoordinateSet = OJoint -> upd_CoordinateSet();
  OCoordinateSet[0].setName("joint_O");
  OCoordinateSet[0].setDefaultValue(OAngleAtZero);
  andrewsMechanism.addBody(OF);

  //********************************
  // Create FE element
  //********************************
  SimTK::Inertia EFbarInertia(0.1,0.1,EFinertia);
  OpenSim::Body *EF = new OpenSim::Body("EF", EFmass, EFMassCenter, EFbarInertia);

  //Set visualization properties
  EF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visEF = EF->updDisplayer();
  visEF -> updTransform() =  trans;
  visEF -> setScaleFactors(SimTK::Vec3(0.001,EFlength, 0.001));
  visEF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *FJoint = new OpenSim::PinJoint("joint_F", *OF, SimTK::Vec3(OFlength/2,0,0), orientationInParent, *EF, SimTK::Vec3(EFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& FCoordinateSet = FJoint -> upd_CoordinateSet();
  FCoordinateSet[0].setName("joint_F");
  FCoordinateSet[0].setDefaultValue(FAngleAtZero);
  andrewsMechanism.addBody(EF);

  //********************************
  // Create EG element
  //********************************
  SimTK::Inertia GEbarInertia(0.1,0.1,GEinertia);
  OpenSim::Body *GE = new OpenSim::Body("GE", GEmass, GEMassCenter, GEbarInertia);

  //Set visualization properties
  GE->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visGE = GE->updDisplayer();
  visGE -> updTransform() =  trans;
  visGE -> setScaleFactors(SimTK::Vec3(0.001,GElength, 0.001));
  visGE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E1Joint = new OpenSim::PinJoint("joint_E1", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *GE, SimTK::Vec3(GElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E1CoordinateSet = E1Joint -> upd_CoordinateSet();
  E1CoordinateSet[0].setName("joint_E1");
  E1CoordinateSet[0].setDefaultValue(E1AngleAtZero);
  andrewsMechanism.addBody(GE);

  //********************************
  // Create AG element
  //********************************
  SimTK::Inertia AGbarInertia(0.1,0.1,AGinertia);
  OpenSim::Body *AG = new OpenSim::Body("AG", AGmass, AGMassCenter, AGbarInertia);

  //Set visualization properties
  AG->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visAG = AG->updDisplayer();
  visAG -> updTransform() =  trans;
  visAG -> setScaleFactors(SimTK::Vec3(0.001,AGlength, 0.001));
  visAG -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *GJoint = new OpenSim::PinJoint("joint_G", *GE, SimTK::Vec3(-GElength/2,0,0), orientationInParent, *AG, SimTK::Vec3(AGlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& GCoordinateSet = GJoint -> upd_CoordinateSet();
  GCoordinateSet[0].setName("joint_G");
  GCoordinateSet[0].setDefaultValue(GAngleAtZero);
  andrewsMechanism.addBody(AG);

  //********************************
  // Create point constraint between AG element and ground to simulate joint A
  //********************************
   createPointCostraint(andrewsMechanism, std::string("ground"), SimTK::Vec3(-0.06934, -0.00227,0), std::string("AG"), SimTK::Vec3(-AGlength/2,0,0));

  //********************************
  // Create HE element
  //********************************
  SimTK::Inertia HEbarInertia(0.1,0.1,GEinertia);
  OpenSim::Body *HE = new OpenSim::Body("HE", HEmass, HEMassCenter, HEbarInertia);

  //Set visualization properties
  HE->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visHE = HE->updDisplayer();
  visHE -> updTransform() =  trans;
  visHE -> setScaleFactors(SimTK::Vec3(0.001,HElength, 0.001));
  visHE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E2Joint = new OpenSim::PinJoint("joint_E2", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *HE, SimTK::Vec3(HElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E2CoordinateSet = E2Joint -> upd_CoordinateSet();
  E2CoordinateSet[0].setName("joint_E2");
  E2CoordinateSet[0].setDefaultValue(E2AngleAtZero);
  andrewsMechanism.addBody(HE);

  //********************************
  //Create AH element
  //********************************
  SimTK::Inertia AHbarInertia(0.1,0.1,AHinertia);
  OpenSim::Body *AH = new OpenSim::Body("AH", AHmass, AHMassCenter, AHbarInertia);

  //Set visualization properties
  AH->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visAH = AH->updDisplayer();
  visAH -> updTransform() =  trans;
  visAH -> setScaleFactors(SimTK::Vec3(0.001,AHlength, 0.001));
  visAH -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *HJoint = new OpenSim::PinJoint("joint_H", *HE, SimTK::Vec3(-HElength/2,0,0), orientationInParent, *AH, SimTK::Vec3(AHlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& HCoordinateSet = HJoint -> upd_CoordinateSet();
  HCoordinateSet[0].setName("joint_H");
  HCoordinateSet[0].setDefaultValue(HAngleAtZero);
  andrewsMechanism.addBody(AH);

  //********************************
  //Create point constraint between AH element and ground to simulate joint A
  //********************************
  createPointCostraint(andrewsMechanism, std::string("ground"), SimTK::Vec3(-0.06934, -0.00227,0), std::string("AH"), SimTK::Vec3(-AHlength/2,0,0));

  //********************************
  // Create BDE element
  //********************************
  SimTK::Inertia BDEInertia(0.1,0.1,BDEinertia);
  OpenSim::Body *BDE = new OpenSim::Body("BDE", BDEmass, BDEMassCenter, BDEInertia);

  //Set visualization properties
  BDE->addDisplayGeometry(triangleGeometry);
  OpenSim::VisibleObject* visBDE = BDE->updDisplayer();
  visBDE -> updTransform() =  trans;
  visBDE -> setScaleFactors(SimTK::Vec3(0.01, 0.01, 0.0005));
  visBDE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E3Joint = new OpenSim::PinJoint("joint_E3", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *BDE, SimTK::Vec3(BElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E3CoordinateSet = E3Joint -> upd_CoordinateSet();
  E3CoordinateSet[0].setName("joint_E3");
  E3CoordinateSet[0].setDefaultValue(E3AngleAtZero);
  andrewsMechanism.addBody(BDE);

  //********************************
  // Create point constraint between BDE element and ground to simulate joint B
  //********************************
  createPointCostraint(andrewsMechanism, std::string("ground"), SimTK::Vec3(-0.03635, 0.03273,0), std::string("BDE"), SimTK::Vec3(-BElength/2,0,0));

  //********************************
  // Add the spring between BDE and ground
  //********************************
  OpenSim::PointToPointSpring *spring = new OpenSim::PointToPointSpring(std::string("ground"), SimTK::Vec3(0.014,0.072,0), std::string("BDE"), SimTK::Vec3(-BElength/2+0.018, 0.02,0), springK, springRestLength);
  andrewsMechanism.addComponent(spring);

  // Save to file the model
  andrewsMechanism.print((dataDir+"/"+modelName+std::string(".osim")).c_str());

  cout << "Model stored in: " << dataDir << "/" << modelName << ".osim" << endl;
}
示例#18
0
//==============================================================================
// Test Cases
//==============================================================================
void testTorqueActuator()
{
    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
    const Ground& ground = model->getGround();

    //Cylindrical bodies
    double r = 0.25, h = 1.0;
    double m1 = 1.0, m2 = 2.0;
    Inertia j1 = m1*Inertia::cylinderAlongY(r, h);
    Inertia j2 = m2*Inertia::cylinderAlongY(r, h);

    //OpenSim bodies
    OpenSim::Body* bodyA 
        = new OpenSim::Body("A", m1, Vec3(0), j1);
    
    OpenSim::Body* bodyB 
        = new OpenSim::Body("B", m2, Vec3(0), j2);

    // connect bodyA to ground with 6dofs
    FreeJoint* base = 
        new FreeJoint("base", ground, Vec3(0), Vec3(0), *bodyA, Vec3(0), Vec3(0));

    model->addBody(bodyA);
    model->addJoint(base);

    // connect bodyA to bodyB by a Ball joint
    BallJoint* bInA = new BallJoint("bInA", *bodyA, Vec3(0,-h/2, 0), Vec3(0), 
                           *bodyB, Vec3(0, h/2, 0), Vec3(0));

    model->addBody(bodyB);
    model->addJoint(bInA);

    // specify magnitude and direction of applied torque
    double torqueMag = 2.1234567890;
    Vec3 torqueAxis(1/sqrt(2.0), 0, 1/sqrt(2.0));
    Vec3 torqueInG = torqueMag*torqueAxis;

    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 torque");
    model->getMatterSubsystem().addInBodyTorque(state, bodyA->getMobilizedBodyIndex(),
        torqueMag*torqueAxis, bodyForces);
    model->getMatterSubsystem().addInBodyTorque(state, bodyB->getMobilizedBodyIndex(),
        -torqueMag*torqueAxis, bodyForces);
    bodyForces.dump("Body Forces after applying torque to bodyA and bodyB");

    model->getMultibodySystem().realize(state, Stage::Acceleration);
    const 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[6+i] = torqueInG[i]; 
    }

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

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

    // clear the mobility forces
    mobilityForces = 0;

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

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

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

    model->addController(controller);

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

    //model->addProbe(powerProbe);

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

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

    model->computeStateVariableDerivatives(state);

    const Vector &udotTorqueActuator = state.getUDot();

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

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

    // determine the initial kinetic energy of the system
    /*double iKE = */model->getMatterSubsystem().calcKineticEnergy(state);

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

    model->computeStateVariableDerivatives(state);

    /*double fKE = */model->getMatterSubsystem().calcKineticEnergy(state);

    // Change in system kinetic energy can only be attributable to actuator work
    //double actuatorWork = (powerProbe->getProbeOutputs(state))[0];
    // test that this is true
    //ASSERT_EQUAL(actuatorWork, fKE-iKE, integ_accuracy);

    // Before exiting lets see if copying the spring works
    TorqueActuator* copyOfActuator = actuator->clone();
    ASSERT(*copyOfActuator == *actuator);
    
    // Check that de/serialization works
    Model modelFromFile("TestTorqueActuatorModel.osim");
    ASSERT(modelFromFile == *model, __FILE__, __LINE__,
        "Model from file FAILED to match model in memory.");

    std::cout << " ********** Test TorqueActuator time =  ********** " << 
        1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
示例#19
0
int main() {
    Model model;
    model.setName("bicep_curl");
#ifdef VISUALIZE
    model.setUseVisualizer(true);
#endif

    // Create two links, each with a mass of 1 kg, center of mass at the body's
    // origin, and moments and products of inertia of zero.
    OpenSim::Body* humerus = new OpenSim::Body("humerus", 1, Vec3(0), Inertia(0));
    OpenSim::Body* radius  = new OpenSim::Body("radius",  1, Vec3(0), Inertia(0));

    // Connect the bodies with pin joints. Assume each body is 1 m long.
    PinJoint* shoulder = new PinJoint("shoulder",
            // Parent body, location in parent, orientation in parent.
            model.getGround(), Vec3(0), Vec3(0),
            // Child body, location in child, orientation in child.
            *humerus, Vec3(0, 1, 0), Vec3(0));
    PinJoint* elbow = new PinJoint("elbow",
            *humerus, Vec3(0), Vec3(0), *radius, Vec3(0, 1, 0), Vec3(0));

    // Add a muscle that flexes the elbow.
    Millard2012EquilibriumMuscle* biceps = new
        Millard2012EquilibriumMuscle("biceps", 200, 0.6, 0.55, 0);
    biceps->addNewPathPoint("origin",    *humerus, Vec3(0, 0.8, 0));
    biceps->addNewPathPoint("insertion", *radius,  Vec3(0, 0.7, 0));

    // Add a controller that specifies the excitation of the muscle.
    PrescribedController* brain = new PrescribedController();
    brain->addActuator(*biceps);
    // Muscle excitation is 0.3 for the first 0.5 seconds, then increases to 1.
    brain->prescribeControlForActuator("biceps",
            new StepFunction(0.5, 3, 0.3, 1));

    // Add components to the model.
    model.addBody(humerus);    model.addBody(radius);
    model.addJoint(shoulder);  model.addJoint(elbow);
    model.addForce(biceps);
    model.addController(brain);

    // Add a console reporter to print the muscle fiber force and elbow angle.
    ConsoleReporter* reporter = new ConsoleReporter();
    reporter->set_report_time_interval(1.0);
    reporter->addToReport(biceps->getOutput("fiber_force"));
    reporter->addToReport(
        elbow->getCoordinate(PinJoint::Coord::RotationZ).getOutput("value"),
        "elbow_angle");
    model.addComponent(reporter);

    // Add display geometry.
    Ellipsoid bodyGeometry(0.1, 0.5, 0.1);
    bodyGeometry.setColor(Gray);
    // Attach an ellipsoid to a frame located at the center of each body.
    PhysicalOffsetFrame* humerusCenter = new PhysicalOffsetFrame(
        "humerusCenter", *humerus, Transform(Vec3(0, 0.5, 0)));
    humerus->addComponent(humerusCenter);
    humerusCenter->attachGeometry(bodyGeometry.clone());
    PhysicalOffsetFrame* radiusCenter = new PhysicalOffsetFrame(
        "radiusCenter", *radius, Transform(Vec3(0, 0.5, 0)));
    radius->addComponent(radiusCenter);
    radiusCenter->attachGeometry(bodyGeometry.clone());

    // Configure the model.
    State& state = model.initSystem();
    // Fix the shoulder at its default angle and begin with the elbow flexed.
    shoulder->getCoordinate().setLocked(state, true);
    elbow->getCoordinate().setValue(state, 0.5 * Pi);
    model.equilibrateMuscles(state);

    // Configure the visualizer.
#ifdef VISUALIZE
    model.updMatterSubsystem().setShowDefaultGeometry(true);
    Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
    viz.setBackgroundType(viz.SolidColor);
    viz.setBackgroundColor(White);
#endif

    // Simulate.
    simulate(model, state, 10.0);

    return 0;
};