Model::formStateStorage() return value: are all coordinate values specified?

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<std::string> 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;
 }
 
 /**

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;
 

OpenSim/Simulation/Test/testModelInterface.cpp

@@ -32,13 +32,15 @@ using namespace std;
 
 void testModelFinalizePropertiesAndConnections();
 void testModelTopologyErrors();
+void testFormStateStorage();
 
 int main() {
     LoadOpenSimLibrary("osimActuators");
 
     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<std::string> 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));
+}
+
+
+
+
+
+
+
+
+