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 7d9ad9c676..2cf45d8fb3 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 1b6660266d..c8f9b84636 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,