<commit_before>/* -------------------------------------------------------------------------- *
 *                          OpenSim:  testForces.cpp                          *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): <NAME>                                                       *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//==============================================================================
//
//  Tests Include:
//      1. PointToPointSpring
//      2. BushingForce
//      3. ElasticFoundationForce
//      4. HuntCrossleyForce
//      5. CoordinateLimitForce
//      6. RotationalCoordinateLimitForce
//      7. ExternalForce
//      8. PathSpring
//      9. ExpressionBasedPointToPointForce
//      
//     Add tests here as Forces are added to OpenSim
//
//==============================================================================
#include <ctime>  // clock(), clock_t, CLOCKS_PER_SEC
#include <OpenSim/Simulation/osimSimulation.h>
#include <OpenSim/Analyses/osimAnalyses.h>
#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>

using namespace OpenSim;
using namespace std;

//==============================================================================
// Common Parameters for the simulations are just global.
const static double integ_accuracy = 1.0e-4;
const static SimTK::Vec3 gravity_vec = SimTK::Vec3(0, -9.8065, 0);
//==============================================================================

void testPathSpring();
void testExternalForce();
void testSpringMass();
void testBushingForce();
void testFunctionBasedBushingForce();
void testExpressionBasedBushingForceTranslational();
void testExpressionBasedBushingForceRotational();
void testElasticFoundation();
void testHuntCrossleyForce();
void testCoordinateLimitForce();
void testCoordinateLimitForceRotational();
void testExpressionBasedPointToPointForce();
void testExpressionBasedCoordinateForce();

int main()
{
    SimTK::Array_<std::string> failures;

    try { testPathSpring(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testPathSpring");
    }
        
    try { testExternalForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testExternalForce");
    }

    try { testSpringMass(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testP2PSpringMass");
    }

    try { testBushingForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testBushingForce");
    }

    try { testFunctionBasedBushingForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testFunctionBasedBushingForce");
    }

    try { testExpressionBasedBushingForceTranslational(); }
    catch (const std::exception& e){
        cout << e.what() <<endl;
        failures.push_back("testExpressionBasedBushingForceTranslational");
    }

    try { testExpressionBasedBushingForceRotational(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testExpressionBasedBushingForceRotational");
    }

    try { testElasticFoundation(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testElasticFoundation");
    }

    try { testHuntCrossleyForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testHuntCrossleyForce");
    }

    try { testCoordinateLimitForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testCoordinateLimitForce");
    }

    try { testCoordinateLimitForceRotational(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testCoordinateLimitForceRotational");
    }

    try { testExpressionBasedPointToPointForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testExpressionBasedPointToPointForce");
    }

    try { testExpressionBasedCoordinateForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testExpressionBasedCoordinateForce");
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done. All cases passed." << endl;

    return 0;
}

//==============================================================================
// Test Cases
//==============================================================================

void testExpressionBasedCoordinateForce()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double damp_coeff = 5;
    double start_h = 0.5;
    double start_v = 0;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);
    // note: test case designed for 0 <= zeta < 1 (under damped system) 
    double zeta = damp_coeff / (2*sqrt(mass*stiffness));
    double damp_freq = omega*sqrt(1-pow(zeta, 2));

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("SpringMass");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass ,Vec3(0),  mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    osimModel.setGravity(gravity_vec);

    // ode for basic mass-spring-dampener system
    ExpressionBasedCoordinateForce spring("ball_h", "-10*q-5*qdot");

    osimModel.addForce(&spring);

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    // move ball to initial conditions
    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================
    // Compute the force at the specified times.
    
    double final_t = 2.0;
    double nsteps = 10;
    double dt = final_t/nsteps;

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = exp(-1*zeta*omega*osim_state.getTime()) *
                        (
                            (start_h-dh)*cos(damp_freq*osim_state.getTime())
                            +
                            (
                                (1/damp_freq)*(zeta*omega*(start_h-dh)+start_v)
                                *
                                sin(damp_freq*osim_state.getTime())
                            )
                        )  
                        + dh;

        ASSERT_EQUAL(height, pos(1), 1e-6);

        manager.setInitialTime(dt*i);
    }
    
    // Test copying
    ExpressionBasedCoordinateForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);

    osimModel.print("ExpressionBasedCoordinateForceModel.osim");

    osimModel.disownAllComponents();
}

void testExpressionBasedPointToPointForce()
{
    using namespace SimTK;

    double mass = 100;
    double ball_radius = 0.25;

    Random::Uniform rand;
    Vec3 p1(rand.getValue(), rand.getValue(), rand.getValue());
    Vec3 p2(rand.getValue(), rand.getValue(), rand.getValue());

    // Setup OpenSim model
    Model model{};
    model.setName("ExpressionBasedPointToPointForce");
    //OpenSim bodies
    const Ground& ground = model.getGround();
    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(ball_radius));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // define body's joint
    FreeJoint free("free", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
    
    model.addBody(&ball);
    model.addJoint(&free);

    string expression("2/(d^2)-3.0*(d-0.2)*(1+0.0123456789*ddot)");

    ExpressionBasedPointToPointForce* p2pForce = 
        new ExpressionBasedPointToPointForce("ground", p1, "ball", p2, expression);
    p2pForce->setName("P2PTestForce");

    model.addForce(p2pForce);

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&model);
    model.addAnalysis(reporter);

    //model.setUseVisualizer(true);
    SimTK::State& state = model.initSystem();

    model.print("ExpressionBasedPointToPointForceModel.osim");

    Vector& q = state.updQ();
    Vector& u = state.updU();

    for(int i=0; i < state.getNU(); ++i){
        q[i] = rand.getValue();
        u[i] = rand.getValue();
    }

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

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

    double final_t = 1.0;

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

    //manager.getStateStorage().print("testExpressionBasedPointToPointForce.sto");

    // force is only velocity dependent but is only compute in Dynamics
    model.getMultibodySystem().realize(state, Stage::Dynamics);

    // Now check that the force reported by spring
    double model_force = p2pForce->getForceMagnitude(state);
    
    // Save the forces
    //reporter->getForceStorage().print("path_spring_forces.mot");
    double d = model.getSimbodyEngine().calcDistance(state, ground, p1, ball, p2);
    const MobilizedBody& b1 = ground.getMobilizedBody();
    const MobilizedBody& b2 = ball.getMobilizedBody();

    double ddot = b1.calcStationToStationDistanceTimeDerivative(state, p1, b2, p2);


    //string expression("2/(d^2)-3.0*(d-0.2)*(1+0.0123456789*ddot)");
    double analytical_force = 2/(d*d)-3.0*(d-0.2)*(1+0.0123456789*ddot);
    
    // something is wrong if the block does not reach equilibrium
    ASSERT_EQUAL(analytical_force, model_force, 1e-5);

    // Before exiting lets see if copying the P2P force works
    ExpressionBasedPointToPointForce *copyOfP2pForce = p2pForce->clone();
    ASSERT(*copyOfP2pForce == *p2pForce);

    model.disownAllComponents();
}

void testPathSpring()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double restlength = 0.5;
    double dissipation = 0.1;
    double start_h = 0.5;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("PathSpring");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body pulleyBody("PulleyBody", mass ,Vec3(0),  mass*SimTK::Inertia::brick(0.1, 0.1, 0.1));
    OpenSim::Body block("block", mass ,Vec3(0),  mass*SimTK::Inertia::brick(0.2, 0.1, 0.1));
    block.attachMeshGeometry("box.vtp");
    block.scale(Vec3(0.2, 0.1, 0.1), false);
    
    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("pulley", ground, Vec3(0, 1.0, 0), Vec3(0), pulleyBody, Vec3(0), Vec3(0));
    SliderJoint slider("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);

    osimModel.addBody(&block);
    osimModel.addJoint(&weld);

    osimModel.addBody(&pulleyBody);
    osimModel.addJoint(&slider);

    osimModel.setGravity(gravity_vec);

    PathSpring spring("spring", restlength, stiffness, dissipation);
    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
    osimModel.addForce(&spring);

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    //osimModel.setUseVisualizer(true);
    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);

    double final_t = 10.0;

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

    // tension should only be velocity dependent
    osimModel.getMultibodySystem().realize(osim_state, Stage::Velocity);

    // Now check that the force reported by spring
    double model_force = spring.getTension(osim_state);

    // get acceleration of the block
    osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
    double hddot = osimModel.getCoordinateSet().get("block_h").getAccelerationValue(osim_state);

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

    // Save the forces
    reporter->getForceStorage().print("path_spring_forces.mot");  
    
    // something is wrong if the block does not reach equilibrium
    ASSERT_EQUAL(analytical_force, model_force, 1e-3);

    // Before exiting lets see if copying the spring works
    PathSpring *copyOfSpring = spring.clone();
    ASSERT(*copyOfSpring == spring);
    
    osimModel.disownAllComponents();
}

void testSpringMass()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double restlength = 1.0;
    double start_h = 0.5;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("SpringMass");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass ,Vec3(0),  mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    osimModel.setGravity(gravity_vec);

    PointToPointSpring spring(osimModel.updGround(), 
        Vec3(0.,restlength,0.), 
        ball, 
        Vec3(0.), 
        stiffness, 
        restlength);

    osimModel.addForce(&spring);

    //osimModel.print("SpringMassModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

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

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

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
        ASSERT_EQUAL(height, pos(1), 1e-5);

        //Now check that the force reported by spring
        Array<double> model_force = spring.getRecordValues(osim_state);

        // get the forces applied to the ground and ball
        double analytical_force = -stiffness*height;
        // analytical force corresponds in direction to the force on the ball Y index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 1e-4);

        manager.setInitialTime(dt*i);
    }

    // Save the forces
    osimModel.disownAllComponents();

    // Before exiting lets see if copying the spring works
    PointToPointSpring *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);
}

void testBushingForce()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double start_h = 0.5;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("BushingTest");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    Vec3 rotStiffness(0);
    Vec3 transStiffness(stiffness);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);

    osimModel.setGravity(gravity_vec);

    BushingForce spring("bushing", "ground", "ball",
        transStiffness, rotStiffness, transDamping, rotDamping);

    osimModel.addForce(&spring);
    const BushingForce& bushingForce =
        osimModel.getComponent<BushingForce>("bushing");

    osimModel.print("BushingForceModel.osim");

    Model previousVersionModel("BushingForceModel_30000.osim", true);
    previousVersionModel.print("BushingForceModel_30000_in_Latest.osim");

    const BushingForce& bushingForceFromPrevious =
        previousVersionModel.getComponent<BushingForce>("bushing");

    ASSERT(bushingForce == bushingForceFromPrevious, __FILE__, __LINE__,
        "current bushing force FAILED to match bushing force from previous model.");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

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

    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
        ASSERT_EQUAL(height, pos(1), 1e-4);

        //Now check that the force reported by spring
        Array<double> model_force = spring.getRecordValues(osim_state);

        // get the forces applied to the ground and ball
        double analytical_force = -stiffness*height;
        // analytical force corresponds in direction to the force on the ball Y index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);

        manager.setInitialTime(dt*i);
    }

    manager.getStateStorage().print("bushing_model_states.sto");

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

    // Before exiting lets see if copying the spring works
    BushingForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);

    osimModel.disownAllComponents();
}

void testFunctionBasedBushingForce()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double start_h = 0.5;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("FunctionBasedBushingTest");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), 
                                 ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    Vec3 rotStiffness(0);
    Vec3 transStiffness(stiffness);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);

    osimModel.setGravity(gravity_vec);

    FunctionBasedBushingForce spring("linear_bushing",
                    "ground", Vec3(0), Vec3(0), 
                    "ball", Vec3(0), Vec3(0),
                    transStiffness, rotStiffness, transDamping, rotDamping);

    osimModel.addForce(&spring);

    osimModel.print("FunctionBasedBushingForceModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
        ASSERT_EQUAL(height, pos(1), 1e-4);

        //Now check that the force reported by spring
        Array<double> model_force = spring.getRecordValues(osim_state);

        // get the forces applied to the ground and ball
        double analytical_force = -stiffness*height;
        // analytical force corresponds in direction to the force on the ball Y index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);

        manager.setInitialTime(dt*i);
    }

    osimModel.disownAllComponents();

    manager.getStateStorage().print("function_based_bushing_model_states.sto");

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

    // Before exiting lets see if copying the spring works
    FunctionBasedBushingForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);
}

void testExpressionBasedBushingForceTranslational()
{
    using namespace SimTK;
    
    double mass = 1;
    double stiffness = 10;
    double start_h = 0.5;
    double ball_radius = 0.25;
    
    double omega = sqrt(stiffness/mass);
    
    double dh = mass*gravity_vec(1)/stiffness;
    
    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("ExpressionBasedBushingTranslationTest");
    osimModel.setGravity(gravity_vec);

    // Create ball body and attach it to ground
    // with a vertical slider

    const Ground& ground = osimModel.getGround();

    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);
    
    SliderJoint sliderY("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
    
    double positionRange[2] = {-10, 10};
    // Rename coordinates for a slider joint
    CoordinateSet &slider_coords = sliderY.upd_CoordinateSet();
    slider_coords[0].setName("ball_h");
    slider_coords[0].setRange(positionRange);
    
    osimModel.addBody(&ball);
    osimModel.addJoint(&sliderY);
    
    // Create base body and attach it to ground with a weld

    OpenSim::Body base("base_body", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    base.attachMeshGeometry("sphere.vtp");
    base.scale(Vec3(ball_radius), false);
    
    WeldJoint weld("weld", ground, Vec3(0), Vec3(0), base, Vec3(0), Vec3(0));
    osimModel.addBody(&base);
    osimModel.addJoint(&weld);
    
    // create an ExpressionBasedBushingForce that represents an
    // uncoupled, linear bushing between the ball body and welded base body
    Vec3 rotStiffness(0);
    Vec3 transStiffness(stiffness);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);
    
    ExpressionBasedBushingForce spring("linear_bushing",
        "base_body", Vec3(0), Vec3(0), 
        "ball", Vec3(0), Vec3(0), 
        transStiffness, rotStiffness, transDamping, rotDamping);
    
    spring.setName("translational_linear_bushing");
    
    osimModel.addForce(&spring);
    
    osimModel.print("ExpressionBasedBushingForceTranslationalModel.osim");
    
    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);
    
    SimTK::State& osim_state = osimModel.initSystem();
    
    // set the initial height of the ball on slider
    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );
    
    //==========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);
    
    double final_t = 2.0;
    double nsteps = 10;
    double dt = final_t/nsteps;
    
    for(int i = 1; i <=nsteps; ++i){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        // compute the height based on the analytic solution for 1-D spring-mass
        // system with zero-velocity at initial offset.
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;

        // check that the simulated solution is equivalent to the analytic solution
        ASSERT_EQUAL(height, pos(1), 1e-4);
        
        // get the forces applied to the base and ball
        Array<double> model_force = spring.getRecordValues(osim_state);
        
        // compute the expected force on the ball
        double analytical_force = -stiffness*height;

        // check analytical force corresponds to the force on the ball 
        // in the Y direction, index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);
        
        manager.setInitialTime(dt*i);
    }
    
    osimModel.disownAllComponents();
    
    manager.getStateStorage().print("expression_based_bushing_translational_model_states.sto");
    
    // Save the forces
    reporter->getForceStorage().print("expression_based_bushing_translational_model_forces.mot");
    
    // Before exiting lets see if copying the spring works
    ExpressionBasedBushingForce *copyOfSpring = spring.clone();
    
    ASSERT(*copyOfSpring == spring);
}

void testExpressionBasedBushingForceRotational()
{
    using namespace SimTK;

    double mass = 5;
    double stiffness = 2;
    double start_theta = Pi/6;
    double ball_radius = 0.25;

    // Setup OpenSim model
    Model *osimModel = new Model;
    osimModel->setName("ExpressionBasedBushingRotationalTest");
    const Ground& ground = osimModel->getGround();

    // Create base body and attach it to ground with a weld

    OpenSim::Body base("base_body", mass, Vec3(0),
        mass*SimTK::Inertia::sphere(ball_radius));

    WeldJoint weld("weld", ground, Vec3(0), Vec3(0), base, Vec3(0), Vec3(0));
    osimModel->addBody(&base);
    osimModel->addJoint(&weld);

    // Create ball body and attach it to ground
    // with a pin joint

    OpenSim::Body ball("ball", mass, Vec3(0), 
        mass*SimTK::Inertia::sphere(ball_radius));

    PinJoint pin("pin", ground, Vec3(0), Vec3( Pi / 2, 0, 0), 
        ball, Vec3(0), Vec3(Pi / 2, 0, 0));

    double thetaRange[2] = { -2*Pi, 2*Pi };
    // Rename coordinates for a pin joint
    CoordinateSet &pin_coords = pin.upd_CoordinateSet();
    pin_coords[0].setName("ball_theta");
    pin_coords[0].setRange(thetaRange);

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

    

    // create an ExpressionBasedBushingForce that represents an
    // uncoupled, linear bushing between the ball body and welded base body
    Vec3 rotStiffness(stiffness);
    Vec3 transStiffness(0);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);

    ExpressionBasedBushingForce spring("rotatinal_spring", 
        "base_body", Vec3(0), Vec3(0),
        "ball", Vec3(0), Vec3(0),
        transStiffness, rotStiffness, transDamping, rotDamping);

    spring.setName("rotational_linear_bushing");

    osimModel->addForce(&spring);

    osimModel->print("ExpressionBasedBushingForceRotationalModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(osimModel);
    osimModel->addAnalysis(reporter);

    SimTK::State& osim_state = osimModel->initSystem();

    // set the initial pin joint angle
    pin_coords[0].setValue(osim_state, start_theta);
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position);

    //=========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem());
    integrator.setAccuracy(1e-6);
    Manager manager(*osimModel, integrator);
    manager.setInitialTime(0.0);

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

    double I_y = ball.getInertia().getMoments()[1];
    
    double omega = sqrt(stiffness / I_y);

    for (int i = 1; i <= nsteps; ++i) {
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);

        // compute the current rotation about the y axis
        double simulated_theta = pin_coords[0].getValue(osim_state);

        // compute the rotation about y-axis from the analytic solution 
        // for 1-D spring-mass system with zero-velocity at initial offset.
        double analytical_theta = start_theta*cos(omega*osim_state.getTime());

        // check that the simulated solution is 
        //  equivalent to the analytic solution
        ASSERT_EQUAL(analytical_theta, simulated_theta, 1e-4);

        // get the forces applied to the base and ball
        Array<double> model_forces = spring.getRecordValues(osim_state);

        // compute the expected force on the ball
        double analytical_moment = -stiffness*analytical_theta;

        // check analytical moment corresponds to the moment on the ball 
        // in the Y direction, index = 4
        ASSERT_EQUAL(analytical_moment, model_forces[4], 2e-4);

        manager.setInitialTime(dt*i);
    }

    osimModel->disownAllComponents();

    manager.getStateStorage().print("expression_based_bushing_rotational_model_states.sto");

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

    // Before exiting lets see if copying the spring works
    ExpressionBasedBushingForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);
}

// Test our wrapping of elastic foundation in OpenSim
// Simple simulation of bouncing ball with dissipation should generate contact
// forces that settle to ball weight.
void testElasticFoundation()
{
    using namespace SimTK;

    double start_h = 0.5;

    // Setup OpenSim model
    Model *osimModel = new Model("BouncingBallModelEF.osim");
    
    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(osimModel);
    osimModel->addAnalysis(reporter);

    SimTK::State& osim_state = osimModel->initSystem();

    osimModel->getCoordinateSet().get("ball_ty").setValue(osim_state, start_h);<commit_msg>No need to use dynamically allocated Model.<commit_after>/* -------------------------------------------------------------------------- *
 *                          OpenSim:  testForces.cpp                          *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): <NAME>                                                       *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//==============================================================================
//
//  Tests Include:
//      1. PointToPointSpring
//      2. BushingForce
//      3. ElasticFoundationForce
//      4. HuntCrossleyForce
//      5. CoordinateLimitForce
//      6. RotationalCoordinateLimitForce
//      7. ExternalForce
//      8. PathSpring
//      9. ExpressionBasedPointToPointForce
//      
//     Add tests here as Forces are added to OpenSim
//
//==============================================================================
#include <ctime>  // clock(), clock_t, CLOCKS_PER_SEC
#include <OpenSim/Simulation/osimSimulation.h>
#include <OpenSim/Analyses/osimAnalyses.h>
#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>

using namespace OpenSim;
using namespace std;

//==============================================================================
// Common Parameters for the simulations are just global.
const static double integ_accuracy = 1.0e-4;
const static SimTK::Vec3 gravity_vec = SimTK::Vec3(0, -9.8065, 0);
//==============================================================================

void testPathSpring();
void testExternalForce();
void testSpringMass();
void testBushingForce();
void testFunctionBasedBushingForce();
void testExpressionBasedBushingForceTranslational();
void testExpressionBasedBushingForceRotational();
void testElasticFoundation();
void testHuntCrossleyForce();
void testCoordinateLimitForce();
void testCoordinateLimitForceRotational();
void testExpressionBasedPointToPointForce();
void testExpressionBasedCoordinateForce();

int main()
{
    SimTK::Array_<std::string> failures;

    try { testPathSpring(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testPathSpring");
    }
        
    try { testExternalForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testExternalForce");
    }

    try { testSpringMass(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testP2PSpringMass");
    }

    try { testBushingForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testBushingForce");
    }

    try { testFunctionBasedBushingForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testFunctionBasedBushingForce");
    }

    try { testExpressionBasedBushingForceTranslational(); }
    catch (const std::exception& e){
        cout << e.what() <<endl;
        failures.push_back("testExpressionBasedBushingForceTranslational");
    }

    try { testExpressionBasedBushingForceRotational(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testExpressionBasedBushingForceRotational");
    }

    try { testElasticFoundation(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testElasticFoundation");
    }

    try { testHuntCrossleyForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testHuntCrossleyForce");
    }

    try { testCoordinateLimitForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testCoordinateLimitForce");
    }

    try { testCoordinateLimitForceRotational(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testCoordinateLimitForceRotational");
    }

    try { testExpressionBasedPointToPointForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testExpressionBasedPointToPointForce");
    }

    try { testExpressionBasedCoordinateForce(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testExpressionBasedCoordinateForce");
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done. All cases passed." << endl;

    return 0;
}

//==============================================================================
// Test Cases
//==============================================================================

void testExpressionBasedCoordinateForce()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double damp_coeff = 5;
    double start_h = 0.5;
    double start_v = 0;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);
    // note: test case designed for 0 <= zeta < 1 (under damped system) 
    double zeta = damp_coeff / (2*sqrt(mass*stiffness));
    double damp_freq = omega*sqrt(1-pow(zeta, 2));

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("SpringMass");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass ,Vec3(0),  mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    osimModel.setGravity(gravity_vec);

    // ode for basic mass-spring-dampener system
    ExpressionBasedCoordinateForce spring("ball_h", "-10*q-5*qdot");

    osimModel.addForce(&spring);

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    // move ball to initial conditions
    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================
    // Compute the force at the specified times.
    
    double final_t = 2.0;
    double nsteps = 10;
    double dt = final_t/nsteps;

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = exp(-1*zeta*omega*osim_state.getTime()) *
                        (
                            (start_h-dh)*cos(damp_freq*osim_state.getTime())
                            +
                            (
                                (1/damp_freq)*(zeta*omega*(start_h-dh)+start_v)
                                *
                                sin(damp_freq*osim_state.getTime())
                            )
                        )  
                        + dh;

        ASSERT_EQUAL(height, pos(1), 1e-6);

        manager.setInitialTime(dt*i);
    }
    
    // Test copying
    ExpressionBasedCoordinateForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);

    osimModel.print("ExpressionBasedCoordinateForceModel.osim");

    osimModel.disownAllComponents();
}

void testExpressionBasedPointToPointForce()
{
    using namespace SimTK;

    double mass = 100;
    double ball_radius = 0.25;

    Random::Uniform rand;
    Vec3 p1(rand.getValue(), rand.getValue(), rand.getValue());
    Vec3 p2(rand.getValue(), rand.getValue(), rand.getValue());

    // Setup OpenSim model
    Model model{};
    model.setName("ExpressionBasedPointToPointForce");
    //OpenSim bodies
    const Ground& ground = model.getGround();
    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(ball_radius));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // define body's joint
    FreeJoint free("free", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
    
    model.addBody(&ball);
    model.addJoint(&free);

    string expression("2/(d^2)-3.0*(d-0.2)*(1+0.0123456789*ddot)");

    ExpressionBasedPointToPointForce* p2pForce = 
        new ExpressionBasedPointToPointForce("ground", p1, "ball", p2, expression);
    p2pForce->setName("P2PTestForce");

    model.addForce(p2pForce);

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&model);
    model.addAnalysis(reporter);

    //model.setUseVisualizer(true);
    SimTK::State& state = model.initSystem();

    model.print("ExpressionBasedPointToPointForceModel.osim");

    Vector& q = state.updQ();
    Vector& u = state.updU();

    for(int i=0; i < state.getNU(); ++i){
        q[i] = rand.getValue();
        u[i] = rand.getValue();
    }

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

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

    double final_t = 1.0;

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

    //manager.getStateStorage().print("testExpressionBasedPointToPointForce.sto");

    // force is only velocity dependent but is only compute in Dynamics
    model.getMultibodySystem().realize(state, Stage::Dynamics);

    // Now check that the force reported by spring
    double model_force = p2pForce->getForceMagnitude(state);
    
    // Save the forces
    //reporter->getForceStorage().print("path_spring_forces.mot");
    double d = model.getSimbodyEngine().calcDistance(state, ground, p1, ball, p2);
    const MobilizedBody& b1 = ground.getMobilizedBody();
    const MobilizedBody& b2 = ball.getMobilizedBody();

    double ddot = b1.calcStationToStationDistanceTimeDerivative(state, p1, b2, p2);


    //string expression("2/(d^2)-3.0*(d-0.2)*(1+0.0123456789*ddot)");
    double analytical_force = 2/(d*d)-3.0*(d-0.2)*(1+0.0123456789*ddot);
    
    // something is wrong if the block does not reach equilibrium
    ASSERT_EQUAL(analytical_force, model_force, 1e-5);

    // Before exiting lets see if copying the P2P force works
    ExpressionBasedPointToPointForce *copyOfP2pForce = p2pForce->clone();
    ASSERT(*copyOfP2pForce == *p2pForce);

    model.disownAllComponents();
}

void testPathSpring()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double restlength = 0.5;
    double dissipation = 0.1;
    double start_h = 0.5;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("PathSpring");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body pulleyBody("PulleyBody", mass ,Vec3(0),  mass*SimTK::Inertia::brick(0.1, 0.1, 0.1));
    OpenSim::Body block("block", mass ,Vec3(0),  mass*SimTK::Inertia::brick(0.2, 0.1, 0.1));
    block.attachMeshGeometry("box.vtp");
    block.scale(Vec3(0.2, 0.1, 0.1), false);
    
    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("pulley", ground, Vec3(0, 1.0, 0), Vec3(0), pulleyBody, Vec3(0), Vec3(0));
    SliderJoint slider("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);

    osimModel.addBody(&block);
    osimModel.addJoint(&weld);

    osimModel.addBody(&pulleyBody);
    osimModel.addJoint(&slider);

    osimModel.setGravity(gravity_vec);

    PathSpring spring("spring", restlength, stiffness, dissipation);
    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
    osimModel.addForce(&spring);

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    //osimModel.setUseVisualizer(true);
    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);

    double final_t = 10.0;

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

    // tension should only be velocity dependent
    osimModel.getMultibodySystem().realize(osim_state, Stage::Velocity);

    // Now check that the force reported by spring
    double model_force = spring.getTension(osim_state);

    // get acceleration of the block
    osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
    double hddot = osimModel.getCoordinateSet().get("block_h").getAccelerationValue(osim_state);

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

    // Save the forces
    reporter->getForceStorage().print("path_spring_forces.mot");  
    
    // something is wrong if the block does not reach equilibrium
    ASSERT_EQUAL(analytical_force, model_force, 1e-3);

    // Before exiting lets see if copying the spring works
    PathSpring *copyOfSpring = spring.clone();
    ASSERT(*copyOfSpring == spring);
    
    osimModel.disownAllComponents();
}

void testSpringMass()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double restlength = 1.0;
    double start_h = 0.5;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("SpringMass");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass ,Vec3(0),  mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    osimModel.setGravity(gravity_vec);

    PointToPointSpring spring(osimModel.updGround(), 
        Vec3(0.,restlength,0.), 
        ball, 
        Vec3(0.), 
        stiffness, 
        restlength);

    osimModel.addForce(&spring);

    //osimModel.print("SpringMassModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

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

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

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
        ASSERT_EQUAL(height, pos(1), 1e-5);

        //Now check that the force reported by spring
        Array<double> model_force = spring.getRecordValues(osim_state);

        // get the forces applied to the ground and ball
        double analytical_force = -stiffness*height;
        // analytical force corresponds in direction to the force on the ball Y index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 1e-4);

        manager.setInitialTime(dt*i);
    }

    // Save the forces
    osimModel.disownAllComponents();

    // Before exiting lets see if copying the spring works
    PointToPointSpring *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);
}

void testBushingForce()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double start_h = 0.5;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("BushingTest");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    Vec3 rotStiffness(0);
    Vec3 transStiffness(stiffness);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);

    osimModel.setGravity(gravity_vec);

    BushingForce spring("bushing", "ground", "ball",
        transStiffness, rotStiffness, transDamping, rotDamping);

    osimModel.addForce(&spring);
    const BushingForce& bushingForce =
        osimModel.getComponent<BushingForce>("bushing");

    osimModel.print("BushingForceModel.osim");

    Model previousVersionModel("BushingForceModel_30000.osim", true);
    previousVersionModel.print("BushingForceModel_30000_in_Latest.osim");

    const BushingForce& bushingForceFromPrevious =
        previousVersionModel.getComponent<BushingForce>("bushing");

    ASSERT(bushingForce == bushingForceFromPrevious, __FILE__, __LINE__,
        "current bushing force FAILED to match bushing force from previous model.");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

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

    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
        ASSERT_EQUAL(height, pos(1), 1e-4);

        //Now check that the force reported by spring
        Array<double> model_force = spring.getRecordValues(osim_state);

        // get the forces applied to the ground and ball
        double analytical_force = -stiffness*height;
        // analytical force corresponds in direction to the force on the ball Y index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);

        manager.setInitialTime(dt*i);
    }

    manager.getStateStorage().print("bushing_model_states.sto");

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

    // Before exiting lets see if copying the spring works
    BushingForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);

    osimModel.disownAllComponents();
}

void testFunctionBasedBushingForce()
{
    using namespace SimTK;

    double mass = 1;
    double stiffness = 10;
    double start_h = 0.5;
    double ball_radius = 0.25;

    double omega = sqrt(stiffness/mass);

    double dh = mass*gravity_vec(1)/stiffness;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("FunctionBasedBushingTest");
    //OpenSim bodies
    const Ground& ground = osimModel.getGround();;
    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);

    // Add joints
    SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), 
                                 ball, 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("ball_h");
    slider_coords[0].setRange(positionRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&slider);

    Vec3 rotStiffness(0);
    Vec3 transStiffness(stiffness);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);

    osimModel.setGravity(gravity_vec);

    FunctionBasedBushingForce spring("linear_bushing",
                    "ground", Vec3(0), Vec3(0), 
                    "ball", Vec3(0), Vec3(0),
                    transStiffness, rotStiffness, transDamping, rotDamping);

    osimModel.addForce(&spring);

    osimModel.print("FunctionBasedBushingForceModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);

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

    for(int i = 1; i <=nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
        ASSERT_EQUAL(height, pos(1), 1e-4);

        //Now check that the force reported by spring
        Array<double> model_force = spring.getRecordValues(osim_state);

        // get the forces applied to the ground and ball
        double analytical_force = -stiffness*height;
        // analytical force corresponds in direction to the force on the ball Y index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);

        manager.setInitialTime(dt*i);
    }

    osimModel.disownAllComponents();

    manager.getStateStorage().print("function_based_bushing_model_states.sto");

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

    // Before exiting lets see if copying the spring works
    FunctionBasedBushingForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);
}

void testExpressionBasedBushingForceTranslational()
{
    using namespace SimTK;
    
    double mass = 1;
    double stiffness = 10;
    double start_h = 0.5;
    double ball_radius = 0.25;
    
    double omega = sqrt(stiffness/mass);
    
    double dh = mass*gravity_vec(1)/stiffness;
    
    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("ExpressionBasedBushingTranslationTest");
    osimModel.setGravity(gravity_vec);

    // Create ball body and attach it to ground
    // with a vertical slider

    const Ground& ground = osimModel.getGround();

    OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    ball.attachMeshGeometry("sphere.vtp");
    ball.scale(Vec3(ball_radius), false);
    
    SliderJoint sliderY("slider", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
    
    double positionRange[2] = {-10, 10};
    // Rename coordinates for a slider joint
    CoordinateSet &slider_coords = sliderY.upd_CoordinateSet();
    slider_coords[0].setName("ball_h");
    slider_coords[0].setRange(positionRange);
    
    osimModel.addBody(&ball);
    osimModel.addJoint(&sliderY);
    
    // Create base body and attach it to ground with a weld

    OpenSim::Body base("base_body", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
    base.attachMeshGeometry("sphere.vtp");
    base.scale(Vec3(ball_radius), false);
    
    WeldJoint weld("weld", ground, Vec3(0), Vec3(0), base, Vec3(0), Vec3(0));
    osimModel.addBody(&base);
    osimModel.addJoint(&weld);
    
    // create an ExpressionBasedBushingForce that represents an
    // uncoupled, linear bushing between the ball body and welded base body
    Vec3 rotStiffness(0);
    Vec3 transStiffness(stiffness);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);
    
    ExpressionBasedBushingForce spring("linear_bushing",
        "base_body", Vec3(0), Vec3(0), 
        "ball", Vec3(0), Vec3(0), 
        transStiffness, rotStiffness, transDamping, rotDamping);
    
    spring.setName("translational_linear_bushing");
    
    osimModel.addForce(&spring);
    
    osimModel.print("ExpressionBasedBushingForceTranslationalModel.osim");
    
    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);
    
    SimTK::State& osim_state = osimModel.initSystem();
    
    // set the initial height of the ball on slider
    slider_coords[0].setValue(osim_state, start_h);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position );
    
    //==========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem() );
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel,  integrator);
    manager.setInitialTime(0.0);
    
    double final_t = 2.0;
    double nsteps = 10;
    double dt = final_t/nsteps;
    
    for(int i = 1; i <=nsteps; ++i){
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 pos;
        osimModel.updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
        
        // compute the height based on the analytic solution for 1-D spring-mass
        // system with zero-velocity at initial offset.
        double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;

        // check that the simulated solution is equivalent to the analytic solution
        ASSERT_EQUAL(height, pos(1), 1e-4);
        
        // get the forces applied to the base and ball
        Array<double> model_force = spring.getRecordValues(osim_state);
        
        // compute the expected force on the ball
        double analytical_force = -stiffness*height;

        // check analytical force corresponds to the force on the ball 
        // in the Y direction, index = 7
        ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);
        
        manager.setInitialTime(dt*i);
    }
    
    osimModel.disownAllComponents();
    
    manager.getStateStorage().print("expression_based_bushing_translational_model_states.sto");
    
    // Save the forces
    reporter->getForceStorage().print("expression_based_bushing_translational_model_forces.mot");
    
    // Before exiting lets see if copying the spring works
    ExpressionBasedBushingForce *copyOfSpring = spring.clone();
    
    ASSERT(*copyOfSpring == spring);
}

void testExpressionBasedBushingForceRotational()
{
    using namespace SimTK;

    double mass = 5;
    double stiffness = 2;
    double start_theta = Pi/6;
    double ball_radius = 0.25;

    // Setup OpenSim model
    Model osimModel{};
    osimModel.setName("ExpressionBasedBushingRotationalTest");
    const Ground& ground = osimModel.getGround();

    // Create base body and attach it to ground with a weld

    OpenSim::Body base("base_body", mass, Vec3(0),
        mass*SimTK::Inertia::sphere(ball_radius));

    WeldJoint weld("weld", ground, Vec3(0), Vec3(0), base, Vec3(0), Vec3(0));
    osimModel.addBody(&base);
    osimModel.addJoint(&weld);

    // Create ball body and attach it to ground
    // with a pin joint

    OpenSim::Body ball("ball", mass, Vec3(0), 
        mass*SimTK::Inertia::sphere(ball_radius));

    PinJoint pin("pin", ground, Vec3(0), Vec3( Pi / 2, 0, 0), 
        ball, Vec3(0), Vec3(Pi / 2, 0, 0));

    double thetaRange[2] = { -2*Pi, 2*Pi };
    // Rename coordinates for a pin joint
    CoordinateSet &pin_coords = pin.upd_CoordinateSet();
    pin_coords[0].setName("ball_theta");
    pin_coords[0].setRange(thetaRange);

    osimModel.addBody(&ball);
    osimModel.addJoint(&pin);

    

    // create an ExpressionBasedBushingForce that represents an
    // uncoupled, linear bushing between the ball body and welded base body
    Vec3 rotStiffness(stiffness);
    Vec3 transStiffness(0);
    Vec3 rotDamping(0);
    Vec3 transDamping(0);

    ExpressionBasedBushingForce spring("rotatinal_spring", 
        "base_body", Vec3(0), Vec3(0),
        "ball", Vec3(0), Vec3(0),
        transStiffness, rotStiffness, transDamping, rotDamping);

    spring.setName("rotational_linear_bushing");

    osimModel.addForce(&spring);

    osimModel.print("ExpressionBasedBushingForceRotationalModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(&osimModel);
    osimModel.addAnalysis(reporter);

    SimTK::State& osim_state = osimModel.initSystem();

    // set the initial pin joint angle
    pin_coords[0].setValue(osim_state, start_theta);
    osimModel.getMultibodySystem().realize(osim_state, Stage::Position);

    //=========================================================================
    // Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
    integrator.setAccuracy(1e-6);
    Manager manager(osimModel, integrator);
    manager.setInitialTime(0.0);

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

    double I_y = ball.getInertia().getMoments()[1];
    
    double omega = sqrt(stiffness / I_y);

    for (int i = 1; i <= nsteps; ++i) {
        manager.setFinalTime(dt*i);
        manager.integrate(osim_state);
        osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration);

        // compute the current rotation about the y axis
        double simulated_theta = pin_coords[0].getValue(osim_state);

        // compute the rotation about y-axis from the analytic solution 
        // for 1-D spring-mass system with zero-velocity at initial offset.
        double analytical_theta = start_theta*cos(omega*osim_state.getTime());

        // check that the simulated solution is 
        //  equivalent to the analytic solution
        ASSERT_EQUAL(analytical_theta, simulated_theta, 1e-4);

        // get the forces applied to the base and ball
        Array<double> model_forces = spring.getRecordValues(osim_state);

        // compute the expected force on the ball
        double analytical_moment = -stiffness*analytical_theta;

        // check analytical moment corresponds to the moment on the ball 
        // in the Y direction, index = 4
        ASSERT_EQUAL(analytical_moment, model_forces[4], 2e-4);

        manager.setInitialTime(dt*i);
    }

    osimModel.disownAllComponents();

    manager.getStateStorage().print("expression_based_bushing_rotational_model_states.sto");

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

    // Before exiting lets see if copying the spring works
    ExpressionBasedBushingForce *copyOfSpring = spring.clone();

    ASSERT(*copyOfSpring == spring);
}

// Test our wrapping of elastic foundation in OpenSim
// Simple simulation of bouncing ball with dissipation should generate contact
// forces that settle to ball weight.
void testElasticFoundation()
{
    using namespace SimTK;

    double start_h = 0.5;

    // Setup OpenSim model
    Model *osimModel = new Model("BouncingBallModelEF.osim");
    
    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(osimModel);
    osimModel->addAnalysis(reporter);

    SimTK::State& osim_state = osimModel->initSystem();

    osimModel->getCoordinateSet().get("ball_ty").setValue(osim_state, start_h);
{'fraction_non_alphanumeric': 0.1512504305890458, 'fraction_numerical': 0.018601446779193936, 'mean_word_length': 3.176074572760228, 'pattern_counts': {'":': 0, '<': 173, '<?xml version=': 0, '>': 71, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 30, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1869897', 'n_tokens_mistral': 23051, 'n_tokens_neox': 21502, 'n_words': 4791}