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 0410781319..855489c5cb 100644 --- a/Bindings/simulation.i +++ b/Bindings/simulation.i @@ -252,6 +252,7 @@ OpenSim::ModelComponentSet; %template(StdVectorIMUs) std::vector< OpenSim::IMU* >; +%include %include // This enables iterating using the getBetween() method. %template(IteratorRangeStatesTrajectoryIterator) 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..0a044da330 --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -0,0 +1,648 @@ +/* -------------------------------------------------------------------------- * + * 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); +} + +//----------------------------------------------------------------------------- +// 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); +} +//_____________________________________________________________________________ +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); + } +} +//_____________________________________________________________________________ +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()); + } + } + +} +//_____________________________________________________________________________ +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); + prepareStatesTrajectory(model, traj); + 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 parced. + + // 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:: +prepareStatesTrajectory(const Model& model, Array_& traj) { + // Create a local copy of the Model and get a default State. + Model localModel(model); + SimTK::State state = localModel.initSystem(); + + // How many State objects should there be? + // The number of objects needs to be the same as the number of time stamps. + // Each State object has a time field, which will be set in + // initializeTime(). + Element rootElt = doc.getRootElement(); + Attribute nTimeAttr = rootElt.getOptionalAttribute("nTime"); + int nTime; + bool success = nTimeAttr.getValue().tryConvertTo(nTime); + SimTK_ASSERT_ALWAYS(success, + "Unable to acquire nTime from root element."); + SimTK_ASSERT1_ALWAYS(nTime > 0, + "Root element attribute numStateObjects=%d; should be > 0.", nTime); + + // Append State objects + for (int i=0; i < nTime; ++i) traj.emplace_back(state); +} +//_____________________________________________________________________________ +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(); +} +//_____________________________________________________________________________ +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:: +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 = traj.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]); +} +//_____________________________________________________________________________ +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()); + } + } +} +//_____________________________________________________________________________ +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()); + } + } +} +//_____________________________________________________________________________ +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..1bdac7570c --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.h @@ -0,0 +1,613 @@ +#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_, +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 presser 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. + +This 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 +not needed. + +### 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 a SimTK::Array_<> 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., Array_). + // 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 SimTK::Array_& traj = reporter->getStateArray(); + 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; + 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(i)->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); + + //------------------------------------------------------------------------- + // 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. See the section called "Deserialization Requirements" + in the introductory documentation for this class for details. + + @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); + +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 prepareStatesTrajectory(const Model& model, + SimTK::Array_ &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}; + 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..7c781cf105 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,23 +173,23 @@ 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; + typedef SimTK::Array_::const_iterator const_iterator; /** A helper type to allow using range for loops over a subset of the * trajectory. */ @@ -289,9 +290,59 @@ 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, + * SimTK::Array_& 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 SimTK::Array_& getStateArray() const { + return m_states; + } + private: - std::vector m_states; + SimTK::Array_ m_states; public: @@ -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..6e8e9d0bfc 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.cpp +++ b/OpenSim/Simulation/StatesTrajectoryReporter.cpp @@ -34,6 +34,11 @@ const StatesTrajectory& StatesTrajectoryReporter::getStates() const { return m_states; } +const SimTK::Array_& +StatesTrajectoryReporter::getStateArray() 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..2d61b7209b 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 SimTK::Array_& getStateArray() 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..933375b9e4 --- /dev/null +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -0,0 +1,771 @@ +/* -------------------------------------------------------------------------- * +* 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; + + +// 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 + return reporter->getStateArray(); +} + +} // 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..45e459879c 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]; + + //#ifdef NDEBUG + // // In DEBUG, Visual Studio puts asserts into the index operator. + // states[states.getSize() + 100]; + // states[4]; + // states[5]; + //#endif + + // 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 - SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange); - SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100), - IndexOutOfRange); } 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,