From 00e4128e16d2530940a7f3f70032d7204d0daa46 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 11 Oct 2024 17:23:52 -0700 Subject: [PATCH 1/6] Add scripting support for analyze with Rotation outputs; add templated MocoStudy::analyze and associated scripting bindings --- Bindings/common.i | 2 +- Bindings/moco.i | 5 +++++ OpenSim/Moco/MocoStudy.cpp | 6 ------ OpenSim/Moco/MocoStudy.h | 16 ++++++++++++++-- 4 files changed, 20 insertions(+), 9 deletions(-) diff --git a/Bindings/common.i b/Bindings/common.i index ec66faca3f..696a8e5b42 100644 --- a/Bindings/common.i +++ b/Bindings/common.i @@ -371,7 +371,7 @@ DATATABLE_CLONE(double, SimTK::Rotation_) %shared_ptr(OpenSim::IMUDataReader) %shared_ptr(OpenSim::XsensDataReader) %shared_ptr(OpenSim::APDMDataReader) -%shared_ptr(OpenSim::STOFileAdapter_) +%shared_ptr(OpenSim::STOFileAdapter_) %shared_ptr(OpenSim::STOFileAdapter_) %shared_ptr(OpenSim::STOFileAdapter_) %shared_ptr(OpenSim::STOFileAdapter_) diff --git a/Bindings/moco.i b/Bindings/moco.i index 95af4e3216..b53d2fa5b3 100644 --- a/Bindings/moco.i +++ b/Bindings/moco.i @@ -138,6 +138,10 @@ namespace OpenSim { %include %include %include +%template(analyzeVec3) OpenSim::MocoStudy::analyze; +%template(analyzeSpatialVec) OpenSim::MocoStudy::analyze; +%template(analyzeRotation) OpenSim::MocoStudy::analyze>; + %include %include @@ -148,5 +152,6 @@ namespace OpenSim { %template(analyzeMocoTrajectory) OpenSim::analyzeMocoTrajectory; %template(analyzeMocoTrajectoryVec3) OpenSim::analyzeMocoTrajectory; %template(analyzeMocoTrajectorySpatialVec) OpenSim::analyzeMocoTrajectory; +%template(analyzeMocoTrajectoryRotation) OpenSim::analyzeMocoTrajectory>; %include diff --git a/OpenSim/Moco/MocoStudy.cpp b/OpenSim/Moco/MocoStudy.cpp index f5b68e5487..cabca37633 100644 --- a/OpenSim/Moco/MocoStudy.cpp +++ b/OpenSim/Moco/MocoStudy.cpp @@ -108,12 +108,6 @@ void MocoStudy::visualize(const MocoTrajectory& trajectory) const { VisualizerUtilities::showMotion(model, trajectory.exportToStatesTable()); } -TimeSeriesTable MocoStudy::analyze(const MocoTrajectory& trajectory, - const std::vector& outputPaths) const { - return OpenSim::analyzeMocoTrajectory( - get_problem().createRep().getModelBase(), trajectory, outputPaths); -} - TimeSeriesTable MocoStudy::calcGeneralizedForces( const MocoTrajectory& trajectory, const std::vector& forcePaths) const { diff --git a/OpenSim/Moco/MocoStudy.h b/OpenSim/Moco/MocoStudy.h index 561cf5a5fc..3ba6a0f8b9 100644 --- a/OpenSim/Moco/MocoStudy.h +++ b/OpenSim/Moco/MocoStudy.h @@ -19,6 +19,8 @@ * -------------------------------------------------------------------------- */ #include "MocoSolver.h" +#include "MocoProblem.h" +#include "MocoUtilities.h" #include #include @@ -162,14 +164,24 @@ class OSIMMOCO_API MocoStudy : public Object { /// ".*activation" gives the activation of all muscles. /// Constraints are not enforced but prescribed motion (e.g., /// PositionMotion) is. - /// @see OpenSim::analyze() + /// @see OpenSim::analyze() OpenSim::analyzeMocoTrajectory() /// @note Parameters in the MocoTrajectory are **not** applied to the model. /// @note If the MocoTrajectory was generated from a MocoStudy with /// Controller%s in the model, first call /// MocoTrajectory::generateControlsFromModelControllers() to populate /// the trajectory with the correct model controls. + template + TimeSeriesTable_ analyze(const MocoTrajectory& traj, + const std::vector& outputPaths) const { + return OpenSim::analyzeMocoTrajectory( + get_problem().createRep().getModelBase(), traj, outputPaths); + } + + // @copydoc analyze() TimeSeriesTable analyze(const MocoTrajectory& traj, - const std::vector& outputPaths) const; + const std::vector& outputPaths) const { + return analyze(traj, outputPaths); + } /// Compute the generalized coordinate forces for the provided trajectory /// based on a set of applied model Force%s. This can be used to compute From ec2a4025e0e4aab5635b88860d9c1adb34aa29f8 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 11 Oct 2024 23:16:09 -0700 Subject: [PATCH 2/6] Add DataTable tests for Python --- Bindings/Python/tests/test_DataTable.py | 50 +++++++++++++++++++++---- Bindings/common.i | 24 ++++++++++++ 2 files changed, 66 insertions(+), 8 deletions(-) diff --git a/Bindings/Python/tests/test_DataTable.py b/Bindings/Python/tests/test_DataTable.py index 8bbe9214bc..9accfe208f 100644 --- a/Bindings/Python/tests/test_DataTable.py +++ b/Bindings/Python/tests/test_DataTable.py @@ -130,7 +130,7 @@ def test_DataTable(self): row[1] == 200 and row[2] == 300 and row[3] == 400) - row0 = table.getRowAtIndex(0) + row0 = table.updRowAtIndex(0) row0[0] = 10 row0[1] = 10 row0[2] = 10 @@ -140,7 +140,7 @@ def test_DataTable(self): row0[1] == 10 and row0[2] == 10 and row0[3] == 10) - row2 = table.getRow(0.3) + row2 = table.updRow(0.3) row2[0] = 20 row2[1] = 20 row2[2] = 20 @@ -152,7 +152,7 @@ def test_DataTable(self): row2[3] == 20) print(table) # Edit columns of the table. - col1 = table.getDependentColumnAtIndex(1) + col1 = table.updDependentColumnAtIndex(1) col1[0] = 30 col1[1] = 30 col1[2] = 30 @@ -160,7 +160,7 @@ def test_DataTable(self): assert (col1[0] == 30 and col1[1] == 30 and col1[2] == 30) - col3 = table.getDependentColumn('3') + col3 = table.updDependentColumn('3') col3[0] = 40 col3[1] = 40 col3[2] = 40 @@ -299,6 +299,40 @@ def test_DataTable(self): assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat) + table = osim.DataTable() + table.setColumnLabels(('col0_x', 'col0_y', 'col0_z', + 'col1_x', 'col1_y', 'col1_z', + 'col2_x', 'col2_y', 'col2_z', + 'col3_x', 'col3_y', 'col3_z', + 'col4_x', 'col4_y', 'col4_z', + 'col5_x', 'col5_y', 'col5_z')) + row = osim.RowVector([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) + table.appendRow(1, row) + row = osim.RowVector([2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]) + table.appendRow(2, row) + row = osim.RowVector([3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) + table.appendRow(3, row) + assert len(table.getColumnLabels()) == 18 + assert table.getNumRows() == 3 + assert table.getNumColumns() == 18 + table.setColumnLabels(('col0_0', 'col0_1', 'col0_2', + 'col0_3', 'col0_4', 'col0_5', + 'col0_6', 'col0_7', 'col0_8', + 'col1_0', 'col1_1', 'col1_2', + 'col1_3', 'col1_4', 'col1_5', + 'col1_6', 'col1_7', 'col1_8')) + tableRot = table.packRotation() + tableRot.getColumnLabels() == ('col0', 'col1') + tableRot.getNumRows() == 3 + tableRot.getNumColumns() == 2 + print(tableRot) + tableFlat = tableRot.flatten() + assert len(tableFlat.getColumnLabels()) == 18 + assert tableFlat.getColumnLabel( 0) == 'col0_1' + assert tableFlat.getColumnLabel(15) == 'col1_7' + assert tableFlat.getNumRows() == 3 + assert tableFlat.getNumColumns() == 18 + print(tableFlat) def test_TimeSeriesTable(self): print() @@ -475,7 +509,7 @@ def test_DataTableVec3(self): print(tableDouble) # Edit rows of the table. - row0 = table.getRowAtIndex(0) + row0 = table.updRowAtIndex(0) row0[0] = osim.Vec3(10, 10, 10) row0[1] = osim.Vec3(10, 10, 10) row0[2] = osim.Vec3(10, 10, 10) @@ -483,7 +517,7 @@ def test_DataTableVec3(self): assert (str(row0[0]) == str(osim.Vec3(10, 10, 10)) and str(row0[1]) == str(osim.Vec3(10, 10, 10)) and str(row0[2]) == str(osim.Vec3(10, 10, 10))) - row2 = table.getRow(0.3) + row2 = table.updRow(0.3) row2[0] = osim.Vec3(20, 20, 20) row2[1] = osim.Vec3(20, 20, 20) row2[2] = osim.Vec3(20, 20, 20) @@ -493,7 +527,7 @@ def test_DataTableVec3(self): str(row2[2]) == str(osim.Vec3(20, 20, 20))) print(table) # Edit columns of the table. - col1 = table.getDependentColumnAtIndex(1) + col1 = table.updDependentColumnAtIndex(1) col1[0] = osim.Vec3(30, 30, 30) col1[1] = osim.Vec3(30, 30, 30) col1[2] = osim.Vec3(30, 30, 30) @@ -501,7 +535,7 @@ def test_DataTableVec3(self): assert (str(col1[0]) == str(osim.Vec3(30, 30, 30)) and str(col1[1]) == str(osim.Vec3(30, 30, 30)) and str(col1[2]) == str(osim.Vec3(30, 30, 30))) - col2 = table.getDependentColumn('2') + col2 = table.updDependentColumn('2') col2[0] = osim.Vec3(40, 40, 40) col2[1] = osim.Vec3(40, 40, 40) col2[2] = osim.Vec3(40, 40, 40) diff --git a/Bindings/common.i b/Bindings/common.i index 696a8e5b42..73c9d8b7d6 100644 --- a/Bindings/common.i +++ b/Bindings/common.i @@ -253,6 +253,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_) packSpatialVec(std::vector suffixes) { return $self->pack(); } + DataTable_> + packRotation() { + return $self->pack>(); + } + DataTable_> + packRotation(std::vector suffixes) { + return $self->pack>(); + } } %ignore OpenSim::TimeSeriesTable_::TimeSeriesTable_(TimeSeriesTable_ &&); @@ -294,6 +302,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_) packSpatialVec(std::vector suffixes) { return $self->pack(); } + TimeSeriesTable_> + packRotation() { + return $self->pack>(); + } + TimeSeriesTable_> + packRotation(std::vector suffixes) { + return $self->pack>(); + } } %extend OpenSim::TimeSeriesTable_ { TimeSeriesTable_ flatten() { @@ -335,6 +351,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_) return $self->flatten(suffixes); } } +%extend OpenSim::TimeSeriesTable_> { + TimeSeriesTable_ flatten() { + return $self->flatten(); + } + TimeSeriesTable_ flatten(std::vector suffixes) { + return $self->flatten(suffixes); + } +} %include %include From f3f8cbdbc450528943101a3b759043bd61cb958b Mon Sep 17 00:00:00 2001 From: nickbianco Date: Mon, 14 Oct 2024 16:29:03 -0700 Subject: [PATCH 3/6] Add Java, C++ tests for tables of Rotations --- Bindings/Java/tests/TestTables.java | 30 +++++++ OpenSim/Common/Test/testDataTable.cpp | 113 ++++++++++++++++++++++++++ 2 files changed, 143 insertions(+) diff --git a/Bindings/Java/tests/TestTables.java b/Bindings/Java/tests/TestTables.java index 4111c5ddc1..03d92669b9 100644 --- a/Bindings/Java/tests/TestTables.java +++ b/Bindings/Java/tests/TestTables.java @@ -265,6 +265,36 @@ public static void test_DataTable() { assert tableFlat.getNumRows() == 3; assert tableFlat.getNumColumns() == 12; System.out.println(tableFlat); + table = new DataTable(); + labels = new StdVectorString(); + labels.add("col0.0"); labels.add("col0.1"); labels.add("col0.2"); + labels.add("col0.3"); labels.add("col0.4"); labels.add("col0.5"); + labels.add("col0.6"); labels.add("col0.7"); labels.add("col0.8"); + labels.add("col1.0"); labels.add("col1.1"); labels.add("col1.2"); + labels.add("col1.3"); labels.add("col1.4"); labels.add("col1.5"); + labels.add("col1.6"); labels.add("col1.7"); labels.add("col1.8"); + table.setColumnLabels(labels); + row = new RowVector(18, 1); + table.appendRow(1, row); + row = new RowVector(18, 2); + table.appendRow(2, row); + row = new RowVector(18, 3); + table.appendRow(3, row); + assert table.getColumnLabels().size() == 18; + assert table.getNumRows() == 3; + assert table.getNumColumns() == 18; + DataTableRotation tableRot = table.packRotation(); + assert tableRot.getColumnLabel(0).equals("col0"); + assert tableRot.getNumRows() == 3; + assert tableRot.getNumColumns() == 2; + System.out.println(tableRot); + tableFlat = tableRot.flatten(); + assert tableFlat.getColumnLabels().size() == 18; + assert tableFlat.getColumnLabel( 0).equals("col0_1"); + assert tableFlat.getColumnLabel(15).equals("col1_7"); + assert tableFlat.getNumRows() == 3; + assert tableFlat.getNumColumns() == 18; + System.out.println(tableFlat); } public static void test_DataTableVec3() { diff --git a/OpenSim/Common/Test/testDataTable.cpp b/OpenSim/Common/Test/testDataTable.cpp index 3dfdc1fc80..7086ffcf3a 100644 --- a/OpenSim/Common/Test/testDataTable.cpp +++ b/OpenSim/Common/Test/testDataTable.cpp @@ -243,6 +243,9 @@ TEST_CASE("DataTable") { ASSERT((static_cast (DataTable_{})). numComponentsPerElement() == 6); + ASSERT((static_cast + (DataTable_{})). + numComponentsPerElement() == 9); { std::cout << "Test DataTable flattening constructor for Vec3." @@ -371,6 +374,26 @@ TEST_CASE("DataTable") { ASSERT(tableDouble.getNumColumns() == 18); std::cout << tableDouble << std::endl; + + std::cout << "Test DataTable flattening constructor for Rotation." + << std::endl; + DataTable_ tableRotation{}; + tableRotation.setColumnLabels({"col0", "col1"}); + tableRotation.appendRow(0.1, {Rotation(0.1, UnitVec3(1, 0, 0)), + Rotation(0.2, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.2, {Rotation(0.2, UnitVec3(0, 0, 1)), + Rotation(0.1, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.3, {Rotation(0.3, UnitVec3(0, 1, 0)), + Rotation(0.2, UnitVec3(1, 0, 0))}); + + std::cout << tableRotation << std::endl; + + tableDouble = tableRotation; + ASSERT(tableDouble.getColumnLabels().size() == 18); + ASSERT(tableDouble.getNumRows() == 3); + ASSERT(tableDouble.getNumColumns() == 18); + + std::cout << tableDouble << std::endl; } { std::cout << "Test TimeSeriesTable flattening constructor for Vec3" @@ -525,6 +548,43 @@ TEST_CASE("DataTable") { ASSERT(tableDouble.getNumColumns() == 18); std::cout << tableDouble << std::endl; + + std::cout << "Test TimeSeriesTable flattening constructor for " + "Rotation" << std::endl; + DataTable_ tableRotation{}; + tableRotation.setColumnLabels({"col0", "col1"}); + tableRotation.appendRow(0.1, {Rotation(0.1, UnitVec3(1, 0, 0)), + Rotation(0.2, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.2, {Rotation(0.2, UnitVec3(0, 0, 1)), + Rotation(0.1, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.3, {Rotation(0.3, UnitVec3(0, 1, 0)), + Rotation(0.2, UnitVec3(1, 0, 0))}); + + // TODO: RowVector_ is not supported. + // const auto& avgRowRot = tableRotation.averageRow(0.1, 0.2); + // for(int i = 0; i < 3; ++i) { + // OPENSIM_THROW_IF(std::abs(avgRowRot[0][0][i] - 2) > 1e-8/*eps*/, + // OpenSim::Exception, + // "Test failed: averageRow() failed."); + // } + + // const auto& nearRowRot = tableRotation.getNearestRow(0.29); + // for(int i = 0; i < 3; ++i) + // ASSERT(nearRowRot[0][0][i] == 2); + + // tableRotation.updNearestRow(0.29) += Rotation(0.2, UnitVec3(0, 1, 0)); + // tableRotation.updNearestRow(0.29) -= Rotation(0.2, UnitVec3(0, 1, 0)); + // for(int i = 0; i < 3; ++i) + // ASSERT(nearRowRot[0][0][i] == 2); + + std::cout << tableRotation << std::endl; + + tableDouble = tableRotation; + ASSERT(tableDouble.getColumnLabels().size() == 18); + ASSERT(tableDouble.getNumRows() == 3); + ASSERT(tableDouble.getNumColumns() == 18); + + std::cout << tableDouble << std::endl; } { std::cout << "Test DataTable packing." << std::endl; @@ -601,6 +661,32 @@ TEST_CASE("DataTable") { ASSERT(tableSVec.getTableMetaData("string") == "string"); ASSERT(tableSVec.getTableMetaData("int") == 10); std::cout << tableSVec << std::endl; + + std::cout << "Test DataTable packing for Rotation" << std::endl; + DataTable_ table{}; + table.setColumnLabels({"col0_0", "col0_1", "col0_2", + "col0_3", "col0_4", "col0_5", + "col0_6", "col0_7", "col0_8", + "col1_0", "col1_1", "col1_2", + "col1_3", "col1_4", "col1_5", + "col1_6", "col1_7", "col1_8"}); + table.appendRow(1, RowVector(18, 1)); + table.appendRow(2, RowVector(18, 2)); + table.appendRow(3, RowVector(18, 3)); + table.addTableMetaData("string", std::string{"string"}); + table.addTableMetaData("int", 10); + ASSERT(table.getColumnLabels().size() == 18); + ASSERT(table.getNumRows() == 3); + ASSERT(table.getNumColumns() == 18); + + auto tableRot = table.pack(); + expLabels = {"col0", "col1"}; + ASSERT(tableRot.getColumnLabels() == expLabels); + ASSERT(tableRot.getNumRows() == 3); + ASSERT(tableRot.getNumColumns() == 2); + ASSERT(tableRot.getTableMetaData("string") == "string"); + ASSERT(tableRot.getTableMetaData("int") == 10); + std::cout << tableRot << std::endl; } { std::cout << "Test TimeSeriesTable packing." << std::endl; @@ -692,6 +778,33 @@ TEST_CASE("DataTable") { ASSERT(tableSVec.getTableMetaData("string") == "string"); ASSERT(tableSVec.getTableMetaData("int") == 10); std::cout << tableSVec << std::endl; + + std::cout << "Test TimeSeriesTable packing for Rotation" << std::endl; + TimeSeriesTable_ table{}; + table.setColumnLabels({"col0_0", "col0_1", "col0_2", + "col0_3", "col0_4", "col0_5", + "col0_6", "col0_7", "col0_8", + "col1_0", "col1_1", "col1_2", + "col1_3", "col1_4", "col1_5", + "col1_6", "col1_7", "col1_8"}); + table.appendRow(1, RowVector(18, 1)); + table.appendRow(2, RowVector(18, 2)); + table.appendRow(3, RowVector(18, 3)); + table.addTableMetaData("string", std::string{"string"}); + table.addTableMetaData("int", 10); + ASSERT(table.getColumnLabels().size() == 18); + ASSERT(table.getNumRows() == 3); + ASSERT(table.getNumColumns() == 18); + + TimeSeriesTable_ tableRot = + table.pack(); + expLabels = {"col0", "col1"}; + ASSERT(tableRot.getColumnLabels() == expLabels); + ASSERT(tableRot.getNumRows() == 3); + ASSERT(tableRot.getNumColumns() == 2); + ASSERT(tableRot.getTableMetaData("string") == "string"); + ASSERT(tableRot.getTableMetaData("int") == 10); + std::cout << tableRot << std::endl; } { From de972247e42618aef96f75d27b2e2c01892a6d29 Mon Sep 17 00:00:00 2001 From: nickbianco Date: Mon, 14 Oct 2024 16:45:50 -0700 Subject: [PATCH 4/6] Update the CHANGELOG --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1b6660266d..60e5e67be5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -93,6 +93,8 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830) - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834) - Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874). +- Added Python and Java (Matlab) scripting support for `TimeSeriesTable_`. (#3940) +- Added the templatized `MocoStudy::analyze()` and equivalent scripting counterparts: `analyzeVec3`, `analyzeSpatialVec`, `analyzeRotation`. (#3940) v4.5 ==== From a11d3e5b21cdb4466fa66af148bd7470822ab5e9 Mon Sep 17 00:00:00 2001 From: nickbianco Date: Wed, 16 Oct 2024 21:31:35 -0700 Subject: [PATCH 5/6] Add binding for OpenSim::analyze --- Bindings/simulation.i | 1 + 1 file changed, 1 insertion(+) diff --git a/Bindings/simulation.i b/Bindings/simulation.i index 7d9ad9c676..f8d1adc48b 100644 --- a/Bindings/simulation.i +++ b/Bindings/simulation.i @@ -263,6 +263,7 @@ OpenSim::ModelComponentSet; %template(analyze) OpenSim::analyze; %template(analyzeVec3) OpenSim::analyze; %template(analyzeSpatialVec) OpenSim::analyze; +%template(analyzeRotation) OpenSim::analyze>; %include From edbc2e48db065e87f7b986be2aa2171fa1087b15 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 17 Oct 2024 22:04:20 -0500 Subject: [PATCH 6/6] Class StatesDocument (#3902) * Added class StatesDocument * Update StatesDocument.cpp - Modified the names of the Trajectory methods to match the latest names in Component.h. - Reintroduced the precision argument in SimTK::Xml::Elment::setValueAs. * Create testStatesDocument.cpp Added a place holder for the Catch2 unit test for class StatesDocument. testStatesDocument.cpp is now being incorporated in the build, and the test case runs when "build" executed on the target "RUN_TESTS_PARALLEL". * Update StatesDocument.cpp - Moved all utility methods into a struct with local linkage. - Streamlined code by adding `getEltValue()`. Use of this method reduced some code repetition. * Update StatesDocument.cpp - Minor changes to line formatting. * Update testComponentInterface.cpp - Minor corrections/additions to comments * Update testStatesDocument.cpp - Added code borrowed from testComponentInterface.cpp to generate a dummy model with a variety of states. The code compiles and runs with all test cases passing, but this is just a starting point. De/serialization using class StatesDocument is not yet being tested. * Update testStatesDocument.cpp - Removed code borrowed from testComponentInterface.cpp. That code was not the way to go. A legitimate model is needed. I'm starting over. * Update StatesDocument.h - Made small improvements and corrections in the documentation. * Update testStatesDocument.cpp - Now building a simple model and running a simulation. * Update testStatesDocument.cpp - Added code to serialize, deserialize, and then re-serialize the state trajectory recorded during a simulation. The code is functioning as expected! * Removed trailing whitespace in StatesTrajectory.h * Update StatesTrajectory.h - Changed member variable `m_states` from type std::vector to SimTK::Array_ - Added `exportToStatesDocument()` * Update StatesTrajectory.h - Added a `get` method that provides access to the underlying SimTK::Array_ member variable. * Update testStatesDocument.cpp - Added a method that tests equality of 2 state trajectories. The method does not yet test equality for discrete variables or modeling options, but Qs, Us, and Zs are passing. * Update testStatesDocument.cpp - Added a static method that computes maximum rounding error based on the specified precision and value that is rounded. * Update testStatesDocument.cpp - Cleaned up the computation of max_eps. * Update testStatesDocument.cpp - Added equality comparisons for discrete variables and modeling options. * Update testStatesDocument.cpp - Added ExtendedPointToPointSpring. * Merge branch 'opensim-org:main' into ostates_2 * Removed trailing whitespace from testStatesTrajectory.cpp * Altered testBoundsCheck() Since the trajectory was changed from std::vector to SimTK::Array_, bounds checking behavior has changed. SimTK::Array_ only checks bounds in DEBUG. In other words, an IndexOutOfRange exception will not be thrown in a release build. To account for this, I inserted an #ifdef to skip bounds checking unless in DEBUG. * Minor: Added a missing space. * Added discrete variables to the test model. Added the following types: - bool - int - double - Vec2 - Vec3 - Vec4 - Vec5 - Vec6 All tests are passing, and the .ostates file looks right. * Revamped ExtendedPointToPointSpring So that discrete variables can be updated during a simulation, class ExtendedPointToPointSpring is now built on top of class ExtendedTwoPointLinearSpring. ExtendedTwoPointLinearSpring allocates auto-update discrete variables in its own `realizeTopology()` and those discrete variables are updated in its own `realizePosition()`. * Reverted to a simpler ExtendedPointToPointSpring class. The discrete variables can be added in extendRealizeTopology(). And, they can be changed in computeForce(). * Discrete variables are now being altered during simulation - Changed the allocation for discrete variables to the DefaultSystemSubsystem. - Primarily using the subsystem index instead of the pointer to the subsystem. - Discrete variables are being changed during simulation in ExtendedPointToPointSpring::computeForce(). All checks are now passing! * Added ability to set a note for a states document. The note is serialized and deserialized. All checks are passing. * Merge branch 'opensim-org:main' into ostates_2 * Update testStatesDocument.cpp 1. Now looping over over output precision from precision = 2 to precision = 22. 2. Added the ability to change the name of a discrete state for the purpose of testing exceptions when a discrete state is not found. * Update testStatesDocument.cpp - Cleaned up the code a little bit. - Added code to deserialize with a model that has one discrete variable with a changed name. This was done to check exception handling. * Check that model names match * Update testStatesDocument.cpp Cleaned up the code. The following checks are now performed: - Serialization / deserialization for precision 1 to 21. - Check that exception is thrown when model names don't match. - Check that exception is thrown when a discrete state is not found. * Update testStatesDocument.cpp - code cleanup - added the ability to omit a discrete state When a discrete state is omitted, an exception should be thrown but it is not. Need to look into this. Previous checks are still running properly. * All checks passing. * Removed old code * Added read-only accessor for low-level state array const SimTK::Array_& StatesTrajectoryReporter::getStateArray(); * Code cleanup and refinement - Removed StatesDocument::parseDoc(). No reason to have it. - Simplified the name of StatesTrajectory::getUnderlyingStateArray() to getStateArray(). - Refined documentation comments and checked them for accuracy. * Merge branch 'opensim-org:main' into ostates_2 * Minor code cleanup - Deleted unused code. - Updated copyright dates. * Update StatesTrajectory.h - Improved documentation comments. * Fixed typo in StatesDocument.h An error was being issued for Ubuntu and Mac builds because "OPENSIM_STATES_DOCUMENT_H_" was not commented out on line 609. I added the comment token. * Corrected some type compatibilities in StatesDocument.cpp In a number of instances, I changed the type from `int` to `size_t`. * Using std::string::c_str() for strings in SimTK_ASSERT macros Windows can handle a std::string, but Mac and Ubuntu builds are failing. * Update testStatesDocument.cpp - Addressed some type warnings. - Removed the method `ComputeMaxRoundingError()`. * Preventing auto generation of a default constructor * Update StatesDocument.h I temporarily added a default constructor to see if that would resolve some errors that are occurring in the Continuous Integration. * Merge branch 'opensim-org:main' into ostates_2 * Update testStatesDocument.cpp It looks like different return types of size() methods are assumed across operating systems. I inserted a (size_t) cast to address compiler errors on Ubuntu * Add bindings for StatesDocument class * Merge branch 'opensim-org:main' into ostates_2 * Reverted StatesTrajectory to use std::vector(SimTK::State) For reasons of performance and compatibility with Simbody, OpenSim::StatesDocument uses SimTK::Array_<>, instead of std::vector<>, as the container for states trajectories. However, some user-facing classes, like OpenSim::StatesTrajectory, have opted to use std::vector<> as the container. I migrated OpenSim::StatesTrajectory to use SimTK::Array_<>. Unfortunately, this caused errors when bindings for Python and perhaps Java and Matlab are generated. To maintain compatibility with existing code and simplify binding generation, this commit reverts to using std::vector as the container for state trajectories. In addition, several new interface methods were introduced to the StatesDocument API to allow either SimTK::Array_<> or std::vector<> to be used as the container. Internally, however, all state trajectory operations are mediated by SimTK::Array_<>. This flexibility is gained at almost no computational cost by using a shallow copy constructor to convert std::vector objects to SimTK::Array_ objects. * Update StatesDocument.cpp Now settings for `note` and `precision` are updated in the body of the constructor instead of in the initializer list. * Restored commented-out code in testStatesTrajectory.cpp * typo corrected * Update StatesDocument.h Updated the documentation to reflect the fact that std::vector is also an acceptable form of the state trajectory for interfacing with OpenSim. * Update StatesTrajectoryReporter.cpp Migrated StatesTrajectoryReporter::getStateArray() to getVectorOfStateObjects() * Update StatesTrajectoryReporter.h Migrated StatesTrajectoryReporter::getStateArray() to getVectorOfStateObjects(). * Update StatesDocument.cpp I added comments for developers should a demand additional supported variable types emerge. * Update testStatesDocument.cpp Corrected a compile error. * Updated CHANGELOG.md to include the new StatesDocument class (#3902) Co-Authored-By: aymanhab --- Bindings/OpenSimHeaders_simulation.h | 1 + Bindings/simulation.i | 1 + CHANGELOG.md | 22 +- .../Common/Test/testComponentInterface.cpp | 6 +- OpenSim/Simulation/StatesDocument.cpp | 769 +++++++++++++++++ OpenSim/Simulation/StatesDocument.h | 664 +++++++++++++++ OpenSim/Simulation/StatesTrajectory.h | 73 +- .../Simulation/StatesTrajectoryReporter.cpp | 5 + OpenSim/Simulation/StatesTrajectoryReporter.h | 8 +- .../Simulation/Test/testStatesDocument.cpp | 775 ++++++++++++++++++ .../Simulation/Test/testStatesTrajectory.cpp | 26 +- 11 files changed, 2318 insertions(+), 32 deletions(-) create mode 100644 OpenSim/Simulation/StatesDocument.cpp create mode 100644 OpenSim/Simulation/StatesDocument.h create mode 100644 OpenSim/Simulation/Test/testStatesDocument.cpp diff --git a/Bindings/OpenSimHeaders_simulation.h b/Bindings/OpenSimHeaders_simulation.h index 310397e1eb..6e839ea3e8 100644 --- a/Bindings/OpenSimHeaders_simulation.h +++ b/Bindings/OpenSimHeaders_simulation.h @@ -148,6 +148,7 @@ #include #include +#include #include #include diff --git a/Bindings/simulation.i b/Bindings/simulation.i index f8d1adc48b..b2b0f01fdf 100644 --- a/Bindings/simulation.i +++ b/Bindings/simulation.i @@ -253,6 +253,7 @@ OpenSim::ModelComponentSet; %template(StdVectorIMUs) std::vector< OpenSim::IMU* >; +%include %include // This enables iterating using the getBetween() method. %template(IteratorRangeStatesTrajectoryIterator) diff --git a/CHANGELOG.md b/CHANGELOG.md index 60e5e67be5..e6fa747b6e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,8 +12,8 @@ v4.6 the case where provided string is just the name of the value, rather than a path to it (#3782) - Fixed bugs in `MocoStepTimeAsymmetryGoal::printDescriptionImpl()` where there were missing or incorrect values printed. (#3842) - Added `ModOpPrescribeCoordinateValues` which can prescribe motion of joints in a model given a table of data. (#3862) -- Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to - allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from +- Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to + allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from occurring in the output. (#3867) - Added `Output`s to `ExpressionBasedCoordinateForce`, `ExpressionBasedPointToPointForce`, and `ExpressionBasedBushingForce` for accessing force values. (#3872) - `PointForceDirection` no longer has a virtual destructor, is `final`, and its `scale` functionality @@ -23,12 +23,24 @@ v4.6 components. The `ForceProducer` API was also rolled out to a variety of existing `Force` components, which means that API users can now now ask many `Force` components what forces they produce (see #3891 for a comprehensive overview). -- Made improvements to `MocoUtilities::createExternalLoadsTableForGait()`: center of pressure values are now set to zero, rather - than NaN, when vertical force is zero, and the vertical torque is returned in the torque columns (rather than the sum of the +- Made improvements to `MocoUtilities::createExternalLoadsTableForGait()`: center of pressure values are now set to zero, rather + than NaN, when vertical force is zero, and the vertical torque is returned in the torque columns (rather than the sum of the sphere torques) to be consistent with the center of pressure GRF representation. - Fixed an issue where a copy of an `OpenSim::Model` containing a `OpenSim::ExternalLoads` could not be finalized (#3926) -- Updated all code examples to use c++14 (#3929) +- Updated all code examples to use c++14 (#3929) +- Added class `OpenSim::StateDocument` as a systematic means of serializing and deserializing a complete trajectory + (i.e., time history) of all states in the `SimTK::State` to and from a single `.ostates` file. Prior to `StatesDocument`, + only the trajectories of continuous states (e.g., joint angles, joint speeds, muscle forces, and the like) could be systematically + written to file, either in the form of a `Storage` file or as a `TimeSeriesTable` file, leaving discrete states (e.g., muscle + excitations and contact model anchor points) and modeling options (e.g., joint clamps) unserialized. `StatesDocument`, on the + other hand, serializes all continuous states, discrete states, and modeling options registered with `OpenSim::Component`. + Whereas neither `Storage` files nor `TimeSeriesTable` files are currently able to handle mixed variable types, `StatesDocument` + is able to accommodate variables of type `bool`, `int`, `double`, `Vec2`, `Vec3`, `Vec4`, `Vec5`, and `Vec6` all in the same + `.ostates` file. `.ostate` files are written in `XML` format and internally carry the name of the OpenSim model to which the + states belong, a date/time stamp of when the file was written, and a user-specified note. `.ostate` files also support a + configurable output precision. At the highest ouput precsion (~20 significant figures), serialization/deserialization is + a lossless process. (#3902) v4.5.1 ====== diff --git a/OpenSim/Common/Test/testComponentInterface.cpp b/OpenSim/Common/Test/testComponentInterface.cpp index 984760a491..74db2cc5fe 100644 --- a/OpenSim/Common/Test/testComponentInterface.cpp +++ b/OpenSim/Common/Test/testComponentInterface.cpp @@ -390,7 +390,7 @@ class Bar : public Component { // and to access a dv that is not type double. // The following calls put the mo and dv into the maps used to contain // all mo's and dv's exposed in OpenSim. When Stage::Topology is - // realized, they will allocated in class Bar's override of + // realized, they will be allocated in class Bar's override of // extendRealizeTopology(). See below. bool allocate = false; int maxFlagValue = 1; @@ -401,6 +401,8 @@ class Bar : public Component { // Manually allocate and update the index and subsystem for // a discrete variable and a modeling option as though they were // natively allocated in Simbody and brought into OpenSim. + // Note, as of May 2024, this is also what one would need to do in order + // to add a discrete variable that is a type other than double. void extendRealizeTopology(SimTK::State& state) const override { Super::extendRealizeTopology(state); @@ -2057,7 +2059,7 @@ TEST_CASE("Component Interface State Trajectories") } // Create a new state trajectory (as though deserializing) - // newTraj must be must the expected size before any set calls. + // newTraj must be the expected size before any set calls. SimTK::Array_ newTraj; for (int i = 0; i < nsteps; ++i) newTraj.emplace_back(s); // state variables diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp new file mode 100644 index 0000000000..c709976f81 --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -0,0 +1,769 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: StatesDocument.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2022-2024 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +#include "StatesDocument.h" + +using namespace SimTK; +using namespace SimTK::Xml; +using namespace std; +using namespace OpenSim; +using std::cout; + +namespace OpenSim { + +// Anonymous namespace to ensure local linkage +namespace { + +//----------------------------------------------------------------------------- +// Local utility methods for use with class StatesDocument +//----------------------------------------------------------------------------- +struct SDocUtil { + + //_________________________________________________________________________ + template + static + void + appendVarElt(const string& path, const string& tag, const string& type, + const Array_& valArr, Element& parent, int precision) + { + // Create the variable element. + Element varElt(tag); + varElt.setAttributeValue("path", path); + varElt.setAttributeValue("type", type); + + // Append the variable element + varElt.setValueAs>(valArr, precision); + parent.appendNode(varElt); + } + //_________________________________________________________________________ + template + inline + static + void + getEltValue(const string& path, size_t expectedSize, + Element& varElt, Array_& vArr) + { + // Interpret the element value + varElt.getValueAs>(vArr); + + // Check the size + size_t n = vArr.size(); + SimTK_ASSERT3_ALWAYS(n == expectedSize, + "Found %d values in the element for %s, but there should be %d", + n, path.c_str(), expectedSize); + } + //_________________________________________________________________________ + template + inline + static + void + initializeStatesForStateVariable(Element& varElt, const Model& model, + const string& path, Array_ & traj) + { + // Interpret the element an array of type T + Array_ vArr; + getEltValue(path, traj.size(), varElt, vArr); + + // Set variable in the States trajectory + model.setStateVariableTrajectory(path, vArr, traj); + } + //_________________________________________________________________________ + template + inline + static + void + initializeStatesForDiscreteVariable(Element& varElt, const Model& model, + const string& path, Array_ & traj) + { + // Interpret the element an array of type T + Array_ vArr; + getEltValue(path, traj.size(), varElt, vArr); + + // Set variable in the States trajectory + model.setDiscreteVariableTrajectory(path, vArr, traj); + } + //_________________________________________________________________________ + template + inline + static + void + initializeStatesForModelingOption(Element& varElt, const Model& model, + const string& path, Array_ & traj) + { + // Interpret the Element value + Array_ vArr; + varElt.getValueAs>(vArr); + + // Check the sizes. + size_t n = vArr.size(); + SimTK_ASSERT2_ALWAYS(n == traj.size(), + "Found %d values. Should match nTime = %d values.", + n, traj.size()); + + // Set variable in the States trajectory + model.setModelingOptionTrajectory(path, vArr, traj); + } +}; + +} // End anonymous namespace +} // End OpenSim namespace + +// Note that the methods below are still in the OpenSim namespace. +// That namespace declaration is taken care of in the .h file. + +//----------------------------------------------------------------------------- +// Construction +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +StatesDocument:: +StatesDocument(const Model& model, const Array_& trajectory, + const String& note, int p) +{ + this->note = note; + this->precision = clamp(1, p, SimTK::LosslessNumDigitsReal); + formDoc(model, trajectory); +} +//_____________________________________________________________________________ +StatesDocument:: +StatesDocument(const Model& model, const vector& trajectory, + const String& note, int p) +{ + this->note = note; + this->precision = clamp(1, p, SimTK::LosslessNumDigitsReal); + + // Repackage the trajectory of states as a SimTK::Array_<>, which is + // the container type used by this class and also by the underlying + // trajectory-related methods in OpenSim::Component. + // + // The constructor below is shallow; it does not create copies of + // the contained State elements. The Array_<> refers directly to the + // contents of trajectory. Hence, the repackaging is quite inexpensive + // computationally. + // + // Unfortunately, this constructor does not have a const version, so + // the const modifier of trajectory has to be cast away. The vector is, + // however, safe from changes. Note that the method `formDoc()` only + // takes a const trajectory. + vector& trajectoryNonconst = const_cast&>(trajectory); + Array_ traj(trajectoryNonconst, SimTK::DontCopy()); + + formDoc(model, traj); +} + +//----------------------------------------------------------------------------- +// Serialize +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +serialize(const SimTK::String& filename) { + doc.writeToFile(filename); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formDoc(const Model& model, const Array_& traj) { + formRootElement(model, traj); + formNoteElement(model, traj); + formTimeElement(model, traj); + formContinuousElement(model, traj); + formDiscreteElement(model, traj); + formModelingElement(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formRootElement(const Model& model, const Array_& traj) { + // Set the tag of the root element and get an iterator to it. + doc.setRootTag("ostates"); + Element rootElt = doc.getRootElement(); + + // Insert a comment at the top level, just before the root node. + string info = "OpenSim States Document (Version: "; + info += std::to_string(model.getDocumentFileVersion()); + info += ")"; + Xml::Comment comment(info); + Xml::node_iterator root_it = doc.node_begin(Xml::ElementNode); + doc.insertTopLevelNodeBefore(root_it, comment); + + // Date and time + const std::time_t now = std::time(nullptr); + const char *localeName = "C"; + std::locale::global(std::locale(localeName)); + char buf[64]; + strftime(buf, sizeof buf, "%a %b %e %Y %H:%M:%S %Z", std::localtime(&now)); + + // Add attributes to the root node + rootElt.setAttributeValue("model", model.getName()); + rootElt.setAttributeValue("nTime", std::to_string(traj.size())); + rootElt.setAttributeValue("precision", std::to_string(precision)); + rootElt.setAttributeValue("date", buf); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formNoteElement(const Model& model, const Array_& traj) { + Element noteElt = Element("note"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(noteElt); + noteElt.setValue(note); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formTimeElement(const Model& model, const Array_& traj) { + // Form time element. + Element timeElt = Element("time"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(timeElt); + + // Get time values from the StatesTrajectory + int n = (int)traj.size(); + SimTK::Array_ time(n); + for (int i = 0; i < n; ++i) { + time[i] = traj[i].getTime(); + } + + // Set the text value on the element + timeElt.setValueAs>(time, precision); +} +//_____________________________________________________________________________ +// Supported continuous variable type (October 2024): double +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +formContinuousElement(const Model& model, const Array_& traj) { + // Form continuous element. + Element contElt = Element("continuous"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(contElt); + + // Get a list of all state variables names from the model. + OpenSim::Array paths = model.getStateVariableNames(); + + // Loop over the names. + // Get the vector of values of each and append as a child element. + int n = paths.getSize(); + for (int i = 0; i < n; ++i) { + Array_ val; + model.getStateVariableTrajectory(paths[i], traj, val); + SDocUtil::appendVarElt(paths[i], "variable", "double", + val, contElt, precision); + } +} +//_____________________________________________________________________________ +// Supported discrete variable types (October 2024): +// bool, int, float, double, Vec2, Vec3, Vec4, Vec5, Vec6 +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim by adding the appropriate `else if` block below. +// +void +StatesDocument:: +formDiscreteElement(const Model& model, const Array_& traj) { + // Form discrete element. + Element discreteElt = Element("discrete"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(discreteElt); + + // Get a list of all discrete variable names from the model. + OpenSim::Array paths = model.getDiscreteVariableNames(); + + // Loop over the names. + // Get the vector of values for each and append as a child element. + int n = paths.getSize(); + for (int i = 0; i < n; ++i) { + // Get a single discrete variable so that its type can be discerned + const AbstractValue &v = + model.getDiscreteVariableAbstractValue(traj[0], paths[i]); + + // Append the vector according to type + if (SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "bool", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "int", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "float", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "double", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec2", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec3", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec4", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec5", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec6", + vArr, discreteElt, precision); + } + else { + string msg = "Unrecognized type: " + v.getTypeName(); + SimTK_ASSERT(false, msg.c_str()); + } + } + +} +//_____________________________________________________________________________ +// Supported modeling option type (October 2024): int +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +formModelingElement(const Model& model, const Array_& traj) { + // Form continuous element. + Element modelingElt = Element("modeling"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(modelingElt); + + // Get a list of all modeling option names from the model. + OpenSim::Array paths = model.getModelingOptionNames(); + + // Loop over the names. + // Get the vector of values of each and append as a child element. + int n = paths.getSize(); + for (int i = 0; i < n; ++i) { + Array_ val; + model.getModelingOptionTrajectory(paths[i], traj, val); + SDocUtil::appendVarElt(paths[i], "option", "int", + val, modelingElt, precision); + } +} + + +//----------------------------------------------------------------------------- +// Deserialize +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +deserialize(const Model& model, Array_& traj) { + checkDocConsistencyWithModel(model); + initializeNumberOfStateObjects(); + initializePrecision(); + initializeNote(); + prepareStatesTrajectory(model, traj); + initializeTime(traj); + initializeContinuousVariables(model, traj); + initializeDiscreteVariables(model, traj); + initializeModelingOptions(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +deserialize(const Model& model, vector& trajectory) { + checkDocConsistencyWithModel(model); + initializeNumberOfStateObjects(); + initializePrecision(); + initializeNote(); + + // Repackage the trajectory of states as a SimTK::Array_<>, which is + // the container type used by this class and also by the underlying + // trajectory-related methods in OpenSim::Component. + // The following constructor is shallow; it does not create copies of + // the contained State elements. The Array_<> refers directly to the + // contents of trajectory. Hence, 1) the repackaging is quite inexpensive + // computationally, and 2) when the contents of `traj` are changed, + // so are the contents of `trajectory`. + prepareStatesTrajectory(model, trajectory); + Array_ traj(trajectory, SimTK::DontCopy()); + + initializeTime(traj); + initializeContinuousVariables(model, traj); + initializeDiscreteVariables(model, traj); + initializeModelingOptions(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +checkDocConsistencyWithModel(const Model& model) { + // At this point, only the model name is checked here. + // Many other aspects are checked for consistency than just the model + // name. Those are more easily checked as the doc is parsed. + + // Check that name of the model in the doc matches the name of the model'. + Element rootElt = doc.getRootElement(); + Attribute modelNameAttr = rootElt.getOptionalAttribute("model"); + SimTK_ASSERT1(modelNameAttr.isValid(), + "The 'model' attribute of the root element was not found in file %s.", + filename.c_str()); + const SimTK::String& modelName = modelNameAttr.getValue(); + if (modelName != model.getName()) { + SimTK::String msg = "The model name (" + modelName + ")"; + msg += " in states document " + filename + " does not match"; + msg += " the name of the OpenSim model (" + model.getName() + ")"; + msg += " for which the states are being deserialized."; + SimTK_ASSERT_ALWAYS(false, msg.c_str()); + } + +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializeNumberOfStateObjects() { + // The number of State objects should be the same as the number of time + // stamps. That is, nStateObjects = nTime. + Element rootElt = doc.getRootElement(); + Attribute nTimeAttr = rootElt.getOptionalAttribute("nTime"); + bool success = nTimeAttr.getValue().tryConvertTo(nStateObjects); + SimTK_ASSERT_ALWAYS(success, + "Unable to acquire nTime from root element."); + SimTK_ASSERT1_ALWAYS(nStateObjects > 0, + "Root element attribute numStateObjects=%d; should be > 0.", + nStateObjects); +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializePrecision() { + // Find the element + Element rootElt = doc.getRootElement(); + Attribute precisionAttr = rootElt.getOptionalAttribute("precision"); + int p; + bool success = precisionAttr.getValue().tryConvertTo(p); + SimTK_ASSERT_ALWAYS(success, + "Unable to acquire the precision from the root element."); + this->precision = clamp(1, p, SimTK::LosslessNumDigitsReal); +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializeNote() { + // Find the element + Element rootElt = doc.getRootElement(); + Array_ noteElts = rootElt.getAllElements("note"); + + // Check the number of note elements found. Should be 1. + if (noteElts.size() == 0) { + this->note = ""; + } + else if (noteElts.size() > 1) { + cout << "StatesDocument: More than 1 `note` element found; "; + cout << "using just the first one." << endl; + } + + // Get the value + this->note = noteElts[0].getValue(); +} +//_____________________________________________________________________________ +// Note that this method is overloaded to permit users the flexibility of +// using either SimTK::Array_<> or std::vector<> as the trajectory container. +void +StatesDocument:: +prepareStatesTrajectory(const Model& model, Array_& traj) { + // Create a local copy of the Model and get its default State. + Model localModel(model); + SimTK::State defaultState = localModel.initSystem(); + + // Append the needed number of state objects to the trajectory. + // A copy of the default state is made with each call of emplace_back(). + // These copies will be initialized during the rest of the deserialization + // process. + for (int i=0; i < nStateObjects; ++i) traj.emplace_back(defaultState); +} +//_____________________________________________________________________________ +// Note that this method is overloaded to permit users the flexibility of +// using either SimTK::Array_<> or std::vector<> as the trajectory container. +void +StatesDocument:: +prepareStatesTrajectory(const Model& model, vector& traj) { + // Create a local copy of the Model and get its default State. + Model localModel(model); + SimTK::State defaultState = localModel.initSystem(); + + // Append the needed number of state objects to the trajectory. + // A copy of the default state is made with each call of emplace_back(). + // These copies will be initialized during the rest of the deserialization + // process. + for (int i=0; i < nStateObjects; ++i) traj.emplace_back(defaultState); +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializeTime(Array_& traj) { + // Find the element + Element rootElt = doc.getRootElement(); + Array_ timeElts = rootElt.getAllElements("time"); + + // Check the number of time elements found. Should be 1. + SimTK_ASSERT1_ALWAYS(timeElts.size() == 1, + "%d time elements found. Only 1 should be found.", timeElts.size()); + + // Get the values + Array_ timeArr; + timeElts[0].getValueAs>(timeArr); + + // Check the size of the time array. + size_t n = timeArr.size(); + SimTK_ASSERT2_ALWAYS(n == traj.size(), + "Found %d time values. Should match numStateObjects = %d", + n, traj.size()); + + // Initialize the State objects + for (size_t i = 0; i < n; ++i) traj[i].setTime(timeArr[i]); +} +//_____________________________________________________________________________ +// Supported continuous variable type (October 2024): double +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +initializeContinuousVariables(const Model& model, SimTK::Array_& traj) { + // Find the 'continuous' element + SimTK::String tag = "continuous"; + Element rootElt = doc.getRootElement(); + Array_ contElts = rootElt.getAllElements(tag); + SimTK_ASSERT1_ALWAYS(contElts.size() == 1, + "Found %d elements with tag 'continuous'. Should only be 1.", + contElts.size()); + + // Find all the child 'variable' elements + SimTK::String childTag = "variable"; + Array_ varElts = contElts[0].getAllElements(childTag); + + // Check that the number matches the number of continous variables. + // In OpenSim, a continuous variable is referred to as a StateVariable. + OpenSim::Array varNames = model.getStateVariableNames(); + int n = varElts.size(); + int m = varNames.size(); + SimTK_ASSERT2_ALWAYS(n == m, + "Found %d continuous variable elements. Should be %d.", n, m); + + // Loop over the variable elements + SimTK::Array_ varArr; + for (int i = 0; i < n; ++i) { + // type + Attribute typeAttr = varElts[i].getOptionalAttribute("type"); + const SimTK::String &type = typeAttr.getValue(); + + // path + Attribute pathAttr = varElts[i].getOptionalAttribute("path"); + const SimTK::String path = pathAttr.getValue(); + + // Switch based on the type. + // Type double is expected for continuous variable elements. + if (type == "double") { + SDocUtil::initializeStatesForStateVariable(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} +//_____________________________________________________________________________ +// Supported discrete variable types (October 2024): +// bool, int, float, double, Vec2, Vec3, Vec4, Vec5, Vec6 +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim by adding the appropriate `else if` block below. +// +void +StatesDocument:: +initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { + Element rootElt = doc.getRootElement(); + Array_ discElts = rootElt.getAllElements("discrete"); + SimTK_ASSERT1_ALWAYS(discElts.size() == 1, + "Found %d elements with tag 'discrete'. Only 1 should be found.", + discElts.size()); + + // Find all the child 'variable' elements + SimTK::String childTag = "variable"; + Array_ varElts = discElts[0].getAllElements(childTag); + + // Check that # children matches the number of discrete variables. + OpenSim::Array varNames = model.getDiscreteVariableNames(); + int n = varElts.size(); + int m = varNames.size(); + SimTK_ASSERT2_ALWAYS(n == m, + "Found %d discrete variable elements. Should be %d.", n, m); + + // Loop over the variable elements + for (int i = 0; i < n; ++i) { + // type + Attribute typeAttr = varElts[i].getOptionalAttribute("type"); + const SimTK::String &type = typeAttr.getValue(); + + // path + Attribute pathAttr = varElts[i].getOptionalAttribute("path"); + const SimTK::String path = pathAttr.getValue(); + + // Switch based on the type + // Append the vector according to type + if (type == "bool") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "int") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "float") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "double") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec2") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec3") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec4") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec5") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec6") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} +//_____________________________________________________________________________ +// Supported continuous variable type (October 2024): int +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +initializeModelingOptions(const Model& model, SimTK::Array_& traj) { + // Find the element + Element rootElt = doc.getRootElement(); + Array_ modlElts = rootElt.getAllElements("modeling"); + SimTK_ASSERT1_ALWAYS(modlElts.size() == 1, + "%d modeling elements found. Only 1 should be found.", + modlElts.size()); + Element modlElt = modlElts[0]; + + // Find all the child 'variable' elements. + SimTK::String childTag = "option"; + Array_ varElts = modlElts[0].getAllElements(childTag); + + // Check that the number matches the number of continous variables. + // In OpenSim, a continuous variable is referred to as a StateVariable. + OpenSim::Array varNames = model.getModelingOptionNames(); + int n = varElts.size(); + int m = varNames.size(); + SimTK_ASSERT2_ALWAYS(n == m, + "Found %d modeling option elements. Should be %d.", n, m); + + // Loop over the modeling options + SimTK::Array_ varArr; + for (int i = 0; i < n; ++i) { + // type + Attribute typeAttr = varElts[i].getOptionalAttribute("type"); + const SimTK::String &type = typeAttr.getValue(); + + // path + Attribute pathAttr = varElts[i].getOptionalAttribute("path"); + const SimTK::String path = pathAttr.getValue(); + + // Switch based on the type. + // Type int is expected for modeling option elements. + if (type == "int") { + SDocUtil::initializeStatesForModelingOption(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h new file mode 100644 index 0000000000..2032212799 --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.h @@ -0,0 +1,664 @@ +#ifndef OPENSIM_STATES_DOCUMENT_H_ +#define OPENSIM_STATES_DOCUMENT_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: StatesDocument.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2023-2024 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +// INCLUDE +#include +#include "osimSimulationDLL.h" +#include + +namespace OpenSim { + +//============================================================================= +//============================================================================= +/** Class StatesDocument provides a means of writing (serializing) and +reading (deserializing) a complete time history of model states to and from +a file. This capability is key when analyzing model behavior, visualizing +simulation results, and conducting a variety of computationally demanding +tasks (e.g., fitting a model to experimental data, solving optimal +control problems, etc.). + +The states of an OpenSim::Model consist of all the independent variables that +change (or can change) during a simulation. At each time step during a +simulation, the underlying SimTK infrastructure captures the states in a +SimTK::State object. A state variable falls into one of the following +categories: + + 1) Continuous Variables (aka OpenSim::StateVariable%s) + 2) Discrete Variables + 3) Modeling Options + +Continuous Variables are governed by differential equations. They are +numerically integrated during a simulation based on the values of their +derivatives. Examples include joint coordinates, joint speeds, and muscle +activations. In OpenSim, because Continuous Variables are the most commonly +encountered kind of state, they are simply referred to as State Variables. All +concrete instances of Continuous Variables in OpenSim are derived from the +abstract class OpenSim::StateVariable. + +Discrete Variable are not governed by differential equations and so can change +discontinuously during a simulation. Examples can include inputs to a +simulation, like muscle excitations, coefficients of friction, and torque +motor voltages. Examples can also include outputs to a simulation, like points +of contact between colliding bodies and whether those bodies are experiencing +static or kinetic frictional conditions. Such output discrete variables are +updated at each time step during numerical integration. Unlike continuous +states, however, they are updated based on closed-form algebraic expressions +rather than based on their derivatives. In the underlying SimTK infrastructure, +an output discrete variable is implemented as a specialized kind of +discrete variable called an Auto-Update Discrete Variable. + +Modeling Options are flags, usually of type int, that are used to choose +between viable ways to model a SimTK::System or whether or not to apply a +constraint. Examples include a flag that specifies whether Euler angles or +quaternions are used to represent rotation or a flag that specifies whether a +particular joint coordinate is locked. When a Modeling Option is changed, +low-level aspects of the System must be reconstituted or, in SimTK +terminology, re-realized through SimTK::Stage::Model. + +Prior to the introduction of this class, only Continuous Variables (i.e., +OpenSim::StateVariable%s) were routinely and systematically serialized, +most commonly via the OpenSim::Manager as an OpenSim::Storage file +or via class OpenSim::StatesTrajectory as an OpenSim::TimeSeriesTable. +Discrete Variables and Modeling Options, if serialized, had to be stored in +separate files or handled as OpenSim::Property objects. In addition, prior to +this class, all Discrete Variables in OpenSim were assumed to be type double, +which is not a requirement of the underlying SimTK infrastructure. + +With the introduction of this class, all state variables {i.e., Continuous +Variables (OpenSim::StateVariable%s), Discrete Variables, and Modeling Options} +can be serialized in a single file, which by convention has the `.ostates` +file name exention. In addition, a variety of types (e.g., bool, int, double, +Vec3, Vec4, etc.) are supported for Discrete Variables. Continuous States are +still assumed to be type double, and Modeling Options are still assumed to be +type `int`. Note, however, that the `.ostates` file format has the +flexibility to relax these assumptions and include other types if needed. + +@note A point of clarification about Data Cache Variables... +By definition, state variables are independent. That is, the value of one +cannot be determined from the values of others. If a quantity of interest can +be computed from values of state variables, particularly if that quantity is +needed frequently, that quantity is often formalized as a Data Cache Variable. +The value of a Data Cach Variable is computed at each time step of a simulation +and stored in the SimTK::State. However, because a Data Cache Variable can +always be computed from the Continuous Variables, Discrete Variables, and +Modeling Options, they are not serialized. + + SimTK::State Contents | Serialized in `.ostates`? + ------------------------ | ----------------------- + Continuous Variables | yes + Discrete Variables | yes + Modeling Options | yes + Data Cache Variables | no + + +----------------- +Design Notes +----------------- + +### Dependencies +Most operations in class StatesDocument rely on underlying SimTK classes, +most notably SimTK::String, SimTK::Array, SimTK::State, and SimTK::Xml. + +StatesDocument has just one key OpenSim dependency: OpenSim::Model. +OpenSim::Model brings with it all the methods it inherits from class +OpenSim::Component, which are essential for getting and setting state +information in OpenSim. StatesDocument does not know about classes like +OpenSim::Storage, OpenSim::TimeSeriesTable, OpenSim::StatesTrajectory, or +OpenSim::Manager. + +Exchanges of state information between class StatesDocument and the rest of +OpenSim are accomplished via objects of type SimTK::Array_, or +alternatively std::vector, which are informally referred to as +state trajectories (see directly below). + +### Trajectories +In many methods of this class, as well as in related classes, you will +encounter the term 'trajectory'. In these contexts, the term connotes a +time-ordered sequence, or a time-history, of values. + +An array of knee angles (-10.0, -2.3, 4.5, 6.2, 7.1) would be termed a knee +angle trajectory if those knee angles were recorded sequentially during a +simulation. Similarly, an array of SimTK::State objects, if time ordered, +would be called a states trajectory. + +Because of the flexibility and computational speed of the SimTK::Array_ +container class, you will often see trajectories passed in argument lists as +SimTK::Array_%s. SimTK::Array_ might represent the trajectory of a +knee angle. SimTK::Array_ might represent the trajectory of the +center of pressure between a foot and the floor during a walking motion. +SimTK::Array_ is used to capture the full trajectory of states +(continuous variables, discrete variables, and modeling options) recorded +during a simulation. + +@Note SimTK::Array_ is preferred over std::vector +for reasons of performance, binary compatibility with Simbody libraries, and +consistency with Simbody's underlying code base. For a variety of common +operations, like indexing through an array, SimTK::Array_ is about +twice as fast as std::vector_ on Windows systems. Such speed differences +may not be as large on Mac or Ubuntu systems, but it is safe to assume that +SimTK::Array_ will be just as fast or have a speed advantage. + +Th `StatesDocument` class relies heavily on a few trjectory-centric methods +available in the OpenSim::Component class. A few examples follow. + +``` + template + Component::getDiscreteVariableTrajectory( + const std::string& path, + const SimTK::Array_& input, + SimTK::Array_& output) const +``` +A call to the above method first finds a Discrete Variable in the component +hierarchy based on the specifed path (`path`). Then, from the input states +trajectory (`input`), the method extracts the values of the specified +Discrete Variable and returns its trajectory as the output (`output`). +Notice that the type of the Discrete Variable can be specified by the caller +(i.e., T = int, double, Vec3, Vec4, etc.). + +``` + template + void setDiscreteVariableTrajectory( + const std::string& path, + const SimTK::Array_& input, + SimTK::Array_& output) const +``` +On the other hand, based on the input trajectory of a specified Discrete +Variable (`input`), a call to the above method sets the appropriate +element in each of the SimTK::State objects held in the states trajectory +(`output`). Notice again that the type T of the Discrete Variable can be +specified by the caller. + +### Complete and Constant XML Document upon Construction +Upon construction, a StatesDocument instance always contains a complete +internal XML document that represents a complete serialization of a specific +model's state trajectory. Moreover, that internal XML document cannot be +altered after construction! + +If a model is changed (e.g., a muscle or contact element is added) or +a change has occurred in its state trajectory, the intended way to generate +an XML document that reflects those changes is to construct a new +StatesDocument instance. Constructing a new instance is the most reliable +approach for ensuring an accurate serialization. This approach also greatly +simplifies the implementation of the StatesDocument class, as methods for +selectively editing aspects of the internal XML document are consequently +unnecessary. + +### Output Precision +The precision with which numbers are serialized to a `.ostates` file can be +specified at the time of construction. The `precision` parameter specifies +the maximum number of significant digits used to represent numbers. If a +number can be represented without data loss with fewer digits, fewer digits +are used. In other words, trailing zeros are not written to file, thus +reducing file size. For example, if `precision` = 5, the number +1.50000000000000000000 would be represented in a `.ostates` file +as '1.5'; however, π would be represented as '3.1415'. + +By default, the `precision` parameter of a `StatesDocument` is set to the +constant `SimTK::LosslessNumDigitsReal`, which results in lossless +serialization. When `precision` = `SimTK::LosslessNumDigitsReal`, the +`SimTK::State` can be serialized and deserialized repeatedly without loss +of information. `SimTK::LosslessNumDigitsReal` is platform dependent but +typically has a value of about `20`. In applications where exact values of the +states are needed, lossless precision should be used. In applications where +exact values of the states are not needed, a smaller number of digits can be +used (e.g., `precsion = 6`) as a means of reducing the size of a `.ostates` +file or simplifying some types of post analysis (e.g., plotting where the extra +significant figures would go unnoticed). + + +------------------- +.ostate File Format +------------------- +XML is used as the organizing framework for `.ostates` files +(see SimTK::Xml), allowing them to be viewed and edited with a text editor. +Internet browsers can be also be used to view a `.ostate` file but may +require a `.xml` file extension to be added to the file name for the +XML format to be recognized. + +### Sample `.ostates` File +``` + + + + + + (0,7.14, ...) + (0,7.81, ...) + ... + + + (~[2.1,-1.1,0],~[1.82,-1.1,0], ...) + (0.5,0.5, ...) + (0.7,0.7, ...) + (1,1, ...) + ... + + + + + ... + + +``` + +### Deserialization Requirements +Successful deserialization of a .ostates file and full initialization of a +states trajectory for an OpenSim::Model requires the following: + + 1) The name of the `OpenSim::Model` must match the value of the + `model` attribute of the top-level `ostates` element. + + 2) The number of continuous variables, discrete variables, and modeling + options in the .ostates file must match the corresponding numbers in the + OpenSim::Model. + + 3) The number of values recorded for each `variable` and each + `option` in the `.ostates` file must be equal to the value of the + `nTime` attribute of the top-level `ostates` element. + + 4) All `variable` and `option` paths must be found in the model + OpenSim::Component heirarchy. + + 5) The type must be supported. As of September 2024, the following types + are supported: + + SimTK::State Category | Supported Type(s) + ------------------------ | ----------------------- + Continuous Variables | double + | + Discrete Variables | bool, int, float, double, + | Vec2, Vec3, Vec4, Vec5, Vec6 + | + Modeling Options | int + + +-------------------------- +Using Class StatesDocument +-------------------------- +Below are some code snippets that show how the StatesDocument class can be +used. Example 1 shows how to obtain a states trajectory from a simulation and +then serialize those states to file. Example 2 shows how to follow up and +deserialize those same states and use them to accomplish a few basic things. + +### Example 1: Serializing Simulated States +``` + // --------------- + // Build the Model + // --------------- + // Building a model can be done in many ways. The most common approach is + // to construct a model from an OpenSim model file. Here, an empty model is + // constructed with place holders for components that are typically added. + OpenSim::Model model(); + model.setGravity( Vec3(0.0,-9.8,0.0) ); + model.setName("BouncingBlock"); + // Add bodies... + // Add joints... + // Add actuators & contact elements... + + // ------------------------------- + // Add a StatesTrajectory Reporter + // ------------------------------- + // The reporter records the SimTK::State in an std::vector<> at a + // specified time interval. + OpenSim::StatesTrajectoryReporter* reporter = + new StatesTrajectoryReporter(); + reporter->setName("states_reporter"); + double interval = 0.01; + reporter->set_report_time_interval(interval); + model->addComponent(reporter); + + // ----------------------------------------- + // Build the System and Initialize the State + // ----------------------------------------- + model.buildSystem(); + SimTK::State& state = model.initializeState(); + + // --------- + // Integrate + // --------- + Manager manager(*model); + manager.getIntegrator().setMaximumStepSize(0.01); + manager.setIntegratorAccuracy(1.0e-5); + double ti = 0.0; + double tf = 5.0; + state.setTime(ti); + manager.initialize(state); + state = manager.integrate(tf); + + // ----------------------- + // Create a StatesDocument + // ----------------------- + // The reporter that was added to the system collects the states in an + // OpenSim::StatesTrajectory object. Underneath the covers, the states are + // accumulated in a private array of state objects (i.e., vector). + // The StatesTrajectory class knows how to export a StatesDocument based + // on those states. This "export" functionality is what is used below. + const StatesTrajectory& trajectory = reporter->getStates(); + StatesDocument doc = trajectory.exportToStatesDocument(model); + + // Alternatively, a read-only reference to the underlying state array + // can be obtained from the reporter and used to construct a + // StatesDocument directly: + const std::vector& traj = reporter->getVectorOfStateObjects(); + StatesDocument doc(model, traj); + + // ---------------------------- + // Serialize the States to File + // ---------------------------- + // The file name (see below), can be any string supported by the file + // system. The recommended convention is for the file name to carry the + // suffix ".ostates". Below, the suffix ".ostates" is simply added to + // the name of the model, and the document is saved to the current working + // directory. The file name can also incorporate a valid system path (e.g., + // "C:/Users/smith/Documents/Work/BouncingBlock.ostates"). + SimTK::String statesFileName = model.getName() + ".ostates"; + doc.serializeToFile(statesFileName); + + // ---------------------- + // Save the Model to File + // ---------------------- + SimTK::String modelFileName = model.getName() + ".osim"; + model->print(modelFileName); + +``` + +### Example 2: Deserializing States +``` + // ----------------------------- + // Construct the Model from File + // ----------------------------- + SimTK::String name = "BouncingBlock"; + SimTK::String modelFileName = name + ".osim"; + OpenSim::Model model(modelFileName); + model.buildSystem(); + SimTK::State& initState = model->initializeState(); + + // ----------------------------------------------- + // Construct the StatesDocument Instance from File + // ----------------------------------------------- + SimTK::String statesFileName = name + ".ostates"; + StatesDocument doc(statesFileName); + + // ---------------------- + // Deserialize the States + // ---------------------- + // Note that model and document must be entirely consistent with each + // other for the deserialization to be successful. + // See StatesDocument::deserialize() for details. + SimTK::Array_ traj; // Or, std::vector traj; + doc.deserialize(model, traj); + + // Below are some things that can be done once a deserialized state + // trajectory has been obtained. + + // --------------------------------------------------- + // Iterate through the State Trajectory Getting Values + // --------------------------------------------------- + std::string path; + const SimTK::State* iter; + for(iter = traj.cbegin(); iter!=traj.cend(); ++iter) { + + // Get time + double t = iter->getTime(); + + // Get the value of a continuous state + path = "/jointset/free/free_coord_0/value"; + double x = model.getStateVariableValue(*iter, path); + + // Get the value of a discrete state of type double + path = "/forceset/EC0/sliding"; + double sliding = model.getDiscreteVariableValue(*iter, path); + + // Get the value of a discrete state of type Vec3 + path = "/forceset/EC0/anchor" + const SimTK::AbstractValue& valAbs = + model.getDiscreteVariableAbstractValue(*iter, path); + SimTK::Value valVec3 = SimTK::Value::downcast( valAbs ); + Vec3 anchor = valVec3.get(); + + // Get the value of a modeling option + path = "/jointset/free/free_coord_0/is_clamped"; + int clamped = model.getModelingOption(*iter, path); + + // Access the value of a data cache variable. Note that this will + // require state realization at the appropriate stage. + system.realize(*iter, SimTK::Stage::Dynamics); + Vec3 force = forces.getContactElement(0)->getForce(); + } + + // ---------------------------------------------------- + // Extract a Complete Trajectory for a Particular State + // ---------------------------------------------------- + // Continuous (double) + path = "/jointset/free/free_coord_0/value"; + SimTK::Array_ xTraj; + model.getStateVariableTrajectory(path, traj, xTraj); + + // Discrete (Vec3) + path = "/forceset/EC0/anchor"; + SimTK::Array_ anchorTraj; + model.getDiscreteVariableTrajectory(path, traj, anchorTraj); + + // Modeling (int) + path = "/jointset/free/free_coord_0/is_clamped"; + SimTK::Array_ clampedTraj; + model.getModelingOptionTrajectory(path, traj, clampedTraj); + + // ---------------------- + // Form a TimeSeriesTable + // ---------------------- + // Note that the table will only include the continuous states. + // This might be done for plotting, post analysis, etc. + StatesTrajectory trajectory(model, doc); + OpenSim::TimesSeriesTable table = traj.exportToTable(model); + +``` + +### A Final Note +Because Storage files (*.sto) and TimeSeriesTable files (*.tst) typically +capture only the continuous states of a system, using these files as the basis +for deserialization runs the risk of leaving discrete variables and modeling +options in the SimTK::State uninitialized. In such an approach, additional +steps may be needed to properly initialize all variables in the SimTK::State +(e.g., by relying on OpenSim::Properties and/or on supplemental input files). + +In contrast, the StatesDocument class can be relied upon to yield a complete +serialization and deserialization of the SimTK::State. If the StatesDocument +class is used to serialize and then deserialize a state trajectory that was +recorded during a simulation, all state variables in the State (continuous, +discrete, and modeling) will be saved to a single file during serizaliztion +and initialized upon deserialization of the document. + +@authors F. C. Anderson **/ +class OSIMSIMULATION_API StatesDocument { + +public: + //------------------------------------------------------------------------- + // Construction + //------------------------------------------------------------------------- + /** The default constructor serves no purpose other than satisfying a + compiler demand. */ + StatesDocument() { } + + /** Construct a StatesDocument instance from an XML file in preparation + for deserialzing the states into a states trajectory. Once constructed, + the document is not designed to be modified; it is a fixed snapshot of the + states stored by the file at the time of construction. If the XML file + changes, the intended mechanism for obtaining a document that is + consistent with the modifed XML file is simply to construct a new document. + By convention (and not requirement), a StatesDocument filename has + ".ostates" as its suffix. To deserialize the states, call + StatesDocument::deserialize() on the constructed document. Note that the + validity of the XML file is not tested until StatesDocument::deserialize() + is called. + + @param filename The name of the file, which may be prepended by the system + path at which the file resides (e.g., "C:/Documents/block.ostates"). */ + StatesDocument(const SimTK::String& filename) : filename(filename) { + doc.readFromFile(filename); + initializeNote(); + initializePrecision(); + } + + /** Construct a StatesDocument instance from a states trajectory in + preparation for serializing the trajectory to file. Once constructed, the + document is not designed to be modified; it is a fixed snapshot of the + states trajectory at the time of construction. The intended mechanism for + obtaining a document that is consistent with a modified or new states + trajectory is simply to construct a new document. To serialize the + constructed document to file, call StatesDocument::serialize(). + + @param model The OpenSim::Model to which the states belong. + @param trajectory An array containing the time-ordered sequence of + SimTK::State objects. + @param note Annotation note for this states document. By default, the note + is an empty string. + @param precision The number of significant figures with which numerical + values are converted to strings. The default value is + SimTK:LosslessNumDigitsReal (about 20), which allows for lossless + reproduction of state. */ + StatesDocument(const OpenSim::Model& model, + const SimTK::Array_& trajectory, + const SimTK::String& note = "", + int precision = SimTK::LosslessNumDigitsReal); + + /** Construct a StatesDocument instance from a states trajectory in + preparation for serializing the trajectory to file. Once constructed, the + document is not designed to be modified; it is a fixed snapshot of the + states trajectory at the time of construction. The intended mechanism for + obtaining a document that is consistent with a modified or new states + trajectory is simply to construct a new document. To serialize the + constructed document to file, call StatesDocument::serialize(). + + @param model The OpenSim::Model to which the states belong. + @param trajectory An array containing the time-ordered sequence of + SimTK::State objects. + @param note Annotation note for this states document. By default, the note + is an empty string. + @param precision The number of significant figures with which numerical + values are converted to strings. The default value is + SimTK:LosslessNumDigitsReal (about 20), which allows for lossless + reproduction of state. */ + StatesDocument(const OpenSim::Model& model, + const std::vector& trajectory, + const SimTK::String& note = "", + int precision = SimTK::LosslessNumDigitsReal); + + //------------------------------------------------------------------------- + // Accessors + //------------------------------------------------------------------------- + /** Get the annotation note for this states document. */ + const SimTK::String& getNote() const { return note; } + /** Get the precision for this states document. */ + int getPrecision() const { return precision; } + + //------------------------------------------------------------------------- + // Serialization + //------------------------------------------------------------------------- + /** Serialize the document to file. By convention (and not requirement), + a StatesDocument filename has ".ostates" as its suffix. + + @param filename The name of the file, which may include the file system + path at which to write the file (e.g., "C:/Documents/block.ostates"). */ + void serialize(const SimTK::String& filename); + + //------------------------------------------------------------------------- + // Deserialization + //------------------------------------------------------------------------- + /** Deserialize the states held by this document into a states trajectory. + If deserialization fails, an exception describing the reason for the + failure is thrown. For details, see the section called "Deserialization + Requirements" in the introductory documentation for this class. + @note This method is overloaded to allow users the flexibility to use + either `SimTK::Array_<>` or `std::vector` as the trajectory container. + + @param model The OpenSim::Model with which the states are to be associated. + @param trajectory The array into which the time-ordered sequence of + SimTK::State objects will be deserialized. + @throws SimTK::Exception */ + void deserialize(const OpenSim::Model& model, + SimTK::Array_& trajectory); + + /** Deserialize the states held by this document into a states trajectory. + If deserialization fails, an exception describing the reason for the + failure is thrown. For details, see the section called "Deserialization + Requirements" in the introductory documentation for this class. + @note This method is overloaded to allow users the flexibility to use + either `SimTK::Array_<>` or `std::vector` as the trajectory container. + + @param model The OpenSim::Model with which the states are to be associated. + @param trajectory The array into which the time-ordered sequence of + SimTK::State objects will be deserialized. + @throws SimTK::Exception */ + void deserialize(const OpenSim::Model& model, + std::vector& trajectory); + +protected: + // Serialization Helpers. + void formDoc(const Model& model, + const SimTK::Array_& traj); + void formRootElement(const Model& model, + const SimTK::Array_& traj); + void formNoteElement(const Model& model, + const SimTK::Array_& traj); + void formTimeElement(const Model& model, + const SimTK::Array_& traj); + void formContinuousElement(const Model& model, + const SimTK::Array_& traj); + void formDiscreteElement(const Model& model, + const SimTK::Array_& traj); + void formModelingElement(const Model& model, + const SimTK::Array_& traj); + + // Deserialization Helpers. + void checkDocConsistencyWithModel(const Model& model); + void initializeNumberOfStateObjects(); + void prepareStatesTrajectory(const Model& model, + SimTK::Array_ &traj); + void prepareStatesTrajectory(const Model& model, + std::vector &traj); + void initializeNote(); + void initializePrecision(); + void initializeTime(SimTK::Array_ &traj); + void initializeContinuousVariables(const Model& model, + SimTK::Array_ &traj); + void initializeDiscreteVariables(const Model& model, + SimTK::Array_ &traj); + void initializeModelingOptions(const Model& model, + SimTK::Array_ &traj); + +private: + // Member Variables + int precision{SimTK::LosslessNumDigitsReal}; + int nStateObjects{0}; + SimTK::Xml::Document doc; + SimTK::String filename{""}; + SimTK::String note{""}; + +}; // END of class StatesDocument + +} // end of namespace OpenSim + +#endif // OPENSIM_STATES_DOCUMENT_H_ diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index 8a4b265dcf..133aecab9d 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -28,6 +28,7 @@ #include #include #include +#include #include "osimSimulationDLL.h" @@ -47,7 +48,7 @@ class Model; // TODO See the bottom of this file for a class description to use once the // OSTATES file format is implemented. // -/** +/** * \section StatesTrajectory * This class holds a sequence of SimTK::State%s. You can obtain a * StatesTrajectory during a simulation via the StatesTrajectoryReporter. You @@ -75,7 +76,7 @@ class Model; * Python and MATLAB do not enforce constness and thus allow modifying the * trajectory. * - * \subsection st_using_model Using with an OpenSim:: Model + * \subsection st_using_model Using with an OpenSim:: Model * A StatesTrajectory is not very useful on its own, since neither the * trajectory nor the contained states know how the Component%s name the state * variables they create. You probably want to use the trajectory with an @@ -151,7 +152,7 @@ class OSIMSIMULATION_API StatesTrajectory { /// @{ /** Get a const reference to the state at a given index in the trajectory. * Here's an example of getting a state variable value from the first state - * in the trajectory: + * in the trajectory: * @code{.cpp} * Model model("subject01.osim"); * const StatesTrajectory states = getStatesTrajectorySomehow(); @@ -172,20 +173,20 @@ class OSIMSIMULATION_API StatesTrajectory { try { return m_states.at(index); } catch (const std::out_of_range&) { - OPENSIM_THROW(IndexOutOfRange, index, 0, + OPENSIM_THROW(IndexOutOfRange, index, 0, static_cast(m_states.size() - 1)); } } /** Get a const reference to the first state in the trajectory. */ - const SimTK::State& front() const { + const SimTK::State& front() const { return m_states.front(); } /** Get a const reference to the last state in the trajectory. */ - const SimTK::State& back() const { + const SimTK::State& back() const { return m_states.back(); } /// @} - + /** Iterator type that does not allow modifying the trajectory. * Most users do not need to understand what this is. */ typedef std::vector::const_iterator const_iterator; @@ -289,6 +290,56 @@ class OSIMSIMULATION_API StatesTrajectory { TimeSeriesTable exportToTable(const Model& model, const std::vector& stateVars = {}) const; + /** Export a complete trajectory of states (i.e., one that includes + * all continuous, discrete, and modeling states) to an + * OpenSim::StatesDocument. That StatesDocument instance can then be + * used to serialize the states to an OSTATES file or document string by + * calling `StatesDocument::serialize()`. + * + * Once the states have been serialized, they can be deserialized by + * constructing a new StatesDocument by calling + * ``` + * StatesDocument(const SimTK::String& filename) + * ``` + * and then calling: + * ``` + * StatesDocument::deserialize(const OpenSim::Model& model, + * std::vector& trajectory) + * ``` + * + * The .ostates format is plain-text XML (see SimTK::Xml) with a + * specifiable precision between 1 and 20 significant figures. A precision + * of 20 digits results in losselss de/serialization. + * + * A note of CAUTION: + * Using either + * + * StatesTrajectory StatesTrajectory::createFromStatesStorage() or + * StatesTrajectory StatesTrajectory::createFromStatesTable() + * + * to construct a StatesTrajectory instance will likely leave discrete + * states (i.e., OpenSim::DiscreteVariable%s) and modeling states + * (i.e., OpenSim::ModelingOptions%s) uninitialized. The reason is that + * Storage and TimeSeriesTable objects include only the continuous states + * (i.e., OpenSim::StateVariable%s). + * + * Thus, when relying on serialization and deserialization to reproduce a + * complete StatesTrajectory, a StatesDocument is the preferred means as + * it will include continuous, discrete, and modeling states. + */ + OpenSim::StatesDocument + exportToStatesDocument(const OpenSim::Model& model, + const SimTK::String& note = "", + int precision = SimTK::LosslessNumDigitsReal) const + { + return OpenSim::StatesDocument(model, m_states, note, precision); + } + + /** Get a read-only reference to the underlying state array. */ + const std::vector& getStateArray() const { + return m_states; + } + private: std::vector m_states; @@ -337,11 +388,11 @@ class OSIMSIMULATION_API StatesTrajectory { msg += " " + missingStates[i] + "\n"; } msg += " " + missingStates.back(); - + addMessage(msg); } }; - + /** Thrown when trying to create a StatesTrajectory from states data, and * the data contains columns that do not correspond to continuous state * variables. */ @@ -360,7 +411,7 @@ class OSIMSIMULATION_API StatesTrajectory { msg += " " + extraStates[i] + "\n"; } msg += " " + extraStates.back(); - + addMessage(msg); } }; @@ -519,7 +570,7 @@ class OSIMSIMULATION_API StatesTrajectory { * * A SimTK::State object contains many different types of data, but only some * are saved into the OSTATES file: - * + * * type of data | saved in OSTATES? * ---------------------------- | ----------------- * (continuous) state variables | yes diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.cpp b/OpenSim/Simulation/StatesTrajectoryReporter.cpp index 4e0939e41a..e738544231 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.cpp +++ b/OpenSim/Simulation/StatesTrajectoryReporter.cpp @@ -34,6 +34,11 @@ const StatesTrajectory& StatesTrajectoryReporter::getStates() const { return m_states; } +const std::vector& +StatesTrajectoryReporter::getVectorOfStateObjects() const { + return m_states.getStateArray(); +} + /* TODO we have to discuss if the trajectory should be cleared. void StatesTrajectoryReporter::extendRealizeInstance(const SimTK::State& state) const { diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.h b/OpenSim/Simulation/StatesTrajectoryReporter.h index 3ef28fb640..a17e750413 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.h +++ b/OpenSim/Simulation/StatesTrajectoryReporter.h @@ -41,9 +41,11 @@ class OSIMSIMULATION_API StatesTrajectoryReporter : public AbstractReporter { OpenSim_DECLARE_CONCRETE_OBJECT(StatesTrajectoryReporter, AbstractReporter); public: - /** Access the accumulated states. */ - const StatesTrajectory& getStates() const; - /** Clear the accumulated states. */ + /** Obtain the accumulated states as a StatesTrajectory object. */ + const StatesTrajectory& getStates() const; + /** Obtain the accumulated states as a low-level array of states. */ + const std::vector& getVectorOfStateObjects() const; + /** Clear the accumulated states. */ void clear(); protected: diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp new file mode 100644 index 0000000000..698efccde3 --- /dev/null +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -0,0 +1,775 @@ +/* -------------------------------------------------------------------------- * +* OpenSim: testComponentInterface.cpp * +* -------------------------------------------------------------------------- * +* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * +* See http://opensim.stanford.edu and the NOTICE file for more information. * +* OpenSim is developed at Stanford University and supported by the US * +* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * +* through the Warrior Web program. * +* * +* Copyright (c) 2024 Stanford University and the Authors * +* Author(s): F. C. Anderson * +* * +* Licensed under the Apache License, Version 2.0 (the "License"); you may * +* not use this file except in compliance with the License. You may obtain a * +* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * +* * +* Unless required by applicable law or agreed to in writing, software * +* distributed under the License is distributed on an "AS IS" BASIS, * +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * +* See the License for the specific language governing permissions and * +* limitations under the License. * +* -------------------------------------------------------------------------- */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace SimTK; +using namespace OpenSim; +using std::cout; +using std::endl; +using std::string; +using std::vector; + + +// Internal static methods and classes. +namespace +{ + +// Constant used to determine equality tolerances +const double padFactor = 1.0 + SimTK::SignificantReal; + + +//----------------------------------------------------------------------------- +// Create a force component derived from PointToPointSpring that adds a +// discrete state of each supported type (bool, int, double, Vec2, Vec3, +// Vec4, Vec5, Vec6). +class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring +{ + OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, + OpenSim::PointToPointSpring); + +private: + // Subsystem index + SubsystemIndex indexSS; + // Indexes of discrete variables + DiscreteVariableIndex indexBool; + DiscreteVariableIndex indexInt; + DiscreteVariableIndex indexDbl; + DiscreteVariableIndex indexVec2; + DiscreteVariableIndex indexVec3; + DiscreteVariableIndex indexVec4; + DiscreteVariableIndex indexVec5; + DiscreteVariableIndex indexVec6; + // Names of discrete variables + string nameBool{"dvBool"}; + string nameInt{"dvInt"}; + string nameDbl{"dvDbl"}; + string nameVec2{"dvVec2"}; + string nameVec3{"dvVec3"}; + string nameVec4{"dvVec4"}; + string nameVec5{"dvVec5"}; + string nameVec6{"dvVec6"}; + // Omit a discrete state altogether + int omit; + +public: + + // Constructor + // @param which Specify which discrete state name (0 to 7) to append the + // suffix to. + // @param suffix String to append to the discrete state name. + // @param omit Specify the discrete state to omit. + ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, + const PhysicalFrame& body2, SimTK::Vec3 point2, + double stiffness, double restlength, + int which = -1, const string& suffix = "", int omit = -1) : + PointToPointSpring(body1, point1, body2, point2, stiffness, restlength), + omit(omit) + { + switch (which) { + case(0) : + nameBool += suffix; + break; + case(1) : + nameInt += suffix; + break; + case(2) : + nameDbl += suffix; + break; + case(3) : + nameVec2 += suffix; + break; + case(4) : + nameVec3 += suffix; + break; + case(5) : + nameVec4 += suffix; + break; + case(6) : + nameVec5 += suffix; + break; + case(7) : + nameVec6 += suffix; + break; + } + } + + void + extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const override + { + Super::extendAddToSystemAfterSubcomponents(system); + + // Add the discrete state to the list of OpenSim Components + // For exception testing purposes, the member variable 'omit' is used + // to omit one state. + bool allocate = false; + if(omit!=0) addDiscreteVariable(nameBool, Stage::Position, allocate); + if(omit!=1) addDiscreteVariable(nameInt, Stage::Position, allocate); + if(omit!=2) addDiscreteVariable(nameDbl, Stage::Position, allocate); + if(omit!=3) addDiscreteVariable(nameVec2, Stage::Position, allocate); + if(omit!=4) addDiscreteVariable(nameVec3, Stage::Position, allocate); + if(omit!=5) addDiscreteVariable(nameVec4, Stage::Position, allocate); + if(omit!=6) addDiscreteVariable(nameVec5, Stage::Position, allocate); + if(omit!=7) addDiscreteVariable(nameVec6, Stage::Position, allocate); + } + + void + extendRealizeTopology(SimTK::State& s) const override + { + Super::extendRealizeTopology(s); + + // Create a mutableThis + ExtendedPointToPointSpring* mutableThis = + const_cast(this); + + // Get the Subsystem + const DefaultSystemSubsystem& fsub = getModel().getDefaultSubsystem(); + mutableThis->indexSS = fsub.getMySubsystemIndex(); + + // 0 Bool + if(omit != 0) { + bool dvBool{false}; + mutableThis->indexBool = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvBool), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameBool, indexSS, indexBool); + } + + // 1 Int + if(omit != 1) { + int dvInt{0}; + mutableThis->indexInt = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvInt), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameInt, indexSS, indexInt); + } + + // 2 Dbl + if(omit != 2) { + double dvDbl{0.0}; + mutableThis->indexDbl = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvDbl), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameDbl, indexSS, indexDbl); + } + + // 3 Vec2 + if(omit != 3) { + Vec2 dvVec2(0.1, 0.2); + mutableThis->indexVec2 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec2), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec2, indexSS, indexVec2); + } + + // 4 Vec3 + if(omit != 4) { + Vec3 dvVec3(0.1, 0.2, 0.3); + mutableThis->indexVec3 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec3), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec3, indexSS, indexVec3); + } + + // 5 Vec4 + if(omit != 5) { + Vec4 dvVec4(0.1, 0.2, 0.3, 0.4); + mutableThis->indexVec4 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec4), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec4, indexSS, indexVec4); + } + + // 6 Vec5 + if(omit != 6) { + Vec5 dvVec5(0.1, 0.2, 0.3, 0.4, 0.5); + mutableThis->indexVec5 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec5), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec5, indexSS, indexVec5); + } + + // 7 Vec6 + if(omit != 7) { + Vec6 dvVec6(0.1, 0.2, 0.3, 0.4, 0.5, 0.6); + mutableThis->indexVec6 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec6), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec6, indexSS, indexVec6); + } + } + + // Set the values of the discrete variables. + // The actual force calculation is done in SimTK::TwoPointLinearSpring. + // This method just provides a means of setting the added discrete + // variables so that they change during the course of a simulation. + // The discrete variables are just set to the generalized speeds. + virtual void computeForce(const SimTK::State& state, + SimTK::Vector_& bodyForces, + SimTK::Vector& generalizedForces) const override + { + Super::computeForce(state, bodyForces, generalizedForces); + + const SimTK::Vector& u = state.getU(); + + // 0 Bool + if (omit != 0) { + bool& vBool = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexBool)); + vBool = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexBool); + } + + // 1 Int + if (omit != 1) { + SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexInt)) = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexInt); + } + + // 2 Dbl + if (omit != 2) { + SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexDbl)) = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexDbl); + } + + // 3 Vec2 + if (omit != 3) { + Vec2& v2 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec2)); + v2[0] = u[0]; + v2[1] = u[1]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec2); + } + + // 4 Vec3 + if (omit != 4) { + Vec3& v3 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec3)); + v3[0] = u[0]; + v3[1] = u[1]; + v3[2] = u[2]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec3); + } + + // 5 Vec4 + if (omit != 5) { + Vec4& v4 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec4)); + v4[0] = u[0]; + v4[1] = u[1]; + v4[2] = u[2]; + v4[3] = u[3]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec4); + } + + // 6 Vec5 + if (omit != 6) { + Vec5& v5 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec5)); + v5[0] = u[0]; + v5[1] = u[1]; + v5[2] = u[2]; + v5[3] = u[3]; + v5[4] = u[4]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec5); + } + + // 7 Vec6 + if (omit != 7) { + Vec6& v6 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec6)); + v6[0] = u[0]; + v6[1] = u[1]; + v6[2] = u[2]; + v6[3] = u[3]; + v6[4] = u[4]; + v6[5] = u[5]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec6); + } + } + +}; // End of class ExtendedPointToPointSpring + + +//----------------------------------------------------------------------------- +// Other Local Static Methods +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +/** +Compute the maximum error that can result from rounding a value at a +specified precision. This method assumes a base-10 representation of the value. +@param value Value to be rounded. +@param precision Number of significant figures that will be retained in the +value. +@return Maximum rounding error. + +double +computeMaxRoundingError(double value, int precision) { + if (value == 0) return 0.0; + int p = clamp(1, precision, SimTK::LosslessNumDigitsReal); + double leastSigDigit = trunc(log10(fabs(value))-precision); + double max_eps = 0.5*pow(10.0, leastSigDigit); + if(max_eps < SimTK::LeastPositiveReal) return SimTK::LeastPositiveReal; + return max_eps; +} +*/ // No longer used, but might be useful elsewhere, so saving. + +//_____________________________________________________________________________ +/** +Compute the expected error that will occur as a result of rounding a value at +a specified precision. +@param value Value to be rounded. +@param precision Number of significant figures that will be retained in the +value. +@return Expected rounding error. +*/ +double +computeRoundingError(const double& value, int precision) { + int p = clamp(1, precision, SimTK::LosslessNumDigitsReal); + SimTK::String valueStr(value, precision); + double valueDbl; + if(!valueStr.tryConvertToDouble(valueDbl)) + cout << "Conversion to double failed" << endl; + return fabs(valueDbl - value); +} + +//_____________________________________________________________________________ +// Test for equality of the continuous variables in two state trajectories. +void +testEqualityForContinuousVariables(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + // Continuous variables are gathered efficiently without using any + // OpenSim::Component methods by using state.getQ(), state.getU(), and + // state.getZ(). + double tol; + double tA, tB; + const State* stateA = trajA.cbegin(); + const State* stateB = trajB.cbegin(); + + // Loop over time + for(int iTime=0; stateA!=trajA.cend(); ++iTime, ++stateA, ++stateB) { + + // Check subsystem consistency + // This checks that basic parameters like number of subystem, nq, nu, + // and nz are the same for two state objects. + REQUIRE(stateA->isConsistent(*stateB)); + + // Get time + tA = stateA->getTime(); + tB = stateB->getTime(); + tol = padFactor * computeRoundingError(tA, precision); + CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, tol)); + + // Check the number of subsystesm + int nsubA = stateA->getNumSubsystems(); + int nsubB = stateB->getNumSubsystems(); + REQUIRE(nsubA == nsubB); + + // Q + double diff; + const Vector& qA = stateA->getQ(); + const Vector& qB = stateB->getQ(); + int nq = qA.size(); + for (int i = 0; i < nq; ++i) { + tol = padFactor * computeRoundingError(qA[i], precision); + CHECK_THAT(qB[i], Catch::Matchers::WithinAbs(qA[i], tol)); + } + // U + const Vector& uA = stateA->getU(); + const Vector& uB = stateB->getU(); + int nu = uA.size(); + for (int i = 0; i < nu; ++i) { + tol = padFactor * computeRoundingError(uA[i], precision); + CHECK_THAT(uB[i], Catch::Matchers::WithinAbs(uA[i], tol)); + } + // Z + const Vector& zA = stateA->getZ(); + const Vector& zB = stateB->getZ(); + int nz = zA.size(); + for (int i = 0; i < nz; ++i) { + tol = padFactor * computeRoundingError(zA[i], precision); + CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], tol)); + } + } +} + +//_____________________________________________________________________________ +// Test for equality of a scalar variable in two state trajectories. +template +void +checkScalar(const Array_& a, const Array_& b, int precision) +{ + double tol; + Array_ dvA, dvB; + for (size_t i = 0; i < (size_t)dvA.size(); ++i) { + tol = padFactor*computeRoundingError(a[i], precision); + CHECK_THAT(b[i], Catch::Matchers::WithinAbs(a[i], tol)); + } +} + +//_____________________________________________________________________________ +// Test for equality of a Vector variable in two state trajectories. +template +void +checkVector(const Array_& a, const Array_& b, int precision) +{ + double tol; + for (size_t i = 0; i < (size_t)a.size(); ++i) { + for (size_t j = 0; j < (size_t)a[i].size(); ++j) { + tol = padFactor*computeRoundingError(a[i][j], precision); + CHECK_THAT(b[i][j], Catch::Matchers::WithinAbs(a[i][j], tol)); + } + } +} + +//_____________________________________________________________________________ +// Test the equality of the discrete variables. +// +// The SimTK API does not allow an exhaustive, low-level comparison of +// discrete variables on the SimTK side. +// +// The comparision is done only for the discrete variables registered +// in the OpenSim Component heirarchy. Any discrete variable that is +// not registered in OpenSim will not be serialized, deserialized, or +// compared in this unit test. +void +testEqualityForDiscreteVariables(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + // Loop over the named variables + OpenSim::Array paths = model.getDiscreteVariableNames(); + int nPaths = paths.getSize(); + for (int i = 0; i < nPaths; ++i) { + + // Get one variable so that its type can be ascertained. + const AbstractValue& abstractVal = + model.getDiscreteVariableAbstractValue(trajA[i],paths[i]); + + // Get the trajectory for the discrete variable + if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + for (size_t j = 0; j < (size_t)dvA.size(); ++j) + CHECK(dvB[j] == dvA[j]); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + for (size_t j = 0; j < (size_t)dvA.size(); ++j) + CHECK(dvB[j] == dvA[j]); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkScalar(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkScalar(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else { + String msg = "Unrecognized type: " + abstractVal.getTypeName(); + SimTK_ASSERT(false, msg.c_str()); + } + } +} + +//_____________________________________________________________________________ +// Test the equality of the modeling options. +// +// The SimTK API does not allow an exhaustive, low-level comparison of +// modeling options on the SimTK side. +// +// The comparision is done only for the modeling options registered +// in the OpenSim Component heirarchy. Any modeling option that is +// not registered in the OpenSim Component hierarchy will not be serialized, +// deserialized, or compared. +void +testEqualityForModelingOptions(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + // Loop over the named variables + OpenSim::Array paths = model.getModelingOptionNames(); + int nPaths = paths.getSize(); + for (int i = 0; i < nPaths; ++i) { + Array_ moA, moB; + model.getModelingOptionTrajectory(paths[i], trajA, moA); + model.getModelingOptionTrajectory(paths[i], trajB, moB); + for (size_t j = 0; j<(size_t)moA.size(); ++j) CHECK(moB[j] == moA[j]); + } +} + +//_____________________________________________________________________________ +// Test for equality of two state trajectories. +// If a state variable fails an equality test, it is likely that that +// variable has not been added to OpenSim's Component heirarchy and therefore +// has not been serialized. +void +testStateEquality(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + testEqualityForContinuousVariables(model, trajA, trajB, precision); + testEqualityForDiscreteVariables(model, trajA, trajB, precision); + testEqualityForModelingOptions(model, trajA, trajB, precision); +} + +//_____________________________________________________________________________ +// Build the model +Model* +buildModel(int whichDiscreteState = -1, + const string& discreteStateSuffix = "", int omit = -1) { + + // Create an empty model + Model* model = new Model(); + Vec3 gravity(0.0, -10.0, 0.0); + model->setGravity(gravity); + model->setName("BlockOnASpringFreeJoint"); + + // Add bodies and joints + OpenSim::Ground& ground = model->updGround(); + + // Body + std::string name = "block"; + OpenSim::Body* block = new OpenSim::Body(); + double mass = 10.0; + block->setName(name); + block->set_mass(mass); + block->set_mass_center(Vec3(0)); + block->setInertia(Inertia(1.0)); + model->addBody(block); + + // Joint + name = "free"; + FreeJoint *free = new + FreeJoint(name, ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + model->addJoint(free); + + // Point-To-Point Spring + // This actuator connects the origin of the block to the orgin of the + // coordinate frame. + double kp = 1000.0; // Stiffness + double kv = 100.0; // Viscosity (under-damped) + double restlength = 0.0; + Vec3 origin(0.0); + Vec3 insertion(0.1, 0.1, 0.025); + ExtendedPointToPointSpring* spring = new ExtendedPointToPointSpring( + ground, origin, *block, insertion, kp, restlength, + whichDiscreteState, discreteStateSuffix, omit); + model->addForce(spring); + + return model; +} + +//_____________________________________________________________________________ +// Simulate +SimTK::Array_ +simulate(Model* model) { + + // Add a StatesTrajectoryReporter + // The reporter records the SimTK::State in a SimTK::Array_<> at a + // specified time interval. + OpenSim::StatesTrajectoryReporter* reporter = + new StatesTrajectoryReporter(); + reporter->setName("states_reporter"); + double interval = 0.1; + reporter->set_report_time_interval(interval); + model->addComponent(reporter); + + // Build the system + model->buildSystem(); + SimTK::State& state = model->initializeState(); + + // Integrate + Manager manager(*model); + manager.getIntegrator().setMaximumStepSize(0.01); + manager.setIntegratorAccuracy(1.0e-5); + double ti = 0.0; + double tf = 5.0; + state.setTime(ti); + manager.initialize(state); + state = manager.integrate(tf); + + // Return a copy of the underlying state array, after repackaging it + // as a SimTK::Array_. + const vector& trajectory = reporter->getVectorOfStateObjects(); + const Array_ traj(trajectory); + return traj; +} + +} // End anonymous namespace + + +TEST_CASE("Serialization and Deserialization") +{ + // Build the model and run a simulation. + // The output of simulate() is the state trajectory. + // Note that a copy of the state trajectory is returned, so we don't have + // to worry about the reporter (or any other object) going out of scope + // or being deleted. + Model *model = buildModel(); + Array_ traj = simulate(model); + + // Serialize + SimTK::String filename = "BlockOnASpring.ostates"; + SimTK::String note = "Output from `testStatesDocument.cpp`."; + for (int p = 1; p < 22; ++p) { + cout << "Testing for precision = " << p << endl; + + StatesDocument doc(*model, traj, note, p); + doc.serialize(filename); + + // (A) Deserialize + StatesDocument docA(filename); + Array_ trajA; + docA.deserialize(*model, trajA); + + // Check the note and the precision. + CHECK(docA.getNote() == doc.getNote()); + CHECK(docA.getPrecision() == doc.getPrecision()); + + // Check size + REQUIRE(traj.size() == traj.size()); + + // Realize both state trajectories to Stage::Report + for (size_t i = 0; i < (size_t)trajA.size(); ++i) { + model->getSystem().realize(traj[i], Stage::Report); + model->getSystem().realize(trajA[i], Stage::Report); + } + + // Are state trajectories A and B the same? + testStateEquality(*model, traj, trajA, p); + } + + delete model; +} + +TEST_CASE("Exceptions") +{ + // Build the default model and run a simulation + Model *model = buildModel(); + Array_ traj = simulate(model); + + // Serialize the default model + SimTK::String filename = "BlockOnASpring.ostates"; + SimTK::String note = "Output from `testStatesDocument.cpp`."; + int precision = 6; + StatesDocument doc(*model, traj, note, precision); + doc.serialize(filename); + + // (A) Model names don't match + const string& name = model->getName(); + model->setName(name + "_diff"); + StatesDocument docA(filename); + Array_ trajA; + CHECK_THROWS(docA.deserialize(*model, trajA), + "Model names should not match."); + model->setName(name); // return the original name + + // (B) A discrete state is not found because no name matches + // In each model, the name of one discrete state is changed. + string suffix{"_ShouldNotBeFound"}; + for (int which = 0; which < 8; ++which) { + cout << "Changing the name of discrete state " << which << endl; + + // Build a model that is different only with respect to one name of a + // specified discrete state. + Model* modelB = buildModel(which, suffix); + Array_ trajDoNotNeed = simulate(modelB); + + // Deserialize using modelB + // This should fail when name of a discrete state has been changed. + StatesDocument docB(filename); + Array_ trajB; + CHECK_THROWS(docB.deserialize(*modelB, trajB), + "Discrete state should not be found"); + + delete modelB; + } + + // (C) A discrete state is not found because the state doesn't exist. + // An exception should be thrown because the number of states don't match. + for (int which = -1, omit = 0; omit < 8; ++omit) { + cout << "Omitting discrete state " << omit << endl; + + // Build a model that is different only in that one discrete state + // is omitted. + Model* modelC = buildModel(which, suffix, omit); + Array_ trajDoNotNeed = simulate(modelC); + + // Deserialize using modelC + StatesDocument docC(filename); + Array_ trajC; + CHECK_THROWS(docC.deserialize(*modelC, trajC), + "Expected number of discrete states should be wrong"); + + delete modelC; + } + + delete model; +} \ No newline at end of file diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index 649eb310bf..aa150dc55a 100644 --- a/OpenSim/Simulation/Test/testStatesTrajectory.cpp +++ b/OpenSim/Simulation/Test/testStatesTrajectory.cpp @@ -67,13 +67,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 +84,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()); @@ -149,7 +149,7 @@ void createStateStorageFile() { // histories. auto* controller = new PrescribedController(); // For consistent results, use same seed each time. - std::default_random_engine generator(0); + std::default_random_engine generator(0); // Uniform distribution between 0.1 and 0.9. std::uniform_real_distribution distribution(0.1, 0.8); @@ -399,7 +399,7 @@ void testFromStatesStorageUniqueColumnLabels() { 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]; @@ -478,7 +478,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. @@ -570,16 +570,20 @@ 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]; states[4]; states[5]; #endif - SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange); - SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100), - IndexOutOfRange); + + // SimTK::Array_<> only throws exceptions when in a DEBUG build + #ifdef DEBUG + SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange); + SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100), + IndexOutOfRange); + #endif } void testIntegrityChecks() { @@ -676,7 +680,7 @@ 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. + // and Z's both pass the check. } void tableAndTrajectoryMatch(const Model& model,