<NME> testAddComponents.cpp
<BEF> /* -------------------------------------------------------------------------- *
 *                      OpenSim:  testAddComponents.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-2017 Stanford University and the Authors                *
 * Author(s):                                                                 *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

/* 
 * Test cases to verify that the identical system and simulation is created
 * when generic addComponent() calls are used to build-up the model opposed
 * to the old add#ComponentType#() type specific methods (e.g. addBody(),
 * addJoint(), addConstraint(), ...) which are no longer functionally necessary
 * but exist for backward compatibility.
 */
//==============================================================================
//==============================================================================
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"
#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>

#include <ctime>    // for clock()

using namespace OpenSim;
using namespace SimTK;
using namespace std;

/////////////////////////////////////////////////
// DEFINE THE SIMULATION START AND FINAL TIMES //
/////////////////////////////////////////////////
static const double initialTime = 0.0;
static const double finalTime = 3.00;

void addComponentsToModel(Model& osimModel)
{
    //////////////////////
    // 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 //
    ///////////////////////////////////////////

    // GROUND FRAME
    // Get a reference to the model's ground frame
    Ground& ground = osimModel.updGround();
    // Create Frames to attach Geometry to
    // Left brick
    OpenSim::PhysicalFrame* leftAnchorFrame = 
        new PhysicalOffsetFrame(ground, Transform(Vec3(0, 0.05, 0.35)));
    leftAnchorFrame->setName("LeftAnchor");
    osimModel.addComponent(leftAnchorFrame);
    // Right brick
    OpenSim::PhysicalFrame* rightAnchorFrame = 
        new PhysicalOffsetFrame(ground, Transform(Vec3(0, 0.05, -0.35)));
    rightAnchorFrame->setName("RightAnchor");
    osimModel.addComponent(rightAnchorFrame);
    // Cylinder
    OpenSim::PhysicalFrame* cylFrame = 
        new PhysicalOffsetFrame(ground, Transform(Vec3(-.2, 0.0, 0.)));
    cylFrame->setName("CylAnchor");
    osimModel.addComponent(cylFrame);
    // Ellipsoid
    OpenSim::PhysicalFrame* ellipsoidFrame = 
        new PhysicalOffsetFrame(ground, Transform(Vec3(-.6, 0.6, 0.)));
    ellipsoidFrame->setName("EllipsoidAnchor");
    osimModel.addComponent(ellipsoidFrame);

    // Add display geometry to the ground to visualize in the Visualizer and GUI
    // add a checkered floor
    ground.attachGeometry(new Mesh("checkered_floor.vtp"));
    // add anchors for the muscles to be fixed to
    Brick* leftAnchorGeometry = new Brick(SimTK::Vec3(0.05, 0.05, 0.05));
    leftAnchorGeometry->upd_Appearance().set_color(SimTK::Vec3(0.0, 1.0, 0.0));
    Brick* rightAnchorGeometry = new Brick(SimTK::Vec3(0.05, 0.05, 0.05));
    rightAnchorGeometry->upd_Appearance().set_color(SimTK::Vec3(1.0, 1.0, 0.0));
    rightAnchorGeometry->upd_Appearance().set_opacity(0.5);

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

    // scale the anchors
    leftAnchorGeometry->set_scale_factors(Vec3(5, 1, 1));
    rightAnchorGeometry->set_scale_factors(Vec3(5, 1, 1));
    // position the anchors
    leftAnchorFrame->attachGeometry(leftAnchorGeometry);
    rightAnchorFrame->attachGeometry(rightAnchorGeometry);

    Geometry* cylGeometry = new Cylinder(0.2, .3);
    cylGeometry->upd_Appearance().set_representation(VisualRepresentation::DrawWireframe);
    cylFrame->attachGeometry(cylGeometry);

    Geometry* ellipsoidGeometry = new Ellipsoid(0.2, .7, .5);
    ellipsoidGeometry->setColor(SimTK::Vec3(1.0, .5, 0.1));
    ellipsoidFrame->attachGeometry(ellipsoidGeometry);

    // 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->attachGeometry(new Mesh("block.vtp"));

    // Use attachGeometry to set frame name & addGeometry
    block->attachGeometry(new Sphere(0.1));

    // FREE JOINT

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

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

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

    // Define non-zero default states for the free joint
    blockToGround->updCoordinate(FreeJoint::Coord::TranslationX)
                   .setDefaultValue(constantDistance);
    double h_start = blockMass*gravity[1] /
                     (stiffness*blockSideLength*blockSideLength);
    blockToGround->updCoordinate(FreeJoint::Coord::TranslationY)
                   .setDefaultValue(h_start); //y-translation is height

    // Add the block and joint to the model
    osimModel.addComponent(block);
    osimModel.addComponent(blockToGround);

    /////////////////////////////////////////////
    // 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.addComponent(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.addComponent(muscle1);
    osimModel.addComponent(muscle2);

    // CONTACT FORCE
    // Define contact geometry
    // Create new floor contact halfspace
    ContactHalfSpace *floor =
        new ContactHalfSpace(Vec3(0), 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.addComponent(floor);
    osimModel.addComponent(cube);

    // Define contact parameters for elastic foundation force
    auto *contactParams =
        new OpenSim::ElasticFoundationForce::
        ContactParameters(stiffness, dissipation, friction, friction, viscosity);
    
    /* These are NOT Component adds to the model. They are defining which 
       ContactGeometry are to be associated with a group of ContactParameters. */
    /**/contactParams->addGeometry("cube");
    /**/contactParams->addGeometry("floor");

    // Create a new elastic foundation (contact) force between the floor and cube.
    auto *contactForce = new OpenSim::ElasticFoundationForce(contactParams);
    contactForce->setName("contactForce");
    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

    // Specify properties of the force function to be applied to the block
    double time[2] = { 0, finalTime };                     // times 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.addComponent(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->set_actuator_list(0, "muscle1");
    muscleController->set_actuator_list(1, "muscle2");
    // Define linear functions for the control values for the two muscles
    Array<double> slopeAndIntercept1(0.0, 2);  // array of 2 doubles
    Array<double> slopeAndIntercept2(0.0, 2);
    // muscle1 control has slope of -1 starting 1 at t = 0
    slopeAndIntercept1[0] = -1.0 / (finalTime - initialTime);
    slopeAndIntercept1[1] = 1.0;
    // muscle2 control has slope of 0.95 starting 0.05 at t = 0
    slopeAndIntercept2[0] = 0.95 / (finalTime - initialTime);
    slopeAndIntercept2[1] = 0.05;

    // Set the individual muscle control functions for the prescribed muscle controller
    muscleController->upd_ControlFunctions().adoptAndAppend(
        new LinearFunction(slopeAndIntercept1));
    muscleController->upd_ControlFunctions()[0].setName("muscle1");
    muscleController->upd_ControlFunctions().adoptAndAppend(
        new LinearFunction(slopeAndIntercept2));
    muscleController->upd_ControlFunctions()[1].setName("muscle2");

    // Add the muscle controller to the model
    osimModel.addComponent(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);
}

void compareResultsToStandard() {
    try {
        Storage result1("tugOfWar_states.sto"),
            standard1("std_tugOfWar_states.sto");

        CHECK_STORAGE_AGAINST_STANDARD(result1, standard1,
            std::vector<double>(16, 0.02), __FILE__, __LINE__,
            "testAddComponents::tugOfWar states failed");
        cout << "testAddComponents::tugOfWar states passed\n";

        Storage result2("tugOfWar_forces.sto"),
            standard2("std_tugOfWar_forces.mot");

        std::vector<double> tols(20, 1.0);
        // 10N is 1% of the muscles maximum isometric force
        tols[0] = tols[1] = 10;

        CHECK_STORAGE_AGAINST_STANDARD(result2, standard2,
            tols, __FILE__, __LINE__,
            "testAddComponents::tugOfWar forces failed");
        cout << "testAddComponents::tugOfWar forces passed\n";
    }
    catch (const OpenSim::Exception& e) {
        e.print(cerr);
    }
}


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

    try {
        // Create an OpenSim model and set its name
        Model osimModel;
        osimModel.setName("testAddComponents_model");
        addComponentsToModel(osimModel);

        osimModel.printSubcomponentInfo();
        osimModel.finalizeConnections(); // Needed so sockets have correct absolute path on print

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

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

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

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

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

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

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

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

        //////////////////////////////
        // SAVE THE RESULTS TO FILE //
        //////////////////////////////
        // Save the model states from forward integration
        auto statesTable = manager.getStatesTable();
        STOFileAdapter_<double>::write(statesTable, "tugOfWar_states.sto");

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

        compareResultsToStandard();
    }
    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 testAddComponents completed successfully." << endl;

    return 0;
}

