/**
 * 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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("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->attachMeshGeometry("luxo_head_meters.obj");
    head->attachMeshGeometry("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));
    }
    
}
//==========================================================================================================
// 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;
}
/**
 * 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 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;
}