diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index 3831a3628c..4d33910102 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -1867,7 +1867,7 @@ void Model::setAllControllersEnabled( bool enabled ) { _allControllersEnabled = enabled; } -void Model::formStateStorage(const Storage& originalStorage, +bool Model::formStateStorage(const Storage& originalStorage, Storage& statesStorage, bool warnUnspecifiedStates) const { @@ -1951,6 +1951,31 @@ void Model::formStateStorage(const Storage& originalStorage, } rStateNames.insert(0, "time"); statesStorage.setColumnLabels(rStateNames); + + + // Determine the return value. + // --------------------------- + // Get coordinate state variable names. + const auto coords = getCoordinatesInMultibodyTreeOrder(); + // Coordinate value names must be unique. + std::set coordValueNames; + const auto modelPath = getAbsolutePath(); + for (size_t i = 0; i < coords.size(); ++i) { + const auto& coord = coords[i]; + const auto coordPath = + coord->getAbsolutePath().formRelativePath(modelPath).toString(); + const auto& coordValueStateVarName = coordPath + "/value"; + coordValueNames.insert(coordValueStateVarName); + } + + // Check if any of the unspecified states are coordinate values. + for (int i = 0; i < mapColumns.size(); ++i) { + // Must add 1 because, at this point, rStateNames includes "time". + if (mapColumns[i] == -1 && + coordValueNames.find(rStateNames[i+1]) != coordValueNames.end()) + return false; + } + return true; } /** diff --git a/OpenSim/Simulation/Model/Model.h b/OpenSim/Simulation/Model/Model.h index bef1c47f4b..e1d13dfbfe 100644 --- a/OpenSim/Simulation/Model/Model.h +++ b/OpenSim/Simulation/Model/Model.h @@ -424,8 +424,12 @@ OpenSim_OBJECT_NONTEMPLATE_DEFS(Model, ModelComponent); * the state value unspecified in originalStorage. The input originalStorage * must be in meters or radians for Coordinate values and their speeds * (m/s, rad/s) otherwise an Exception is thrown. + * + * @returns true if originalStorage has columns for the all coordinate + * values (including dependent coordinates), even if columns for other + * states (speeds, auxiliary states) are absent. */ - void formStateStorage(const Storage& originalStorage, + bool formStateStorage(const Storage& originalStorage, Storage& statesStorage, bool warnUnspecifiedStates = true) const; diff --git a/OpenSim/Simulation/Test/testModelInterface.cpp b/OpenSim/Simulation/Test/testModelInterface.cpp index a53edd72ff..3ebced0e82 100644 --- a/OpenSim/Simulation/Test/testModelInterface.cpp +++ b/OpenSim/Simulation/Test/testModelInterface.cpp @@ -32,6 +32,7 @@ using namespace std; void testModelFinalizePropertiesAndConnections(); void testModelTopologyErrors(); +void testFormStateStorage(); int main() { LoadOpenSimLibrary("osimActuators"); @@ -39,6 +40,7 @@ int main() { SimTK_START_TEST("testModelInterface"); SimTK_SUBTEST(testModelFinalizePropertiesAndConnections); SimTK_SUBTEST(testModelTopologyErrors); + SimTK_SUBTEST(testFormStateStorage); SimTK_END_TEST(); } @@ -226,6 +228,46 @@ void testModelTopologyErrors() ASSERT_THROW(JointFramesHaveSameBaseFrame, degenerate.initSystem()); } +// Test the return value of Model::formStateStorage(). +void testFormStateStorage() { + + Model model("arm26.osim"); + + // Create an "origStorage." + SimTK::State state = model.initSystem(); + Manager manager(model, state); + manager.integrate(0.05); + Storage origStorage = manager.getStateStorage(); + Array origLabels = origStorage.getColumnLabels(); + + Storage statesStorage; + + // Ensure that a complete states storage causes a return value of true. + SimTK_TEST(model.formStateStorage(origStorage, statesStorage)); + + // "Removing" a state variable that is not a coordinate value: the return + // value should still be true. + origLabels[origLabels.findIndex("TRIlong/activation")] = "hide1"; + origStorage.setColumnLabels(origLabels); + SimTK_TEST(model.formStateStorage(origStorage, statesStorage)); + origLabels[origLabels.findIndex("r_elbow/r_elbow_flex/speed")]= "hide2"; + origStorage.setColumnLabels(origLabels); + SimTK_TEST(model.formStateStorage(origStorage, statesStorage)); + + // "Removing" a coordinate state variable causes a return value of false. + origLabels[origLabels.findIndex("r_elbow/r_elbow_flex/value")] = "hide3"; + origStorage.setColumnLabels(origLabels); + SimTK_TEST(!model.formStateStorage(origStorage, statesStorage)); +} + + + + + + + + +