<MSG> update test replacing addForce(contactForce) with addComponent(...)

<DFF> @@ -238,7 +238,7 @@ void addComponentsToModel(Model& osimModel)
     contactForce->setName("contactForce");
 
     // Add the new elastic foundation force to the model
-    osimModel.addForce(contactForce);
+    osimModel.addComponent(contactForce);
 
     // PRESCRIBED FORCE
     // Create a new prescribed force to be applied to the block

{'commit': 'diff --git a/OpenSim/Tests/AddComponents/testAddComponents.cpp b/OpenSim/Tests/AddComponents/testAddComponents.cpp\nindex 32cf13385..625d42cc9 100644\n--- a/OpenSim/Tests/AddComponents/testAddComponents.cpp\n+++ b/OpenSim/Tests/AddComponents/testAddComponents.cpp\n@@ -238,7 +238,7 @@ void addComponentsToModel(Model& osimModel)\n     contactForce->setName("contactForce");\n \n     // Add the new elastic foundation force to the model\n-    osimModel.addForce(contactForce);\n+    osimModel.addComponent(contactForce);\n \n     // PRESCRIBED FORCE\n     // Create a new prescribed force to be applied to the block\n', 'commit_subject': 'update test replacing addForce(contactForce) with addComponent(...)', 'repo_name': 'opensim-org/opensim-core', 'committer': {'name': 'Ajay Seth', 'email': 'b506629e4698a937c5166aa6069ad6008e969ba5@stanford.edu', 'time_sec': '1466030422', 'tz_offset': '-420', 'date': {'seconds': '1466030422'}}, 'commit_tree': 'a80d13141dcbbe2189cc17b3347b6a26cce83626', 'commit_message': 'update test replacing addForce(contactForce) with addComponent(...)\n', 'language_name': 'HTML', 'license': 'apache-2.0', 'hunks': '@@ -238,7 +238,7 @@ void addComponentsToModel(Model& osimModel)\n     contactForce->setName("contactForce");\n \n     // Add the new elastic foundation force to the model\n-    osimModel.addForce(contactForce);\n+    osimModel.addComponent(contactForce);\n \n     // PRESCRIBED FORCE\n     // Create a new prescribed force to be applied to the block\n', 'addition_count': 1, 'deletion_count': 1, 'src_file': 'a/OpenSim/Tests/AddComponents/testAddComponents.cpp', 'tgt_file': 'b/OpenSim/Tests/AddComponents/testAddComponents.cpp', 'file_extension': '.cpp', 'before_file': '/* -------------------------------------------------------------------------- *\n *                      OpenSim:  testAddComponents.cpp                       *\n * -------------------------------------------------------------------------- *\n * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *\n * See http://opensim.stanford.edu and the NOTICE file for more information.  *\n * OpenSim is developed at Stanford University and supported by the US        *\n * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *\n * through the Warrior Web program.                                           *\n *                                                                            *\n * Copyright (c) 2005-2017 Stanford University and the Authors                *\n * Author(s):                                                                 *\n *                                                                            *\n * Licensed under the Apache License, Version 2.0 (the "License"); you may    *\n * not use this file except in compliance with the License. You may obtain a  *\n * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *\n *                                                                            *\n * Unless required by applicable law or agreed to in writing, software        *\n * distributed under the License is distributed on an "AS IS" BASIS,          *\n * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *\n * See the License for the specific language governing permissions and        *\n * limitations under the License.                                             *\n * -------------------------------------------------------------------------- */\n\n/* \n * Test cases to verify that the identical system and simulation is created\n * when generic addComponent() calls are used to build-up the model opposed\n * to the old add#ComponentType#() type specific methods (e.g. addBody(),\n * addJoint(), addConstraint(), ...) which are no longer functionally necessary\n * but exist for backward compatibility.\n */\n//==============================================================================\n//==============================================================================\n#include <OpenSim/OpenSim.h>\n#include "OpenSim/Common/STOFileAdapter.h"\n#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>\n\n#include <ctime>    // for clock()\n\nusing namespace OpenSim;\nusing namespace SimTK;\nusing namespace std;\n\n/////////////////////////////////////////////////\n// DEFINE THE SIMULATION START AND FINAL TIMES //\n/////////////////////////////////////////////////\nstatic const double initialTime = 0.0;\nstatic const double finalTime = 3.00;\n\nvoid addComponentsToModel(Model& osimModel)\n{\n    //////////////////////\n    // MODEL PARAMETERS //\n    //////////////////////\n\n    // Specify body mass of a 20 kg, 0.1m sides of cubed block body\n    double blockMass = 20.0, blockSideLength = 0.1;\n\n    // Constant distance of constraint to limit the block\'s motion\n    double constantDistance = 0.2;\n\n    // Contact parameters\n    double stiffness = 1.0e7, dissipation = 0.1, friction = 0.2, viscosity = 0.01;\n\n    ///////////////////////////////////////////\n    // DEFINE BODIES AND JOINTS OF THE MODEL //\n    ///////////////////////////////////////////\n\n    // GROUND FRAME\n    // Get a reference to the model\'s ground frame\n    Ground& ground = osimModel.updGround();\n    // Create Frames to attach Geometry to\n    // Left brick\n    OpenSim::PhysicalFrame* leftAnchorFrame = \n        new PhysicalOffsetFrame(ground, Transform(Vec3(0, 0.05, 0.35)));\n    leftAnchorFrame->setName("LeftAnchor");\n    osimModel.addComponent(leftAnchorFrame);\n    // Right brick\n    OpenSim::PhysicalFrame* rightAnchorFrame = \n        new PhysicalOffsetFrame(ground, Transform(Vec3(0, 0.05, -0.35)));\n    rightAnchorFrame->setName("RightAnchor");\n    osimModel.addComponent(rightAnchorFrame);\n    // Cylinder\n    OpenSim::PhysicalFrame* cylFrame = \n        new PhysicalOffsetFrame(ground, Transform(Vec3(-.2, 0.0, 0.)));\n    cylFrame->setName("CylAnchor");\n    osimModel.addComponent(cylFrame);\n    // Ellipsoid\n    OpenSim::PhysicalFrame* ellipsoidFrame = \n        new PhysicalOffsetFrame(ground, Transform(Vec3(-.6, 0.6, 0.)));\n    ellipsoidFrame->setName("EllipsoidAnchor");\n    osimModel.addComponent(ellipsoidFrame);\n\n    // Add display geometry to the ground to visualize in the Visualizer and GUI\n    // add a checkered floor\n    ground.attachGeometry(new Mesh("checkered_floor.vtp"));\n    // add anchors for the muscles to be fixed to\n    Brick* leftAnchorGeometry = new Brick(SimTK::Vec3(0.05, 0.05, 0.05));\n    leftAnchorGeometry->upd_Appearance().set_color(SimTK::Vec3(0.0, 1.0, 0.0));\n    Brick* rightAnchorGeometry = new Brick(SimTK::Vec3(0.05, 0.05, 0.05));\n    rightAnchorGeometry->upd_Appearance().set_color(SimTK::Vec3(1.0, 1.0, 0.0));\n    rightAnchorGeometry->upd_Appearance().set_opacity(0.5);\n\n    // block is 0.1 by 0.1 by 0.1m cube and centered at origin. \n    // transform anchors to be placed at the two extremes of the sliding block (to come)\n\n    // scale the anchors\n    leftAnchorGeometry->set_scale_factors(Vec3(5, 1, 1));\n    rightAnchorGeometry->set_scale_factors(Vec3(5, 1, 1));\n    // position the anchors\n    leftAnchorFrame->attachGeometry(leftAnchorGeometry);\n    rightAnchorFrame->attachGeometry(rightAnchorGeometry);\n\n    Geometry* cylGeometry = new Cylinder(0.2, .3);\n    cylGeometry->upd_Appearance().set_representation(VisualRepresentation::DrawWireframe);\n    cylFrame->attachGeometry(cylGeometry);\n\n    Geometry* ellipsoidGeometry = new Ellipsoid(0.2, .7, .5);\n    ellipsoidGeometry->setColor(SimTK::Vec3(1.0, .5, 0.1));\n    ellipsoidFrame->attachGeometry(ellipsoidGeometry);\n\n    // BLOCK BODY\n    Vec3 blockMassCenter(0);\n    Inertia blockInertia =\n        blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);\n\n    // Create a new block body with the specified properties\n    OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);\n\n    // Add display geometry to the block to visualize in the GUI\n    block->attachGeometry(new Mesh("block.vtp"));\n\n    // Use attachGeometry to set frame name & addGeometry\n    block->attachGeometry(new Sphere(0.1));\n\n    // FREE JOINT\n\n    // Create a new free joint with 6 degrees-of-freedom (coordinates) between\n    // the block and ground frames\n    Vec3 locationInParent(0, blockSideLength / 2, 0),\n        orientationInParent(0), locationInBody(0), orientationInBody(0);\n    FreeJoint *blockToGround =\n        new FreeJoint("blockToGround", ground, locationInParent, orientationInParent,\n                                       *block, locationInBody, orientationInBody);\n\n    // Set the angle and position ranges for the free (6-degree-of-freedom)\n    // joint between the block and ground frames.\n    double angleRange[2] = { -SimTK::Pi / 2, SimTK::Pi / 2 };\n    double positionRange[2] = { -1, 1 };\n    blockToGround->updCoordinate(FreeJoint::Coord::Rotation1X).setRange(angleRange);\n    blockToGround->updCoordinate(FreeJoint::Coord::Rotation2Y).setRange(angleRange);\n    blockToGround->updCoordinate(FreeJoint::Coord::Rotation3Z).setRange(angleRange);\n    blockToGround->updCoordinate(FreeJoint::Coord::TranslationX).setRange(positionRange);\n    blockToGround->updCoordinate(FreeJoint::Coord::TranslationY).setRange(positionRange);\n    blockToGround->updCoordinate(FreeJoint::Coord::TranslationZ).setRange(positionRange);\n\n    // GRAVITY\n    // Obtain the default acceleration due to gravity\n    Vec3 gravity = osimModel.getGravity();\n\n    // Define non-zero default states for the free joint\n    blockToGround->updCoordinate(FreeJoint::Coord::TranslationX)\n                   .setDefaultValue(constantDistance);\n    double h_start = blockMass*gravity[1] /\n                     (stiffness*blockSideLength*blockSideLength);\n    blockToGround->updCoordinate(FreeJoint::Coord::TranslationY)\n                   .setDefaultValue(h_start); //y-translation is height\n\n    // Add the block and joint to the model\n    osimModel.addComponent(block);\n    osimModel.addComponent(blockToGround);\n\n    /////////////////////////////////////////////\n    // DEFINE CONSTRAINTS IMPOSED ON THE MODEL //\n    /////////////////////////////////////////////\n    Vec3 pointOnGround(0, blockSideLength / 2, 0);\n    Vec3 pointOnBlock(0, 0, 0);\n\n    // Create a new constant distance constraint\n    ConstantDistanceConstraint *constDist =\n        new ConstantDistanceConstraint(ground,\n            pointOnGround, *block, pointOnBlock, constantDistance);\n\n    // Add the new point on a line constraint to the model\n    osimModel.addComponent(constDist);\n\n    ///////////////////////////////////////\n    // DEFINE FORCES ACTING ON THE MODEL //\n    ///////////////////////////////////////\n    // MUSCLE FORCES\n    // Create two new muscles with identical properties\n    double maxIsometricForce = 1000.0, optimalFiberLength = 0.25,\n        tendonSlackLength = 0.1, pennationAngle = 0.0;\n    Thelen2003Muscle *muscle1 =\n        new Thelen2003Muscle("muscle1",\n            maxIsometricForce, optimalFiberLength, tendonSlackLength, pennationAngle);\n    Thelen2003Muscle *muscle2 =\n        new Thelen2003Muscle("muscle2",\n            maxIsometricForce, optimalFiberLength, tendonSlackLength, pennationAngle);\n\n    // Specify the paths for the two muscles\n    // Path for muscle 1\n    muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0, 0.05, -0.35));\n    muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0, 0.0, -0.05));\n    // Path for muscle 2\n    muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0, 0.05, 0.35));\n    muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0, 0.0, 0.05));\n\n    // Add the two muscles (as forces) to the model\n    osimModel.addComponent(muscle1);\n    osimModel.addComponent(muscle2);\n\n    // CONTACT FORCE\n    // Define contact geometry\n    // Create new floor contact halfspace\n    ContactHalfSpace *floor =\n        new ContactHalfSpace(Vec3(0), Vec3(0, 0, -0.5*SimTK_PI), ground, "floor");\n    // Create new cube contact mesh\n    OpenSim::ContactMesh *cube =\n        new OpenSim::ContactMesh("blockMesh.obj", SimTK::Vec3(0), SimTK::Vec3(0), *block, "cube");\n\n    // Add contact geometry to the model\n    osimModel.addComponent(floor);\n    osimModel.addComponent(cube);\n\n    // Define contact parameters for elastic foundation force\n    auto *contactParams =\n        new OpenSim::ElasticFoundationForce::\n        ContactParameters(stiffness, dissipation, friction, friction, viscosity);\n    \n    /* These are NOT Component adds to the model. They are defining which \n       ContactGeometry are to be associated with a group of ContactParameters. */\n    /**/contactParams->addGeometry("cube");\n    /**/contactParams->addGeometry("floor");\n\n    // Create a new elastic foundation (contact) force between the floor and cube.\n    auto *contactForce = new OpenSim::ElasticFoundationForce(contactParams);\n    contactForce->setName("contactForce");\n    contactForce->setName("contactForce");\n\n    // Add the new elastic foundation force to the model\n    osimModel.addForce(contactForce);\n\n    // PRESCRIBED FORCE\n    // Create a new prescribed force to be applied to the block\n\n    // Specify properties of the force function to be applied to the block\n    double time[2] = { 0, finalTime };                     // times for linear function\n    double fXofT[2] = { 0,  -blockMass*gravity[1] * 3.0 }; // force values at t1 and t2\n\n    // Create linear function for the force components\n    PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);\n    // Set the force and point functions for the new prescribed force\n    prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));\n    prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));\n\n    // Add the new prescribed force to the model\n    osimModel.addComponent(prescribedForce);\n\n    ///////////////////////////////////\n    // DEFINE CONTROLS FOR THE MODEL //\n    ///////////////////////////////////\n    // Create a prescribed controller that simply applies controls as function of time\n    // For muscles, controls are normalized motor-neuron excitations\n    PrescribedController *muscleController = new PrescribedController();\n    muscleController->set_actuator_list(0, "muscle1");\n    muscleController->set_actuator_list(1, "muscle2");\n    // Define linear functions for the control values for the two muscles\n    Array<double> slopeAndIntercept1(0.0, 2);  // array of 2 doubles\n    Array<double> slopeAndIntercept2(0.0, 2);\n    // muscle1 control has slope of -1 starting 1 at t = 0\n    slopeAndIntercept1[0] = -1.0 / (finalTime - initialTime);\n    slopeAndIntercept1[1] = 1.0;\n    // muscle2 control has slope of 0.95 starting 0.05 at t = 0\n    slopeAndIntercept2[0] = 0.95 / (finalTime - initialTime);\n    slopeAndIntercept2[1] = 0.05;\n\n    // Set the individual muscle control functions for the prescribed muscle controller\n    muscleController->upd_ControlFunctions().adoptAndAppend(\n        new LinearFunction(slopeAndIntercept1));\n    muscleController->upd_ControlFunctions()[0].setName("muscle1");\n    muscleController->upd_ControlFunctions().adoptAndAppend(\n        new LinearFunction(slopeAndIntercept2));\n    muscleController->upd_ControlFunctions()[1].setName("muscle2");\n\n    // Add the muscle controller to the model\n    osimModel.addComponent(muscleController);\n\n    ///////////////////////////////////\n    // SPECIFY MODEL DEFAULT STATES  //\n    ///////////////////////////////////\n    // Define the default states for the two muscles\n    // Activation\n    muscle1->setDefaultActivation(slopeAndIntercept1[1]);\n    muscle2->setDefaultActivation(slopeAndIntercept2[1]);\n    // Fiber length\n    muscle2->setDefaultFiberLength(optimalFiberLength);\n    muscle1->setDefaultFiberLength(optimalFiberLength);\n}\n\nvoid compareResultsToStandard() {\n    try {\n        Storage result1("tugOfWar_states.sto"),\n            standard1("std_tugOfWar_states.sto");\n\n        CHECK_STORAGE_AGAINST_STANDARD(result1, standard1,\n            std::vector<double>(16, 0.02), __FILE__, __LINE__,\n            "testAddComponents::tugOfWar states failed");\n        cout << "testAddComponents::tugOfWar states passed\\n";\n\n        Storage result2("tugOfWar_forces.sto"),\n            standard2("std_tugOfWar_forces.mot");\n\n        std::vector<double> tols(20, 1.0);\n        // 10N is 1% of the muscles maximum isometric force\n        tols[0] = tols[1] = 10;\n\n        CHECK_STORAGE_AGAINST_STANDARD(result2, standard2,\n            tols, __FILE__, __LINE__,\n            "testAddComponents::tugOfWar forces failed");\n        cout << "testAddComponents::tugOfWar forces passed\\n";\n    }\n    catch (const OpenSim::Exception& e) {\n        e.print(cerr);\n    }\n}\n\n\n//______________________________________________________________________________\n/**\n * Run a simulation of block sliding with contact on by two muscles sliding with contact \n */\nint main()\n{\n    clock_t startTime = clock();\n\n    try {\n        // Create an OpenSim model and set its name\n        Model osimModel;\n        osimModel.setName("testAddComponents_model");\n        addComponentsToModel(osimModel);\n\n        osimModel.printSubcomponentInfo();\n        osimModel.finalizeConnections(); // Needed so sockets have correct absolute path on print\n\n        // Save the model to a file\n        osimModel.print(osimModel.getName()+".osim");\n\n        // Create the force reporter for obtaining the forces applied to the model\n        // during a forward simulation\n        ForceReporter* reporter = new ForceReporter(&osimModel);\n        osimModel.addAnalysis(reporter);\n\n        //////////////////////////\n        // PERFORM A SIMULATION //\n        //////////////////////////\n        // set use visualizer to true to visualize the simulation live\n        osimModel.setUseVisualizer(false);\n\n        // Initialize the system and get the default state\n        SimTK::State& si = osimModel.initSystem();\n\n        osimModel.getMultibodySystem().realize(si, Stage::Velocity);\n\n        // Compute initial conditions for muscles\n        osimModel.equilibrateMuscles(si);\n\n        // Create the manager managing the forward integration and its outputs\n        Manager manager(osimModel);\n        manager.setIntegratorAccuracy(1.0e-6);\n\n        // Print out details of the model\n        osimModel.printDetailedInfo(si, cout);\n\n        // Integrate from initial time to final time\n        si.setTime(initialTime);\n        manager.initialize(si);\n        cout<<"\\nIntegrating from "<<initialTime<<" to "<<finalTime<<endl;\n        manager.integrate(finalTime);\n\n        //////////////////////////////\n        // SAVE THE RESULTS TO FILE //\n        //////////////////////////////\n        // Save the model states from forward integration\n        auto statesTable = manager.getStatesTable();\n        STOFileAdapter_<double>::write(statesTable, "tugOfWar_states.sto");\n\n        auto forcesTable = reporter->getForcesTable();\n        STOFileAdapter_<double>::write(forcesTable, "tugOfWar_forces.sto");\n\n        compareResultsToStandard();\n    }\n    catch (const std::exception& ex) {\n        cerr << ex.what() << endl;\n        return 1;\n    }\n    catch (...) {\n        cerr << "UNRECOGNIZED EXCEPTION" << endl;\n        return 1;\n    }\n\n    cout << "main() routine time = " << 1.e3*(clock()-startTime)/CLOCKS_PER_SEC << "ms\\n";\n\n    cout << "OpenSim testAddComponents completed successfully." << endl;\n\n    return 0;\n}\n', 'message': 'update test replacing addForce(contactForce) with addComponent(...)\n', 'author_name': 'Ajay Seth', 'lang': 'cpp'}