diff --git a/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp b/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp index 7f4f306de7..62dd6e4413 100644 --- a/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp +++ b/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp @@ -33,118 +33,236 @@ #include #include #include +#include using namespace OpenSim; using namespace std; +namespace { + // Utility function to build a simple pendulum with markers attached + Model* constructPendulumWithMarkers() + { + Model* pendulum = new Model(); + pendulum->setName("pendulum"); + Body* ball = + new Body("ball", 1.0, SimTK::Vec3(0), SimTK::Inertia::sphere(0.05)); + pendulum->addBody(ball); + + // PinJoint hinge is 1m above ground origin and 1m above the ball in the + // ball reference frame such that the ball center is at the origin with + // the hinge angle is zero + PinJoint* hinge = new PinJoint("hinge", pendulum->getGround(), + SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0), + *ball, SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0)); + hinge->updCoordinate().setName("theta"); + pendulum->addJoint(hinge); + + // Add Markers + Marker*m0 = new Marker(); + m0->setName("m0"); + m0->setParentFrame(*ball); + m0->set_location(SimTK::Vec3(0)); + pendulum->addMarker(m0); + + // Shifted Right 1cm + Marker*mR = new Marker(); + mR->setName("mR"); + mR->setParentFrame(*ball); + mR->set_location(SimTK::Vec3(0.01, 0, 0)); + pendulum->addMarker(mR); + + // Shifted Left 2cm + Marker*mL = new Marker(); + mL->setName("mL"); + mL->setParentFrame(*ball); + mL->set_location(SimTK::Vec3(-0.02, 0, 0)); + pendulum->addMarker(mL); + + return pendulum; + } + + // Using a model with markers and trajectory of states, create synthetic + // marker data. If noiseRadius is provided use it to scale the noise + // that perturbs the marker data. Optionally, use the constantOffset + // parameter true to use the same noise for each time frame, otherwise + // randomly select the noise to be added at each frame. + TimeSeriesTable_ + generateMarkerDataFromModelAndStates(const Model& model, + const StatesTrajectory& states, + const SimTK::RowVector_& biases, + double noiseRadius = 0, + bool isConstantOffset = false) { + // use a fixed seed so that we can reproduce and debug failures. + std::mt19937 gen(0); + std::normal_distribution noise(0.0, 1); + + unique_ptr m{ model.clone() }; + + auto* markerReporter = new TableReporterVec3(); + + auto markers = m->updComponentList(); + int cnt = 0; + for (auto& marker : markers) { + marker.set_location(marker.get_location() + biases[cnt++]); + markerReporter->addToReport( + marker.getOutput("location"), marker.getName()); + } -// Verify that the marker weight are consistent with the initial Set -// of MarkerWeights used to construct the MarkersReference -void testMarkersReference(); -// Verify that the orientations sensor weights are consistent with the initial -// Set of OrientationWeights used to construct the OrientationsReference -void testOrientationsReference(); - -// Utility function to build a simple pendulum with markers attached -Model* constructPendulumWithMarkers(); -// Using a model with markers and trajectory of states, create synthetic -// marker data. If noiseRadius is provided use it to scale the noise -// that perturbs the marker data. Optionally, use the constantOffset -// parameter true to use the same noise for each time frame, otherwise -// randomly select the noise to be added at each frame. -TimeSeriesTable_ -generateMarkerDataFromModelAndStates(const Model& model, - const StatesTrajectory& states, - const SimTK::RowVector_& biases, - double noiseRadius = 0, - bool constantOffset = false); - -// Utility function to build a simple 3dof leg model -Model* constructLegWithOrientationFrames(); -// Using a model with orientation reference frames and a trajectory of -// states, create synthetic orientation data. If noiseRadius is provided -// use it to scale the noise that perturbs the orientation data. Optionally, -// use the constantOffset parameter true to use the same noise for each time -// frame, otherwise randomly select the noise to be added at each frame. -TimeSeriesTable_ -generateOrientationsDataFromModelAndStates(const Model& model, - const StatesTrajectory& states, - const SimTK::RowVector_& biases, - double noiseRadius, - bool constantOffset = false); + m->addComponent(markerReporter); -// Verify that accuracy improves the number of decimals points to which -// the solver solution (coordinates) can be trusted as it is tightened. -void testAccuracy(); -// Verify that the marker weights impact the solver and has the expected -// effect of reducing the error for the marker weight that is increased. -void testUpdateMarkerWeights(); -// Verify that the track() solution is also effected by updating marker -// weights and marker error is being reduced as its weighting increases. -void testTrackWithUpdateMarkerWeights(); + SimTK::State s = m->initSystem(); -// Verify that solver does not confuse/mismanage markers when reference -// has more markers than the model, order is changed or marker reference -// includes intervals with NaNs (no observation) -void testNumberOfMarkersMismatch(); -void testNumberOfOrientationsMismatch(); + for (const auto& state : states) { + // collect results into reporter + m->realizeReport(state); + } -int main() -{ - SimTK::Array_ failures; + // make a copy of the reported table + auto results = markerReporter->getTable(); - try { testMarkersReference(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testMarkersReference"); - } - try { testOrientationsReference(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testOrientationsReference"); - } - - try { testAccuracy(); } - catch (const std::exception& e) { - cout << e.what() << endl; failures.push_back("testAccuracy"); - } - try { testUpdateMarkerWeights(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testUpdateMarkerWeights"); - } - try { testTrackWithUpdateMarkerWeights(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testTrackWithUpdateMarkerWeights"); - } + SimTK::Vec3 offset = noiseRadius*SimTK::Vec3(double(noise(gen)), + double(noise(gen)), + double(noise(gen))); - try { testNumberOfMarkersMismatch(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testNumberOfMarkersMismatch"); - } + if (noiseRadius >= SimTK::Eps) { + for (size_t i = 0; i < results.getNumRows(); ++i) { + auto row = results.updRowAtIndex(i); + for (int j = 0; j < row.size(); ++j) { + if (!isConstantOffset) { + offset = noiseRadius*SimTK::Vec3(double(noise(gen)), + double(noise(gen)), + double(noise(gen))); + } + // add noise to each marker + row[j] += offset; + } + } + } - try { testNumberOfOrientationsMismatch(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testNumberOfOrientationsMismatch"); - } + return results; + } + + + // Utility function to build a simple 3dof leg model + Model* constructLegWithOrientationFrames() + { + std::unique_ptr leg{ new Model() }; + leg->setName("leg"); + Body* thigh = + new Body("thigh", 5.0, SimTK::Vec3(0), + SimTK::Inertia::cylinderAlongY(0.1, 0.5) ); + leg->addBody(thigh); + Body* shank = + new Body("shank", 2.0, SimTK::Vec3(0), + SimTK::Inertia::cylinderAlongY(0.04, 0.4) ); + leg->addBody(shank); + Body* foot = + new Body("foot", 1.0, SimTK::Vec3(0), + SimTK::Inertia::cylinderAlongY(0.02, 0.1)); + leg->addBody(foot); + + + // PinJoint hip is 1m above ground origin and 1m above the ball in the + // ball reference frame such that the ball center is at the origin with + // the hinge angle is zero + PinJoint* hip = new PinJoint("hip", leg->getGround(), + SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0), + *thigh, SimTK::Vec3(0, 0.25, 0), SimTK::Vec3(0)); + hip->updCoordinate().setName("flex"); + leg->addJoint(hip); + + PinJoint* knee = new PinJoint("knee", *thigh, + SimTK::Vec3(0, -0.25, 0), SimTK::Vec3(0), + *shank, SimTK::Vec3(0, 0.2, 0), SimTK::Vec3(0)); + knee->updCoordinate().setName("flex"); + leg->addJoint(knee); + + PinJoint* ankle = new PinJoint("ankle", *shank, + SimTK::Vec3(0, -0.2, 0), SimTK::Vec3(0), + *foot, SimTK::Vec3(0, 0.1, 0), SimTK::Vec3(0)); + ankle->updCoordinate().setName("flex"); + leg->addJoint(ankle); + + // Add Orientation Sensor Frames + SimTK::Transform offset(SimTK::Rotation(0.378, SimTK::YAxis) ); + thigh->addComponent(new PhysicalOffsetFrame("thigh_imu", *thigh, offset)); + shank->addComponent(new PhysicalOffsetFrame("shank_imu", *shank, offset)); + foot->addComponent(new PhysicalOffsetFrame("foot_imu", *foot, offset)); + + return leg.release(); + } + + // Using a model with orientation reference frames and a trajectory of + // states, create synthetic orientation data. If noiseRadius is provided + // use it to scale the noise that perturbs the orientation data. Optionally, + // use the constantOffset parameter true to use the same noise for each time + // frame, otherwise randomly select the noise to be added at each frame. + TimeSeriesTable_ + generateOrientationsDataFromModelAndStates(const Model& model, + const StatesTrajectory& states, + const SimTK::RowVector_& biases, + double noiseLevel, // noise standard deviation in radians + bool constantOffset) { + // use a fixed seed so that we can reproduce and debug failures. + std::mt19937 gen(0); + std::normal_distribution noise(0.0, 1); + + unique_ptr m{ model.clone() }; + + auto* orientationsReporter = new TableReporter_(); + + auto imus = m->updComponentList(); + int cnt = 0; + for (auto& imu : imus) { + if (imu.getName().find("_imu") != std::string::npos) { + imu.setOffsetTransform( + SimTK::Transform(imu.getOffsetTransform().R()*biases[cnt++])); + orientationsReporter->addToReport( + imu.getOutput("rotation"), imu.getName()); + } + } - if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; - return 1; - } + m->addComponent(orientationsReporter); + + SimTK::State s = m->initSystem(); - cout << "Done. All cases passed." << endl; + for (const auto& state : states) { + // collect results into reporter + m->realizeReport(state); + } - return 0; + // make a copy of the reported table + auto results = orientationsReporter->getTable(); + + SimTK::Rotation offset = SimTK::Rotation( + SimTK::BodyRotationSequence, + noiseLevel*double(noise(gen)), SimTK::XAxis, + noiseLevel*double(noise(gen)), SimTK::YAxis, + noiseLevel*double(noise(gen)), SimTK::ZAxis ); + + if (noiseLevel >= SimTK::Eps) { + for (size_t i = 0; i < results.getNumRows(); ++i) { + auto row = results.updRowAtIndex(i); + for (int j = 0; j < row.size(); ++j) { + if (!constantOffset) { + offset = SimTK::Rotation(SimTK::BodyRotationSequence, + noiseLevel*double(noise(gen)), SimTK::XAxis, + noiseLevel*double(noise(gen)), SimTK::YAxis, + noiseLevel*double(noise(gen)), SimTK::ZAxis); + } + // add noise to each orientation sensor + row[j] *= offset; + } + } + } + + return results; + } } -//============================================================================= -// Test Cases -//============================================================================= -void testMarkersReference() +// Verify that the marker weight are consistent with the initial Set +// of MarkerWeights used to construct the MarkersReference +TEST_CASE("testMarkersReference") { // column labels for marker data vector labels{ "A", "B", "C", "D", "E", "F" }; @@ -212,7 +330,9 @@ void testMarkersReference() } } -void testOrientationsReference() { +// Verify that the orientations sensor weights are consistent with the initial +// Set of OrientationWeights used to construct the OrientationsReference +TEST_CASE("testOrientationsReference") { // column labels for orientation sensor data vector labels{"A", "B", "C", "D", "E", "F"}; // for testing construct a set of marker weights in a different order @@ -280,8 +400,9 @@ void testOrientationsReference() { } } - -void testAccuracy() +// Verify that accuracy improves the number of decimals points to which +// the solver solution (coordinates) can be trusted as it is tightened. +TEST_CASE("testAccuracy") { cout << "\ntestInverseKinematicsSolver::testAccuracy()" << endl; @@ -397,7 +518,9 @@ void testAccuracy() "when accuracy was tightened."); } -void testUpdateMarkerWeights() +// Verify that the marker weights impact the solver and has the expected +// effect of reducing the error for the marker weight that is increased. +TEST_CASE("testUpdateMarkerWeights") { cout << "\ntestInverseKinematicsSolver::testUpdateMarkerWeights()" << endl; @@ -502,7 +625,9 @@ void testUpdateMarkerWeights() "marker weight was increased."); } -void testTrackWithUpdateMarkerWeights() +// Verify that the track() solution is also effected by updating marker +// weights and marker error is being reduced as its weighting increases. +TEST_CASE("testTrackWithUpdateMarkerWeights") { cout << "\ntestInverseKinematicsSolver::testTrackWithUpdateMarkerWeights()" @@ -576,7 +701,10 @@ void testTrackWithUpdateMarkerWeights() } } -void testNumberOfMarkersMismatch() +// Verify that solver does not confuse/mismanage markers when reference +// has more markers than the model, order is changed or marker reference +// includes intervals with NaNs (no observation) +TEST_CASE("testNumberOfMarkersMismatch") { cout << "\ntestInverseKinematicsSolver::testNumberOfMarkersMismatch()" @@ -689,7 +817,7 @@ void testNumberOfMarkersMismatch() } } -void testNumberOfOrientationsMismatch() +TEST_CASE("testNumberOfOrientationsMismatch") { cout << "\ntestInverseKinematicsSolver::testNumberOfOrientationsMismatch()" @@ -805,212 +933,3 @@ void testNumberOfOrientationsMismatch() cout << endl; } } - -Model* constructPendulumWithMarkers() -{ - Model* pendulum = new Model(); - pendulum->setName("pendulum"); - Body* ball = - new Body("ball", 1.0, SimTK::Vec3(0), SimTK::Inertia::sphere(0.05)); - pendulum->addBody(ball); - - // PinJoint hinge is 1m above ground origin and 1m above the ball in the ball - // reference frame such that the ball center is at the origin with the hinge - // angle is zero - PinJoint* hinge = new PinJoint("hinge", pendulum->getGround(), - SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0), - *ball, SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0)); - hinge->updCoordinate().setName("theta"); - pendulum->addJoint(hinge); - - // Add Markers - Marker*m0 = new Marker(); - m0->setName("m0"); - m0->setParentFrame(*ball); - m0->set_location(SimTK::Vec3(0)); - pendulum->addMarker(m0); - - // Shifted Right 1cm - Marker*mR = new Marker(); - mR->setName("mR"); - mR->setParentFrame(*ball); - mR->set_location(SimTK::Vec3(0.01, 0, 0)); - pendulum->addMarker(mR); - - // Shifted Left 2cm - Marker*mL = new Marker(); - mL->setName("mL"); - mL->setParentFrame(*ball); - mL->set_location(SimTK::Vec3(-0.02, 0, 0)); - pendulum->addMarker(mL); - - return pendulum; -} - -TimeSeriesTable_ -generateMarkerDataFromModelAndStates(const Model& model, - const StatesTrajectory& states, - const SimTK::RowVector_& biases, - double noiseRadius, - bool isConstantOffset) { - // use a fixed seed so that we can reproduce and debug failures. - std::mt19937 gen(0); - std::normal_distribution noise(0.0, 1); - - unique_ptr m{ model.clone() }; - - auto* markerReporter = new TableReporterVec3(); - - auto markers = m->updComponentList(); - int cnt = 0; - for (auto& marker : markers) { - marker.set_location(marker.get_location() + biases[cnt++]); - markerReporter->addToReport( - marker.getOutput("location"), marker.getName()); - } - - m->addComponent(markerReporter); - - SimTK::State s = m->initSystem(); - - for (const auto& state : states) { - // collect results into reporter - m->realizeReport(state); - } - - // make a copy of the reported table - auto results = markerReporter->getTable(); - - SimTK::Vec3 offset = noiseRadius*SimTK::Vec3(double(noise(gen)), - double(noise(gen)), - double(noise(gen))); - - if (noiseRadius >= SimTK::Eps) { - for (size_t i = 0; i < results.getNumRows(); ++i) { - auto row = results.updRowAtIndex(i); - for (int j = 0; j < row.size(); ++j) { - if (!isConstantOffset) { - offset = noiseRadius*SimTK::Vec3(double(noise(gen)), - double(noise(gen)), - double(noise(gen))); - } - // add noise to each marker - row[j] += offset; - } - } - } - - return results; -} - -Model* constructLegWithOrientationFrames() -{ - std::unique_ptr leg{ new Model() }; - leg->setName("leg"); - Body* thigh = - new Body("thigh", 5.0, SimTK::Vec3(0), - SimTK::Inertia::cylinderAlongY(0.1, 0.5) ); - leg->addBody(thigh); - Body* shank = - new Body("shank", 2.0, SimTK::Vec3(0), - SimTK::Inertia::cylinderAlongY(0.04, 0.4) ); - leg->addBody(shank); - Body* foot = - new Body("foot", 1.0, SimTK::Vec3(0), - SimTK::Inertia::cylinderAlongY(0.02, 0.1)); - leg->addBody(foot); - - - // PinJoint hip is 1m above ground origin and 1m above the ball in the ball - // reference frame such that the ball center is at the origin with the hinge - // angle is zero - PinJoint* hip = new PinJoint("hip", leg->getGround(), - SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0), - *thigh, SimTK::Vec3(0, 0.25, 0), SimTK::Vec3(0)); - hip->updCoordinate().setName("flex"); - leg->addJoint(hip); - - PinJoint* knee = new PinJoint("knee", *thigh, - SimTK::Vec3(0, -0.25, 0), SimTK::Vec3(0), - *shank, SimTK::Vec3(0, 0.2, 0), SimTK::Vec3(0)); - knee->updCoordinate().setName("flex"); - leg->addJoint(knee); - - PinJoint* ankle = new PinJoint("ankle", *shank, - SimTK::Vec3(0, -0.2, 0), SimTK::Vec3(0), - *foot, SimTK::Vec3(0, 0.1, 0), SimTK::Vec3(0)); - ankle->updCoordinate().setName("flex"); - leg->addJoint(ankle); - - // Add Orientation Sensor Frames - SimTK::Transform offset(SimTK::Rotation(0.378, SimTK::YAxis) ); - thigh->addComponent(new PhysicalOffsetFrame("thigh_imu", *thigh, offset)); - shank->addComponent(new PhysicalOffsetFrame("shank_imu", *shank, offset)); - foot->addComponent(new PhysicalOffsetFrame("foot_imu", *foot, offset)); - - return leg.release(); -} - -TimeSeriesTable_ -generateOrientationsDataFromModelAndStates(const Model& model, - const StatesTrajectory& states, - const SimTK::RowVector_& biases, - double noiseLevel, // noise standard deviation in radians - bool constantOffset) { - // use a fixed seed so that we can reproduce and debug failures. - std::mt19937 gen(0); - std::normal_distribution noise(0.0, 1); - - unique_ptr m{ model.clone() }; - - auto* orientationsReporter = new TableReporter_(); - - auto imus = m->updComponentList(); - int cnt = 0; - for (auto& imu : imus) { - if (imu.getName().find("_imu") != std::string::npos) { - imu.setOffsetTransform( - SimTK::Transform(imu.getOffsetTransform().R()*biases[cnt++])); - orientationsReporter->addToReport( - imu.getOutput("rotation"), imu.getName()); - } - } - - m->addComponent(orientationsReporter); - - SimTK::State s = m->initSystem(); - - for (const auto& state : states) { - // collect results into reporter - m->realizeReport(state); - } - - // make a copy of the reported table - auto results = orientationsReporter->getTable(); - - SimTK::Rotation offset = SimTK::Rotation( - SimTK::BodyRotationSequence, - noiseLevel*double(noise(gen)), SimTK::XAxis, - noiseLevel*double(noise(gen)), SimTK::YAxis, - noiseLevel*double(noise(gen)), SimTK::ZAxis ); - - cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl; - - if (noiseLevel >= SimTK::Eps) { - for (size_t i = 0; i < results.getNumRows(); ++i) { - auto row = results.updRowAtIndex(i); - for (int j = 0; j < row.size(); ++j) { - if (!constantOffset) { - offset = SimTK::Rotation(SimTK::BodyRotationSequence, - noiseLevel*double(noise(gen)), SimTK::XAxis, - noiseLevel*double(noise(gen)), SimTK::YAxis, - noiseLevel*double(noise(gen)), SimTK::ZAxis); - } - // add noise to each orientation sensor - row[j] *= offset; - } - } - } - - return results; -} \ No newline at end of file diff --git a/OpenSim/Simulation/Test/testManager.cpp b/OpenSim/Simulation/Test/testManager.cpp index b7db780ec6..4f22797d8d 100644 --- a/OpenSim/Simulation/Test/testManager.cpp +++ b/OpenSim/Simulation/Test/testManager.cpp @@ -47,71 +47,12 @@ Manager Tests: #include #include #include +#include using namespace OpenSim; using namespace std; -void testStationCalcWithManager(); -void testStateChangesBetweenIntegration(); -void testExcitationUpdatesWithManager(); -void testConstructors(); -void testIntegratorInterface(); -void testExceptions(); - -int main() -{ - SimTK::Array_ failures; - - try { testStationCalcWithManager(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testStationCalcWithManager"); - } - - try { testStateChangesBetweenIntegration(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testStateChangesBetweenIntegration"); - } - - try { testExcitationUpdatesWithManager(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testExcitationUpdatesWithManager"); - } - - try { testConstructors(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testConstructors"); - } - - try { testIntegratorInterface(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testIntegratorInterface"); - } - - try { testExceptions(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testExceptions"); - } - - if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; - return 1; - } - - cout << "Done. All cases passed." << endl; - - return 0; -} - -//============================================================================== -// Test Cases -//============================================================================== -void testStationCalcWithManager() +TEST_CASE("testStationCalcWithManager") { using SimTK::Vec3; @@ -185,7 +126,7 @@ void testStationCalcWithManager() } } -void testStateChangesBetweenIntegration() +TEST_CASE("testStateChangesBetweenIntegration") { cout << "Running testStateChangesBetweenIntegration" << endl; @@ -275,7 +216,7 @@ void testStateChangesBetweenIntegration() } -void testExcitationUpdatesWithManager() +TEST_CASE("testExcitationUpdatesWithManager") { cout << "Running testExcitationUpdatesWithManager" << endl; LoadOpenSimLibrary("osimActuators"); @@ -324,7 +265,7 @@ void testExcitationUpdatesWithManager() } } -void testConstructors() +TEST_CASE("testConstructors") { cout << "Running testConstructors" << endl; @@ -382,7 +323,7 @@ void testConstructors() SimTK_TEST_EQ(sliderCoord.getSpeedValue(outState4), finalSpeed); } -void testIntegratorInterface() +TEST_CASE("testIntegratorInterface") { cout << "Running testIntegratorInterface" << endl; @@ -450,7 +391,7 @@ void testIntegratorInterface() manager.setIntegratorInternalStepLimit(nSteps); } -void testExceptions() +TEST_CASE("testExceptions") { cout << "Running testExceptions" << endl; LoadOpenSimLibrary("osimActuators"); diff --git a/OpenSim/Simulation/Test/testModelInterface.cpp b/OpenSim/Simulation/Test/testModelInterface.cpp index ff7339cfc9..0ebdfe19ad 100644 --- a/OpenSim/Simulation/Test/testModelInterface.cpp +++ b/OpenSim/Simulation/Test/testModelInterface.cpp @@ -28,126 +28,114 @@ #include #include +#include using namespace OpenSim; using namespace std; -void testModelFinalizePropertiesAndConnections(); -void testModelTopologyErrors(); -void testDoesNotSegfaultWithUnusualConnections(); - -int main() { +TEST_CASE("testModelFinalizePropertiesAndConnections") +{ LoadOpenSimLibrary("osimActuators"); + Model model("arm26.osim"); - SimTK_START_TEST("testModelInterface"); - SimTK_SUBTEST(testModelFinalizePropertiesAndConnections); - SimTK_SUBTEST(testModelTopologyErrors); - SimTK_SUBTEST(testDoesNotSegfaultWithUnusualConnections); - SimTK_END_TEST(); -} - - -void testModelFinalizePropertiesAndConnections() -{ - Model model("arm26.osim"); + model.printSubcomponentInfo(); - model.printSubcomponentInfo(); + // all subcomponents are accounted for since Model constructor invokes + // finalizeFromProperties(). + ASSERT(model.countNumComponents() > 0); - // all subcomponents are accounted for since Model constructor invokes - // finalizeFromProperties(). - ASSERT(model.countNumComponents() > 0); + // model must be up-to-date with its properties + ASSERT(model.isObjectUpToDateWithProperties()); - // model must be up-to-date with its properties - ASSERT(model.isObjectUpToDateWithProperties()); + // get writable access to Components contained in the model's Set + auto& muscles = model.updMuscles(); + // to make edits, for example, muscles[0].upd_min_control() = 0.02; + ASSERT(!model.isObjectUpToDateWithProperties()); - // get writable access to Components contained in the model's Set - auto& muscles = model.updMuscles(); - // to make edits, for example, muscles[0].upd_min_control() = 0.02; - ASSERT(!model.isObjectUpToDateWithProperties()); + model.finalizeFromProperties(); + ASSERT(model.isObjectUpToDateWithProperties()); - model.finalizeFromProperties(); - ASSERT(model.isObjectUpToDateWithProperties()); + // get writable access to another Set for the purpose of editing + auto& bodies = model.updBodySet(); + // for example, bodies[1].upd_mass() = 0.05; + ASSERT(!model.isObjectUpToDateWithProperties()); - // get writable access to another Set for the purpose of editing - auto& bodies = model.updBodySet(); - // for example, bodies[1].upd_mass() = 0.05; - ASSERT(!model.isObjectUpToDateWithProperties()); + model.finalizeFromProperties(); + ASSERT(model.isObjectUpToDateWithProperties()); - model.finalizeFromProperties(); - ASSERT(model.isObjectUpToDateWithProperties()); + // make an edit through model's ComponentList access + for (auto& body : model.updComponentList()) { + body.upd_mass_center() = SimTK::Vec3(0); + break; + } + ASSERT(!model.isObjectUpToDateWithProperties()); - // make an edit through model's ComponentList access - for (auto& body : model.updComponentList()) { - body.upd_mass_center() = SimTK::Vec3(0); - break; - } - ASSERT(!model.isObjectUpToDateWithProperties()); + SimTK::State dummy_state; - SimTK::State dummy_state; + ASSERT_THROW(ComponentHasNoSystem, model.getSystem()); + ASSERT_THROW(ComponentHasNoSystem, model.realizeDynamics(dummy_state)); + ASSERT_THROW(ComponentHasNoSystem, + model.computeStateVariableDerivatives(dummy_state)); + // should not be able to create a Manager either + ASSERT_THROW(ComponentHasNoSystem, + Manager manager(model)); - ASSERT_THROW(ComponentHasNoSystem, model.getSystem()); - ASSERT_THROW(ComponentHasNoSystem, model.realizeDynamics(dummy_state)); - ASSERT_THROW(ComponentHasNoSystem, - model.computeStateVariableDerivatives(dummy_state)); - // should not be able to create a Manager either - ASSERT_THROW(ComponentHasNoSystem, - Manager manager(model)); + // get a valid System and corresponding state + SimTK::State state = model.initSystem(); + Manager manager(model); + state.setTime(0.0); - // get a valid System and corresponding state - SimTK::State state = model.initSystem(); - Manager manager(model); - state.setTime(0.0); + // this should invalidate the System + model.finalizeFromProperties(); - // this should invalidate the System - model.finalizeFromProperties(); + // verify that finalizeFromProperties() wipes out the underlying System + ASSERT_THROW(ComponentHasNoSystem, model.getSystem()); - // verify that finalizeFromProperties() wipes out the underlying System - ASSERT_THROW(ComponentHasNoSystem, model.getSystem()); + // should not be able to "trick" the manager into integrating a model + // given a stale but compatible state + ASSERT_THROW(ComponentHasNoSystem, manager.initialize(state)); - // should not be able to "trick" the manager into integrating a model - // given a stale but compatible state - ASSERT_THROW(ComponentHasNoSystem, manager.initialize(state)); + // once again, get a valid System and corresponding state + state = model.initSystem(); - // once again, get a valid System and corresponding state - state = model.initSystem(); - - // Test for the effects of calling finalizeConnections() on the model - // after initSystem() has been called. - // In this case, there are no changes to the connections to be finalized. - model.finalizeConnections(); + // Test for the effects of calling finalizeConnections() on the model + // after initSystem() has been called. + // In this case, there are no changes to the connections to be finalized. + model.finalizeConnections(); - // verify that finalizeConnections() does not wipe out the underlying - // System when there are no changes to the connections - auto& sys = model.getSystem(); + // verify that finalizeConnections() does not wipe out the underlying + // System when there are no changes to the connections + auto& sys = model.getSystem(); - auto elbowInHumerus = new PhysicalOffsetFrame("elbow_in_humerus", - model.getComponent("./bodyset/r_humerus"), - SimTK::Transform(SimTK::Vec3(0, -0.33, 0)) ); + auto elbowInHumerus = new PhysicalOffsetFrame("elbow_in_humerus", + model.getComponent("./bodyset/r_humerus"), + SimTK::Transform(SimTK::Vec3(0, -0.33, 0)) ); - model.addComponent(elbowInHumerus); + model.addComponent(elbowInHumerus); - // establish the new offset frame as part of the model but not - // used by any joints, constraints or forces - state = model.initSystem(); + // establish the new offset frame as part of the model but not + // used by any joints, constraints or forces + state = model.initSystem(); - // update the elbow Joint and connect its socket to the new frame - Joint& elbow = model.updComponent("./jointset/r_elbow"); - elbow.connectSocket_parent_frame(*elbowInHumerus); + // update the elbow Joint and connect its socket to the new frame + Joint& elbow = model.updComponent("./jointset/r_elbow"); + elbow.connectSocket_parent_frame(*elbowInHumerus); - // satisfy the new connections in the model - model.finalizeConnections(); + // satisfy the new connections in the model + model.finalizeConnections(); - // now finalizing the connections will invalidate the System because - // a Component (the elbow Joint and its connection) was updated - ASSERT_THROW(ComponentHasNoSystem, model.getSystem()); + // now finalizing the connections will invalidate the System because + // a Component (the elbow Joint and its connection) was updated + ASSERT_THROW(ComponentHasNoSystem, model.getSystem()); - // verify the new connection was made - ASSERT(model.getComponent("./jointset/r_elbow") - .getParentFrame().getName() == "elbow_in_humerus"); + // verify the new connection was made + ASSERT(model.getComponent("./jointset/r_elbow") + .getParentFrame().getName() == "elbow_in_humerus"); } -void testModelTopologyErrors() +TEST_CASE("testModelTopologyErrors") { + LoadOpenSimLibrary("osimActuators"); Model model("arm26.osim"); model.initSystem(); @@ -232,7 +220,7 @@ void testModelTopologyErrors() ASSERT_THROW(JointFramesHaveSameBaseFrame, degenerate.initSystem()); } -void testDoesNotSegfaultWithUnusualConnections() +TEST_CASE("testDoesNotSegfaultWithUnusualConnections") { // automated reproduction for bug reported in #3299 // @@ -242,6 +230,7 @@ void testDoesNotSegfaultWithUnusualConnections() try { + LoadOpenSimLibrary("osimActuators"); OpenSim::Model m; OpenSim::PhysicalFrame const& groundFrame = m.getGround(); diff --git a/OpenSim/Simulation/Test/testMomentArms.cpp b/OpenSim/Simulation/Test/testMomentArms.cpp index ab9af57578..b8a41477de 100644 --- a/OpenSim/Simulation/Test/testMomentArms.cpp +++ b/OpenSim/Simulation/Test/testMomentArms.cpp @@ -40,405 +40,449 @@ #include #include "SimulationComponentsForTesting.h" +#include using namespace OpenSim; using namespace std; -//============================================================================== -// Common Parameters for the simulations are just global. -const static double integ_accuracy = 1.0e-3; +namespace { + //========================================================================== + // Common Parameters for the simulations are just global. + const static double integ_accuracy = 1.0e-3; -void testMomentArmDefinitionForModel(const string &filename, - const string &coordName = "", - const string &muscleName = "", - SimTK::Vec2 rom = SimTK::Vec2(-SimTK::Pi/2,0), - double mass = -1.0, string errorMessage = ""); + //========================================================================== + // moment_arm = dl/dtheta, definition using inexact perturbation technique + //========================================================================== + double computeMomentArmFromDefinition(const SimTK::State &s, + const GeometryPath &path, + const Coordinate &coord){ + using namespace SimTK; -void testMomentArmsAcrossCompoundJoint(); + //Compute r = dl/dtheta + SimTK::State s_ma = s; + coord.setClamped(s_ma, false); + coord.setLocked(s_ma, false); + double theta = coord.getValue(s); + double dtheta = 0.1*integ_accuracy; -int main() -{ - clock_t startTime = clock(); - LoadOpenSimLibrary("osimActuators"); - Object::registerType(CompoundJoint()); + // Compute length 1 + coord.setValue(s_ma, theta-dtheta, false); - try { + // satisfy constraints using project since we are close to the solution + coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); + coord.getModel().getMultibodySystem().projectQ(s_ma, 1e-8); - testMomentArmsAcrossCompoundJoint(); - cout << "Joint composed of more than one mobilized body: PASSED\n" << endl; + double theta1 = coord.getValue(s_ma); + coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); - testMomentArmDefinitionForModel("BothLegs22.osim", "r_knee_angle", "VASINT", - SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), 0.0, - "VASINT of BothLegs with no mass: FAILED"); - cout << "VASINT of BothLegs with no mass: PASSED\n" << endl; + double len1 = path.getLength(s_ma); - testMomentArmDefinitionForModel("testMomentArmsConstraintB.osim", - "hip_flexion_r", "rect_fem_r", SimTK::Vec2(-SimTK::Pi/3, SimTK::Pi/3), - -1.0, "Rectus Femoris at hip with muscle attachment on patella defined w.r.t Femur: FAILED"); - cout << "Rectus Femoris at hip with muscle attachment on patella defined w.r.t Femur: PASSED\n" << endl; + // Compute length 2 + coord.setValue(s_ma, theta+dtheta, false); - testMomentArmDefinitionForModel("testMomentArmsConstraintB.osim", "knee_angle_r", "rect_fem_r", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, "Rectus Femoris with muscle attachment on patella defined w.r.t Femur: FAILED"); - cout << "Rectus Femoris with muscle attachment on patella defined w.r.t Femur: PASSED\n" << endl; + // satisfy constraints using project since we are close to the solution + coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); + coord.getModel().getMultibodySystem().projectQ(s_ma, 1e-8); - testMomentArmDefinitionForModel("testMomentArmsConstraintB.osim", "knee_angle_r", "vas_int_r", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, "Knee with Vasti attachment on patella defined w.r.t Femur: FAILED"); - cout << "Knee with Vasti attachment on patella defined w.r.t Femur: PASSED\n" << endl; + double theta2 = coord.getValue(s_ma); + coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); - testMomentArmDefinitionForModel("testMomentArmsConstraintA.osim", "knee_angle_r", "vas_int_r", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, "Knee with Vasti attachment on patella w.r.t Tibia: FAILED"); - cout << "Knee with Vasti attachment on patella w.r.t Tibia: PASSED\n" << endl; + double len2 = path.getLength(s_ma); - testMomentArmDefinitionForModel("MovingPathPointMomentArmTest.osim", "", "", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, "Moving path point across PinJoint: FAILED"); - cout << "Moving path point across PinJoint: PASSED\n" << endl; + return (len1-len2)/(theta2-theta1); + } - testMomentArmDefinitionForModel("gait2354_simbody.osim", "knee_angle_r", "vas_int_r", SimTK::Vec2(-119*SimTK::Pi/180, 9*SimTK::Pi/180), -1.0, "Knee with moving muscle point (no patella): FAILED"); - cout << "Knee with moving muscle point (no patella): PASSED\n" << endl; - - //massless should not break moment-arm solver - testMomentArmDefinitionForModel("wrist_mass.osim", "flexion", "ECU_post-surgery", SimTK::Vec2(-SimTK::Pi/3, SimTK::Pi/3), 0.0, "WRIST ECU TEST with MASSLESS BODIES: FAILED"); - cout << "WRIST ECU TEST with MASSLESS BODIES: PASSED\n" << endl; - testMomentArmDefinitionForModel("wrist_mass.osim", "flexion", "ECU_post-surgery", SimTK::Vec2(-SimTK::Pi/3, SimTK::Pi/3), 1.0, "WRIST ECU TEST with MASS = 1.0 : FAILED"); - cout << "WRIST ECU TEST with MASS = 1.0 : PASSED\n" << endl; + SimTK::Vector computeGenForceScaling(const Model &osimModel, + const SimTK::State &s, + const Coordinate &coord, + const Array &coupledCoords){ + using namespace SimTK; + + //Local modifiable copy of the state + State s_ma = s; + + osimModel.getMultibodySystem().realize(s_ma, SimTK::Stage::Instance); + + // Calculate coupling matrix C to determine the influence of other coordinates + // (mobilities) on the coordinate of interest due to constraints + + s_ma.updU() = 0; + // Light-up speed of coordinate of interest and see how other + // coordinates affected by constraints respond + coord.setSpeedValue(s_ma, 1); + + // Satisfy velocity constraints. Note that the speed we just set may + // change here too so be sure to retrieve the modified value. + osimModel.getMultibodySystem().realize(s_ma, SimTK::Stage::Velocity); + osimModel.getMultibodySystem().projectU(s_ma, 1e-10); + + // Now calculate C. by checking how speeds of other coordinates change + // normalized by how much the speed of the coordinate of interest changed. + const Vector C = s_ma.getU() / coord.getSpeedValue(s_ma); + + // Compute the scaling matrix for converting gen_forces to torques + // Unlike C, ignore all coupling that are not explicit coordinate + // coupling that defines theta = sum(q_i) or q_i = w_i*theta. + // Also do not consider coupled torques for coordinates not spanned by + // the path of interest. + Vector W(osimModel.getNumSpeeds(), 0.0); + + for(int i=0; i< osimModel.getCoordinateSet().getSize(); i++){ + Coordinate &ac = osimModel.getCoordinateSet()[i]; + //If a coordinate is kinematically coupled (ac.getName() == coord.getName()) || + bool found = ((ac.getName() == coord.getName()) || (coupledCoords.findIndex(ac.getName()) > -1)); + + // and not translational (cannot contribute to torque) + if(found && (ac.getMotionType() != Coordinate::Translational) + && (ac.getJoint().getName() != "tib_pat_r") ){ + MobilizedBodyIndex modbodIndex = ac.getBodyIndex(); + const MobilizedBody& mobod = osimModel.getMatterSubsystem().getMobilizedBody(modbodIndex); + SpatialVec Hcol = mobod.getHCol(s, SimTK::MobilizerUIndex(0)); //ac.getMobilizerQIndex())); // get nth column of H + + /*double thetaScale = */Hcol[0].norm(); // magnitude of the rotational part of this column of H + + double Ci = C[mobod.getFirstUIndex(s)+ac.getMobilizerQIndex()]; + // double Wi = 1.0/thetaScale; + //if(thetaScale) + W[mobod.getFirstUIndex(s)+ac.getMobilizerQIndex()] = Ci; + } + } - testMomentArmDefinitionForModel("wrist_mass.osim", "flexion", "ECU_post-surgery", SimTK::Vec2(-SimTK::Pi/3, SimTK::Pi/3), 100.0, "WRIST ECU TEST with MASS = 100.0 : FAILED"); - cout << "WRIST ECU TEST with MASS = 100.0 : PASSED\n" << endl; + return W; + } - testMomentArmDefinitionForModel("P2PBallJointMomentArmTest.osim", "", "", SimTK::Vec2(-SimTK::Pi/2,0), -1.0, "Point to point muscle across BallJoint: FAILED"); - cout << "Point to point muscle across BallJoint: PASSED\n" << endl; + //========================================================================== + // Main test driver can be used on any model so test cases should be very + // easy to add + //========================================================================== + void testMomentArmDefinitionForModel(const string &filename, + const string &coordName = "", + const string &muscleName = "", + SimTK::Vec2 rom = SimTK::Vec2(-SimTK::Pi/2,0), + double mass = -1.0, string errorMessage = "") + { + using namespace SimTK; + + bool passesDefinition = true; + bool passesDynamicConsistency = true; + + // Load OpenSim model + Model osimModel(filename); + osimModel.initSystem(); + + // Create the moment-arm solver to solve for moment-arms + MomentArmSolver maSolver(osimModel); + + Coordinate &coord = (coordName != "") ? + osimModel.updCoordinateSet().get(coordName) : + osimModel.updCoordinateSet()[0]; + + // Consider one force, which is the muscle of interest + Muscle &muscle = (muscleName != "") ? + dynamic_cast((osimModel.updMuscles().get(muscleName))) : + dynamic_cast(osimModel.updMuscles()[0]); + + if( mass >= 0.0){ + for(int i=0; i coupledCoordNames; + for(int i=0; i(aConstraint); + Array coordNames = coupler.getIndependentCoordinateNames(); + coordNames.append(coupler.getDependentCoordinateName()); + + int ind = coordNames.findIndex(coord.getName()); + if (ind > -1){ + for(int j=0; jaddNewPathPoint("p1", model.updGround(), SimTK::Vec3(0.05, 0, 0)); - musc->addNewPathPoint("p2", *leg, SimTK::Vec3(0.05, 0.25, 0.01)); - model.addForce(musc); + // Verify that the moment-arm calculated is dynamically consistent with moment generated + if (mass!=0 ) { + muscle.overrideActuation(s, true); + muscle.setOverrideActuation(s, 10); + osimModel.getMultibodySystem().realize(s, Stage::Acceleration); - SimTK::State& s = model.initSystem(); + double force = muscle.getActuation(s); - const Coordinate& c = model.getCoordinateSet()[0]; + // Get muscle's applied body forces + const Vector_& appliedBodyForces = osimModel.getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics); + //appliedBodyForces.dump("Applied Body Force resulting from muscle"); - model.print("testMomentArmsAcrossCompoundJoint.osim"); - testMomentArmDefinitionForModel("testMomentArmsAcrossCompoundJoint.osim", - c.getName(), "muscle", SimTK::Vec2(-2 * SimTK::Pi / 3, SimTK::Pi / 18), - 0.0, "testMomentArmsAcrossCompoundJoint: FAILED"); -} + // And any applied mobility (gen) forces due to gearing (moving path point) + const Vector& appliedGenForce = osimModel.getMultibodySystem().getMobilityForces(s, Stage::Dynamics); -//========================================================================================================== -// moment_arm = dl/dtheta, definition using inexact perturbation technique -//========================================================================================================== -double computeMomentArmFromDefinition(const SimTK::State &s, const GeometryPath &path, const Coordinate &coord) -{ - using namespace SimTK; + // Get current system accelerations + const Vector& knownUDots = s.getUDot(); + //knownUDots.dump("Acceleration due to ECU muscle:"); - //Compute r = dl/dtheta - SimTK::State s_ma = s; - coord.setClamped(s_ma, false); - coord.setLocked(s_ma, false); - double theta = coord.getValue(s); - double dtheta = 0.1*integ_accuracy; - - // Compute length 1 - coord.setValue(s_ma, theta-dtheta, false); + // Convert body forces to equivalent mobility forces (joint torques) + Vector equivalentGenForce(s.getNU(), 0.0); + osimModel.getMultibodySystem().getMatterSubsystem() + .calcTreeEquivalentMobilityForces(s, appliedBodyForces, + equivalentGenForce); + if(s.getSystemStage() < SimTK::Stage::Dynamics) + osimModel.getMultibodySystem().realize(s,SimTK::Stage::Dynamics); - // satisfy constraints using project since we are close to the solution - coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); - coord.getModel().getMultibodySystem().projectQ(s_ma, 1e-8); + // include any directly applied gen force from the path (muscle) tension + equivalentGenForce += appliedGenForce; - double theta1 = coord.getValue(s_ma); - coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); + // Determine the contribution of constraints (if any) to the effective torque + Vector_ constraintForcesInParent; + Vector constraintMobilityForces; - double len1 = path.getLength(s_ma); + // Get all forces applied to model by constraints + osimModel.getMultibodySystem().getMatterSubsystem().calcConstraintForcesFromMultipliers(s, -s.getMultipliers(), + constraintForcesInParent, constraintMobilityForces); - // Compute length 2 - coord.setValue(s_ma, theta+dtheta, false); + // Perform inverse dynamics + Vector ivdGenForces; + osimModel.getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s, + constraintMobilityForces, constraintForcesInParent, knownUDots, ivdGenForces); - // satisfy constraints using project since we are close to the solution - coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); - coord.getModel().getMultibodySystem().projectQ(s_ma, 1e-8); + //constraintForcesInParent.dump("Constraint Body Forces"); + //constraintMobilityForces.dump("Constraint Mobility Forces"); - double theta2 = coord.getValue(s_ma); - coord.getModel().getMultibodySystem().realize(s_ma, SimTK::Stage::Position); + Vector W = computeGenForceScaling(osimModel, s, coord, coupledCoordNames); - double len2 = path.getLength(s_ma); + double equivalentMuscleTorque = ~W*equivalentGenForce; + double equivalentIvdMuscleTorque = ~W*(ivdGenForces); //+constraintMobilityForces); - return (len1-len2)/(theta2-theta1); -} + cout << " Tau = " << equivalentIvdMuscleTorque <<"::" << equivalentMuscleTorque + << " r*fm = " << ma*force <<"::" << ma_dldtheta*force << endl; -SimTK::Vector computeGenForceScaling(const Model &osimModel, const SimTK::State &s, const Coordinate &coord, - const Array &coupledCoords) -{ - using namespace SimTK; - - //Local modifiable copy of the state - State s_ma = s; - - osimModel.getMultibodySystem().realize(s_ma, SimTK::Stage::Instance); - - // Calculate coupling matrix C to determine the influence of other coordinates - // (mobilities) on the coordinate of interest due to constraints - - s_ma.updU() = 0; - // Light-up speed of coordinate of interest and see how other coordinates - // affected by constraints respond - coord.setSpeedValue(s_ma, 1); - - // Satisfy velocity constraints. Note that the speed we just set may - // change here too so be sure to retrieve the modified value. - osimModel.getMultibodySystem().realize(s_ma, SimTK::Stage::Velocity); - osimModel.getMultibodySystem().projectU(s_ma, 1e-10); - - // Now calculate C. by checking how speeds of other coordinates change - // normalized by how much the speed of the coordinate of interest changed. - const Vector C = s_ma.getU() / coord.getSpeedValue(s_ma); - - // Compute the scaling matrix for converting gen_forces to torques - // Unlike C, ignore all coupling that are not explicit coordinate - // coupling that defines theta = sum(q_i) or q_i = w_i*theta. - // Also do not consider coupled torques for coordinates not spanned by - // the path of interest. - Vector W(osimModel.getNumSpeeds(), 0.0); - - for(int i=0; i< osimModel.getCoordinateSet().getSize(); i++){ - Coordinate &ac = osimModel.getCoordinateSet()[i]; - //If a coordinate is kinematically coupled (ac.getName() == coord.getName()) || - bool found = ((ac.getName() == coord.getName()) || (coupledCoords.findIndex(ac.getName()) > -1)); - - // and not translational (cannot contribute to torque) - if(found && (ac.getMotionType() != Coordinate::Translational) - && (ac.getJoint().getName() != "tib_pat_r") ){ - MobilizedBodyIndex modbodIndex = ac.getBodyIndex(); - const MobilizedBody& mobod = osimModel.getMatterSubsystem().getMobilizedBody(modbodIndex); - SpatialVec Hcol = mobod.getHCol(s, SimTK::MobilizerUIndex(0)); //ac.getMobilizerQIndex())); // get nth column of H - - /*double thetaScale = */Hcol[0].norm(); // magnitude of the rotational part of this column of H - - double Ci = C[mobod.getFirstUIndex(s)+ac.getMobilizerQIndex()]; - // double Wi = 1.0/thetaScale; - //if(thetaScale) - W[mobod.getFirstUIndex(s)+ac.getMobilizerQIndex()] = Ci; + try { + // Resulting torque from ID (no constraints) + constraints = equivalent applied torque + ASSERT_EQUAL(0.0, (equivalentIvdMuscleTorque-equivalentMuscleTorque)/equivalentIvdMuscleTorque, integ_accuracy); + // verify that equivalent torque is in fact moment-arm*force + ASSERT_EQUAL(0.0, (ma*force-equivalentMuscleTorque)/equivalentMuscleTorque, integ_accuracy); + } + catch (const OpenSim::Exception&) { + passesDynamicConsistency = false; + } + } else { + cout << endl; + } + + // Increment the joint angle + q += dq; } - } - return W; + if(!passesDefinition) + cout << "WARNING: Moment arm did not satisfy dL/dTheta equivalence." << endl; + if(!passesDynamicConsistency) + cout << "WARNING: Moment arm * force did not satisfy Torque equivalence." << endl; + + // Minimum requirement to pass is that calculated moment-arm satisfies either + // dL/dTheta definition or is at least dynamically consistent, in which dL/dTheta is not + ASSERT(passesDefinition || passesDynamicConsistency, __FILE__, __LINE__, errorMessage); + } } -//========================================================================================================== -// Main test driver can be used on any model so test cases should be very easy to add -//========================================================================================================== -void testMomentArmDefinitionForModel(const string &filename, const string &coordName, - const string &muscleName, SimTK::Vec2 rom, - double mass, string errorMessage) -{ - using namespace SimTK; +TEST_CASE("testMomentArmDefinitionForModel") { + LoadOpenSimLibrary("osimActuators"); + Object::registerType(CompoundJoint()); + + SECTION("VASINT of BothLegs with no mass") { + testMomentArmDefinitionForModel("BothLegs22.osim", "r_knee_angle", "VASINT", + SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), 0.0, + "VASINT of BothLegs with no mass: FAILED"); + } + + SECTION("Rectus Femoris at hip with muscle attachment on patella defined w.r.t Femur") { + testMomentArmDefinitionForModel("testMomentArmsConstraintB.osim", + "hip_flexion_r", "rect_fem_r", SimTK::Vec2(-SimTK::Pi/3, SimTK::Pi/3), + -1.0, "Rectus Femoris at hip with muscle attachment on patella defined w.r.t Femur: FAILED"); + } - bool passesDefinition = true; - bool passesDynamicConsistency = true; + SECTION("Rectus Femoris with muscle attachment on patella defined w.r.t Femur") { + testMomentArmDefinitionForModel("testMomentArmsConstraintB.osim", "knee_angle_r", + "rect_fem_r", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, + "Rectus Femoris with muscle attachment on patella defined w.r.t Femur: FAILED"); + } - // Load OpenSim model - Model osimModel(filename); - osimModel.initSystem(); + SECTION("Knee with Vasti attachment on patella defined w.r.t Femur") { + testMomentArmDefinitionForModel("testMomentArmsConstraintB.osim", "knee_angle_r", + "vas_int_r", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, + "Knee with Vasti attachment on patella defined w.r.t Femur: FAILED"); + } - // Create the moment-arm solver to solve for moment-arms - MomentArmSolver maSolver(osimModel); + SECTION("Knee with Vasti attachment on patella w.r.t Tibia") { + testMomentArmDefinitionForModel("testMomentArmsConstraintA.osim", "knee_angle_r", + "vas_int_r", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, + "Knee with Vasti attachment on patella w.r.t Tibia: FAILED"); + } - Coordinate &coord = (coordName != "") ? osimModel.updCoordinateSet().get(coordName) : - osimModel.updCoordinateSet()[0]; + SECTION("Moving path point across PinJoint") { + testMomentArmDefinitionForModel("MovingPathPointMomentArmTest.osim", "", + "", SimTK::Vec2(-2*SimTK::Pi/3, SimTK::Pi/18), -1.0, + "Moving path point across PinJoint: FAILED"); + } - // Consider one force, which is the muscle of interest - Muscle &muscle = (muscleName != "") ? dynamic_cast((osimModel.updMuscles().get(muscleName))) : - dynamic_cast(osimModel.updMuscles()[0]); + SECTION("Knee with moving muscle point (no patella)") { + testMomentArmDefinitionForModel("gait2354_simbody.osim", "knee_angle_r", + "vas_int_r", SimTK::Vec2(-119*SimTK::Pi/180, 9*SimTK::Pi/180), -1.0, + "Knee with moving muscle point (no patella): FAILED"); + } - if( mass >= 0.0){ - for(int i=0; i coupledCoordNames; - for(int i=0; i(aConstraint); - Array coordNames = coupler.getIndependentCoordinateNames(); - coordNames.append(coupler.getDependentCoordinateName()); - - int ind = coordNames.findIndex(coord.getName()); - if (ind > -1){ - for(int j=0; j constraintForcesInParent; - Vector constraintMobilityForces; - - // Get all forces applied to model by constraints - osimModel.getMultibodySystem().getMatterSubsystem().calcConstraintForcesFromMultipliers(s, -s.getMultipliers(), - constraintForcesInParent, constraintMobilityForces); - - // Perform inverse dynamics - Vector ivdGenForces; - osimModel.getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s, - constraintMobilityForces, constraintForcesInParent, knownUDots, ivdGenForces); - - //constraintForcesInParent.dump("Constraint Body Forces"); - //constraintMobilityForces.dump("Constraint Mobility Forces"); - - Vector W = computeGenForceScaling(osimModel, s, coord, coupledCoordNames); - - double equivalentMuscleTorque = ~W*equivalentGenForce; - double equivalentIvdMuscleTorque = ~W*(ivdGenForces); //+constraintMobilityForces); - - cout << " Tau = " << equivalentIvdMuscleTorque <<"::" << equivalentMuscleTorque - << " r*fm = " << ma*force <<"::" << ma_dldtheta*force << endl; - - - try { - // Resulting torque from ID (no constraints) + constraints = equivalent applied torque - ASSERT_EQUAL(0.0, (equivalentIvdMuscleTorque-equivalentMuscleTorque)/equivalentIvdMuscleTorque, integ_accuracy); - // verify that equivalent torque is in fact moment-arm*force - ASSERT_EQUAL(0.0, (ma*force-equivalentMuscleTorque)/equivalentMuscleTorque, integ_accuracy); - } - catch (const OpenSim::Exception&) { - passesDynamicConsistency = false; - } - } else { - cout << endl; - } + SECTION("Point to point muscle across a ball implemented by CustomJoint") { + testMomentArmDefinitionForModel("P2PBallCustomJointMomentArmTest.osim", + "", "", SimTK::Vec2(-SimTK::Pi/2, 0), -1.0, + "Point to point muscle across a ball implemented by CustomJoint: FAILED"); + } + + SECTION("Moving path point across CustomJoint") { + testMomentArmDefinitionForModel("MovingPointCustomJointMomentArmTest.osim", + "", "", SimTK::Vec2(-SimTK::Pi/2, 0), -1.0, + "Moving path point across CustomJoint: FAILED"); + } + + SECTION("Path with wrapping across CustomJoint") { + testMomentArmDefinitionForModel("WrapPathCustomJointMomentArmTest.osim", + "", "", SimTK::Vec2(-SimTK::Pi/2, 0), -1.0, + "Path with wrapping across CustomJoint: FAILED"); + } + + SECTION("Path on constrained body across CustomJoint") { + testMomentArmDefinitionForModel("PathOnConstrainedBodyMomentArmTest.osim", + "", "", SimTK::Vec2(-SimTK::Pi/2, 0), -1.0, + "Path on constrained body across CustomJoint: FAILED"); + } + + SECTION("Path on constrained body across CustomJoint") { + testMomentArmDefinitionForModel("PathOnConstrainedBodyMomentArmTest.osim", + "", "", SimTK::Vec2(-SimTK::Pi/2, 0), -1.0, + "Path on constrained body across CustomJoint: FAILED"); + } + + SECTION("Multiple moving path points test 1") { + testMomentArmDefinitionForModel("MultipleMPPsMomentArmTest.osim", "knee_angle_1", + "vas_int_r", SimTK::Vec2(-1.99 * SimTK::Pi/3, SimTK::Pi/18), -1.0, + "Multiple moving path points: FAILED"); + } - // Increment the joint angle - q += dq; + SECTION("Multiple moving path points test 2") { + testMomentArmDefinitionForModel("MultipleMPPsMomentArmTest.osim", "knee_angle_2", + "vas_int_r", SimTK::Vec2(-1.99 * SimTK::Pi/3, SimTK::Pi/18), -1.0, + "Multiple moving path points: FAILED"); } - if(!passesDefinition) - cout << "WARNING: Moment arm did not satisfy dL/dTheta equivalence." << endl; - if(!passesDynamicConsistency) - cout << "WARNING: Moment arm * force did not satisfy Torque equivalence." << endl; + SECTION("Multiple moving path points coupled coordinates test") { + testMomentArmDefinitionForModel("CoupledCoordinatesMPPsMomentArmTest.osim", + "foot_angle", "vas_int_r", SimTK::Vec2(-2 * SimTK::Pi/3, SimTK::Pi/18), + -1.0, "Multiple moving path points coupled coordinates test: FAILED"); + } +} + +TEST_CASE("testMomentArmsAcrossCompoundJoint") +{ + LoadOpenSimLibrary("osimActuators"); + Object::registerType(CompoundJoint()); + + Model model; - // Minimum requirement to pass is that calculated moment-arm satisfies either - // dL/dTheta definition or is at least dynamically consistent, in which dL/dTheta is not - ASSERT(passesDefinition || passesDynamicConsistency, __FILE__, __LINE__, errorMessage); + Body* leg = new Body("leg", 10., SimTK::Vec3(0,1,0), SimTK::Inertia(1,1,1)); + model.addComponent(leg); + + CompoundJoint* hip = new CompoundJoint("hip", + model.getGround(), SimTK::Vec3(0), SimTK::Vec3(0), + *leg, SimTK::Vec3(0, 0.5, 0), SimTK::Vec3(0)); + model.addComponent(hip); + + Thelen2003Muscle* musc = new Thelen2003Muscle("muscle", 10., 0.1, 0.2, 0.); + musc->addNewPathPoint("p1", model.updGround(), SimTK::Vec3(0.05, 0, 0)); + musc->addNewPathPoint("p2", *leg, SimTK::Vec3(0.05, 0.25, 0.01)); + model.addForce(musc); + + SimTK::State& s = model.initSystem(); + + const Coordinate& c = model.getCoordinateSet()[0]; + + model.print("testMomentArmsAcrossCompoundJoint.osim"); + testMomentArmDefinitionForModel("testMomentArmsAcrossCompoundJoint.osim", + c.getName(), "muscle", SimTK::Vec2(-2 * SimTK::Pi / 3, SimTK::Pi / 18), + 0.0, "testMomentArmsAcrossCompoundJoint: FAILED"); } diff --git a/OpenSim/Simulation/Test/testMuscleMetabolicsProbes.cpp b/OpenSim/Simulation/Test/testMuscleMetabolicsProbes.cpp index 9da77d6506..640140c841 100644 --- a/OpenSim/Simulation/Test/testMuscleMetabolicsProbes.cpp +++ b/OpenSim/Simulation/Test/testMuscleMetabolicsProbes.cpp @@ -55,6 +55,7 @@ #include #include #include +#include // The zeroth-order muscle activation dynamics model can be used only once the // API supports subcomponents that have states. @@ -90,366 +91,410 @@ class MyExcitationGetter : public MuscleActivationDynamics::ExcitationGetter { }; #endif - -//============================================================================== -// UMBERGER MUSCLE -//============================================================================== -// Muscle model used by Umberger et al., which is a slightly modified version of -// the contractile element described by van Soest and Bobbert. -class UmbergerMuscle : public Muscle { -OpenSim_DECLARE_CONCRETE_OBJECT(UmbergerMuscle, Muscle); -public: - OpenSim_DECLARE_PROPERTY(width, double, - "Normalized width of the active-force-length curve."); - OpenSim_DECLARE_PROPERTY(Arel, double, - "Arel = A_Hill * fiberActiveForceLengthMultiplier / maxIsometricForce"); - OpenSim_DECLARE_PROPERTY(Brel, double, - "Brel = B_Hill / optimalFiberLength"); - OpenSim_DECLARE_PROPERTY(FmaxEccentric, double, - "Asymptote on the eccentric side of the force-velocity curve."); - - #ifdef USE_ACTIVATION_DYNAMICS_MODEL - OpenSim_DECLARE_UNNAMED_PROPERTY(ZerothOrderMuscleActivationDynamics, - "Activation dynamic model that simply sets activation to excitation."); - #endif - - //-------------------------------------------------------------------------- - // CONSTRUCTOR - //-------------------------------------------------------------------------- - UmbergerMuscle(const std::string& muscleName, double maxIsometricForce, - double optimalFiberLength, double width, double Arel, - double Brel, double FmaxEccentric) - { - setName(muscleName); - setMaxIsometricForce(maxIsometricForce); - setOptimalFiberLength(optimalFiberLength); - setMaxContractionVelocity(Brel/Arel); - - constructProperty_width(width); - constructProperty_Arel(Arel); - constructProperty_Brel(Brel); - constructProperty_FmaxEccentric(FmaxEccentric); +namespace { + //========================================================================== + // UMBERGER MUSCLE + //========================================================================== + // Muscle model used by Umberger et al., which is a slightly modified + // version of the contractile element described by van Soest and Bobbert. + class UmbergerMuscle : public Muscle { + OpenSim_DECLARE_CONCRETE_OBJECT(UmbergerMuscle, Muscle); + public: + OpenSim_DECLARE_PROPERTY(width, double, + "Normalized width of the active-force-length curve."); + OpenSim_DECLARE_PROPERTY(Arel, double, + "Arel = A_Hill * fiberActiveForceLengthMultiplier / maxIsometricForce"); + OpenSim_DECLARE_PROPERTY(Brel, double, + "Brel = B_Hill / optimalFiberLength"); + OpenSim_DECLARE_PROPERTY(FmaxEccentric, double, + "Asymptote on the eccentric side of the force-velocity curve."); #ifdef USE_ACTIVATION_DYNAMICS_MODEL - constructProperty_ZerothOrderMuscleActivationDynamics( - ZerothOrderMuscleActivationDynamics()); - upd_ZerothOrderMuscleActivationDynamics().setExcitationGetter( - new MyExcitationGetter(*this)); + OpenSim_DECLARE_UNNAMED_PROPERTY(ZerothOrderMuscleActivationDynamics, + "Activation dynamic model that simply sets activation to excitation."); #endif - } - //-------------------------------------------------------------------------- - // SET MUSCLE STATES - //-------------------------------------------------------------------------- - void setFiberLength(SimTK::State& s, double fiberLength) const - { - setStateVariableValue(s, stateName_fiberLength, fiberLength); - markCacheVariableInvalid(s, _lengthInfoCV); - markCacheVariableInvalid(s, _velInfoCV); - markCacheVariableInvalid(s, _dynamicsInfoCV); - } + //---------------------------------------------------------------------- + // CONSTRUCTOR + //---------------------------------------------------------------------- + UmbergerMuscle(const std::string& muscleName, double maxIsometricForce, + double optimalFiberLength, double width, double Arel, + double Brel, double FmaxEccentric) + { + setName(muscleName); + setMaxIsometricForce(maxIsometricForce); + setOptimalFiberLength(optimalFiberLength); + setMaxContractionVelocity(Brel/Arel); + + constructProperty_width(width); + constructProperty_Arel(Arel); + constructProperty_Brel(Brel); + constructProperty_FmaxEccentric(FmaxEccentric); + + #ifdef USE_ACTIVATION_DYNAMICS_MODEL + constructProperty_ZerothOrderMuscleActivationDynamics( + ZerothOrderMuscleActivationDynamics()); + upd_ZerothOrderMuscleActivationDynamics().setExcitationGetter( + new MyExcitationGetter(*this)); + #endif + } - void setNormFiberVelocity(SimTK::State& s, double normFiberVelocity) const - { - setStateVariableValue(s, stateName_fiberVelocity, normFiberVelocity * - getMaxContractionVelocity() * getOptimalFiberLength()); - markCacheVariableInvalid(s, _velInfoCV); - markCacheVariableInvalid(s, _dynamicsInfoCV); - } + //---------------------------------------------------------------------- + // SET MUSCLE STATES + //---------------------------------------------------------------------- + void setFiberLength(SimTK::State& s, double fiberLength) const + { + setStateVariableValue(s, stateName_fiberLength, fiberLength); + markCacheVariableInvalid(s, _lengthInfoCV); + markCacheVariableInvalid(s, _velInfoCV); + markCacheVariableInvalid(s, _dynamicsInfoCV); + } - //-------------------------------------------------------------------------- - // MODELCOMPONENT INTERFACE - //-------------------------------------------------------------------------- - void extendConnectToModel(Model& model) override - { - Super::extendConnectToModel(model); - #ifdef USE_ACTIVATION_DYNAMICS_MODEL - ZerothOrderMuscleActivationDynamics &zomad = - upd_ZerothOrderMuscleActivationDynamics(); - includeAsSubComponent(&zomad); - #endif - } + void setNormFiberVelocity(SimTK::State& s, double normFiberVelocity) const + { + setStateVariableValue(s, stateName_fiberVelocity, normFiberVelocity * + getMaxContractionVelocity() * getOptimalFiberLength()); + markCacheVariableInvalid(s, _velInfoCV); + markCacheVariableInvalid(s, _dynamicsInfoCV); + } - void extendAddToSystem(SimTK::MultibodySystem& system) const override - { - Super::extendAddToSystem(system); - addStateVariable(stateName_fiberLength); - addStateVariable(stateName_fiberVelocity); - } + //---------------------------------------------------------------------- + // MODELCOMPONENT INTERFACE + //---------------------------------------------------------------------- + void extendConnectToModel(Model& model) override + { + Super::extendConnectToModel(model); + #ifdef USE_ACTIVATION_DYNAMICS_MODEL + ZerothOrderMuscleActivationDynamics &zomad = + upd_ZerothOrderMuscleActivationDynamics(); + includeAsSubComponent(&zomad); + #endif + } - void extendInitStateFromProperties(SimTK::State& s) const override - { - Super::extendInitStateFromProperties(s); - setFiberLength(s, getOptimalFiberLength()); - } + void extendAddToSystem(SimTK::MultibodySystem& system) const override + { + Super::extendAddToSystem(system); + addStateVariable(stateName_fiberLength); + addStateVariable(stateName_fiberVelocity); + } - void extendSetPropertiesFromState(const SimTK::State& s) override - { - Super::extendSetPropertiesFromState(s); - setOptimalFiberLength(getFiberLength(s)); - } + void extendInitStateFromProperties(SimTK::State& s) const override + { + Super::extendInitStateFromProperties(s); + setFiberLength(s, getOptimalFiberLength()); + } - void computeStateVariableDerivatives(const SimTK::State& s) const override - { - // This implementation is not intended for use in dynamic simulations. - /*const int n = */getNumStateVariables(); - setStateVariableDerivativeValue(s, stateName_fiberLength, 0.0); - setStateVariableDerivativeValue(s, stateName_fiberVelocity, 0.0); - } + void extendSetPropertiesFromState(const SimTK::State& s) override + { + Super::extendSetPropertiesFromState(s); + setOptimalFiberLength(getFiberLength(s)); + } - //-------------------------------------------------------------------------- - // MUSCLE INTERFACE - //-------------------------------------------------------------------------- - void computeInitialFiberEquilibrium(SimTK::State& s) const override {} + void computeStateVariableDerivatives(const SimTK::State& s) const override + { + // This implementation is not intended for use in dynamic simulations. + /*const int n = */getNumStateVariables(); + setStateVariableDerivativeValue(s, stateName_fiberLength, 0.0); + setStateVariableDerivativeValue(s, stateName_fiberVelocity, 0.0); + } - void setActivation(SimTK::State& s, double activation) const override - { - #ifdef USE_ACTIVATION_DYNAMICS_MODEL - get_ZerothOrderMuscleActivationDynamics().setActivation(s, activation); - #else - setExcitation(s, activation); - #endif - } + //---------------------------------------------------------------------- + // MUSCLE INTERFACE + //---------------------------------------------------------------------- + void computeInitialFiberEquilibrium(SimTK::State& s) const override {} + + void setActivation(SimTK::State& s, double activation) const override + { + #ifdef USE_ACTIVATION_DYNAMICS_MODEL + get_ZerothOrderMuscleActivationDynamics().setActivation(s, activation); + #else + setExcitation(s, activation); + #endif + } - double computeActuation(const SimTK::State& s) const override - { - const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s); - setActuation(s, mdi.tendonForce); - return mdi.tendonForce; - } + double computeActuation(const SimTK::State& s) const override + { + const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s); + setActuation(s, mdi.tendonForce); + return mdi.tendonForce; + } - // Calculate position-level variables. - void calcMuscleLengthInfo(const SimTK::State& s, MuscleLengthInfo& mli) - const override - { - mli.fiberLength = getStateVariableValue(s, stateName_fiberLength); - mli.fiberLengthAlongTendon = mli.fiberLength; - mli.normFiberLength = mli.fiberLength / getOptimalFiberLength(); - mli.tendonLength = 0; - mli.normTendonLength = 0; - mli.tendonStrain = 0; - mli.pennationAngle = 0; - mli.cosPennationAngle = 1; - mli.sinPennationAngle = 0; - mli.fiberPassiveForceLengthMultiplier = 0; - - // The fiberActiveForceLengthMultiplier (referred to as 'Fisom' in [3]) - // is the proportion of maxIsometricForce that would be delivered - // isometrically at maximal activation. Fisom=1 if Lce=Lceopt. - if (mli.fiberLength < (1 - get_width()) * getOptimalFiberLength() || - mli.fiberLength > (1 + get_width()) * getOptimalFiberLength()) - mli.fiberActiveForceLengthMultiplier = 0; - else { - double c = -1.0 / (get_width() * get_width()); - double t1 = mli.fiberLength / getOptimalFiberLength(); - mli.fiberActiveForceLengthMultiplier = c*t1*(t1-2) + c + 1; + // Calculate position-level variables. + void calcMuscleLengthInfo(const SimTK::State& s, MuscleLengthInfo& mli) + const override + { + mli.fiberLength = getStateVariableValue(s, stateName_fiberLength); + mli.fiberLengthAlongTendon = mli.fiberLength; + mli.normFiberLength = mli.fiberLength / getOptimalFiberLength(); + mli.tendonLength = 0; + mli.normTendonLength = 0; + mli.tendonStrain = 0; + mli.pennationAngle = 0; + mli.cosPennationAngle = 1; + mli.sinPennationAngle = 0; + mli.fiberPassiveForceLengthMultiplier = 0; + + // The fiberActiveForceLengthMultiplier (referred to as 'Fisom' in [3]) + // is the proportion of maxIsometricForce that would be delivered + // isometrically at maximal activation. Fisom=1 if Lce=Lceopt. + if (mli.fiberLength < (1 - get_width()) * getOptimalFiberLength() || + mli.fiberLength > (1 + get_width()) * getOptimalFiberLength()) + mli.fiberActiveForceLengthMultiplier = 0; + else { + double c = -1.0 / (get_width() * get_width()); + double t1 = mli.fiberLength / getOptimalFiberLength(); + mli.fiberActiveForceLengthMultiplier = c*t1*(t1-2) + c + 1; + } } - } - // Calculate velocity-level variables. - void calcFiberVelocityInfo(const SimTK::State& s, FiberVelocityInfo& fvi) - const override - { - fvi.fiberVelocity = getStateVariableValue(s, stateName_fiberVelocity); - fvi.fiberVelocityAlongTendon = fvi.fiberVelocity; - fvi.normFiberVelocity = fvi.fiberVelocity / - (getMaxContractionVelocity() * getOptimalFiberLength()); - - fvi.pennationAngularVelocity = 0; - fvi.tendonVelocity = 0; - fvi.normTendonVelocity = 0; - fvi.fiberForceVelocityMultiplier = 1; - } + // Calculate velocity-level variables. + void calcFiberVelocityInfo(const SimTK::State& s, FiberVelocityInfo& fvi) + const override + { + fvi.fiberVelocity = getStateVariableValue(s, stateName_fiberVelocity); + fvi.fiberVelocityAlongTendon = fvi.fiberVelocity; + fvi.normFiberVelocity = fvi.fiberVelocity / + (getMaxContractionVelocity() * getOptimalFiberLength()); + + fvi.pennationAngularVelocity = 0; + fvi.tendonVelocity = 0; + fvi.normTendonVelocity = 0; + fvi.fiberForceVelocityMultiplier = 1; + } - // Calculate dynamics-level variables. - void calcMuscleDynamicsInfo(const SimTK::State& s, MuscleDynamicsInfo& mdi) - const override - { - #ifdef USE_ACTIVATION_DYNAMICS_MODEL - mdi.activation = - get_ZerothOrderMuscleActivationDynamics().getActivation(s); - #else - mdi.activation = getExcitation(s); - #endif + // Calculate dynamics-level variables. + void calcMuscleDynamicsInfo(const SimTK::State& s, MuscleDynamicsInfo& mdi) + const override + { + #ifdef USE_ACTIVATION_DYNAMICS_MODEL + mdi.activation = + get_ZerothOrderMuscleActivationDynamics().getActivation(s); + #else + mdi.activation = getExcitation(s); + #endif + + // These expressions were obtained by solving the 'Vce' equations in [3] + // for force F, then applying the modifications described in [1]. + // Negative fiber velocity corresponds to concentric contraction. + double ArelStar = pow(mdi.activation,-0.3) * get_Arel(); + if (getFiberVelocity(s) <= 0) { + double v = max(getFiberVelocity(s), + -getMaxContractionVelocity() * getOptimalFiberLength()); + double t1 = get_Brel() * getOptimalFiberLength(); + mdi.fiberForce = (t1*getActiveForceLengthMultiplier(s) + ArelStar*v) + / (t1 - v); + } else { + double c2 = -get_FmaxEccentric() / mdi.activation; + double c3 = (get_FmaxEccentric()-1) * get_Brel() / (mdi.activation * + 2 * (getActiveForceLengthMultiplier(s) + ArelStar)); + double c1 = (get_FmaxEccentric()-1) * c3 / mdi.activation; + mdi.fiberForce = -(getOptimalFiberLength() * (c1 + c2*c3) + + c2*getFiberVelocity(s)) / + (getFiberVelocity(s) + c3*getOptimalFiberLength()); + } + mdi.fiberForce *= getMaxIsometricForce() * mdi.activation; + + mdi.fiberForceAlongTendon = mdi.fiberForce; + mdi.normFiberForce = mdi.fiberForce / getMaxIsometricForce(); + mdi.activeFiberForce = mdi.fiberForce; + mdi.passiveFiberForce = 0; + mdi.tendonForce = mdi.fiberForce; + mdi.normTendonForce = mdi.normFiberForce; + mdi.fiberStiffness = 0; + mdi.fiberStiffnessAlongTendon = 0; + mdi.tendonStiffness = 0; + mdi.fiberActivePower = 0; + mdi.fiberPassivePower = 0; + mdi.tendonPower = 0; + } - // These expressions were obtained by solving the 'Vce' equations in [3] - // for force F, then applying the modifications described in [1]. - // Negative fiber velocity corresponds to concentric contraction. - double ArelStar = pow(mdi.activation,-0.3) * get_Arel(); - if (getFiberVelocity(s) <= 0) { - double v = max(getFiberVelocity(s), - -getMaxContractionVelocity() * getOptimalFiberLength()); - double t1 = get_Brel() * getOptimalFiberLength(); - mdi.fiberForce = (t1*getActiveForceLengthMultiplier(s) + ArelStar*v) - / (t1 - v); - } else { - double c2 = -get_FmaxEccentric() / mdi.activation; - double c3 = (get_FmaxEccentric()-1) * get_Brel() / (mdi.activation * - 2 * (getActiveForceLengthMultiplier(s) + ArelStar)); - double c1 = (get_FmaxEccentric()-1) * c3 / mdi.activation; - mdi.fiberForce = -(getOptimalFiberLength() * (c1 + c2*c3) - + c2*getFiberVelocity(s)) / - (getFiberVelocity(s) + c3*getOptimalFiberLength()); + private: + static const std::string stateName_fiberLength; + static const std::string stateName_fiberVelocity; + }; + const std::string UmbergerMuscle::stateName_fiberLength = "fiber_length"; + const std::string UmbergerMuscle::stateName_fiberVelocity = "fiber_velocity"; + + + //========================================================================== + // CONSTANT-EXCITATION MUSCLE CONTROLLER + //========================================================================== + // Simple controller to maintain all muscle excitations at the same constant + // value. The metabolic probes depend on both excitation and activation. + class ConstantExcitationMuscleController : public Controller { + OpenSim_DECLARE_CONCRETE_OBJECT(ConstantExcitationMuscleController, Controller); + public: + ConstantExcitationMuscleController(double u) : _u(u) {} + + void computeControls(const SimTK::State& s, SimTK::Vector &controls) const + override + { + for (int i=0; i<_model->getMuscles().getSize(); ++i) + controls[i] = _u; } - mdi.fiberForce *= getMaxIsometricForce() * mdi.activation; - - mdi.fiberForceAlongTendon = mdi.fiberForce; - mdi.normFiberForce = mdi.fiberForce / getMaxIsometricForce(); - mdi.activeFiberForce = mdi.fiberForce; - mdi.passiveFiberForce = 0; - mdi.tendonForce = mdi.fiberForce; - mdi.normTendonForce = mdi.normFiberForce; - mdi.fiberStiffness = 0; - mdi.fiberStiffnessAlongTendon = 0; - mdi.tendonStiffness = 0; - mdi.fiberActivePower = 0; - mdi.fiberPassivePower = 0; - mdi.tendonPower = 0; - } -private: - static const std::string stateName_fiberLength; - static const std::string stateName_fiberVelocity; -}; -const std::string UmbergerMuscle::stateName_fiberLength = "fiber_length"; -const std::string UmbergerMuscle::stateName_fiberVelocity = "fiber_velocity"; + void setConstantExcitation(double u) { _u = u; } + private: + double _u; + }; -//============================================================================== -// CONSTANT-EXCITATION MUSCLE CONTROLLER -//============================================================================== -// Simple controller to maintain all muscle excitations at the same constant -// value. The metabolic probes depend on both excitation and activation. -class ConstantExcitationMuscleController : public Controller { -OpenSim_DECLARE_CONCRETE_OBJECT(ConstantExcitationMuscleController, Controller); -public: - ConstantExcitationMuscleController(double u) : _u(u) {} - void computeControls(const SimTK::State& s, SimTK::Vector &controls) const - override + //========================================================================== + // COMPARE UMBERGER PROBE TO PUBLISHED RESULTS + //========================================================================== + // Normalized force, mechanical power, and total energy liberation rate are + // calculated as functions of normalized shortening velocity for the soleus and + // rectus femoris muscles. These data are then compared to either analytical + // expressions or polynomials fit to the results published by Umberger et al. + const int numPoints = 21; + void generateUmbergerMuscleData(const std::string& muscleName, + double maxIsometricForce, + double optimalFiberLength, + double width, + double Arel, + double Brel, + double FmaxEccentric, + double slowTwitchRatio, + double muscleMass, + double* normalizedForce, + double* mechanicalPower, + double* totalEnergyRate) { - for (int i=0; i<_model->getMuscles().getSize(); ++i) - controls[i] = _u; - } - - void setConstantExcitation(double u) { _u = u; } - -private: - double _u; -}; - - -//============================================================================== -// COMPARE UMBERGER PROBE TO PUBLISHED RESULTS -//============================================================================== -// Normalized force, mechanical power, and total energy liberation rate are -// calculated as functions of normalized shortening velocity for the soleus and -// rectus femoris muscles. These data are then compared to either analytical -// expressions or polynomials fit to the results published by Umberger et al. -const int numPoints = 21; -void generateUmbergerMuscleData(const std::string& muscleName, - double maxIsometricForce, - double optimalFiberLength, - double width, - double Arel, - double Brel, - double FmaxEccentric, - double slowTwitchRatio, - double muscleMass, - double* normalizedForce, - double* mechanicalPower, - double* totalEnergyRate) -{ - // Create OpenSim model. - Model model; - model.setName("testModel_"+muscleName); - Ground& ground = model.updGround(); - - // Create block. The length and velocity of the muscle will be specified, so - // the properties of the block are irrelevant. - const double blockMass = 1.0; - const double blockSideLength = 0.1; - Inertia blockInertia = blockMass * Inertia::brick(Vec3(blockSideLength/2)); - OpenSim::Body *block = new OpenSim::Body("block", blockMass, Vec3(0), - blockInertia); - - // Create slider joint between ground and block. - SliderJoint* prismatic = new SliderJoint("prismatic", ground, Vec3(0), Vec3(0), - *block, Vec3(0), Vec3(0)); - prismatic->updCoordinate().setName("xTranslation"); - model.addBody(block); - model.addJoint(prismatic); - - // Create muscle attached to ground and block. - UmbergerMuscle *muscle = new UmbergerMuscle(muscleName, maxIsometricForce, - optimalFiberLength, width, Arel, Brel, FmaxEccentric); - muscle->addNewPathPoint("muscle-ground", ground, Vec3(0)); - muscle->addNewPathPoint("muscle-block", *block, Vec3(0)); - model.addForce(muscle); + // Create OpenSim model. + Model model; + model.setName("testModel_"+muscleName); + Ground& ground = model.updGround(); + + // Create block. The length and velocity of the muscle will be specified, + // so the properties of the block are irrelevant. + const double blockMass = 1.0; + const double blockSideLength = 0.1; + Inertia blockInertia = blockMass * Inertia::brick(Vec3(blockSideLength/2)); + OpenSim::Body *block = new OpenSim::Body("block", blockMass, Vec3(0), + blockInertia); + + // Create slider joint between ground and block. + SliderJoint* prismatic = new SliderJoint("prismatic", ground, Vec3(0), + Vec3(0), *block, Vec3(0), Vec3(0)); + prismatic->updCoordinate().setName("xTranslation"); + model.addBody(block); + model.addJoint(prismatic); + + // Create muscle attached to ground and block. + UmbergerMuscle *muscle = new UmbergerMuscle(muscleName, maxIsometricForce, + optimalFiberLength, width, Arel, Brel, FmaxEccentric); + muscle->addNewPathPoint("muscle-ground", ground, Vec3(0)); + muscle->addNewPathPoint("muscle-block", *block, Vec3(0)); + model.addForce(muscle); + + // Attach muscle controller. + const double constantActivation = 1.0; + ConstantExcitationMuscleController* controller = + new ConstantExcitationMuscleController(constantActivation); + controller->setActuators(model.updActuators()); + model.addController(controller); + + // Attach Umberger probes. Must call addProbe() before addMuscle(). + Umberger2010MuscleMetabolicsProbe* mechanicalPowerProbe = + new Umberger2010MuscleMetabolicsProbe(false, false, false, true); + model.addProbe(mechanicalPowerProbe); + + model.setup(); + mechanicalPowerProbe->setName("mechanicalPowerProbe"); + mechanicalPowerProbe->setOperation("value"); + mechanicalPowerProbe->addMuscle(muscleName, slowTwitchRatio, muscleMass); + + Umberger2010MuscleMetabolicsProbe* totalEnergyRateProbe = + new Umberger2010MuscleMetabolicsProbe(true, true, false, true); + model.addProbe(totalEnergyRateProbe); + totalEnergyRateProbe->setName("totalEnergyRateProbe"); + totalEnergyRateProbe->setOperation("value"); + totalEnergyRateProbe->addMuscle(muscleName, slowTwitchRatio, muscleMass); + totalEnergyRateProbe->set_aerobic_factor(1.0); + + // Initialize. + SimTK::State& state = model.initSystem(); + muscle->setFiberLength(state, muscle->getOptimalFiberLength()); + + // Calculate normalized force, mechanical power [W/kg], and total energy + // liberation rate [W/kg] at numPoints evenly-spaced normalized shortening + // velocities between 0 and 1. + for (int i=0; isetNormFiberVelocity(state, -(double)i/(numPoints-1)); + model.getMultibodySystem().realize(state, SimTK::Stage::Report); + normalizedForce[i] = muscle->getFiberForce(state) + / muscle->getMaxIsometricForce(); + mechanicalPower[i] = mechanicalPowerProbe->computeProbeInputs(state)[0] + / muscleMass; + totalEnergyRate[i] = totalEnergyRateProbe->computeProbeInputs(state)[0] + / muscleMass; + } - // Attach muscle controller. - const double constantActivation = 1.0; - ConstantExcitationMuscleController* controller = - new ConstantExcitationMuscleController(constantActivation); - controller->setActuators(model.updActuators()); - model.addController(controller); + // Print for debugging. + if (DISPLAY_PROBE_OUTPUTS) { + const int w = 16; + cout << "\n\nResults for " << muscleName << " shortening test" << endl; + for (int i=0; i<4*w; ++i) {cout << "=";} cout << endl; + cout << setw(w) << "Velocity" + << setw(w) << "Force" + << setw(w) << "Mech power" + << setw(w) << "Total rate" << endl; + for (int i=0; i<4*w; ++i) {cout << "=";} cout << endl; + for (int i=0; isetName("mechanicalPowerProbe"); - mechanicalPowerProbe->setOperation("value"); - mechanicalPowerProbe->addMuscle(muscleName, slowTwitchRatio, muscleMass); - - Umberger2010MuscleMetabolicsProbe* totalEnergyRateProbe = - new Umberger2010MuscleMetabolicsProbe(true, true, false, true); - model.addProbe(totalEnergyRateProbe); - totalEnergyRateProbe->setName("totalEnergyRateProbe"); - totalEnergyRateProbe->setOperation("value"); - totalEnergyRateProbe->addMuscle(muscleName, slowTwitchRatio, muscleMass); - totalEnergyRateProbe->set_aerobic_factor(1.0); - - // Initialize. - SimTK::State& state = model.initSystem(); - muscle->setFiberLength(state, muscle->getOptimalFiberLength()); - - // Calculate normalized force, mechanical power [W/kg], and total energy - // liberation rate [W/kg] at numPoints evenly-spaced normalized shortening - // velocities between 0 and 1. - for (int i=0; isetNormFiberVelocity(state, -(double)i/(numPoints-1)); - model.getMultibodySystem().realize(state, SimTK::Stage::Report); - normalizedForce[i] = muscle->getFiberForce(state) - / muscle->getMaxIsometricForce(); - mechanicalPower[i] = mechanicalPowerProbe->computeProbeInputs(state)[0] - / muscleMass; - totalEnergyRate[i] = totalEnergyRateProbe->computeProbeInputs(state)[0] - / muscleMass; - } - // Print for debugging. - if (DISPLAY_PROBE_OUTPUTS) { - const int w = 16; - cout << "\n\nResults for " << muscleName << " shortening test" << endl; - for (int i=0; i<4*w; ++i) {cout << "=";} cout << endl; - cout << setw(w) << "Velocity" - << setw(w) << "Force" - << setw(w) << "Mech power" - << setw(w) << "Total rate" << endl; - for (int i=0; i<4*w; ++i) {cout << "=";} cout << endl; - for (int i=0; i failures; - - printf("\n"); horizontalRule(); - cout << "Comparing Umberger2010 probe output to published results" << endl; - horizontalRule(); - try { compareUmbergerProbeToPublishedResults(); - cout << "\ncompareUmbergerProbeToPublishedResults test passed\n" << endl; - } catch (const OpenSim::Exception& e) { - e.print(cerr); - failures.push_back("compareUmbergerProbeToPublishedResults"); - } - - printf("\n"); horizontalRule(); - cout << "Testing Umberger2010 and Bhargava2004 probes in simulation" << endl; - horizontalRule(); - try { testProbesUsingMillardMuscleSimulation(); - cout << "\ntestProbesUsingMillardMuscleSimulation test passed\n" << endl; - } catch (const OpenSim::Exception& e) { - e.print(cerr); - failures.push_back("testProbesUsingMillardMuscleSimulation"); - } - - printf("\n"); horizontalRule(); horizontalRule(); - if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; - return 1; - } - - cout << "testMuscleMetabolicsProbes passed\n" << endl; - return 0; -} diff --git a/OpenSim/Simulation/Test/testNestedModelComponents.cpp b/OpenSim/Simulation/Test/testNestedModelComponents.cpp index 86d6a02d13..17bafbc74b 100644 --- a/OpenSim/Simulation/Test/testNestedModelComponents.cpp +++ b/OpenSim/Simulation/Test/testNestedModelComponents.cpp @@ -40,90 +40,73 @@ Tests Include: #include #include #include +#include using namespace OpenSim; using namespace std; -// Create Device as Concrete Container Component (like Model) of Components -class Device : public ModelComponent { - OpenSim_DECLARE_CONCRETE_OBJECT(Device, ModelComponent); -}; - - -//============================================================================== -// Test Case Driver -//============================================================================== -template -void testPendulumModelWithNestedJoints() -{ - using namespace SimTK; - Vec3 tolerance(SimTK::Eps); - - cout << "Running testPendulumModelWithNestedJoints<" << - typeid(C).name() << ">" << endl; - - // Load the pendulum model - Model* pendulum = new Model("double_pendulum.osim"); - - // Create a new empty device; - C* device = new C(); - device->setName("device"); - - // Build the device - // Create bodies - auto* cuffA = new OpenSim::Body("cuffA", 1.0, Vec3(0), Inertia(0.5)); - auto* cuffB = new OpenSim::Body("cuffB", 1.0, Vec3(0), Inertia(0.5)); - - // add Bodies to the device - device->addComponent(cuffA); - device->addComponent(cuffB); - - // Create WeldJoints to anchor cuff Bodies to the pendulum. - auto* anchorA = new WeldJoint(); - anchorA->setName("anchorA"); - anchorA->connectSocket_child_frame(*cuffA); - - auto* anchorB = new WeldJoint(); - anchorB->setName("anchorB"); - anchorB->connectSocket_child_frame(*cuffB); - - // add anchors to the Device - device->addComponent(anchorA); - device->addComponent(anchorB); - - // add the device to the pendulum model - pendulum->addModelComponent(device); - - // Connect the device to bodies of the pendulum - const auto& rod1 = pendulum->getComponent("bodyset/rod1"); - const auto& rod2 = pendulum->getComponent("bodyset/rod2"); - anchorA->connectSocket_parent_frame(rod1); - anchorB->connectSocket_parent_frame(rod2); - - State& s = pendulum->initSystem(); -} - -int main() -{ - SimTK::Array_ failures; - - try { testPendulumModelWithNestedJoints(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testPendulumModelWithNestedJoints"); +namespace { + // Create Device as Concrete Container Component (like Model) of Components + class Device : public ModelComponent { + OpenSim_DECLARE_CONCRETE_OBJECT(Device, ModelComponent); + }; + + // Test Case Driver + template + void testPendulumModelWithNestedJoints() + { + using namespace SimTK; + Vec3 tolerance(SimTK::Eps); + + cout << "Running testPendulumModelWithNestedJoints<" << + typeid(C).name() << ">" << endl; + + // Load the pendulum model + Model* pendulum = new Model("double_pendulum.osim"); + + // Create a new empty device; + C* device = new C(); + device->setName("device"); + + // Build the device + // Create bodies + auto* cuffA = new OpenSim::Body("cuffA", 1.0, Vec3(0), Inertia(0.5)); + auto* cuffB = new OpenSim::Body("cuffB", 1.0, Vec3(0), Inertia(0.5)); + + // add Bodies to the device + device->addComponent(cuffA); + device->addComponent(cuffB); + + // Create WeldJoints to anchor cuff Bodies to the pendulum. + auto* anchorA = new WeldJoint(); + anchorA->setName("anchorA"); + anchorA->connectSocket_child_frame(*cuffA); + + auto* anchorB = new WeldJoint(); + anchorB->setName("anchorB"); + anchorB->connectSocket_child_frame(*cuffB); + + // add anchors to the Device + device->addComponent(anchorA); + device->addComponent(anchorB); + + // add the device to the pendulum model + pendulum->addModelComponent(device); + + // Connect the device to bodies of the pendulum + const auto& rod1 = pendulum->getComponent("bodyset/rod1"); + const auto& rod2 = pendulum->getComponent("bodyset/rod2"); + anchorA->connectSocket_parent_frame(rod1); + anchorB->connectSocket_parent_frame(rod2); + + State& s = pendulum->initSystem(); } - try { testPendulumModelWithNestedJoints(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testPendulumModelWithNestedJoints"); - } - - if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; - return 1; - } - - cout << "Done. All cases passed." << endl; +} - return 0; +TEST_CASE("Device") { + testPendulumModelWithNestedJoints(); } + +TEST_CASE("Model") { + testPendulumModelWithNestedJoints(); +} \ No newline at end of file diff --git a/OpenSim/Simulation/Test/testPoints.cpp b/OpenSim/Simulation/Test/testPoints.cpp index d54019e871..c2efed7df9 100644 --- a/OpenSim/Simulation/Test/testPoints.cpp +++ b/OpenSim/Simulation/Test/testPoints.cpp @@ -36,55 +36,25 @@ Tests Include: #include #include #include +#include using namespace OpenSim; using namespace std; using SimTK::Transform; -void testStationOnBody(); -void testStationOnOffsetFrame(); +namespace { + class OrdinaryOffsetFrame : public OffsetFrame < Frame > { + OpenSim_DECLARE_CONCRETE_OBJECT(OrdinaryOffsetFrame, OffsetFrame); + public: + OrdinaryOffsetFrame() : OffsetFrame() {} + virtual ~OrdinaryOffsetFrame() {} -class OrdinaryOffsetFrame : public OffsetFrame < Frame > { - OpenSim_DECLARE_CONCRETE_OBJECT(OrdinaryOffsetFrame, OffsetFrame); -public: - OrdinaryOffsetFrame() : OffsetFrame() {} - virtual ~OrdinaryOffsetFrame() {} - - OrdinaryOffsetFrame(const Frame& parent, const SimTK::Transform& offset) : - OffsetFrame(parent, offset) {} -}; - - -int main() -{ - SimTK::Array_ failures; - - try { testStationOnBody(); } - catch (const std::exception& e){ - cout << e.what() << endl; failures.push_back("testStationOnBody"); - } - - try { testStationOnOffsetFrame(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testStationOnOffsetFrame"); - } - - if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; - return 1; - } - - cout << "Done. All cases passed." << endl; - - return 0; + OrdinaryOffsetFrame(const Frame& parent, const SimTK::Transform& offset) : + OffsetFrame(parent, offset) {} + }; } -//============================================================================== -// Test Cases -//============================================================================== - -void testStationOnBody() +TEST_CASE("testStationOnBody") { SimTK::Vec3 tolerance(SimTK::Eps); @@ -127,7 +97,7 @@ void testStationOnBody() } } -void testStationOnOffsetFrame() +TEST_CASE("testStationOnOffsetFrame") { using SimTK::Vec3; SimTK::Vec3 tolerance(SimTK::Eps); diff --git a/OpenSim/Simulation/Test/testPrescribedForce.cpp b/OpenSim/Simulation/Test/testPrescribedForce.cpp index 2c75e67ef9..28fdfd5c05 100644 --- a/OpenSim/Simulation/Test/testPrescribedForce.cpp +++ b/OpenSim/Simulation/Test/testPrescribedForce.cpp @@ -53,143 +53,103 @@ #include #include "SimTKsimbody.h" #include "SimTKmath.h" +#include using namespace OpenSim; using namespace std; -//========================================================================================================== -// Common Parameters for the simulations are just global. -const static double integ_accuracy = 1.0e-6; -const static double duration = 1.0; -const static SimTK::Vec3 gravity_vec = SimTK::Vec3(0, -9.8065, 0); - -SimTK::MassProperties ballMass = SimTK::MassProperties(8.806, SimTK::Vec3(0), SimTK::Inertia(SimTK::Vec3(0.1268, 0.0332, 0.1337))); -//========================================================================================================== -static int counter=0; - -void testNoForce(); -void testForceAtOrigin(); -void testForceAtPoint(); -void testTorque(); - -int main() -{ - SimTK::Array_ failures; - - try { testNoForce(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testNoForce"); - } - try { testForceAtOrigin(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testForceAtOrigin"); - } - try { testForceAtPoint(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testForceAtPoint"); - } - try { testTorque(); } - catch (const std::exception& e) { - cout << e.what() << endl; - failures.push_back("testTorque"); - } - - if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; - return 1; - } - - cout << "testPrescribedForce Done" << endl; - return 0; -} - -//========================================================================================================== -// Test Cases -//========================================================================================================== -void testPrescribedForce(OpenSim::Function* forceX, OpenSim::Function* forceY, OpenSim::Function* forceZ, - OpenSim::Function* pointX, OpenSim::Function* pointY, OpenSim::Function* pointZ, - OpenSim::Function* torqueX, OpenSim::Function* torqueY, OpenSim::Function* torqueZ, - vector& times, vector& accelerations, vector& angularAccelerations) -{ - using namespace SimTK; - - //========================================================================================================== - // Setup OpenSim model - Model *osimModel = new Model; - //OpenSim bodies - const Ground& ground = osimModel->getGround(); - OpenSim::Body ball; - ball.setName("ball"); - ball.setMass(0); - // Add joints - FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0)); - - // Rename coordinates for a free joint - for(int i=0; iaddBody(&ball); - osimModel->addJoint(&free); - - // Add a PrescribedForce. - PrescribedForce force("forceOnBall", ball); - if (forceX != NULL) - force.setForceFunctions(forceX, forceY, forceZ); - if (pointX != NULL) - force.setPointFunctions(pointX, pointY, pointZ); - if (torqueX != NULL) - force.setTorqueFunctions(torqueX, torqueY, torqueZ); - - counter++; - osimModel->updForceSet().append(&force); - - // BAD: have to set memoryOwner to false or program will crash when this test is complete. - osimModel->disownAllComponents(); - - //Set mass - ball.setMass(ballMass.getMass()); - ball.setMassCenter(ballMass.getMassCenter()); - ball.setInertia(ballMass.getInertia()); - - osimModel->setGravity(gravity_vec); - osimModel->finalizeConnections(); - osimModel->print("TestPrescribedForceModel.osim"); - - delete osimModel; - // Check that serialization/deserialization is working correctly as well - osimModel = new Model("TestPrescribedForceModel.osim"); - SimTK::State& osim_state = osimModel->initSystem(); - osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); - - //========================================================================================================== - // Compute the force and torque at the specified times. - - const OpenSim::Body& body = osimModel->getBodySet().get("ball"); - osim_state.setTime(0.0); - Manager manager(*osimModel); - manager.initialize(osim_state); - - for (unsigned int i = 0; i < times.size(); ++i) +namespace { + const static double integ_accuracy = 1.0e-6; + const static double duration = 1.0; + const static SimTK::Vec3 gravity_vec = SimTK::Vec3(0, -9.8065, 0); + + SimTK::MassProperties ballMass = SimTK::MassProperties(8.806, SimTK::Vec3(0), SimTK::Inertia(SimTK::Vec3(0.1268, 0.0332, 0.1337))); + static int counter=0; + + void testPrescribedForce(OpenSim::Function* forceX, OpenSim::Function* forceY, + OpenSim::Function* forceZ, OpenSim::Function* pointX, + OpenSim::Function* pointY, OpenSim::Function* pointZ, + OpenSim::Function* torqueX, OpenSim::Function* torqueY, + OpenSim::Function* torqueZ, vector& times, + vector& accelerations, + vector& angularAccelerations) { - osim_state = manager.integrate(times[i]); - ASSERT_EQUAL(osim_state.getTime(), times[i], SimTK::Eps); - - osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); - Vec3 accel = body.findStationAccelerationInGround(osim_state, Vec3(0)); - Vec3 angularAccel = body.getAccelerationInGround(osim_state)[0]; - - ASSERT_EQUAL(accelerations[i], accel, integ_accuracy); - ASSERT_EQUAL(angularAccelerations[i], angularAccel, integ_accuracy); + using namespace SimTK; + + // Setup OpenSim model + Model *osimModel = new Model; + //OpenSim bodies + const Ground& ground = osimModel->getGround(); + OpenSim::Body ball; + ball.setName("ball"); + ball.setMass(0); + // Add joints + FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0)); + + // Rename coordinates for a free joint + for(int i=0; iaddBody(&ball); + osimModel->addJoint(&free); + + // Add a PrescribedForce. + PrescribedForce force("forceOnBall", ball); + if (forceX != NULL) + force.setForceFunctions(forceX, forceY, forceZ); + if (pointX != NULL) + force.setPointFunctions(pointX, pointY, pointZ); + if (torqueX != NULL) + force.setTorqueFunctions(torqueX, torqueY, torqueZ); + + counter++; + osimModel->updForceSet().append(&force); + + // BAD: have to set memoryOwner to false or program will crash when this + // test is complete. + osimModel->disownAllComponents(); + + //Set mass + ball.setMass(ballMass.getMass()); + ball.setMassCenter(ballMass.getMassCenter()); + ball.setInertia(ballMass.getInertia()); + + osimModel->setGravity(gravity_vec); + osimModel->finalizeConnections(); + osimModel->print("TestPrescribedForceModel.osim"); + + delete osimModel; + // Check that serialization/deserialization is working correctly as well + osimModel = new Model("TestPrescribedForceModel.osim"); + SimTK::State& osim_state = osimModel->initSystem(); + osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); + + // Compute the force and torque at the specified times. + + const OpenSim::Body& body = osimModel->getBodySet().get("ball"); + osim_state.setTime(0.0); + Manager manager(*osimModel); + manager.initialize(osim_state); + + for (unsigned int i = 0; i < times.size(); ++i) + { + osim_state = manager.integrate(times[i]); + ASSERT_EQUAL(osim_state.getTime(), times[i], SimTK::Eps); + + osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); + Vec3 accel = body.findStationAccelerationInGround(osim_state, Vec3(0)); + Vec3 angularAccel = body.getAccelerationInGround(osim_state)[0]; + + ASSERT_EQUAL(accelerations[i], accel, integ_accuracy); + ASSERT_EQUAL(angularAccelerations[i], angularAccel, integ_accuracy); + } } } -void testNoForce() +TEST_CASE("testNoForce") { using namespace SimTK; @@ -208,7 +168,7 @@ void testNoForce() testPrescribedForce(NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, times, accel, angularAccel); } -void testForceAtOrigin() +TEST_CASE("testForceAtOrigin") { using namespace SimTK; @@ -233,7 +193,7 @@ void testForceAtOrigin() testPrescribedForce(forceX, forceY, forceZ, NULL, NULL, NULL, NULL, NULL, NULL, times, accel, angularAccel); } -void testForceAtPoint() +TEST_CASE("testForceAtPoint") { using namespace SimTK; @@ -255,7 +215,7 @@ void testForceAtPoint() testPrescribedForce(forceX, forceY, forceZ, pointX, pointY, pointZ, NULL, NULL, NULL, times, accel, angularAccel); } -void testTorque() +TEST_CASE("testTorque") { using namespace SimTK; diff --git a/OpenSim/Simulation/Test/testProbes.cpp b/OpenSim/Simulation/Test/testProbes.cpp index f8d9441264..873e9a3acc 100644 --- a/OpenSim/Simulation/Test/testProbes.cpp +++ b/OpenSim/Simulation/Test/testProbes.cpp @@ -43,67 +43,516 @@ #include #include +#include using namespace OpenSim; using namespace std; -//============================================================================== -static const double IntegrationAccuracy = 1e-8; -static const double SimulationTestTolerance = 1e-6; -static const double InitializationTestTolerance = 1e-8; -static const double CorrectnessTestTolerance = 1e-8; - -static const int SimulationTest = 0; -static const int InitializationTest = 1; -static const int CorrectnessTest = 2; - -// MUSCLE CONSTANTS -static const double MaxIsometricForce0 = 100.0, -OptimalFiberLength0 = 0.1, -TendonSlackLength0 = 0.2, - PennationAngle0 = 0.0; -// PennationAngle1 = SimTK::Pi / 4; - -static const double Activation0 = 0.01, -Deactivation0 = 0.4, -ShutteDelpActivation1 = 7.6, -ShutteDelpActivation2 = 2.5; - -/* -This function completes a controlled activation, controlled stretch simulation -of a muscle. After the simulation has completed, the results can be -tested in a number of different ways to ensure that the muscle model is -functioning - -@param aMuscle a path actuator -@param startX the starting position of the muscle anchor. I have no idea -why this value is included. -@param act0 the initial i of the muscle -@param motion the forced stretch of the simulation -@param control the activation control signal that is applied to the muscle -@param accuracy the desired accuracy of the integrated solution -@param testType 0: No test, just simulate the muscle -1: Initialization test -2: Correctness test: ensure that d/dt(KE+PE-W) = 0 -@param testTolerance the desired tolerance associated with the test -@param printResults print the osim model associated with this test. -*/ -void simulateMuscle(const Muscle &aMuscle, - double startX, - double act0, - const Function *motion, - const Function *control, - double integrationAccuracy, - int testType, - double testTolerance, - bool printResults); - - -int main() -{ - SimTK::Array_ failures; +namespace { + static const double IntegrationAccuracy = 1e-8; + static const double SimulationTestTolerance = 1e-6; + static const double InitializationTestTolerance = 1e-8; + static const double CorrectnessTestTolerance = 1e-8; + + static const int SimulationTest = 0; + static const int InitializationTest = 1; + static const int CorrectnessTest = 2; + + // MUSCLE CONSTANTS + static const double MaxIsometricForce0 = 100.0, + OptimalFiberLength0 = 0.1, + TendonSlackLength0 = 0.2, + PennationAngle0 = 0.0; + // PennationAngle1 = SimTK::Pi / 4; + + static const double Activation0 = 0.01, + Deactivation0 = 0.4, + ShutteDelpActivation1 = 7.6, + ShutteDelpActivation2 = 2.5; + + + /*========================================================================== + Main test driver to be used on any muscle model (derived from Muscle) so new + cases should be easy to add currently, the test only verifies that the work + done by the muscle corresponds to the change in system energy. + + TODO: Test will fail with prescribed motion until the work done by this + constraint is accounted for. + ============================================================================ + This function completes a controlled activation, controlled stretch simulation + of a muscle. After the simulation has completed, the results can be + tested in a number of different ways to ensure that the muscle model is + functioning + + @param aMuscle a path actuator + @param startX the starting position of the muscle anchor. I have no idea + why this value is included. + @param act0 the initial i of the muscle + @param motion the forced stretch of the simulation + @param control the activation control signal that is applied to the muscle + @param accuracy the desired accuracy of the integrated solution + @param testType 0: No test, just simulate the muscle + 1: Initialization test + 2: Correctness test: ensure that d/dt(KE+PE-W) = 0 + @param testTolerance the desired tolerance associated with the test + @param printResults print the osim model associated with this test. + */ + void simulateMuscle( + const Muscle &aMuscModel, + double startX, + double act0, + const Function *motion, // prescribe motion of free end of muscle + const Function *control, // prescribed excitation signal to the muscle + double integrationAccuracy, + int testType, + double testTolerance, + bool printResults) + { + string prescribed = (motion == NULL) ? "." : " with Prescribed Motion."; + + cout << "\n******************************************************" << endl; + cout << "Test " << aMuscModel.getConcreteClassName() + << " Model" << prescribed << endl; + cout << "******************************************************" << endl; + using SimTK::Vec3; + + //====================================================================== + // 0. SIMULATION SETUP: Create the block and ground + //====================================================================== + + // Define the initial and final simulation times + double initialTime = 0.0; + double finalTime = 4.0; + + //Physical properties of the model + double ballMass = 10; + double ballRadius = 0.05; + double anchorWidth = 0.1; + + // Create an OpenSim model + Model model; + + double optimalFiberLength = aMuscModel.getOptimalFiberLength(); + double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength(); + double tendonSlackLength = aMuscModel.getTendonSlackLength(); + + // Use a copy of the muscle model passed in to add path points later + PathActuator *aMuscle = aMuscModel.clone(); + + // Get a reference to the model's ground body + Ground& ground = model.updGround(); + + OpenSim::Body * ball = new OpenSim::Body("ball", + ballMass, + Vec3(0), + ballMass*SimTK::Inertia::sphere(ballRadius)); + + ball->attachGeometry(new Sphere(ballRadius)); + // ball connected to ground via a slider along X + double xSinG = optimalFiberLength*cos(pennationAngle) + tendonSlackLength; + + SliderJoint* slider = new SliderJoint("slider", + ground, + Vec3(anchorWidth / 2 + xSinG, 0, 0), + Vec3(0), + *ball, + Vec3(0), + Vec3(0)); + + auto& sliderCoord = slider->updCoordinate(); + sliderCoord.setName("tx"); + sliderCoord.setDefaultValue(1.0); + sliderCoord.setRangeMin(0); + sliderCoord.setRangeMax(1.0); + + if (motion != NULL){ + sliderCoord.setPrescribedFunction(*motion); + sliderCoord.setDefaultIsPrescribed(true); + } + // add ball to model + model.addBody(ball); + model.addJoint(slider); + + + //====================================================================== + // 1. SIMULATION SETUP: Add the muscle + //====================================================================== + + //Attach the muscle + /*const string &actuatorType = */aMuscle->getConcreteClassName(); + aMuscle->setName("muscle"); + aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth / 2, 0, 0)); + aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius, 0, 0)); + + ActivationFiberLengthMuscle_Deprecated *aflMuscle + = dynamic_cast(aMuscle); + if (aflMuscle){ + // Define the default states for the muscle that has + //activation and fiber-length states + aflMuscle->setDefaultActivation(act0); + aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength()); + } + else{ + ActivationFiberLengthMuscle *aflMuscle2 + = dynamic_cast(aMuscle); + if (aflMuscle2){ + // Define the default states for the muscle + //that has activation and fiber-length states + aflMuscle2->setDefaultActivation(act0); + aflMuscle2->setDefaultFiberLength(aflMuscle2 + ->getOptimalFiberLength()); + } + } + + model.addForce(aMuscle); + + // Create a prescribed controller that simply + //applies controls as function of time + PrescribedController* muscleController = new PrescribedController(); + if (control != NULL){ + muscleController->setActuators(model.updActuators()); + // Set the individual muscle control functions + //for the prescribed muscle controller + muscleController->prescribeControlForActuator("muscle", *control->clone()); + + // Add the control set controller to the model + model.addController(muscleController); + } + + // Set names for muscles / joints. + Array muscNames; + muscNames.append(aMuscle->getName()); + Array jointNames; + jointNames.append("slider"); + + //====================================================================== + // 2. SIMULATION SETUP: Instrument the test with probes + //====================================================================== + + Array muscNamesTwice = muscNames; + muscNamesTwice.append(muscNames.get(0)); + cout << "------------\nPROBES\n------------" << endl; + int probeCounter = 1; + + // Add ActuatorPowerProbe to measure work done by the muscle + ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1); + //muscWorkProbe->setName("ActuatorWork"); + muscWorkProbe->setOperation("integrate"); + SimTK::Vector ic1(1); + ic1 = 9.0; // some arbitrary initial condition. + muscWorkProbe->setInitialConditions(ic1); + model.addProbe(muscWorkProbe); + model.setup(); + cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl; + + // Add ActuatorPowerProbe to measure power generated by the muscle + ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor + muscPowerProbe->setName("ActuatorPower"); + muscPowerProbe->setOperation("value"); + model.addProbe(muscPowerProbe); + cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl; + + // Add ActuatorPowerProbe to report the muscle power MINIMUM + ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor + powerProbeMinimum->setName("ActuatorPowerMinimum"); + powerProbeMinimum->setOperation("minimum"); + model.addProbe(powerProbeMinimum); + cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl; + + // Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM + ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor + powerProbeMinAbs->setName("ActuatorPowerMinAbs"); + powerProbeMinAbs->setOperation("minabs"); + model.addProbe(powerProbeMinAbs); + cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl; + + // Add ActuatorPowerProbe to report the muscle power MAXIMUM + ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor + powerProbeMaximum->setName("ActuatorPowerMaximum"); + powerProbeMaximum->setOperation("maximum"); + model.addProbe(powerProbeMaximum); + cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl; + + // Add ActuatorPowerProbe to report the muscle power MAXABS + ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor + powerProbeMaxAbs->setName("ActuatorPowerMaxAbs"); + powerProbeMaxAbs->setOperation("maxabs"); + model.addProbe(powerProbeMaxAbs); + cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl; + + + // Add ActuatorPowerProbe to measure the square of the power generated by the muscle + ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor + muscPowerSquaredProbe->setName("ActuatorPowerSquared"); + muscPowerSquaredProbe->setExponent(2.0); + model.addProbe(muscPowerSquaredProbe); + cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl; + + // Add JointInternalPowerProbe to measure work done by the joint + JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1); + jointWorkProbe->setName("JointWork"); + jointWorkProbe->setOperation("integrate"); + jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0)); + model.addProbe(jointWorkProbe); + cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl; + + // Add JointPowerProbe to measure power generated by the joint + JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor + jointPowerProbe->setName("JointPower"); + jointPowerProbe->setOperation("value"); + model.addProbe(jointPowerProbe); + cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl; + + // Add ActuatorForceProbe to measure the impulse of the muscle force + ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1); + impulseProbe->setName("ActuatorImpulse"); + impulseProbe->setOperation("integrate"); + impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0)); + model.addProbe(impulseProbe); + cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl; + + // Add ActuatorForceProbe to report the muscle force + ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor + forceProbe->setName("ActuatorForce"); + forceProbe->setOperation("value"); + model.addProbe(forceProbe); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl; + + // Add ActuatorForceProbe to report the square of the muscle force + ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor + forceSquaredProbe->setName("ActuatorForceSquared"); + forceSquaredProbe->setExponent(2.0); + model.addProbe(forceSquaredProbe); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl; + + // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice + ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor + forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice"); + forceSquaredProbeTwice->setSumForcesTogether(true); + forceSquaredProbeTwice->setActuatorNames(muscNamesTwice); + model.addProbe(forceSquaredProbeTwice); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl; + + // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5 + ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor + forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved"); + double gain1 = 0.5; + forceSquaredProbeTwiceScaled->setGain(gain1); + model.addProbe(forceSquaredProbeTwiceScaled); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl; + + // Add ActuatorForceProbe to report -3.5X the muscle force + double gain2 = -3.50; + ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor + forceProbeScale->setName("ScaleActuatorForce"); + forceProbeScale->setOperation("value"); + forceProbeScale->setGain(gain2); + model.addProbe(forceProbeScale); + cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl; + + // Add ActuatorForceProbe to report the differentiated muscle force + ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor + forceProbeDiff->setName("DifferentiateActuatorForce"); + forceProbeDiff->setOperation("differentiate"); + model.addProbe(forceProbeDiff); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl; + + // Add SystemEnergyProbe to measure the system KE+PE + SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true); + sysEnergyProbe->setName("SystemEnergy"); + sysEnergyProbe->setOperation("value"); + sysEnergyProbe->setComputeKineticEnergy(true); + sysEnergyProbe->setComputePotentialEnergy(true); + model.addProbe(sysEnergyProbe); + cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl; + + // Add SystemEnergyProbe to measure system power (d/dt system KE+PE) + SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor + sysPowerProbe->setName("SystemPower"); + sysPowerProbe->setEnabled(true); + sysPowerProbe->setOperation("differentiate"); + model.addProbe(sysPowerProbe); + cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl; + + // Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS + ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor + forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR"); + forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually + forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice); + //cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl; + forceSquaredProbeTwiceReportedIndividually1->setOperation("value"); + model.addProbe(forceSquaredProbeTwiceReportedIndividually1); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl; + + // Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS + ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor + forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR"); + forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually + forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate"); + model.addProbe(forceSquaredProbeTwiceReportedIndividually2); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; + + // Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS + ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor + forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR"); + forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually + forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate"); + SimTK::Vector initCondVec(2); + initCondVec(0) = 0; + initCondVec(1) = 10; + forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec); + model.addProbe(forceSquaredProbeTwiceReportedIndividually3); + cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; + cout << "initCondVec = " << initCondVec << endl; + + /* Since all components are allocated on the stack don't have model + own them (and try to free)*/ + // model.disownAllComponents(); + model.setName("testProbesModel"); + cout << "Saving model... " << endl; + model.print("testProbesModel.osim"); + cout << "Re-loading model... " << endl; + Model reloadedModel = Model("testProbesModel.osim"); + + /* Setup analyses and reporters. */ + ProbeReporter* probeReporter = new ProbeReporter(&model); + model.addAnalysis(probeReporter); + ForceReporter* forceReporter = new ForceReporter(&model); + model.addAnalysis(forceReporter); + MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model); + model.addAnalysis(muscleReporter); + model.print("testProbesModel.osim"); + model.finalizeFromProperties(); + model.printBasicInfo(); + + + + //====================================================================== + // 3. SIMULATION Initialization + //====================================================================== + + // Initialize the system and get the default state + SimTK::State& si = model.initSystem(); + SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si); + + model.getMultibodySystem().realize(si, SimTK::Stage::Dynamics); + model.equilibrateMuscles(si); + + CoordinateSet& modelCoordinateSet = model.updCoordinateSet(); + + // Define non-zero (defaults are 0) states for the free joint + // set x-translation value + modelCoordinateSet[0].setValue(si, startX, true); + + //Copy the initial state + SimTK::State initialState(si); + + // Check muscle is setup correctly + const PathActuator &muscle + = dynamic_cast(model.updActuators().get("muscle")); + double length = muscle.getLength(si); + double trueLength = startX + xSinG - anchorWidth / 2; + + ASSERT_EQUAL(length / trueLength, 1.0, testTolerance, __FILE__, __LINE__, + "testMuscles: path failed to initialize to correct length."); + + model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); + + double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0); + log_debug("Muscle initial energy = {}", Emuscle0); + double Esys0 = model.getMultibodySystem().calcEnergy(si); + Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0)); + double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si); + log_debug("Total system initial energy = {}", Esys0); + log_debug("System potential energy = {}", PEsys0); + + //====================================================================== + // 4. SIMULATION Integration + //====================================================================== + + // Create the manager + Manager manager(model); + manager.setIntegratorAccuracy(integrationAccuracy); + + // Integrate from initial time to final time + si.setTime(initialTime); + cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl; + + // Start timing the simulation + const clock_t start = clock(); + // simulate + manager.initialize(si); + manager.integrate(finalTime); + + // how long did it take? + double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC; + + + + //====================================================================== + // 5. SIMULATION Reporting + //====================================================================== + + double realTimeMultiplier = ((finalTime - initialTime) / comp_time); + printf("testMuscles: Realtime Multiplier: %f\n" + " : simulation duration / clock duration\n" + " > 1 : faster than real time\n" + " = 1 : real time\n" + " < 1 : slower than real time\n", + realTimeMultiplier); + + /* + ASSERT(comp_time <= (finalTime-initialTime)); + printf("testMuscles: PASSED Realtime test\n" + " %s simulation time: %f with accuracy %f\n\n", + actuatorType.c_str(), comp_time , accuracy); + */ + + //An analysis only writes to a dir that exists, so create here. + if (printResults == true){ + Storage states(manager.getStateStorage()); + states.print("testProbes_states.sto"); + probeReporter->getProbeStorage().print("testProbes_probes.sto"); + forceReporter->getForceStorage().print("testProbes_forces.sto"); + muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto"); + cout << "\nDone with printing results..." << endl; + } + + double muscleWork = muscWorkProbe->getProbeOutputs(si)(0); + cout << "Muscle work = " << muscleWork << endl; + + + // Test the resetting of probes + cout << "Resetting muscle work probe..." << endl; + muscWorkProbe->reset(si); + muscleWork = muscWorkProbe->getProbeOutputs(si)(0); + cout << "Muscle work = " << muscleWork << endl; + ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe."); + + + //====================================================================== + // 6. SIMULATION Tests + //====================================================================== + model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); + ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); + ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); + ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe."); + ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe."); + ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2 * pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe."); + for (int i = 0; iattachGeometry(new Sphere(ballRadius)); - // ball connected to ground via a slider along X - double xSinG = optimalFiberLength*cos(pennationAngle) + tendonSlackLength; - - SliderJoint* slider = new SliderJoint("slider", - ground, - Vec3(anchorWidth / 2 + xSinG, 0, 0), - Vec3(0), - *ball, - Vec3(0), - Vec3(0)); - - auto& sliderCoord = slider->updCoordinate(); - sliderCoord.setName("tx"); - sliderCoord.setDefaultValue(1.0); - sliderCoord.setRangeMin(0); - sliderCoord.setRangeMax(1.0); - - if (motion != NULL){ - sliderCoord.setPrescribedFunction(*motion); - sliderCoord.setDefaultIsPrescribed(true); - } - // add ball to model - model.addBody(ball); - model.addJoint(slider); - - - //========================================================================== - // 1. SIMULATION SETUP: Add the muscle - //========================================================================== - - //Attach the muscle - /*const string &actuatorType = */aMuscle->getConcreteClassName(); - aMuscle->setName("muscle"); - aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth / 2, 0, 0)); - aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius, 0, 0)); - - ActivationFiberLengthMuscle_Deprecated *aflMuscle - = dynamic_cast(aMuscle); - if (aflMuscle){ - // Define the default states for the muscle that has - //activation and fiber-length states - aflMuscle->setDefaultActivation(act0); - aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength()); - } - else{ - ActivationFiberLengthMuscle *aflMuscle2 - = dynamic_cast(aMuscle); - if (aflMuscle2){ - // Define the default states for the muscle - //that has activation and fiber-length states - aflMuscle2->setDefaultActivation(act0); - aflMuscle2->setDefaultFiberLength(aflMuscle2 - ->getOptimalFiberLength()); - } - } - - model.addForce(aMuscle); - - // Create a prescribed controller that simply - //applies controls as function of time - PrescribedController* muscleController = new PrescribedController(); - if (control != NULL){ - muscleController->setActuators(model.updActuators()); - // Set the individual muscle control functions - //for the prescribed muscle controller - muscleController->prescribeControlForActuator("muscle", *control->clone()); - - // Add the control set controller to the model - model.addController(muscleController); - } - - // Set names for muscles / joints. - Array muscNames; - muscNames.append(aMuscle->getName()); - Array jointNames; - jointNames.append("slider"); - - //========================================================================== - // 2. SIMULATION SETUP: Instrument the test with probes - //========================================================================== - - Array muscNamesTwice = muscNames; - muscNamesTwice.append(muscNames.get(0)); - cout << "------------\nPROBES\n------------" << endl; - int probeCounter = 1; - - // Add ActuatorPowerProbe to measure work done by the muscle - ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1); - //muscWorkProbe->setName("ActuatorWork"); - muscWorkProbe->setOperation("integrate"); - SimTK::Vector ic1(1); - ic1 = 9.0; // some arbitrary initial condition. - muscWorkProbe->setInitialConditions(ic1); - model.addProbe(muscWorkProbe); - model.setup(); - cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl; - - // Add ActuatorPowerProbe to measure power generated by the muscle - ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor - muscPowerProbe->setName("ActuatorPower"); - muscPowerProbe->setOperation("value"); - model.addProbe(muscPowerProbe); - cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl; - - // Add ActuatorPowerProbe to report the muscle power MINIMUM - ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor - powerProbeMinimum->setName("ActuatorPowerMinimum"); - powerProbeMinimum->setOperation("minimum"); - model.addProbe(powerProbeMinimum); - cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl; - - // Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM - ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor - powerProbeMinAbs->setName("ActuatorPowerMinAbs"); - powerProbeMinAbs->setOperation("minabs"); - model.addProbe(powerProbeMinAbs); - cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl; - - // Add ActuatorPowerProbe to report the muscle power MAXIMUM - ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor - powerProbeMaximum->setName("ActuatorPowerMaximum"); - powerProbeMaximum->setOperation("maximum"); - model.addProbe(powerProbeMaximum); - cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl; - - // Add ActuatorPowerProbe to report the muscle power MAXABS - ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor - powerProbeMaxAbs->setName("ActuatorPowerMaxAbs"); - powerProbeMaxAbs->setOperation("maxabs"); - model.addProbe(powerProbeMaxAbs); - cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl; - - - // Add ActuatorPowerProbe to measure the square of the power generated by the muscle - ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor - muscPowerSquaredProbe->setName("ActuatorPowerSquared"); - muscPowerSquaredProbe->setExponent(2.0); - model.addProbe(muscPowerSquaredProbe); - cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl; - - // Add JointInternalPowerProbe to measure work done by the joint - JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1); - jointWorkProbe->setName("JointWork"); - jointWorkProbe->setOperation("integrate"); - jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0)); - model.addProbe(jointWorkProbe); - cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl; - - // Add JointPowerProbe to measure power generated by the joint - JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor - jointPowerProbe->setName("JointPower"); - jointPowerProbe->setOperation("value"); - model.addProbe(jointPowerProbe); - cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl; - - // Add ActuatorForceProbe to measure the impulse of the muscle force - ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1); - impulseProbe->setName("ActuatorImpulse"); - impulseProbe->setOperation("integrate"); - impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0)); - model.addProbe(impulseProbe); - cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl; - - // Add ActuatorForceProbe to report the muscle force - ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor - forceProbe->setName("ActuatorForce"); - forceProbe->setOperation("value"); - model.addProbe(forceProbe); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl; - - // Add ActuatorForceProbe to report the square of the muscle force - ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor - forceSquaredProbe->setName("ActuatorForceSquared"); - forceSquaredProbe->setExponent(2.0); - model.addProbe(forceSquaredProbe); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl; - - // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice - ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor - forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice"); - forceSquaredProbeTwice->setSumForcesTogether(true); - forceSquaredProbeTwice->setActuatorNames(muscNamesTwice); - model.addProbe(forceSquaredProbeTwice); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl; - - // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5 - ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor - forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved"); - double gain1 = 0.5; - forceSquaredProbeTwiceScaled->setGain(gain1); - model.addProbe(forceSquaredProbeTwiceScaled); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl; - - // Add ActuatorForceProbe to report -3.5X the muscle force - double gain2 = -3.50; - ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor - forceProbeScale->setName("ScaleActuatorForce"); - forceProbeScale->setOperation("value"); - forceProbeScale->setGain(gain2); - model.addProbe(forceProbeScale); - cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl; - - // Add ActuatorForceProbe to report the differentiated muscle force - ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor - forceProbeDiff->setName("DifferentiateActuatorForce"); - forceProbeDiff->setOperation("differentiate"); - model.addProbe(forceProbeDiff); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl; - - // Add SystemEnergyProbe to measure the system KE+PE - SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true); - sysEnergyProbe->setName("SystemEnergy"); - sysEnergyProbe->setOperation("value"); - sysEnergyProbe->setComputeKineticEnergy(true); - sysEnergyProbe->setComputePotentialEnergy(true); - model.addProbe(sysEnergyProbe); - cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl; - - // Add SystemEnergyProbe to measure system power (d/dt system KE+PE) - SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor - sysPowerProbe->setName("SystemPower"); - sysPowerProbe->setEnabled(true); - sysPowerProbe->setOperation("differentiate"); - model.addProbe(sysPowerProbe); - cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl; - - // Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS - ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor - forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR"); - forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually - forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice); - //cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl; - forceSquaredProbeTwiceReportedIndividually1->setOperation("value"); - model.addProbe(forceSquaredProbeTwiceReportedIndividually1); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl; - - // Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS - ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor - forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR"); - forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually - forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate"); - model.addProbe(forceSquaredProbeTwiceReportedIndividually2); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; - - // Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS - ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor - forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR"); - forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually - forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate"); - SimTK::Vector initCondVec(2); - initCondVec(0) = 0; - initCondVec(1) = 10; - forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec); - model.addProbe(forceSquaredProbeTwiceReportedIndividually3); - cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; - cout << "initCondVec = " << initCondVec << endl; - - /* Since all components are allocated on the stack don't have model - own them (and try to free)*/ - // model.disownAllComponents(); - model.setName("testProbesModel"); - cout << "Saving model... " << endl; - model.print("testProbesModel.osim"); - cout << "Re-loading model... " << endl; - Model reloadedModel = Model("testProbesModel.osim"); - - /* Setup analyses and reporters. */ - ProbeReporter* probeReporter = new ProbeReporter(&model); - model.addAnalysis(probeReporter); - ForceReporter* forceReporter = new ForceReporter(&model); - model.addAnalysis(forceReporter); - MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model); - model.addAnalysis(muscleReporter); - model.print("testProbesModel.osim"); - model.finalizeFromProperties(); - model.printBasicInfo(); - - - - //========================================================================== - // 3. SIMULATION Initialization - //========================================================================== - - // Initialize the system and get the default state - SimTK::State& si = model.initSystem(); - SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si); - - model.getMultibodySystem().realize(si, SimTK::Stage::Dynamics); - model.equilibrateMuscles(si); - - CoordinateSet& modelCoordinateSet = model.updCoordinateSet(); - - // Define non-zero (defaults are 0) states for the free joint - // set x-translation value - modelCoordinateSet[0].setValue(si, startX, true); - - //Copy the initial state - SimTK::State initialState(si); - - // Check muscle is setup correctly - const PathActuator &muscle - = dynamic_cast(model.updActuators().get("muscle")); - double length = muscle.getLength(si); - double trueLength = startX + xSinG - anchorWidth / 2; - - ASSERT_EQUAL(length / trueLength, 1.0, testTolerance, __FILE__, __LINE__, - "testMuscles: path failed to initialize to correct length."); - - model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); - - double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0); - log_debug("Muscle initial energy = {}", Emuscle0); - double Esys0 = model.getMultibodySystem().calcEnergy(si); - Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0)); - double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si); - log_debug("Total system initial energy = {}", Esys0); - log_debug("System potential energy = {}", PEsys0); - - //========================================================================== - // 4. SIMULATION Integration - //========================================================================== - - // Create the manager - Manager manager(model); - manager.setIntegratorAccuracy(integrationAccuracy); - - // Integrate from initial time to final time - si.setTime(initialTime); - cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl; - - // Start timing the simulation - const clock_t start = clock(); - // simulate - manager.initialize(si); - manager.integrate(finalTime); - - // how long did it take? - double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC; - - - - //========================================================================== - // 5. SIMULATION Reporting - //========================================================================== - - double realTimeMultiplier = ((finalTime - initialTime) / comp_time); - printf("testMuscles: Realtime Multiplier: %f\n" - " : simulation duration / clock duration\n" - " > 1 : faster than real time\n" - " = 1 : real time\n" - " < 1 : slower than real time\n", - realTimeMultiplier); - - /* - ASSERT(comp_time <= (finalTime-initialTime)); - printf("testMuscles: PASSED Realtime test\n" - " %s simulation time: %f with accuracy %f\n\n", - actuatorType.c_str(), comp_time , accuracy); - */ - - //An analysis only writes to a dir that exists, so create here. - if (printResults == true){ - Storage states(manager.getStateStorage()); - states.print("testProbes_states.sto"); - probeReporter->getProbeStorage().print("testProbes_probes.sto"); - forceReporter->getForceStorage().print("testProbes_forces.sto"); - muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto"); - cout << "\nDone with printing results..." << endl; - } - - double muscleWork = muscWorkProbe->getProbeOutputs(si)(0); - cout << "Muscle work = " << muscleWork << endl; - - - // Test the resetting of probes - cout << "Resetting muscle work probe..." << endl; - muscWorkProbe->reset(si); - muscleWork = muscWorkProbe->getProbeOutputs(si)(0); - cout << "Muscle work = " << muscleWork << endl; - ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe."); - - - - - - //========================================================================== - // 6. SIMULATION Tests - //========================================================================== - model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); - ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); - ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); - ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe."); - ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe."); - ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2 * pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe."); - for (int i = 0; i #include #include +#include using namespace std; using namespace SimTK; using namespace OpenSim; -void testConsoleReporterLabels() { +TEST_CASE("testConsoleReporterLabels") { // Create a model consisting of a falling ball. Model model; model.setName("world"); @@ -82,7 +83,7 @@ void testConsoleReporterLabels() { SimTK_TEST(idxHeading2 < idxHeading3); } -void testTableReporterLabels() { +TEST_CASE("testTableReporterLabels") { // Create a model consisting of a falling ball. Model model; model.setName("world"); @@ -115,10 +116,3 @@ void testTableReporterLabels() { SimTK_TEST(headings[0] == "/jointset/slider/sliderCoord|value"); SimTK_TEST(headings[1] == "height"); } - -int main() { - SimTK_START_TEST("testReporters"); - SimTK_SUBTEST(testConsoleReporterLabels); - SimTK_SUBTEST(testTableReporterLabels); - SimTK_END_TEST(); -}; diff --git a/OpenSim/Simulation/Test/testSimulationUtilities.cpp b/OpenSim/Simulation/Test/testSimulationUtilities.cpp index 4dc7cb895c..e4ab1580b0 100644 --- a/OpenSim/Simulation/Test/testSimulationUtilities.cpp +++ b/OpenSim/Simulation/Test/testSimulationUtilities.cpp @@ -26,22 +26,14 @@ #include #include #include +#include using namespace OpenSim; using namespace std; -void testUpdatePre40KinematicsFor40MotionType(); - -int main() { - LoadOpenSimLibrary("osimActuators"); - - SimTK_START_TEST("testSimulationUtilities"); - SimTK_SUBTEST(testUpdatePre40KinematicsFor40MotionType); - SimTK_END_TEST(); -} // Ensure the simulate() method works as intended. -void testSimulate() { +TEST_CASE("testSimulate") { cout << "Running testSimulate" << endl; using SimTK::Vec3; @@ -82,11 +74,11 @@ void testSimulate() { } } -void testUpdatePre40KinematicsFor40MotionType() { +TEST_CASE("testUpdatePre40KinematicsFor40MotionType") { // The model and motion files for this test are from the opensim-models // repository. This PR is related to issues #2240 and #2088. - + LoadOpenSimLibrary("osimActuators"); Model model("testSimulationUtilities_leg6dof9musc_20303.osim"); // Ensure the model file has an inconsistent motion type @@ -173,23 +165,3 @@ void testUpdatePre40KinematicsFor40MotionType() { Exception); } } - - - - - - - - - - - - - - - - - - - - diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index 649eb310bf..f03ba42c91 100644 --- a/OpenSim/Simulation/Test/testStatesTrajectory.cpp +++ b/OpenSim/Simulation/Test/testStatesTrajectory.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using namespace OpenSim; using namespace SimTK; @@ -41,19 +42,228 @@ using namespace SimTK; // TODO test modeling options (locked coordinates, etc.) // TODO store a model within a StatesTrajectory. -const std::string statesStoFname = "testStatesTrajectory_readStorage_states.sto"; -const std::string pre40StoFname = "std_subject01_walk1_states.sto"; +namespace { + const std::string statesStoFname = "testStatesTrajectory_readStorage_states.sto"; + const std::string pre40StoFname = "std_subject01_walk1_states.sto"; + + // Helper function to get a state variable value from a storage file. + Real getStorageEntry(const Storage& sto, + const int timeIndex, const std::string& columnName) { + Real value; + const int columnIndex = sto.getStateIndex(columnName); + sto.getData(timeIndex, columnIndex, value); + return value; + } + + // Create states storage file to for states storage tests. + void createStateStorageFile() { + + Model model("gait2354_simbody.osim"); + + // To assist with creating interesting (non-zero) coordinate values: + model.updCoordinateSet().get("pelvis_ty").setDefaultLocked(true); + + // Randomly assign muscle excitations to create interesting activation + // histories. + auto* controller = new PrescribedController(); + // For consistent results, use same seed each time. + std::default_random_engine generator(0); + // Uniform distribution between 0.1 and 0.9. + std::uniform_real_distribution distribution(0.1, 0.8); + + for (int im = 0; im < model.getMuscles().getSize(); ++im) { + controller->addActuator(model.getMuscles()[im]); + controller->prescribeControlForActuator( + model.getMuscles()[im].getName(), + Constant(distribution(generator)) + ); + } + + model.addController(controller); + + auto& initState = model.initSystem(); + Manager manager(model); + initState.setTime(0.0); + manager.initialize(initState); + manager.integrate(0.15); + manager.getStateStorage().print(statesStoFname); + } + + Storage newStorageWithRemovedRows(const Storage& origSto, + const std::set& rowsToRemove) { + Storage sto(1000); + auto labels = origSto.getColumnLabels(); + auto numOrigColumns = origSto.getColumnLabels().getSize() - 1; + + // Remove in reverse order so it's easier to keep track of indices. + for (auto it = rowsToRemove.rbegin(); it != rowsToRemove.rend(); ++it) { + labels.remove(*it); + } + sto.setColumnLabels(labels); + + double time; + for (int itime = 0; itime < origSto.getSize(); ++itime) { + SimTK::Vector rowData(numOrigColumns); + origSto.getData(itime, numOrigColumns, rowData); + + SimTK::Vector newRowData(numOrigColumns - (int)rowsToRemove.size()); + int iNew = 0; + for (int iOrig = 0; iOrig < numOrigColumns; ++iOrig) { + if (rowsToRemove.count(iOrig) == 0) { + newRowData[iNew] = rowData[iOrig]; + ++iNew; + } + } + + origSto.getTime(itime, time); + sto.append(time, newRowData); + } + return sto; + } + + void testFromStatesStorageInconsistentModel(const std::string &stoFilepath) { + + // States are missing from the Storage. + // ------------------------------------ + { + Model model("gait2354_simbody.osim"); + model.initSystem(); + + const auto stateNames = model.getStateVariableNames(); + Storage sto(stoFilepath); + // So the test doesn't take so long. + sto.resampleLinear((sto.getLastTime() - sto.getFirstTime()) / 10); + + // Create new Storage with fewer columns. + auto labels = sto.getColumnLabels(); + + auto origLabel10 = stateNames[10]; + auto origLabel15 = stateNames[15]; + Storage stoMissingCols = newStorageWithRemovedRows(sto, { + // gymnastics to be compatible with pre-v4.0 column names: + sto.getStateIndex(origLabel10) + 1, + sto.getStateIndex(origLabel15) + 1}); + + // Test that an exception is thrown. + SimTK_TEST_MUST_THROW_EXC( + StatesTrajectory::createFromStatesStorage(model, stoMissingCols), + StatesTrajectory::MissingColumns + ); + // Check some other similar calls. + SimTK_TEST_MUST_THROW_EXC( + StatesTrajectory::createFromStatesStorage(model, stoMissingCols, + false, true), + StatesTrajectory::MissingColumns + ); + SimTK_TEST_MUST_THROW_EXC( + StatesTrajectory::createFromStatesStorage(model, stoMissingCols, + false, false), + StatesTrajectory::MissingColumns + ); + + // No exception if allowing missing columns. + // The unspecified states are set to NaN (for at least two random + // states). + auto states = StatesTrajectory::createFromStatesStorage( + model, stoMissingCols, true); + SimTK_TEST(SimTK::isNaN( + model.getStateVariableValue(states[0], origLabel10))); + SimTK_TEST(SimTK::isNaN( + model.getStateVariableValue(states[4], origLabel15))); + // Behavior is independent of value for allowMissingColumns. + StatesTrajectory::createFromStatesStorage(model, stoMissingCols, + true, true); + StatesTrajectory::createFromStatesStorage(model, stoMissingCols, + true, false); + } + + // States are missing from the Model. + // ---------------------------------- + { + Model model("gait2354_simbody.osim"); + Storage sto(stoFilepath); + // So the test doesn't take so long. + sto.resampleLinear((sto.getLastTime() - sto.getFirstTime()) / 10); + + // Remove a few of the muscles. + model.updForceSet().remove(0); + model.updForceSet().remove(10); + model.updForceSet().remove(30); + + // Test that an exception is thrown. + // Must call initSystem() here; otherwise the _propertySubcomponents + // would be stale and would still include the forces we removed above. + model.initSystem(); + SimTK_TEST_MUST_THROW_EXC( + StatesTrajectory::createFromStatesStorage(model, sto), + StatesTrajectory::ExtraColumns + ); + SimTK_TEST_MUST_THROW_EXC( + StatesTrajectory::createFromStatesStorage(model, sto, + true, false), + StatesTrajectory::ExtraColumns + ); + SimTK_TEST_MUST_THROW_EXC( + StatesTrajectory::createFromStatesStorage(model, sto, + false, false), + StatesTrajectory::ExtraColumns + ); + + // No exception if allowing extra columns, and behavior is + // independent of value for allowMissingColumns. + StatesTrajectory::createFromStatesStorage(model, sto, true, true); + StatesTrajectory::createFromStatesStorage(model, sto, false, true); + } + } + + void tableAndTrajectoryMatch(const Model& model, + const TimeSeriesTable& table, + const StatesTrajectory& states, + std::vector columns = {}) { + + const auto stateNames = model.getStateVariableNames(); + + size_t numColumns{}; + if (columns.empty()) { + numColumns = stateNames.getSize(); + } else { + numColumns = columns.size(); + } + SimTK_TEST(table.getNumColumns() == numColumns); + SimTK_TEST(table.getNumRows() == states.getSize()); + + const auto& colNames = table.getColumnLabels(); + + std::vector stateValueIndices(colNames.size()); + for (size_t icol = 0; icol < colNames.size(); ++icol) { + stateValueIndices[icol] = stateNames.findIndex(colNames[icol]); + } + + SimTK::Vector stateValues; // working memory -// Helper function to get a state variable value from a storage file. -Real getStorageEntry(const Storage& sto, - const int timeIndex, const std::string& columnName) { - Real value; - const int columnIndex = sto.getStateIndex(columnName); - sto.getData(timeIndex, columnIndex, value); - return value; + // Test that the data table has exactly the same numbers. + for (size_t itime = 0; itime < states.getSize(); ++itime) { + // Test time. + SimTK_TEST(table.getIndependentColumn()[itime] == + states[itime].getTime()); + + stateValues = model.getStateVariableValues(states[itime]); + + // Test state values. + for (size_t icol = 0; icol < table.getNumColumns(); ++icol) { + const auto& valueInStates = stateValues[stateValueIndices[icol]]; + + const auto& column = table.getDependentColumnAtIndex(icol); + const auto& valueInTable = column[static_cast(itime)]; + + SimTK_TEST(valueInStates == valueInTable); + } + } + } } -void testPopulateTrajectoryAndStatesTrajectoryReporter() { +TEST_CASE("testPopulateTrajectoryAndStatesTrajectoryReporter") { + LoadOpenSimLibrary("osimActuators"); Model model("gait2354_simbody.osim"); // To assist with creating interesting (non-zero) coordinate values: @@ -67,13 +277,13 @@ void testPopulateTrajectoryAndStatesTrajectoryReporter() { const double finalTime = 0.05; { auto& state = model.initSystem(); - + SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem()); SimTK::TimeStepper ts(model.getSystem(), integrator); ts.initialize(state); ts.setReportAllSignificantStates(true); integrator.setReturnEveryInternalStep(true); - + StatesTrajectory states; std::vector times; while (ts.getState().getTime() < finalTime) { @@ -84,7 +294,7 @@ void testPopulateTrajectoryAndStatesTrajectoryReporter() { // For the StatesTrajectoryReporter: model.getMultibodySystem().realize(ts.getState(), SimTK::Stage::Report); } - + // Make sure we have all the states SimTK_TEST_EQ((int)states.getSize(), (int)times.size()); SimTK_TEST_EQ((int)statesCol->getStates().getSize(), (int)times.size()); @@ -125,7 +335,8 @@ void testPopulateTrajectoryAndStatesTrajectoryReporter() { } } -void testFrontBack() { +TEST_CASE("testFrontBack") { + LoadOpenSimLibrary("osimActuators"); Model model("arm26.osim"); const auto& state = model.initSystem(); StatesTrajectory states; @@ -137,46 +348,16 @@ void testFrontBack() { SimTK_TEST(&states.back() == &states[2]); } -// Create states storage file to for states storage tests. -void createStateStorageFile() { - - Model model("gait2354_simbody.osim"); - - // To assist with creating interesting (non-zero) coordinate values: - model.updCoordinateSet().get("pelvis_ty").setDefaultLocked(true); - - // Randomly assign muscle excitations to create interesting activation - // histories. - auto* controller = new PrescribedController(); - // For consistent results, use same seed each time. - std::default_random_engine generator(0); - // Uniform distribution between 0.1 and 0.9. - std::uniform_real_distribution distribution(0.1, 0.8); - - for (int im = 0; im < model.getMuscles().getSize(); ++im) { - controller->addActuator(model.getMuscles()[im]); - controller->prescribeControlForActuator( - model.getMuscles()[im].getName(), - Constant(distribution(generator)) - ); - } - model.addController(controller); - - auto& initState = model.initSystem(); - Manager manager(model); - initState.setTime(0.0); - manager.initialize(initState); - manager.integrate(0.15); - manager.getStateStorage().print(statesStoFname); -} - -void testFromStatesStorageGivesCorrectStates() { +TEST_CASE("testFromStatesStorageGivesCorrectStates") { + LoadOpenSimLibrary("osimActuators"); // Read in trajectory. // ------------------- Model model("gait2354_simbody.osim"); + remove(statesStoFname.c_str()); + createStateStorageFile(); Storage sto(statesStoFname); // Note: we are verifying that we can load a trajectory without @@ -268,138 +449,14 @@ void testFromStatesStorageGivesCorrectStates() { } } -Storage newStorageWithRemovedRows(const Storage& origSto, - const std::set& rowsToRemove) { - Storage sto(1000); - auto labels = origSto.getColumnLabels(); - auto numOrigColumns = origSto.getColumnLabels().getSize() - 1; - - // Remove in reverse order so it's easier to keep track of indices. - for (auto it = rowsToRemove.rbegin(); it != rowsToRemove.rend(); ++it) { - labels.remove(*it); - } - sto.setColumnLabels(labels); - - double time; - for (int itime = 0; itime < origSto.getSize(); ++itime) { - SimTK::Vector rowData(numOrigColumns); - origSto.getData(itime, numOrigColumns, rowData); - - SimTK::Vector newRowData(numOrigColumns - (int)rowsToRemove.size()); - int iNew = 0; - for (int iOrig = 0; iOrig < numOrigColumns; ++iOrig) { - if (rowsToRemove.count(iOrig) == 0) { - newRowData[iNew] = rowData[iOrig]; - ++iNew; - } - } - - origSto.getTime(itime, time); - sto.append(time, newRowData); - } - return sto; -} - -void testFromStatesStorageInconsistentModel(const std::string &stoFilepath) { - - // States are missing from the Storage. - // ------------------------------------ - { - Model model("gait2354_simbody.osim"); - model.initSystem(); - - const auto stateNames = model.getStateVariableNames(); - Storage sto(stoFilepath); - // So the test doesn't take so long. - sto.resampleLinear((sto.getLastTime() - sto.getFirstTime()) / 10); - - // Create new Storage with fewer columns. - auto labels = sto.getColumnLabels(); - - auto origLabel10 = stateNames[10]; - auto origLabel15 = stateNames[15]; - Storage stoMissingCols = newStorageWithRemovedRows(sto, { - // gymnastics to be compatible with pre-v4.0 column names: - sto.getStateIndex(origLabel10) + 1, - sto.getStateIndex(origLabel15) + 1}); - - // Test that an exception is thrown. - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, stoMissingCols), - StatesTrajectory::MissingColumns - ); - // Check some other similar calls. - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, stoMissingCols, - false, true), - StatesTrajectory::MissingColumns - ); - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, stoMissingCols, - false, false), - StatesTrajectory::MissingColumns - ); - - // No exception if allowing missing columns. - // The unspecified states are set to NaN (for at least two random - // states). - auto states = StatesTrajectory::createFromStatesStorage( - model, stoMissingCols, true); - SimTK_TEST(SimTK::isNaN( - model.getStateVariableValue(states[0], origLabel10))); - SimTK_TEST(SimTK::isNaN( - model.getStateVariableValue(states[4], origLabel15))); - // Behavior is independent of value for allowMissingColumns. - StatesTrajectory::createFromStatesStorage(model, stoMissingCols, - true, true); - StatesTrajectory::createFromStatesStorage(model, stoMissingCols, - true, false); - } - - // States are missing from the Model. - // ---------------------------------- - { - Model model("gait2354_simbody.osim"); - Storage sto(stoFilepath); - // So the test doesn't take so long. - sto.resampleLinear((sto.getLastTime() - sto.getFirstTime()) / 10); - - // Remove a few of the muscles. - model.updForceSet().remove(0); - model.updForceSet().remove(10); - model.updForceSet().remove(30); - - // Test that an exception is thrown. - // Must call initSystem() here; otherwise the _propertySubcomponents - // would be stale and would still include the forces we removed above. - model.initSystem(); - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, sto), - StatesTrajectory::ExtraColumns - ); - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, sto, - true, false), - StatesTrajectory::ExtraColumns - ); - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, sto, - false, false), - StatesTrajectory::ExtraColumns - ); - - // No exception if allowing extra columns, and behavior is - // independent of value for allowMissingColumns. - StatesTrajectory::createFromStatesStorage(model, sto, true, true); - StatesTrajectory::createFromStatesStorage(model, sto, false, true); - } -} - -void testFromStatesStorageUniqueColumnLabels() { +TEST_CASE("testFromStatesStorageUniqueColumnLabels") { + LoadOpenSimLibrary("osimActuators"); + remove(statesStoFname.c_str()); + createStateStorageFile(); Model model("gait2354_simbody.osim"); Storage sto(statesStoFname); - + // Edit column labels so that they are not unique. auto labels = sto.getColumnLabels(); labels[10] = labels[7]; @@ -425,7 +482,8 @@ void testFromStatesStorageUniqueColumnLabels() { // names (/value and /speed) in the same file. } -void testFromStatesStoragePre40CorrectStates() { +TEST_CASE("testFromStatesStoragePre40CorrectStates") { + LoadOpenSimLibrary("osimActuators"); // This test is very similar to testFromStatesStorageGivesCorrectStates // TODO could avoid the duplicate function since getStateIndex handles // pre-4.0 names. @@ -478,7 +536,7 @@ void testFromStatesStoragePre40CorrectStates() { // Fiber length. SimTK_TEST_EQ( - getStorageEntry(sto, itime, muscleName + ".fiber_length"), + getStorageEntry(sto, itime, muscleName + ".fiber_length"), muscle.getFiberLength(state)); // More complicated computation based on state. @@ -512,7 +570,8 @@ void testFromStatesStoragePre40CorrectStates() { } -void testCopying() { +TEST_CASE("testCopying") { + LoadOpenSimLibrary("osimActuators"); Model model("gait2354_simbody.osim"); auto& state = model.initSystem(); @@ -546,7 +605,8 @@ void testCopying() { } } -void testAppendTimesAreNonDecreasing() { +TEST_CASE("testAppendTimesAreNonDecreasing") { + LoadOpenSimLibrary("osimActuators"); Model model("gait2354_simbody.osim"); auto& state = model.initSystem(); state.setTime(1.0); @@ -562,7 +622,8 @@ void testAppendTimesAreNonDecreasing() { SimTK::Exception::APIArgcheckFailed); } -void testBoundsCheck() { +TEST_CASE("testBoundsCheck") { + LoadOpenSimLibrary("osimActuators"); Model model("gait2354_simbody.osim"); const auto& state = model.initSystem(); StatesTrajectory states; @@ -570,7 +631,7 @@ void testBoundsCheck() { states.append(state); states.append(state); states.append(state); - + #ifdef NDEBUG // In DEBUG, Visual Studio puts asserts into the index operator. states[states.getSize() + 100]; @@ -582,7 +643,8 @@ void testBoundsCheck() { IndexOutOfRange); } -void testIntegrityChecks() { +TEST_CASE("testIntegrityChecks") { + LoadOpenSimLibrary("osimActuators"); Model arm26("arm26.osim"); const auto& s26 = arm26.initSystem(); @@ -676,60 +738,18 @@ void testIntegrityChecks() { } // TODO Show weakness of the test: two models with the same number of Q's, U's, - // and Z's both pass the check. -} - -void tableAndTrajectoryMatch(const Model& model, - const TimeSeriesTable& table, - const StatesTrajectory& states, - std::vector columns = {}) { - - const auto stateNames = model.getStateVariableNames(); - - size_t numColumns{}; - if (columns.empty()) { - numColumns = stateNames.getSize(); - } else { - numColumns = columns.size(); - } - SimTK_TEST(table.getNumColumns() == numColumns); - SimTK_TEST(table.getNumRows() == states.getSize()); - - const auto& colNames = table.getColumnLabels(); - - std::vector stateValueIndices(colNames.size()); - for (size_t icol = 0; icol < colNames.size(); ++icol) { - stateValueIndices[icol] = stateNames.findIndex(colNames[icol]); - } - - SimTK::Vector stateValues; // working memory - - // Test that the data table has exactly the same numbers. - for (size_t itime = 0; itime < states.getSize(); ++itime) { - // Test time. - SimTK_TEST(table.getIndependentColumn()[itime] == - states[itime].getTime()); - - stateValues = model.getStateVariableValues(states[itime]); - - // Test state values. - for (size_t icol = 0; icol < table.getNumColumns(); ++icol) { - const auto& valueInStates = stateValues[stateValueIndices[icol]]; - - const auto& column = table.getDependentColumnAtIndex(icol); - const auto& valueInTable = column[static_cast(itime)]; - - SimTK_TEST(valueInStates == valueInTable); - } - } + // and Z's both pass the check. } -void testExport() { +TEST_CASE("testExport") { + LoadOpenSimLibrary("osimActuators"); Model gait("gait2354_simbody.osim"); gait.initSystem(); // Exported data exactly matches data in the trajectory. const auto stateNames = gait.getStateVariableNames(); + remove(statesStoFname.c_str()); + createStateStorageFile(); Storage sto(statesStoFname); auto states = StatesTrajectory::createFromStatesStorage(gait, sto); @@ -770,38 +790,10 @@ void testExport() { OpenSim::Exception); } -int main() { - SimTK_START_TEST("testStatesTrajectory"); - // actuators library is not loaded automatically (unless using clang). - #if !defined(__clang__) - LoadOpenSimLibrary("osimActuators"); - #endif - - // Make sure the states Storage file doesn't already exist; we'll - // generate it later and we don't want to use a stale one by accident. - remove(statesStoFname.c_str()); - - SimTK_SUBTEST(testPopulateTrajectoryAndStatesTrajectoryReporter); - SimTK_SUBTEST(testFrontBack); - SimTK_SUBTEST(testBoundsCheck); - SimTK_SUBTEST(testIntegrityChecks); - SimTK_SUBTEST(testAppendTimesAreNonDecreasing); - SimTK_SUBTEST(testCopying); - - // Test creation of trajectory from a states storage. - // ------------------------------------------------- - // Using a pre-4.0 states storage file with old column names. - SimTK_SUBTEST(testFromStatesStoragePre40CorrectStates); - SimTK_SUBTEST1(testFromStatesStorageInconsistentModel, pre40StoFname); - - // v4.0 states storage - createStateStorageFile(); - SimTK_SUBTEST(testFromStatesStorageGivesCorrectStates); - SimTK_SUBTEST1(testFromStatesStorageInconsistentModel, statesStoFname); - SimTK_SUBTEST(testFromStatesStorageUniqueColumnLabels); - - // Export to data table. - SimTK_SUBTEST(testExport); - - SimTK_END_TEST(); +TEST_CASE("testFromStatesStorageInconsistentModel") { + LoadOpenSimLibrary("osimActuators"); + remove(statesStoFname.c_str()); + testFromStatesStorageInconsistentModel(pre40StoFname); + createStateStorageFile(); + testFromStatesStorageInconsistentModel(statesStoFname); }