From 6789e7fd759c41f90edd41969863f214e9e8cdca Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 27 May 2024 09:12:20 -0500 Subject: [PATCH 01/56] Added class StatesDocument --- OpenSim/Simulation/StatesDocument.cpp | 667 ++++++++++++++++++++++++++ OpenSim/Simulation/StatesDocument.h | 591 +++++++++++++++++++++++ 2 files changed, 1258 insertions(+) create mode 100644 OpenSim/Simulation/StatesDocument.cpp create mode 100644 OpenSim/Simulation/StatesDocument.h diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp new file mode 100644 index 0000000000..6e4014c8aa --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -0,0 +1,667 @@ +/* -------------------------------------------------------------------------- * + * 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-20232 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; + +//----------------------------------------------------------------------------- +// Local Utility Functions +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +template +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); + varElt.setValueAs>(valArr); + parent.appendNode(varElt); +} +//_____________________________________________________________________________ +template +void +initializeStatesForStateVariable(Element& varElt, const Model& model, + const string& path, Array_ & traj) +{ + // Interpret the Element value + Array_ vArr; + varElt.getValueAs>(vArr); + + // Check the sizes. + int 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.setStatesTrajectoryForStateVariable(path, vArr, traj); +} +//_____________________________________________________________________________ +template +void +initializeStatesForDiscreteVariable(Element& varElt, const Model& model, + const string& path, Array_ & traj) +{ + // Interpret the Element value + Array_ vArr; + varElt.getValueAs>(vArr); + + // Check the sizes. + int 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.setStatesTrajectoryForDiscreteVariable(path, vArr, traj); +} +//_____________________________________________________________________________ +template +void +initializeStatesForModelingOption(Element& varElt, const Model& model, + const string& path, Array_ & traj) +{ + // Interpret the Element value + Array_ vArr; + varElt.getValueAs>(vArr); + + // Check the sizes. + int 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.setStatesTrajectoryForModelingOption(path, vArr, traj); +} + +//----------------------------------------------------------------------------- +// Construction +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +StatesDocument:: +StatesDocument(const Model& model, const Array_& trajectory, int p) { + precision = clamp(1, p, SimTK::LosslessNumDigitsReal); + formDoc(model, trajectory); +} + +//----------------------------------------------------------------------------- +// Serialize +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +formDoc(const Model& model, const Array_& traj) { + formRootElement(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", std::localtime(&now)); + //SimTK::String datetime = buf; + //cout << "ostates datetime = " << datetime << endl; + + // 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:: +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); + //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); + 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); + appendVarElt(paths[i], "variable", "bool", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "int", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "float", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "double", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "Vec2", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "Vec3", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "Vec4", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + appendVarElt(paths[i], "variable", "Vec5", vArr, + discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + 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); + appendVarElt(paths[i], "option", "int", val, modelingElt, + precision); + } +} + + +//----------------------------------------------------------------------------- +// Deserialize +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +deserialize(const Model& model, Array_& traj) { + parseDoc(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +parseDoc(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) const { + +} +//_____________________________________________________________________________ +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:: +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. Should only be 1.", timeElts.size()); + + // Get the values + Array_ timeArr; + timeElts[0].getValueAs>(timeArr); + + // Check the size of the time array. + int 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 (int 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 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") { + 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'. Should only be 1.", + discElts.size()); + + // Find all the child 'variable' elements + SimTK::String childTag = "variable"; + Array_ varElts = discElts[0].getAllElements(childTag); + + // Check that the number 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 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") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "int") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "float") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "double") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec2") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec3") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec4") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec5") { + initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec6") { + 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. Should only be 1.", 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 variable 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") { + initializeStatesForModelingOption(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} + + +//----------------------------------------------------------------------------- +// Testing +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +test() { + // Make up some data and elements. + prototype(); + + // Write to String + SimTK::String docStr; + doc.writeToString(docStr); + cout << endl << "Prototype StatesDocument -----" << endl; + cout << docStr << endl; + + // Write to File + doc.writeToFile( + "C:/Users/fcand/Documents/GitHub/Work/Testing/OpenSim/test.ostates"); +} + +//_____________________________________________________________________________ +void +StatesDocument:: +prototype() { + + // Set the tag of the root element and get an iterator to it. + doc.setRootTag("ostates"); + Xml::Element& root = doc.getRootElement(); + Xml::node_iterator root_it = doc.node_begin(Xml::ElementNode); + + // Insert a comment at the top level, just before the root node. + string info = "Developing class StatesDocument. Version 0.0.1."; + Xml::Comment comment(info); + doc.insertTopLevelNodeBefore(root_it, comment); + + // Add attributes to the root node + root.setAttributeValue("model", "BouncingBlocks"); + + // Add time and state category elements + Xml::Element timeElt("time"); + root.appendNode(timeElt); + Xml::Element continuousElt("continuous"); + root.appendNode(continuousElt); + Xml::Element discreteElt("discrete"); + root.appendNode(discreteElt); + Xml::Element modelingElt("modeling"); + root.appendNode(modelingElt); + + // Number of State% Objects + int i; + int num = 11; + std::string numStr = std::to_string(num); + root.setAttributeValue("numStateObjects", numStr); + + // Time + SimTK::Vector_ time(num); + for (i = 0; i < num; ++i) { time[i] = 0.1 * SimTK::Pi * (double)i; } + //timeElt.setValueAs>(time, precision); + timeElt.setValueAs>(time); + + // Experiment with output precision + cout.unsetf(std::ios::floatfield); + cout << setprecision(precision); + cout << endl << time << endl << endl; + + // Hip Flexion + SimTK::Vector_ q(num); + for (i = 0; i < num; ++i) { q[i] = 1.0e-10 * SimTK::Pi * (double)i; } + Xml::Element hipElt("variable"); + hipElt.setAttributeValue("path", "/jointset/hip/flexion/value"); + //hipElt.setValueAs>(q, precision); + hipElt.setValueAs>(q); + continuousElt.appendNode(hipElt); + + // Elastic Anchor Point + SimTK::Array_ anchor(num); + for (i = 0; i < num; ++i) { + Vec3 val(0.0, 1.10000000001, 1.200000000000002); + anchor[i] = ((double)i) * val; + } + Xml::Element anchorElt("variable"); + anchorElt.setAttributeValue("path", "/forceset/EC0/anchor"); + //anchorElt.setValueAs>(anchor, precision); + anchorElt.setValueAs>(anchor); + discreteElt.appendNode(anchorElt); + + // Now -- Getting Vectors out! + // Time + Vector_ timeOut; + timeElt.getValueAs>(timeOut); + cout << endl << "timeOut: " << timeOut << endl; + // Hip Flexion + Vector_ qOut; + hipElt.getValueAs>(qOut); + cout << endl << "hipOut: " << qOut << endl; + // Anchor + Array_ anchorOut; + anchorElt.getValueAs>(anchorOut); + cout << endl << "anchorOut: " << anchorOut << endl; + + // Asserts + SimTK_ASSERT_ALWAYS(anchor[0] == anchorOut[0], + "Deserialized value not equal to original value."); +} diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h new file mode 100644 index 0000000000..eef12c365d --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.h @@ -0,0 +1,591 @@ +#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, +these output variables are 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 the 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 or unlocked. When a Modeling Options 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 StatesDocument, 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 into 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::Vector_, 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 +frequently 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& pathName, + const SimTK::Array_& input, + SimTK::Array_& output) const +``` +A call to the above method first finds a Discrete Variable in the model +hierarchy based on the specifed path name (```pathName```). 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 setStatesTrajectoryForDiscreteVariable( + const std::string& pathName, + 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 model is added) or +a change has occurred in its state trajectory, the intended way to generate +an internal 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 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. In applications where exact values of the states are needed, +lossless precision should be used. + + +------------------- +.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 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. + + 3) All ```variable``` and ```option``` paths must be found in the model + OpenSim::Component heirarchy. + + 4) The type must be supported. As of January 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(statesReporter); + + // ----------------------------------------- + // 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 does not provide direct access to this + // private array, but it does know 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, if the reporter did provide direct access to the + // underlying array of states (i.e., the Array_) instead of the + // StatesTrajectory object, the StatesDocument would be created by doing + // the following: + const SimTK::Array_& trajectory = reporter->getStates(); + StatesDocument doc(model, trajectory); + + // ---------------------------- + // Serialize the States to File + // ---------------------------- + // The file name (see below), can be any string supported by the file + // system can be. 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; // "traj" is short for "trajectory" + 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); + + } + + // ------------------------------------------ + // Extract Trajectories for Individual States + // ------------------------------------------ + // 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 (*.??) 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 + //------------------------------------------------------------------------- + /** 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 include the file system + path at which the file resides (e.g., "C:/Documents/block.ostates"). */ + StatesDocument(const SimTK::String& filename) { + doc.readFromFile(filename); + } + + /** 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 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 near lossless + reproduction of state. */ + StatesDocument(const OpenSim::Model& model, + const SimTK::Array_& trajectory, + int precision = SimTK::LosslessNumDigitsReal); + + //------------------------------------------------------------------------- + // 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) { + doc.writeToFile(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); + + //------------------------------------------------------------------------- + // Testing + //------------------------------------------------------------------------- + /** An entry point for running basic tests on this class. */ + void test(); + +protected: + // Serialization Helpers. + void formDoc(const Model& model, + const SimTK::Array_& traj); + void formRootElement(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 parseDoc(const Model& m, SimTK::Array_& t); + void checkDocConsistencyWithModel(const Model& model) const; + void prepareStatesTrajectory(const Model& model, + SimTK::Array_ &traj); + 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); + + // Testing + void prototype(); + +private: + // Member Variables + int precision{SimTK::LosslessNumDigitsReal}; + SimTK::Xml::Document doc; + +}; // END of class StatesDocument + +} // end of namespace OpenSim + +#endif OPENSIM_STATES_DOCUMENT_H_ \ No newline at end of file From 03b15d94a79f77c80cae7f735ec60cdd39b1a43d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 27 May 2024 09:38:21 -0500 Subject: [PATCH 02/56] Update StatesDocument.cpp - Modified the names of the Trajectory methods to match the latest names in Component.h. - Reintroduced the precision argument in SimTK::Xml::Elment::setValueAs. --- OpenSim/Simulation/StatesDocument.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 6e4014c8aa..d3431f9137 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -43,8 +43,8 @@ appendVarElt(const string& path, const string& tag, const string& type, varElt.setAttributeValue("type", type); // Append the variable element - //varElt.setValueAs>(valArr, precision); - varElt.setValueAs>(valArr); + varElt.setValueAs>(valArr, precision); + //varElt.setValueAs>(valArr); parent.appendNode(varElt); } //_____________________________________________________________________________ @@ -64,7 +64,7 @@ initializeStatesForStateVariable(Element& varElt, const Model& model, n, traj.size()); // Set variable in the States trajectory - model.setStatesTrajectoryForStateVariable(path, vArr, traj); + model.setStateVariableTrajectory(path, vArr, traj); } //_____________________________________________________________________________ template @@ -83,7 +83,7 @@ initializeStatesForDiscreteVariable(Element& varElt, const Model& model, n, traj.size()); // Set variable in the States trajectory - model.setStatesTrajectoryForDiscreteVariable(path, vArr, traj); + model.setDiscreteVariableTrajectory(path, vArr, traj); } //_____________________________________________________________________________ template @@ -102,7 +102,7 @@ initializeStatesForModelingOption(Element& varElt, const Model& model, n, traj.size()); // Set variable in the States trajectory - model.setStatesTrajectoryForModelingOption(path, vArr, traj); + model.setModelingOptionTrajectory(path, vArr, traj); } //----------------------------------------------------------------------------- @@ -176,8 +176,8 @@ formTimeElement(const Model& model, const Array_& traj) { } // Set the text value on the element - timeElt.setValueAs>(time); - //timeElt.setValueAs>(time, precision); + //timeElt.setValueAs>(time); + timeElt.setValueAs>(time, precision); } //_____________________________________________________________________________ void @@ -618,8 +618,8 @@ prototype() { // Time SimTK::Vector_ time(num); for (i = 0; i < num; ++i) { time[i] = 0.1 * SimTK::Pi * (double)i; } - //timeElt.setValueAs>(time, precision); - timeElt.setValueAs>(time); + timeElt.setValueAs>(time, precision); + //timeElt.setValueAs>(time); // Experiment with output precision cout.unsetf(std::ios::floatfield); @@ -631,8 +631,8 @@ prototype() { for (i = 0; i < num; ++i) { q[i] = 1.0e-10 * SimTK::Pi * (double)i; } Xml::Element hipElt("variable"); hipElt.setAttributeValue("path", "/jointset/hip/flexion/value"); - //hipElt.setValueAs>(q, precision); - hipElt.setValueAs>(q); + hipElt.setValueAs>(q, precision); + //hipElt.setValueAs>(q); continuousElt.appendNode(hipElt); // Elastic Anchor Point @@ -643,8 +643,8 @@ prototype() { } Xml::Element anchorElt("variable"); anchorElt.setAttributeValue("path", "/forceset/EC0/anchor"); - //anchorElt.setValueAs>(anchor, precision); - anchorElt.setValueAs>(anchor); + anchorElt.setValueAs>(anchor, precision); + //anchorElt.setValueAs>(anchor); discreteElt.appendNode(anchorElt); // Now -- Getting Vectors out! From cc9954f10c5e9d5cc16d97b22c1308b0fa695482 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 28 May 2024 09:46:32 -0500 Subject: [PATCH 03/56] Create testStatesDocument.cpp Added a place holder for the Catch2 unit test for class StatesDocument. testStatesDocument.cpp is now being incorporated in the build, and the test case runs when "build" executed on the target "RUN_TESTS_PARALLEL". --- .../Simulation/Test/testStatesDocument.cpp | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 OpenSim/Simulation/Test/testStatesDocument.cpp diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp new file mode 100644 index 0000000000..a8df1b6825 --- /dev/null +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -0,0 +1,42 @@ +/* -------------------------------------------------------------------------- * +* OpenSim: testStatesDocument.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) 2005-2024 Stanford University and the Authors * +* * +* 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 + +//using namespace OpenSim; + +// Internal static methods and classes. +namespace +{ + double customSquare(double x) + { + return(x*x); + } +} + + +TEST_CASE("Getting Started") +{ + double x = 2.0; + double square = customSquare(x); + REQUIRE(square == x*x); +} From 21b1c81624b88a8e86e1455b00ff6fa0b76306a0 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 29 May 2024 13:41:31 -0500 Subject: [PATCH 04/56] Update StatesDocument.cpp - Moved all utility methods into a struct with local linkage. - Streamlined code by adding `getEltValue()`. Use of this method reduced some code repetition. --- OpenSim/Simulation/StatesDocument.cpp | 245 ++++++++++++++------------ 1 file changed, 136 insertions(+), 109 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index d3431f9137..d93becdd26 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -28,82 +28,108 @@ using namespace std; using namespace OpenSim; using std::cout; +namespace OpenSim { + +// Anonymous namespace to ensure local linkage +namespace { + //----------------------------------------------------------------------------- -// Local Utility Functions +// Local utility methods for use with class StatesDocument //----------------------------------------------------------------------------- -//_____________________________________________________________________________ -template -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); - //varElt.setValueAs>(valArr); - parent.appendNode(varElt); -} -//_____________________________________________________________________________ -template -void -initializeStatesForStateVariable(Element& varElt, const Model& model, - const string& path, Array_ & traj) -{ - // Interpret the Element value - Array_ vArr; - varElt.getValueAs>(vArr); - - // Check the sizes. - int 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.setStateVariableTrajectory(path, vArr, traj); -} -//_____________________________________________________________________________ -template -void -initializeStatesForDiscreteVariable(Element& varElt, const Model& model, - const string& path, Array_ & traj) -{ - // Interpret the Element value - Array_ vArr; - varElt.getValueAs>(vArr); - - // Check the sizes. - int n = vArr.size(); - SimTK_ASSERT2_ALWAYS(n == traj.size(), - "Found %d values. Should match nTime = %d values.", - n, traj.size()); +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); + //varElt.setValueAs>(valArr); + parent.appendNode(varElt); + } + //_________________________________________________________________________ + template + inline + static + void + getEltValue(const string& path, int expectedSize, + Element& varElt, Array_& vArr) + { + // Interpret the element value + varElt.getValueAs>(vArr); + + // Check the size + int n = vArr.size(); + SimTK_ASSERT3_ALWAYS(n == expectedSize, + "Found %d values in the element for %s, but there should be %d", + n, path, 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. + int 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); + } +}; - // Set variable in the States trajectory - model.setDiscreteVariableTrajectory(path, vArr, traj); -} -//_____________________________________________________________________________ -template -void -initializeStatesForModelingOption(Element& varElt, const Model& model, - const string& path, Array_ & traj) -{ - // Interpret the Element value - Array_ vArr; - varElt.getValueAs>(vArr); - - // Check the sizes. - int n = vArr.size(); - SimTK_ASSERT2_ALWAYS(n == traj.size(), - "Found %d values. Should match nTime = %d values.", - n, traj.size()); +} // End anonymous namespace +} // End OpenSim namespace - // Set variable in the States trajectory - model.setModelingOptionTrajectory(path, vArr, traj); -} +// Note that the methods below are still in the OpenSim namespace. +// That namespace declaration is taken care of in the .h file. //----------------------------------------------------------------------------- // Construction @@ -197,7 +223,8 @@ formContinuousElement(const Model& model, const Array_& traj) { for (int i = 0; i < n; ++i) { Array_ val; model.getStateVariableTrajectory(paths[i], traj, val); - appendVarElt(paths[i], "variable", "double", val, contElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "double", + val, contElt, precision); } } //_____________________________________________________________________________ @@ -225,64 +252,64 @@ formDiscreteElement(const Model& model, const Array_& traj) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "bool", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "bool", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "int", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "int", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "float", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "float", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "double", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "double", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "Vec2", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "Vec2", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "Vec3", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "Vec3", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "Vec4", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "Vec4", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "Vec5", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "Vec5", + vArr, discreteElt, precision); } else if(SimTK::Value::isA(v)) { Array_ vArr; model.getDiscreteVariableTrajectory( paths[i], traj, vArr); - appendVarElt(paths[i], "variable", "Vec6", vArr, - discreteElt, precision); + SDocUtil::appendVarElt(paths[i], "variable", "Vec6", + vArr, discreteElt, precision); } else { string msg = "Unrecognized type: " + v.getTypeName(); @@ -309,8 +336,8 @@ formModelingElement(const Model& model, const Array_& traj) { for (int i = 0; i < n; ++i) { Array_ val; model.getModelingOptionTrajectory(paths[i], traj, val); - appendVarElt(paths[i], "option", "int", val, modelingElt, - precision); + SDocUtil::appendVarElt(paths[i], "option", "int", + val, modelingElt, precision); } } @@ -428,7 +455,7 @@ initializeContinuousVariables(const Model& model, SimTK::Array_& traj) { // Switch based on the type. // Type double is expected for continuous variable elements. if (type == "double") { - initializeStatesForStateVariable(varElts[i], + SDocUtil::initializeStatesForStateVariable(varElts[i], model, path, traj); } else { @@ -471,39 +498,39 @@ initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { // Switch based on the type // Append the vector according to type if (type == "bool") { - initializeStatesForDiscreteVariable(varElts[i], - model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable( + varElts[i], model, path, traj); } else if(type == "int") { - initializeStatesForDiscreteVariable(varElts[i], - model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable( + varElts[i], model, path, traj); } else if(type == "float") { - initializeStatesForDiscreteVariable(varElts[i], - model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable( + varElts[i], model, path, traj); } else if(type == "double") { - initializeStatesForDiscreteVariable(varElts[i], - model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable( + varElts[i], model, path, traj); } else if(type == "Vec2") { - initializeStatesForDiscreteVariable(varElts[i], - model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable( + varElts[i], model, path, traj); } else if(type == "Vec3") { - initializeStatesForDiscreteVariable(varElts[i], + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], model, path, traj); } else if(type == "Vec4") { - initializeStatesForDiscreteVariable(varElts[i], + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], model, path, traj); } else if(type == "Vec5") { - initializeStatesForDiscreteVariable(varElts[i], + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], model, path, traj); } else if(type == "Vec6") { - initializeStatesForDiscreteVariable(varElts[i], + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], model, path, traj); } else { @@ -549,7 +576,7 @@ initializeModelingOptions(const Model& model, SimTK::Array_& traj) { // Switch based on the type. // Type int is expected for modeling option elements. if (type == "int") { - initializeStatesForModelingOption(varElts[i], + SDocUtil::initializeStatesForModelingOption(varElts[i], model, path, traj); } else { From bb99e948487103efccbb262abec53faffc258237 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 29 May 2024 13:45:08 -0500 Subject: [PATCH 05/56] Update StatesDocument.cpp - Minor changes to line formatting. --- OpenSim/Simulation/StatesDocument.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index d93becdd26..6be7bae6d1 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -498,24 +498,24 @@ initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { // Switch based on the type // Append the vector according to type if (type == "bool") { - SDocUtil::initializeStatesForDiscreteVariable( - varElts[i], model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); } else if(type == "int") { - SDocUtil::initializeStatesForDiscreteVariable( - varElts[i], model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); } else if(type == "float") { - SDocUtil::initializeStatesForDiscreteVariable( - varElts[i], model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); } else if(type == "double") { - SDocUtil::initializeStatesForDiscreteVariable( - varElts[i], model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); } else if(type == "Vec2") { - SDocUtil::initializeStatesForDiscreteVariable( - varElts[i], model, path, traj); + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); } else if(type == "Vec3") { SDocUtil::initializeStatesForDiscreteVariable(varElts[i], From 0538766f61ad76dc20c3b5db99e9be75a928c11b Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 30 May 2024 02:52:40 -0500 Subject: [PATCH 06/56] Update testComponentInterface.cpp - Minor corrections/additions to comments --- OpenSim/Common/Test/testComponentInterface.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/OpenSim/Common/Test/testComponentInterface.cpp b/OpenSim/Common/Test/testComponentInterface.cpp index 61311bc123..83873865ed 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); @@ -2047,7 +2049,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 From b799c0be03aa08b50be28d9666ba61d85d90a56a Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 30 May 2024 02:57:00 -0500 Subject: [PATCH 07/56] Update testStatesDocument.cpp - Added code borrowed from testComponentInterface.cpp to generate a dummy model with a variety of states. The code compiles and runs with all test cases passing, but this is just a starting point. De/serialization using class StatesDocument is not yet being tested. --- .../Simulation/Test/testStatesDocument.cpp | 552 +++++++++++++++++- 1 file changed, 549 insertions(+), 3 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index a8df1b6825..b819bda092 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -1,5 +1,5 @@ /* -------------------------------------------------------------------------- * -* OpenSim: testStatesDocument.cpp * +* 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. * @@ -7,7 +7,8 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * -* Copyright (c) 2005-2024 Stanford University and the Authors * +* 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 * @@ -19,10 +20,397 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include + +//----------------------------------------------------------------------------- +// The section above was taken from testComponentInterface.cpp. +// The code is adapted to generate a dummy model with a SimTK::State that is +// comprised of continuous variables (q, u, z), discrete variables of all +// supported types, and modeling options. +//----------------------------------------------------------------------------- +using namespace OpenSim; +using namespace std; +using namespace SimTK; + +class Foo; +class Bar; + +class Sub : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(Sub, Component); +public: + OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(subState); + Sub() = default; + virtual ~Sub() = default; +private: + void extendAddToSystem(MultibodySystem &system) const override { + Super::extendAddToSystem(system); + addStateVariable("subState", Stage::Dynamics); + addDiscreteVariable("dvX", Stage::Dynamics); + addModelingOption("moX", 2); + } + void computeStateVariableDerivatives(const SimTK::State& s) const override { + double deriv = exp(-2.0*s.getTime()); + setStateVariableDerivativeValue(s, "subState", deriv); + } +}; //end class Sub + +class TheWorld : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(TheWorld, Component); +public: + TheWorld() : Component() { } + + TheWorld(const std::string& fileName, bool updFromXMLNode = false) + : Component(fileName, updFromXMLNode) { + // Propagate XML file values to properties + updateFromXMLDocument(); + // add components listed as properties as sub components. + finalizeFromProperties(); + } + + void add(Component* comp) { + addComponent(comp); + // Edit Sub + /*Sub& subc = */updMemberSubcomponent(intSubix); + } + + // Top level connection method for this all encompassing component, TheWorld + void connect() { + Super::finalizeConnections(*this); + } + void buildUpSystem(MultibodySystem& system) { + connect(); + addToSystem(system); + } + + const SimbodyMatterSubsystem& getMatterSubsystem() const { return *matter; } + SimbodyMatterSubsystem& updMatterSubsystem() const { return *matter; } + + const GeneralForceSubsystem& getForceSubsystem() const { return *forces; } + GeneralForceSubsystem& updForceSubsystem() const { return *forces; } + +protected: + // Component interface implementation + void extendAddToSystem(MultibodySystem& system) const override { + if (system.hasMatterSubsystem()){ + matter = system.updMatterSubsystem(); + } + else{ + // const Sub& subc = getMemberSubcomponent(intSubix); + + SimbodyMatterSubsystem* old_matter = matter.release(); + delete old_matter; + matter = new SimbodyMatterSubsystem(system); + + GeneralForceSubsystem* old_forces = forces.release(); + delete old_forces; + forces = new GeneralForceSubsystem(system); + + SimTK::Force::UniformGravity gravity(*forces, *matter, Vec3(0, -9.816, 0)); + fix = gravity.getForceIndex(); + + system.updMatterSubsystem().setShowDefaultGeometry(true); + } + } + +private: + // Keep track of pointers to the underlying computational subsystems. + mutable ReferencePtr matter; + mutable ReferencePtr forces; + + // keep track of the force added by the component + mutable ForceIndex fix; + + MemberSubcomponentIndex intSubix{ constructSubcomponent("internalSub") }; + +}; // end of TheWorld + + +class Foo : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(Foo, Component); +public: + //============================================================================= + // PROPERTIES + //============================================================================= + OpenSim_DECLARE_PROPERTY(mass, double, "mass (kg)"); + OpenSim_DECLARE_LIST_PROPERTY_SIZE(inertia, double, 6, + "inertia {Ixx, Iyy, Izz, Ixy, Ixz, Iyz}"); + + OpenSim_DECLARE_OUTPUT(Output1, double, getSomething, SimTK::Stage::Time); + OpenSim_DECLARE_OUTPUT(Output2, SimTK::Vec3, calcSomething, + SimTK::Stage::Time); + + OpenSim_DECLARE_OUTPUT(Output3, double, getSomethingElse, SimTK::Stage::Time); + + OpenSim_DECLARE_OUTPUT(Qs, Vector, getQ, SimTK::Stage::Position); + + OpenSim_DECLARE_OUTPUT(BodyAcc, SpatialVec, calcSpatialAcc, + SimTK::Stage::Velocity); + + OpenSim_DECLARE_OUTPUT(return_by_ref, double, getReturnByRef, + SimTK::Stage::Time); + + OpenSim_DECLARE_INPUT(input1, double, SimTK::Stage::Model, ""); + OpenSim_DECLARE_INPUT(AnglesIn, Vector, SimTK::Stage::Model, ""); + OpenSim_DECLARE_INPUT(fiberLength, double, SimTK::Stage::Model, ""); + OpenSim_DECLARE_INPUT(activation, double, SimTK::Stage::Model, ""); + OpenSim_DECLARE_LIST_INPUT(listInput1, double, SimTK::Stage::Model, ""); + + Foo() : Component() { + constructProperties(); + m_ctr = 0; + m_mutableCtr = 0; + } + + double getSomething(const SimTK::State& state) const { + const_cast(this)->m_ctr++; + m_mutableCtr++; + + return state.getTime(); + } + + SimTK::Vec3 calcSomething(const SimTK::State& state) const { + const_cast(this)->m_ctr++; + m_mutableCtr++; + + double t = state.getTime(); + return SimTK::Vec3(t, t*t, sqrt(t)); + } + + double getSomethingElse(const SimTK::State& state) const { + return 1.618; + } + + SimTK::Vector getQ(const SimTK::State& state) const { + return state.getQ(); + } + + SimTK::SpatialVec calcSpatialAcc(const SimTK::State& state) const { + const_cast(this)->m_ctr++; + m_mutableCtr++; + + return getSystem().getMatterSubsystem().getMobilizedBody(bindex) + .getBodyAcceleration(state); + } + + const double& getReturnByRef(const SimTK::State& s) const { + // Must return something that is stored in the state! + return s.getTime(); + } + +protected: + /** Component Interface */ + void extendFinalizeConnections(Component& root) override { + Super::extendFinalizeConnections(root); + // do any internal wiring + world = dynamic_cast(&root); + } + + void extendAddToSystem(MultibodySystem &system) const override { + Super::extendAddToSystem(system); + + SimbodyMatterSubsystem& matter = system.updMatterSubsystem(); + + Vec3 mInB(0.0, 1.0, 0); + Vec3 mInP(0, 0, 0); + + SimTK::Body::Rigid bone( + MassProperties(1, Vec3(0), Inertia::brick(0.5, 1, 0.5))); + + // Thigh connected by hip + MobilizedBody::Pin b1ToGround(matter.updGround(), SimTK::Transform(mInP), + bone, SimTK::Transform(mInB)); + + //Pin knee connects shank + MobilizedBody::Pin b1ToB2(b1ToGround, SimTK::Transform(mInP), + bone, SimTK::Transform(mInB)); + + bindex = b1ToB2.getMobilizedBodyIndex(); + } + +private: + int m_ctr; + mutable int m_mutableCtr; + + + void constructProperties() { + constructProperty_mass(1.0); + Array inertia(0.001, 6); + inertia[0] = inertia[1] = inertia[2] = 0.1; + constructProperty_inertia(inertia); + } + + // Keep indices and reference to the world + mutable MobilizedBodyIndex bindex; + ReferencePtr world; + +}; // End of class Foo + +class Bar : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(Bar, Component); +public: + + OpenSim_DECLARE_SOCKET(parentFoo, Foo, ""); + OpenSim_DECLARE_SOCKET(childFoo, Foo, ""); + OpenSim_DECLARE_LIST_SOCKET(listFoo, Foo, ""); + + // This is used to test output copying and returns the address of the + // component. + OpenSim_DECLARE_OUTPUT(copytesting, size_t, myself, SimTK::Stage::Model); + // Use this member variable to ensure that output functions get copied + // correctly. + double copytestingViaMemberVariable = 5; + OpenSim_DECLARE_OUTPUT(copytestingMemVar, double, getCopytestingMemVar, + SimTK::Stage::Model); + + OpenSim_DECLARE_OUTPUT(PotentialEnergy, double, getPotentialEnergy, + SimTK::Stage::Velocity); + + OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(fiberLength); + OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(activation); + + double getPotentialEnergy(const SimTK::State& state) const { + const GeneralForceSubsystem& forces = world->getForceSubsystem(); + const SimTK::Force& force = forces.getForce(fix); + const auto& spring = SimTK::Force::TwoPointLinearSpring::downcast(force); + + return spring.calcPotentialEnergyContribution(state); + } + + /** Returns the `this` pointer. Used to ensure that the std::function + within Outputs is properly copied when copying components. */ + size_t myself(const SimTK::State& s) const { return size_t(this); } + + double getCopytestingMemVar(const SimTK::State& s) const + { return copytestingViaMemberVariable; } + + class ParentAndFooAreSame : OpenSim::Exception { + public: + using OpenSim::Exception::Exception; + }; +protected: + /** Component Interface */ + void extendFinalizeConnections(Component& root) override{ + Super::extendFinalizeConnections(root); + // do any internal wiring + world = dynamic_cast(&root); + // perform custom checking + if (&updSocket("parentFoo").getConnectee() + == &updSocket("childFoo").getConnectee()){ + string msg = "ERROR - Bar::extendFinalizeConnections()\n"; + msg += " parentFoo and childFoo cannot be the same component."; + throw ParentAndFooAreSame(msg); + } + } + + // Copied here from Component for testing purposes. + + + void extendAddToSystem(MultibodySystem& system) const override{ + + GeneralForceSubsystem& forces = world->updForceSubsystem(); + SimbodyMatterSubsystem& matter = world->updMatterSubsystem(); + + int nb = matter.getNumBodies(); + if (nb > 2) { + const MobilizedBody& b1 = matter.getMobilizedBody(MobilizedBodyIndex(1)); + const MobilizedBody& b2 = matter.getMobilizedBody(MobilizedBodyIndex(2)); + + SimTK::Force::TwoPointLinearSpring + spring(forces, b1, Vec3(0.5,0,0), b2, Vec3(0.5,0,0), 10.0, 0.1); + fix = spring.getForceIndex(); + } + + // We use these to test the Output's that are generated when we + // add a StateVariable. + addStateVariable("fiberLength", SimTK::Stage::Velocity); + addStateVariable("activation", SimTK::Stage::Dynamics); + + // Create a hidden state variable, so we can ensure that hidden state + // variables do not have a corresponding Output. + bool hidden = true; + addStateVariable("hiddenStateVar", SimTK::Stage::Dynamics, hidden); + + // Add a modeling option (mo) and a discrete variable (dv) as though + // they were allocated natively in Simbody. + // This will allow testing the ability to accomodate a mo and dv that + // were allocated from subsystem different than the default subsystem + // 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 be allocated in class Bar's override of + // extendRealizeTopology(). See below. + bool allocate = false; + int maxFlagValue = 1; + addDiscreteVariable("point", Stage::Position, allocate); + addModelingOption("moY", maxFlagValue, allocate); + } + + // 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); + + GeneralForceSubsystem& fsub = world->updForceSubsystem(); + + // Discrete Variable Initialization + std::string dvName = "point"; + Vec3 point(0.0, 0.1, 0.2); + SimTK::DiscreteVariableIndex dvIndex = + fsub.allocateDiscreteVariable(state, Stage::Dynamics, + new Value(point)); + initializeDiscreteVariableIndexes(dvName, + fsub.getMySubsystemIndex(), dvIndex); + + // Modeling Option Initialization + std::string moName = "moY"; + int moVal{0}; + SimTK::DiscreteVariableIndex moIndex = + fsub.allocateDiscreteVariable(state, Stage::Dynamics, + new Value(moVal)); + initializeModelingOptionIndexes(moName, + fsub.getMySubsystemIndex(), moIndex); + } + + void computeStateVariableDerivatives(const SimTK::State& state) const override { + setStateVariableDerivativeValue(state, "fiberLength", 2.0); + setStateVariableDerivativeValue(state, "activation", 3.0 * state.getTime()); + setStateVariableDerivativeValue(state, "hiddenStateVar", + exp(-0.5 * state.getTime())); + } + +private: + + // keep track of the force added by the component + mutable ForceIndex fix; + ReferencePtr world; + +}; // End of class Bar + +//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +// The section above was taken from testComponentInterface.cpp. +// The code is adapted to generate a dummy model with a SimTK::State that is +// comprised of continuous variables (q, u, z), discrete variables of all +// supported types, and modeling options. +//----------------------------------------------------------------------------- + -//using namespace OpenSim; // Internal static methods and classes. namespace @@ -34,9 +422,167 @@ namespace } + TEST_CASE("Getting Started") { double x = 2.0; double square = customSquare(x); REQUIRE(square == x*x); } + +TEST_CASE("Component Interface State Trajectories") +{ + MultibodySystem system; + TheWorld wrld; + wrld.setName("World"); + + Foo& foo = *new Foo(); + foo.setName("Foo"); + wrld.add(&foo); + foo.set_mass(2.0); + + Foo& foo2 = *new Foo(); + foo2.setName("Foo2"); + foo2.set_mass(3.0); + wrld.add(&foo2); + + Bar& bar = *new Bar(); + bar.setName("Bar"); + wrld.add(&bar); + + bar.connectSocket_parentFoo(foo); + bar.connectSocket_childFoo(foo2); + + wrld.connect(); + wrld.buildUpSystem(system); + + State s = system.realizeTopology(); + + // Form the q and u vectors + const Array& svPaths = wrld.getStateVariableNames(); + REQUIRE(svPaths.size() == 4); + const Vector y = Vector(svPaths.size(), 0.1); + + // Get the paths of all discrete variables under "wrld" + const OpenSim::Array& dvPaths = + wrld.getDiscreteVariableNames(); + REQUIRE(dvPaths.size() == 2); + REQUIRE(dvPaths[0] == "/internalSub/dvX"); + REQUIRE(dvPaths[1] == "/Bar/point"); + + // Get the paths of all modeling moptions under "wrld" + const OpenSim::Array& moPaths = + wrld.getModelingOptionNames(); + REQUIRE(moPaths.size() == 2); + REQUIRE(moPaths[0] == "/internalSub/moX"); + REQUIRE(moPaths[1] == "/Bar/moY"); + + // Get the starting value of point + // The starting value should be (0.0, 0.1, 1.0). + // See Bar::extendRealizeTopology(). + Vec3 pointStart = wrld.getDiscreteVariableValue(s, "/Bar/point"); + + // Run an artificial simulation and record the state trajectory + SimTK::Array_ simTraj; + int nsteps{11}; + simTraj.reserve(nsteps); + int moX{0}, moY{0}; + double dvX{0.0}; + Vec3 point(0.0); + for (int i = 0; i < nsteps; ++i){ + // Time + s.updTime() = i*0.01; + // State Variables + wrld.setStateVariableValue(s, svPaths[0], i*y[0] + 0); + wrld.setStateVariableValue(s, svPaths[1], i*y[1] + 1); + wrld.setStateVariableValue(s, svPaths[2], i*y[2] + 2); + wrld.setStateVariableValue(s, svPaths[3], i*y[3] + 3); + // Discrete Variables + dvX = i*0.5 ; + wrld.setDiscreteVariableValue(s, dvPaths[0], dvX); + point = i*pointStart; + wrld.setDiscreteVariableValue(s, dvPaths[1], point); + // Modeling Options + moX = i % 2; + wrld.setModelingOption(s, moPaths[0], moX); + moY = (i+1) % 2; + wrld.setModelingOption(s, moPaths[1], moY); + // Accumulate the simulated state trajectory + system.realize(s, Stage::Report); + simTraj.emplace_back(s); + } + + // Extract individual variable trajectories (as though serializing) + // state variables + SimTK::Array_ y0Traj, y1Traj, y2Traj, y3Traj; + wrld.getStateVariableTrajectory(svPaths[0], simTraj, y0Traj); + wrld.getStateVariableTrajectory(svPaths[1], simTraj, y1Traj); + wrld.getStateVariableTrajectory(svPaths[2], simTraj, y2Traj); + wrld.getStateVariableTrajectory(svPaths[3], simTraj, y3Traj); + // discrete variables + SimTK::Array_ dv0Traj; + SimTK::Array_ dv1Traj; + wrld.getDiscreteVariableTrajectory(dvPaths[0], simTraj, dv0Traj); + wrld.getDiscreteVariableTrajectory(dvPaths[1], simTraj, dv1Traj); + // modeling options + SimTK::Array_ mo0Traj; + wrld.getModelingOptionTrajectory(moPaths[0], simTraj, mo0Traj); + SimTK::Array_ mo1Traj; + wrld.getModelingOptionTrajectory(moPaths[1], simTraj, mo1Traj); + + // Check the individual variable trajectories + for (int i = 0; i < nsteps; ++i){ + // state variables + CHECK(y0Traj[i] == i*y[0] + 0); + CHECK(y1Traj[i] == i*y[1] + 1); + CHECK(y2Traj[i] == i*y[2] + 2); + CHECK(y3Traj[i] == i*y[3] + 3); + // discrete variables + CHECK(dv0Traj[i] == i*0.5); + CHECK(dv1Traj[i] == i*pointStart); + // modeling options + CHECK(mo0Traj[i] == (i%2)); + CHECK(mo1Traj[i] == ((i+1)%2)); + } + + // Create a new state trajectory (as though deserializing) + // 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 + wrld.setStateVariableTrajectory(svPaths[0], y0Traj, newTraj); + wrld.setStateVariableTrajectory(svPaths[1], y1Traj, newTraj); + wrld.setStateVariableTrajectory(svPaths[2], y2Traj, newTraj); + wrld.setStateVariableTrajectory(svPaths[3], y3Traj, newTraj); + // discrete variables + wrld.setDiscreteVariableTrajectory(dvPaths[0], dv0Traj, newTraj); + wrld.setDiscreteVariableTrajectory(dvPaths[1], dv1Traj, newTraj); + // modeling option + wrld.setModelingOptionTrajectory(moPaths[0], mo0Traj, newTraj); + wrld.setModelingOptionTrajectory(moPaths[1], mo1Traj, newTraj); + + // Check the new state trajectory + SimTK::Array_ nq0Traj, nq1Traj, nq2Traj, nq3Traj; + SimTK::Array_ ndv0Traj; + SimTK::Array_ ndv1Traj; + SimTK::Array_ nmo0Traj; + SimTK::Array_ nmo1Traj; + wrld.getStateVariableTrajectory(svPaths[0], newTraj, nq0Traj); + wrld.getStateVariableTrajectory(svPaths[1], newTraj, nq1Traj); + wrld.getStateVariableTrajectory(svPaths[2], newTraj, nq2Traj); + wrld.getStateVariableTrajectory(svPaths[3], newTraj, nq3Traj); + wrld.getDiscreteVariableTrajectory(dvPaths[0], newTraj, ndv0Traj); + wrld.getDiscreteVariableTrajectory(dvPaths[1], newTraj, ndv1Traj); + wrld.getModelingOptionTrajectory(moPaths[0], newTraj, nmo0Traj); + wrld.getModelingOptionTrajectory(moPaths[1], newTraj, nmo1Traj); + for (int i = 0; i < nsteps; ++i){ + CHECK(nq0Traj[i] == i*y[0] + 0); + CHECK(nq1Traj[i] == i*y[1] + 1); + CHECK(nq2Traj[i] == i*y[2] + 2); + CHECK(nq3Traj[i] == i*y[3] + 3); + CHECK(ndv0Traj[i] == i*0.5); + CHECK(ndv1Traj[i] == i*pointStart); + CHECK(nmo0Traj[i] == (i%2)); + CHECK(nmo1Traj[i] == ((i+1)%2)); + } +} From 03a62295b11f6f2982060804ed2ee84b17e72199 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 30 May 2024 10:30:19 -0500 Subject: [PATCH 08/56] Update testStatesDocument.cpp - Removed code borrowed from testComponentInterface.cpp. That code was not the way to go. A legitimate model is needed. I'm starting over. --- .../Simulation/Test/testStatesDocument.cpp | 550 +----------------- 1 file changed, 1 insertion(+), 549 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index b819bda092..4642d4d678 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -20,397 +20,8 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - +#include #include -#include - -//----------------------------------------------------------------------------- -// The section above was taken from testComponentInterface.cpp. -// The code is adapted to generate a dummy model with a SimTK::State that is -// comprised of continuous variables (q, u, z), discrete variables of all -// supported types, and modeling options. -//----------------------------------------------------------------------------- -using namespace OpenSim; -using namespace std; -using namespace SimTK; - -class Foo; -class Bar; - -class Sub : public Component { - OpenSim_DECLARE_CONCRETE_OBJECT(Sub, Component); -public: - OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(subState); - Sub() = default; - virtual ~Sub() = default; -private: - void extendAddToSystem(MultibodySystem &system) const override { - Super::extendAddToSystem(system); - addStateVariable("subState", Stage::Dynamics); - addDiscreteVariable("dvX", Stage::Dynamics); - addModelingOption("moX", 2); - } - void computeStateVariableDerivatives(const SimTK::State& s) const override { - double deriv = exp(-2.0*s.getTime()); - setStateVariableDerivativeValue(s, "subState", deriv); - } -}; //end class Sub - -class TheWorld : public Component { - OpenSim_DECLARE_CONCRETE_OBJECT(TheWorld, Component); -public: - TheWorld() : Component() { } - - TheWorld(const std::string& fileName, bool updFromXMLNode = false) - : Component(fileName, updFromXMLNode) { - // Propagate XML file values to properties - updateFromXMLDocument(); - // add components listed as properties as sub components. - finalizeFromProperties(); - } - - void add(Component* comp) { - addComponent(comp); - // Edit Sub - /*Sub& subc = */updMemberSubcomponent(intSubix); - } - - // Top level connection method for this all encompassing component, TheWorld - void connect() { - Super::finalizeConnections(*this); - } - void buildUpSystem(MultibodySystem& system) { - connect(); - addToSystem(system); - } - - const SimbodyMatterSubsystem& getMatterSubsystem() const { return *matter; } - SimbodyMatterSubsystem& updMatterSubsystem() const { return *matter; } - - const GeneralForceSubsystem& getForceSubsystem() const { return *forces; } - GeneralForceSubsystem& updForceSubsystem() const { return *forces; } - -protected: - // Component interface implementation - void extendAddToSystem(MultibodySystem& system) const override { - if (system.hasMatterSubsystem()){ - matter = system.updMatterSubsystem(); - } - else{ - // const Sub& subc = getMemberSubcomponent(intSubix); - - SimbodyMatterSubsystem* old_matter = matter.release(); - delete old_matter; - matter = new SimbodyMatterSubsystem(system); - - GeneralForceSubsystem* old_forces = forces.release(); - delete old_forces; - forces = new GeneralForceSubsystem(system); - - SimTK::Force::UniformGravity gravity(*forces, *matter, Vec3(0, -9.816, 0)); - fix = gravity.getForceIndex(); - - system.updMatterSubsystem().setShowDefaultGeometry(true); - } - } - -private: - // Keep track of pointers to the underlying computational subsystems. - mutable ReferencePtr matter; - mutable ReferencePtr forces; - - // keep track of the force added by the component - mutable ForceIndex fix; - - MemberSubcomponentIndex intSubix{ constructSubcomponent("internalSub") }; - -}; // end of TheWorld - - -class Foo : public Component { - OpenSim_DECLARE_CONCRETE_OBJECT(Foo, Component); -public: - //============================================================================= - // PROPERTIES - //============================================================================= - OpenSim_DECLARE_PROPERTY(mass, double, "mass (kg)"); - OpenSim_DECLARE_LIST_PROPERTY_SIZE(inertia, double, 6, - "inertia {Ixx, Iyy, Izz, Ixy, Ixz, Iyz}"); - - OpenSim_DECLARE_OUTPUT(Output1, double, getSomething, SimTK::Stage::Time); - OpenSim_DECLARE_OUTPUT(Output2, SimTK::Vec3, calcSomething, - SimTK::Stage::Time); - - OpenSim_DECLARE_OUTPUT(Output3, double, getSomethingElse, SimTK::Stage::Time); - - OpenSim_DECLARE_OUTPUT(Qs, Vector, getQ, SimTK::Stage::Position); - - OpenSim_DECLARE_OUTPUT(BodyAcc, SpatialVec, calcSpatialAcc, - SimTK::Stage::Velocity); - - OpenSim_DECLARE_OUTPUT(return_by_ref, double, getReturnByRef, - SimTK::Stage::Time); - - OpenSim_DECLARE_INPUT(input1, double, SimTK::Stage::Model, ""); - OpenSim_DECLARE_INPUT(AnglesIn, Vector, SimTK::Stage::Model, ""); - OpenSim_DECLARE_INPUT(fiberLength, double, SimTK::Stage::Model, ""); - OpenSim_DECLARE_INPUT(activation, double, SimTK::Stage::Model, ""); - OpenSim_DECLARE_LIST_INPUT(listInput1, double, SimTK::Stage::Model, ""); - - Foo() : Component() { - constructProperties(); - m_ctr = 0; - m_mutableCtr = 0; - } - - double getSomething(const SimTK::State& state) const { - const_cast(this)->m_ctr++; - m_mutableCtr++; - - return state.getTime(); - } - - SimTK::Vec3 calcSomething(const SimTK::State& state) const { - const_cast(this)->m_ctr++; - m_mutableCtr++; - - double t = state.getTime(); - return SimTK::Vec3(t, t*t, sqrt(t)); - } - - double getSomethingElse(const SimTK::State& state) const { - return 1.618; - } - - SimTK::Vector getQ(const SimTK::State& state) const { - return state.getQ(); - } - - SimTK::SpatialVec calcSpatialAcc(const SimTK::State& state) const { - const_cast(this)->m_ctr++; - m_mutableCtr++; - - return getSystem().getMatterSubsystem().getMobilizedBody(bindex) - .getBodyAcceleration(state); - } - - const double& getReturnByRef(const SimTK::State& s) const { - // Must return something that is stored in the state! - return s.getTime(); - } - -protected: - /** Component Interface */ - void extendFinalizeConnections(Component& root) override { - Super::extendFinalizeConnections(root); - // do any internal wiring - world = dynamic_cast(&root); - } - - void extendAddToSystem(MultibodySystem &system) const override { - Super::extendAddToSystem(system); - - SimbodyMatterSubsystem& matter = system.updMatterSubsystem(); - - Vec3 mInB(0.0, 1.0, 0); - Vec3 mInP(0, 0, 0); - - SimTK::Body::Rigid bone( - MassProperties(1, Vec3(0), Inertia::brick(0.5, 1, 0.5))); - - // Thigh connected by hip - MobilizedBody::Pin b1ToGround(matter.updGround(), SimTK::Transform(mInP), - bone, SimTK::Transform(mInB)); - - //Pin knee connects shank - MobilizedBody::Pin b1ToB2(b1ToGround, SimTK::Transform(mInP), - bone, SimTK::Transform(mInB)); - - bindex = b1ToB2.getMobilizedBodyIndex(); - } - -private: - int m_ctr; - mutable int m_mutableCtr; - - - void constructProperties() { - constructProperty_mass(1.0); - Array inertia(0.001, 6); - inertia[0] = inertia[1] = inertia[2] = 0.1; - constructProperty_inertia(inertia); - } - - // Keep indices and reference to the world - mutable MobilizedBodyIndex bindex; - ReferencePtr world; - -}; // End of class Foo - -class Bar : public Component { - OpenSim_DECLARE_CONCRETE_OBJECT(Bar, Component); -public: - - OpenSim_DECLARE_SOCKET(parentFoo, Foo, ""); - OpenSim_DECLARE_SOCKET(childFoo, Foo, ""); - OpenSim_DECLARE_LIST_SOCKET(listFoo, Foo, ""); - - // This is used to test output copying and returns the address of the - // component. - OpenSim_DECLARE_OUTPUT(copytesting, size_t, myself, SimTK::Stage::Model); - // Use this member variable to ensure that output functions get copied - // correctly. - double copytestingViaMemberVariable = 5; - OpenSim_DECLARE_OUTPUT(copytestingMemVar, double, getCopytestingMemVar, - SimTK::Stage::Model); - - OpenSim_DECLARE_OUTPUT(PotentialEnergy, double, getPotentialEnergy, - SimTK::Stage::Velocity); - - OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(fiberLength); - OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(activation); - - double getPotentialEnergy(const SimTK::State& state) const { - const GeneralForceSubsystem& forces = world->getForceSubsystem(); - const SimTK::Force& force = forces.getForce(fix); - const auto& spring = SimTK::Force::TwoPointLinearSpring::downcast(force); - - return spring.calcPotentialEnergyContribution(state); - } - - /** Returns the `this` pointer. Used to ensure that the std::function - within Outputs is properly copied when copying components. */ - size_t myself(const SimTK::State& s) const { return size_t(this); } - - double getCopytestingMemVar(const SimTK::State& s) const - { return copytestingViaMemberVariable; } - - class ParentAndFooAreSame : OpenSim::Exception { - public: - using OpenSim::Exception::Exception; - }; -protected: - /** Component Interface */ - void extendFinalizeConnections(Component& root) override{ - Super::extendFinalizeConnections(root); - // do any internal wiring - world = dynamic_cast(&root); - // perform custom checking - if (&updSocket("parentFoo").getConnectee() - == &updSocket("childFoo").getConnectee()){ - string msg = "ERROR - Bar::extendFinalizeConnections()\n"; - msg += " parentFoo and childFoo cannot be the same component."; - throw ParentAndFooAreSame(msg); - } - } - - // Copied here from Component for testing purposes. - - - void extendAddToSystem(MultibodySystem& system) const override{ - - GeneralForceSubsystem& forces = world->updForceSubsystem(); - SimbodyMatterSubsystem& matter = world->updMatterSubsystem(); - - int nb = matter.getNumBodies(); - if (nb > 2) { - const MobilizedBody& b1 = matter.getMobilizedBody(MobilizedBodyIndex(1)); - const MobilizedBody& b2 = matter.getMobilizedBody(MobilizedBodyIndex(2)); - - SimTK::Force::TwoPointLinearSpring - spring(forces, b1, Vec3(0.5,0,0), b2, Vec3(0.5,0,0), 10.0, 0.1); - fix = spring.getForceIndex(); - } - - // We use these to test the Output's that are generated when we - // add a StateVariable. - addStateVariable("fiberLength", SimTK::Stage::Velocity); - addStateVariable("activation", SimTK::Stage::Dynamics); - - // Create a hidden state variable, so we can ensure that hidden state - // variables do not have a corresponding Output. - bool hidden = true; - addStateVariable("hiddenStateVar", SimTK::Stage::Dynamics, hidden); - - // Add a modeling option (mo) and a discrete variable (dv) as though - // they were allocated natively in Simbody. - // This will allow testing the ability to accomodate a mo and dv that - // were allocated from subsystem different than the default subsystem - // 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 be allocated in class Bar's override of - // extendRealizeTopology(). See below. - bool allocate = false; - int maxFlagValue = 1; - addDiscreteVariable("point", Stage::Position, allocate); - addModelingOption("moY", maxFlagValue, allocate); - } - - // 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); - - GeneralForceSubsystem& fsub = world->updForceSubsystem(); - - // Discrete Variable Initialization - std::string dvName = "point"; - Vec3 point(0.0, 0.1, 0.2); - SimTK::DiscreteVariableIndex dvIndex = - fsub.allocateDiscreteVariable(state, Stage::Dynamics, - new Value(point)); - initializeDiscreteVariableIndexes(dvName, - fsub.getMySubsystemIndex(), dvIndex); - - // Modeling Option Initialization - std::string moName = "moY"; - int moVal{0}; - SimTK::DiscreteVariableIndex moIndex = - fsub.allocateDiscreteVariable(state, Stage::Dynamics, - new Value(moVal)); - initializeModelingOptionIndexes(moName, - fsub.getMySubsystemIndex(), moIndex); - } - - void computeStateVariableDerivatives(const SimTK::State& state) const override { - setStateVariableDerivativeValue(state, "fiberLength", 2.0); - setStateVariableDerivativeValue(state, "activation", 3.0 * state.getTime()); - setStateVariableDerivativeValue(state, "hiddenStateVar", - exp(-0.5 * state.getTime())); - } - -private: - - // keep track of the force added by the component - mutable ForceIndex fix; - ReferencePtr world; - -}; // End of class Bar - -//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -// The section above was taken from testComponentInterface.cpp. -// The code is adapted to generate a dummy model with a SimTK::State that is -// comprised of continuous variables (q, u, z), discrete variables of all -// supported types, and modeling options. -//----------------------------------------------------------------------------- - - // Internal static methods and classes. namespace @@ -421,168 +32,9 @@ namespace } } - - TEST_CASE("Getting Started") { double x = 2.0; double square = customSquare(x); REQUIRE(square == x*x); } - -TEST_CASE("Component Interface State Trajectories") -{ - MultibodySystem system; - TheWorld wrld; - wrld.setName("World"); - - Foo& foo = *new Foo(); - foo.setName("Foo"); - wrld.add(&foo); - foo.set_mass(2.0); - - Foo& foo2 = *new Foo(); - foo2.setName("Foo2"); - foo2.set_mass(3.0); - wrld.add(&foo2); - - Bar& bar = *new Bar(); - bar.setName("Bar"); - wrld.add(&bar); - - bar.connectSocket_parentFoo(foo); - bar.connectSocket_childFoo(foo2); - - wrld.connect(); - wrld.buildUpSystem(system); - - State s = system.realizeTopology(); - - // Form the q and u vectors - const Array& svPaths = wrld.getStateVariableNames(); - REQUIRE(svPaths.size() == 4); - const Vector y = Vector(svPaths.size(), 0.1); - - // Get the paths of all discrete variables under "wrld" - const OpenSim::Array& dvPaths = - wrld.getDiscreteVariableNames(); - REQUIRE(dvPaths.size() == 2); - REQUIRE(dvPaths[0] == "/internalSub/dvX"); - REQUIRE(dvPaths[1] == "/Bar/point"); - - // Get the paths of all modeling moptions under "wrld" - const OpenSim::Array& moPaths = - wrld.getModelingOptionNames(); - REQUIRE(moPaths.size() == 2); - REQUIRE(moPaths[0] == "/internalSub/moX"); - REQUIRE(moPaths[1] == "/Bar/moY"); - - // Get the starting value of point - // The starting value should be (0.0, 0.1, 1.0). - // See Bar::extendRealizeTopology(). - Vec3 pointStart = wrld.getDiscreteVariableValue(s, "/Bar/point"); - - // Run an artificial simulation and record the state trajectory - SimTK::Array_ simTraj; - int nsteps{11}; - simTraj.reserve(nsteps); - int moX{0}, moY{0}; - double dvX{0.0}; - Vec3 point(0.0); - for (int i = 0; i < nsteps; ++i){ - // Time - s.updTime() = i*0.01; - // State Variables - wrld.setStateVariableValue(s, svPaths[0], i*y[0] + 0); - wrld.setStateVariableValue(s, svPaths[1], i*y[1] + 1); - wrld.setStateVariableValue(s, svPaths[2], i*y[2] + 2); - wrld.setStateVariableValue(s, svPaths[3], i*y[3] + 3); - // Discrete Variables - dvX = i*0.5 ; - wrld.setDiscreteVariableValue(s, dvPaths[0], dvX); - point = i*pointStart; - wrld.setDiscreteVariableValue(s, dvPaths[1], point); - // Modeling Options - moX = i % 2; - wrld.setModelingOption(s, moPaths[0], moX); - moY = (i+1) % 2; - wrld.setModelingOption(s, moPaths[1], moY); - // Accumulate the simulated state trajectory - system.realize(s, Stage::Report); - simTraj.emplace_back(s); - } - - // Extract individual variable trajectories (as though serializing) - // state variables - SimTK::Array_ y0Traj, y1Traj, y2Traj, y3Traj; - wrld.getStateVariableTrajectory(svPaths[0], simTraj, y0Traj); - wrld.getStateVariableTrajectory(svPaths[1], simTraj, y1Traj); - wrld.getStateVariableTrajectory(svPaths[2], simTraj, y2Traj); - wrld.getStateVariableTrajectory(svPaths[3], simTraj, y3Traj); - // discrete variables - SimTK::Array_ dv0Traj; - SimTK::Array_ dv1Traj; - wrld.getDiscreteVariableTrajectory(dvPaths[0], simTraj, dv0Traj); - wrld.getDiscreteVariableTrajectory(dvPaths[1], simTraj, dv1Traj); - // modeling options - SimTK::Array_ mo0Traj; - wrld.getModelingOptionTrajectory(moPaths[0], simTraj, mo0Traj); - SimTK::Array_ mo1Traj; - wrld.getModelingOptionTrajectory(moPaths[1], simTraj, mo1Traj); - - // Check the individual variable trajectories - for (int i = 0; i < nsteps; ++i){ - // state variables - CHECK(y0Traj[i] == i*y[0] + 0); - CHECK(y1Traj[i] == i*y[1] + 1); - CHECK(y2Traj[i] == i*y[2] + 2); - CHECK(y3Traj[i] == i*y[3] + 3); - // discrete variables - CHECK(dv0Traj[i] == i*0.5); - CHECK(dv1Traj[i] == i*pointStart); - // modeling options - CHECK(mo0Traj[i] == (i%2)); - CHECK(mo1Traj[i] == ((i+1)%2)); - } - - // Create a new state trajectory (as though deserializing) - // 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 - wrld.setStateVariableTrajectory(svPaths[0], y0Traj, newTraj); - wrld.setStateVariableTrajectory(svPaths[1], y1Traj, newTraj); - wrld.setStateVariableTrajectory(svPaths[2], y2Traj, newTraj); - wrld.setStateVariableTrajectory(svPaths[3], y3Traj, newTraj); - // discrete variables - wrld.setDiscreteVariableTrajectory(dvPaths[0], dv0Traj, newTraj); - wrld.setDiscreteVariableTrajectory(dvPaths[1], dv1Traj, newTraj); - // modeling option - wrld.setModelingOptionTrajectory(moPaths[0], mo0Traj, newTraj); - wrld.setModelingOptionTrajectory(moPaths[1], mo1Traj, newTraj); - - // Check the new state trajectory - SimTK::Array_ nq0Traj, nq1Traj, nq2Traj, nq3Traj; - SimTK::Array_ ndv0Traj; - SimTK::Array_ ndv1Traj; - SimTK::Array_ nmo0Traj; - SimTK::Array_ nmo1Traj; - wrld.getStateVariableTrajectory(svPaths[0], newTraj, nq0Traj); - wrld.getStateVariableTrajectory(svPaths[1], newTraj, nq1Traj); - wrld.getStateVariableTrajectory(svPaths[2], newTraj, nq2Traj); - wrld.getStateVariableTrajectory(svPaths[3], newTraj, nq3Traj); - wrld.getDiscreteVariableTrajectory(dvPaths[0], newTraj, ndv0Traj); - wrld.getDiscreteVariableTrajectory(dvPaths[1], newTraj, ndv1Traj); - wrld.getModelingOptionTrajectory(moPaths[0], newTraj, nmo0Traj); - wrld.getModelingOptionTrajectory(moPaths[1], newTraj, nmo1Traj); - for (int i = 0; i < nsteps; ++i){ - CHECK(nq0Traj[i] == i*y[0] + 0); - CHECK(nq1Traj[i] == i*y[1] + 1); - CHECK(nq2Traj[i] == i*y[2] + 2); - CHECK(nq3Traj[i] == i*y[3] + 3); - CHECK(ndv0Traj[i] == i*0.5); - CHECK(ndv1Traj[i] == i*pointStart); - CHECK(nmo0Traj[i] == (i%2)); - CHECK(nmo1Traj[i] == ((i+1)%2)); - } -} From 368f123870db6dc5e2d441a03436c3d352f4ffc7 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 31 May 2024 11:05:01 -0500 Subject: [PATCH 09/56] Update StatesDocument.h - Made small improvements and corrections in the documentation. --- OpenSim/Simulation/StatesDocument.h | 95 ++++++++++++++++------------- 1 file changed, 52 insertions(+), 43 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index eef12c365d..4a1bd6a963 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -88,11 +88,11 @@ 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 into a single file, which by convention has the```.ostates``` +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 +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... @@ -105,7 +105,7 @@ 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```? + SimTK::State Contents | Serialized in `.ostates`? ------------------------ | ----------------------- Continuous Variables | yes Discrete Variables | yes @@ -119,8 +119,7 @@ Design Notes ### Dependencies Most operations in class StatesDocument rely on underlying SimTK classes, -most notably SimTK::String, SimTK::Vector_, SimTK::Array, SimTK::State, -and SimTK::Xml. +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 @@ -163,23 +162,23 @@ the OpenSim::Component class. A few examples follow. SimTK::Array_& output) const ``` A call to the above method first finds a Discrete Variable in the model -hierarchy based on the specifed path name (```pathName```). Then, from the -input states trajectory (```input```), the method extracts the values of the +hierarchy based on the specifed path (`pathName`). 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 +(`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 setStatesTrajectoryForDiscreteVariable( + void setDiscreteVariableTrajectory( const std::string& pathName, 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 +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 +(`output`). Notice again that the type T of the Discrete Variable can be specified by the caller. ### Complete and Constant XML Document upon Construction @@ -188,42 +187,48 @@ 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 model is added) or +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 internal XML document that reflects those changes is to construct a new +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 not needed. +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 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 +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. In applications where exact values of the states are needed, -lossless precision should be used. +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 +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 +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 +### Sample `.ostates` File ``` @@ -253,14 +258,14 @@ XML format to be recognized. 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. + 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 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. + 2) 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. - 3) All ```variable``` and ```option``` paths must be found in the model + 3) All `variable` and `option` paths must be found in the model OpenSim::Component heirarchy. 4) The type must be supported. As of January 2024, the following types are @@ -304,12 +309,12 @@ deserialize those same states and use them to accomplish a few basic things. // ------------------------------- // The reporter records the SimTK::State in a SimTK::Array_<> at a // specified time interval. - OpenSim::StatesTrajectoryReporter - reporter = new StatesTrajectoryReporter(); + OpenSim::StatesTrajectoryReporter* reporter = + new StatesTrajectoryReporter(); reporter->setName("states_reporter"); double interval = 0.01; reporter->set_report_time_interval(interval); - model->addComponent(statesReporter); + model->addComponent(reporter); // ----------------------------------------- // Build the System and Initialize the State @@ -352,8 +357,8 @@ deserialize those same states and use them to accomplish a few basic things. // Serialize the States to File // ---------------------------- // The file name (see below), can be any string supported by the file - // system can be. The recommended convention is for the file name to carry - // the suffix ".ostates". Below, the suffix ".ostates" is simply added to + // 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"). @@ -403,7 +408,7 @@ deserialize those same states and use them to accomplish a few basic things. std::string path; const SimTK::State* iter; for(iter = traj.cbegin(); iter!=traj.cend(); ++iter) { - + // Get time double t = iter->getTime(); @@ -426,11 +431,15 @@ deserialize those same states and use them to accomplish a few basic things. 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 Trajectories for Individual States - // ------------------------------------------ + // ---------------------------------------------------- + // Extract a Complete Trajectory for a Particular State + // ---------------------------------------------------- // Continuous (double) path = "/jointset/free/free_coord_0/value"; SimTK::Array_ xTraj; From 9585d98b4d50f2a6db7301a91714002ce37ce33b Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 31 May 2024 11:06:36 -0500 Subject: [PATCH 10/56] Update testStatesDocument.cpp - Now building a simple model and running a simulation. --- .../Simulation/Test/testStatesDocument.cpp | 111 +++++++++++++++++- 1 file changed, 107 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 4642d4d678..c02e25f02b 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -20,21 +20,124 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ +#include #include +#include +#include +#include +#include #include +using namespace SimTK; +using namespace OpenSim; + // Internal static methods and classes. namespace { - double customSquare(double x) - { - return(x*x); - } + +//_____________________________________________________________________________ +// Sample internal method +double +customSquare(double x) +{ + return(x*x); } +//_____________________________________________________________________________ +// Build the model +Model* +buildModel() { + + // Create an empty model + Model* model = new Model(); + Vec3 gravity(0.0, -10.0, 0.0); + model->setGravity(gravity); + model->setName("BlockOnASpring"); + + // 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.1); + PointToPointSpring* spring = new PointToPointSpring(ground, origin, + *block, insertion, kp, restlength); + model->addForce(spring); + + return model; +} + +//_____________________________________________________________________________ +// Simulate +const OpenSim::StatesTrajectory* +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.01; + 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 the trajectory + return &reporter->getStates(); +} + +} // End anonymous namespace + + TEST_CASE("Getting Started") { double x = 2.0; double square = customSquare(x); REQUIRE(square == x*x); } + + +TEST_CASE("StatesDocument Serialization and Deserialization") +{ + Model *model = buildModel(); + simulate(model); + + REQUIRE(1 == 1); + + delete model; +} From 7a7698272e047559d7b852f58524c8735aab8c7d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 31 May 2024 14:22:23 -0500 Subject: [PATCH 11/56] Update testStatesDocument.cpp - Added code to serialize, deserialize, and then re-serialize the state trajectory recorded during a simulation. The code is functioning as expected! --- OpenSim/Simulation/Test/testStatesDocument.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index c02e25f02b..fdd6bdf905 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -99,7 +99,7 @@ simulate(Model* model) { OpenSim::StatesTrajectoryReporter* reporter = new StatesTrajectoryReporter(); reporter->setName("states_reporter"); - double interval = 0.01; + double interval = 0.1; reporter->set_report_time_interval(interval); model->addComponent(reporter); @@ -135,7 +135,19 @@ TEST_CASE("Getting Started") TEST_CASE("StatesDocument Serialization and Deserialization") { Model *model = buildModel(); - simulate(model); + const StatesTrajectory *traj = simulate(model); + int precision = 3; + SimTK::String filename = "BlockOnAString.ostates"; + StatesDocument doc1 = traj->exportToStatesDocument(*model, precision); + doc1.serialize(filename); + + StatesDocument doc2(filename); + SimTK::Array_ trajDe; + doc2.deserialize(*model, trajDe); + + SimTK::String filename2 = "BlockOnAString02.ostates"; + StatesDocument doc3(*model, trajDe, precision); + doc3.serialize(filename2); REQUIRE(1 == 1); From 3cf98fa447c22639e882a52a1850965b278dfa70 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 31 May 2024 14:32:26 -0500 Subject: [PATCH 12/56] Removed trailing whitespace in StatesTrajectory.h --- OpenSim/Simulation/StatesTrajectory.h | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index 8a4b265dcf..a986c93c12 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -47,7 +47,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 +75,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 +151,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 +172,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; @@ -337,11 +337,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 +360,7 @@ class OSIMSIMULATION_API StatesTrajectory { msg += " " + extraStates[i] + "\n"; } msg += " " + extraStates.back(); - + addMessage(msg); } }; @@ -519,7 +519,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 From 842cca65bfa29ec9a1ba45b7dceb6de25f30fb67 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 31 May 2024 14:46:36 -0500 Subject: [PATCH 13/56] Update StatesTrajectory.h - Changed member variable `m_states` from type std::vector to SimTK::Array_ - Added `exportToStatesDocument()` --- OpenSim/Simulation/StatesTrajectory.h | 52 +++++++++++++++++++++++++-- 1 file changed, 50 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index a986c93c12..eb735700c5 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -28,6 +28,7 @@ #include #include #include +#include #include "osimSimulationDLL.h" @@ -188,7 +189,7 @@ class OSIMSIMULATION_API StatesTrajectory { /** 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,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 the following: + * + * StatesDocument::serializeToFile() + * StatesDocument::serializeToSring() + * + * Once the states have been serialized, they can be deserialized by + * constructing a new StatesDocument from a file or document string: + * + * StatesDocument(const SimTK::String& filename) + * StatesDocument(const char* xmlDocument) + * + * and then calling the following: + * + * StatesDocument::deserialize(const OpenSim::Model& model, + * SimTK::Array_& trajectory) + * * + * The OSTATES format is plain-text XML (see SimTK::Xml) with precision + * sufficient to restore a time-history of SimTK::State%s with virtually + * no trunction or rounding error. + * + * 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 generally 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 source as + * it will include continuous, discrete, and modeling states. + */ + OpenSim::StatesDocument + exportToStatesDocument(const OpenSim::Model& model, + int precision = SimTK::LosslessNumDigitsReal) const + { + return OpenSim::StatesDocument(model, m_states, precision); + } + private: - std::vector m_states; + SimTK::Array_ m_states; public: From 21699edbc6a00e92461380f5ed0d5207d0a71073 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 1 Jun 2024 17:16:45 -0500 Subject: [PATCH 14/56] Update StatesTrajectory.h - Added a `get` method that provides access to the underlying SimTK::Array_ member variable. --- OpenSim/Simulation/StatesTrajectory.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index eb735700c5..fbd8099196 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -337,6 +337,11 @@ class OSIMSIMULATION_API StatesTrajectory { return OpenSim::StatesDocument(model, m_states, precision); } + /** Get a read-only reference to the underlying state array. */ + const SimTK::Array_& getUnderlyingStateArray() const { + return m_states; + } + private: SimTK::Array_ m_states; From dd18f4c406cd880ab3344eaeb4b567915429ca67 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 1 Jun 2024 17:18:29 -0500 Subject: [PATCH 15/56] Update testStatesDocument.cpp - Added a method that tests equality of 2 state trajectories. The method does not yet test equality for discrete variables or modeling options, but Qs, Us, and Zs are passing. --- .../Simulation/Test/testStatesDocument.cpp | 120 ++++++++++++++++-- 1 file changed, 107 insertions(+), 13 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index fdd6bdf905..463073cb04 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -43,6 +43,93 @@ customSquare(double x) return(x*x); } +//_____________________________________________________________________________ +// Test for exact 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. +bool +testEquality(const Model& model, + const Array_& trajA, const Array_& trajB) +{ + // Check the array size + REQUIRE(trajA.size() == trajB.size()); + + // Get the SimTK::System + const System& sys = model.getSystem(); + + // Iterate through the State Trajectory + std::string path; + double tA, tB; + const State* stateA = trajA.cbegin(); + const State* stateB = trajB.cbegin(); + // Time + for(int iTime=0; stateA!=trajA.cend(); ++iTime, ++stateA, ++stateB) { + + // Check subsystem consistency + REQUIRE(stateA->isConsistent(*stateB)); + + // Get time + tA = stateA->getTime(); + tB = stateB->getTime(); + REQUIRE(tA == tB); + + // Check the number of subsystesm + int nsubA = stateA->getNumSubsystems(); + int nsubB = stateB->getNumSubsystems(); + REQUIRE(nsubA == nsubB); + + // Realize to Stage::Report + // This ensures that all states have been initialized, including + // quaternion parameters, and that all constraints have been enforced. + sys.realize(*stateA, Stage::Report); + sys.realize(*stateB, Stage::Report); + + // Continuous variables. + // This is an exhaustive, low-level comparision on the SimTK side + // that is independent of the OpenSim Component heirarchy. + // Q + const Vector& qA = stateA->getQ(); + const Vector& qB = stateB->getQ(); + int nq = qA.size(); + for (int i = 0; i < nq; ++i) { + if(i==6) std::cout << "q6= " << qA[i] << " " << qB[i] << std::endl; + REQUIRE(qA[i] == qB[i]); + } + // U + const Vector& uA = stateA->getU(); + const Vector& uB = stateB->getU(); + int nu = uA.size(); + for (int i = 0; i < nu; ++i) REQUIRE(uA[i] == uB[i]); + // Z + const Vector& zA = stateA->getZ(); + const Vector& zB = stateB->getZ(); + int nz = zA.size(); + for (int i = 0; i < nz; ++i) REQUIRE(zA[i] == zB[i]); + + // Discrete Variables + // The SimTK API does not allow an exhaustive, low-level comparison + // 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. + + + // Modeling Options + // The SimTK API does not allow an exhaustive, low-level comparison + // across all subsystems. + // The comparision is done only for the modeling options registered + // in the OpenSim Component heirarchy. + // Note that on the SimTK side, modeling options are just a special + // kind of discrete variable. + + + } + + return(true); +} + //_____________________________________________________________________________ // Build the model Model* @@ -90,7 +177,7 @@ buildModel() { //_____________________________________________________________________________ // Simulate -const OpenSim::StatesTrajectory* +SimTK::Array_ simulate(Model* model) { // Add a StatesTrajectoryReporter @@ -117,8 +204,8 @@ simulate(Model* model) { manager.initialize(state); state = manager.integrate(tf); - // Return the trajectory - return &reporter->getStates(); + // Return a copy of the state trajectory + return reporter->getStates().getUnderlyingStateArray(); } } // End anonymous namespace @@ -132,22 +219,29 @@ TEST_CASE("Getting Started") } -TEST_CASE("StatesDocument Serialization and Deserialization") +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(); - const StatesTrajectory *traj = simulate(model); + Array_& trajA = simulate(model); + + // Serialize (A) int precision = 3; SimTK::String filename = "BlockOnAString.ostates"; - StatesDocument doc1 = traj->exportToStatesDocument(*model, precision); - doc1.serialize(filename); + StatesDocument docA(*model, trajA); + docA.serialize(filename); - StatesDocument doc2(filename); - SimTK::Array_ trajDe; - doc2.deserialize(*model, trajDe); + // Deserialize (B) + StatesDocument docB(filename); + Array_ trajB; + docB.deserialize(*model, trajB); - SimTK::String filename2 = "BlockOnAString02.ostates"; - StatesDocument doc3(*model, trajDe, precision); - doc3.serialize(filename2); + // Does A == B? + testEquality(*model, trajA, trajB); REQUIRE(1 == 1); From 651ffb9f49afb7daa1183c884646fa47753b4595 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 2 Jun 2024 10:24:09 -0500 Subject: [PATCH 16/56] Update testStatesDocument.cpp - Added a static method that computes maximum rounding error based on the specified precision and value that is rounded. --- .../Simulation/Test/testStatesDocument.cpp | 99 +++++++++++-------- 1 file changed, 59 insertions(+), 40 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 463073cb04..a1d1f3e939 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -30,6 +30,9 @@ using namespace SimTK; using namespace OpenSim; +using std::cout; +using std::endl; + // Internal static methods and classes. namespace @@ -43,6 +46,20 @@ customSquare(double x) return(x*x); } +//_____________________________________________________________________________ +// Compute the maximum difference that can occur due to rounding a specified +// value at a specified precision. +double +max_eps(int precision, double value) +{ + if (value == 0) return 0.0; + double leastSig = trunc(log10(fabs(value))-precision); + double eps = 0.5*pow(10.0,leastSig); + cout << "least= " << leastSig << " eps= " << eps << endl; + return eps; +} + + //_____________________________________________________________________________ // Test for exact equality of two state trajectories. // If a state variable fails an equality test, it is likely that that @@ -50,16 +67,15 @@ customSquare(double x) // has not been serialized. bool testEquality(const Model& model, - const Array_& trajA, const Array_& trajB) + const Array_& trajA, const Array_& trajB, int precision) { // Check the array size REQUIRE(trajA.size() == trajB.size()); - // Get the SimTK::System - const System& sys = model.getSystem(); - - // Iterate through the State Trajectory - std::string path; + // Continuous Variables + // Continuous variables gathered efficiently without using any + // OpenSim::Component methods by using state.getQ(), state.getU(), and + // state.getZ(). double tA, tB; const State* stateA = trajA.cbegin(); const State* stateB = trajB.cbegin(); @@ -67,65 +83,68 @@ testEquality(const Model& model, 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(); - REQUIRE(tA == tB); + CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, max_eps(precision,tA))); // Check the number of subsystesm int nsubA = stateA->getNumSubsystems(); int nsubB = stateB->getNumSubsystems(); REQUIRE(nsubA == nsubB); - // Realize to Stage::Report - // This ensures that all states have been initialized, including - // quaternion parameters, and that all constraints have been enforced. - sys.realize(*stateA, Stage::Report); - sys.realize(*stateB, Stage::Report); - - // Continuous variables. - // This is an exhaustive, low-level comparision on the SimTK side - // that is independent of the OpenSim Component heirarchy. // Q + double eps; const Vector& qA = stateA->getQ(); const Vector& qB = stateB->getQ(); + double tol = 1.0e-16; + Vector diff = (qB - qA)/tol; int nq = qA.size(); + std::cout << "diff= " << diff << std::endl; for (int i = 0; i < nq; ++i) { - if(i==6) std::cout << "q6= " << qA[i] << " " << qB[i] << std::endl; - REQUIRE(qA[i] == qB[i]); + eps = max_eps(precision, qA[i]); + double diff = qB[i] - qA[i]; + cout << "diff=" << diff << endl; + CHECK_THAT(qB[i], Catch::Matchers::WithinAbs(qA[i], eps)); } // U const Vector& uA = stateA->getU(); const Vector& uB = stateB->getU(); int nu = uA.size(); - for (int i = 0; i < nu; ++i) REQUIRE(uA[i] == uB[i]); + for (int i = 0; i < nu; ++i) { + eps = max_eps(precision, uA[i]); + CHECK_THAT(uB[i], Catch::Matchers::WithinAbs(uA[i], eps)); + } // Z const Vector& zA = stateA->getZ(); const Vector& zB = stateB->getZ(); int nz = zA.size(); - for (int i = 0; i < nz; ++i) REQUIRE(zA[i] == zB[i]); - - // Discrete Variables - // The SimTK API does not allow an exhaustive, low-level comparison - // 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. - + for (int i = 0; i < nz; ++i) { + eps = max_eps(precision, zA[i]); + CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], eps)); + } + } - // Modeling Options - // The SimTK API does not allow an exhaustive, low-level comparison - // across all subsystems. - // The comparision is done only for the modeling options registered - // in the OpenSim Component heirarchy. - // Note that on the SimTK side, modeling options are just a special - // kind of discrete variable. + // Discrete Variables + // The SimTK API does not allow an exhaustive, low-level comparison + // 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. - } + // Modeling Options + // The SimTK API does not allow an exhaustive, low-level comparison + // across all subsystems. + // The comparision is done only for the modeling options registered + // in the OpenSim Component heirarchy. + // Note that on the SimTK side, modeling options are just a special + // kind of discrete variable. return(true); } @@ -167,7 +186,7 @@ buildModel() { double kv = 100.0; // Viscosity (under-damped) double restlength = 0.0; Vec3 origin(0.0); - Vec3 insertion(0.1, 0.1, 0.1); + Vec3 insertion(0.1, 0.1, 0.025); PointToPointSpring* spring = new PointToPointSpring(ground, origin, *block, insertion, kp, restlength); model->addForce(spring); @@ -232,7 +251,7 @@ TEST_CASE("Serialization and Deserialization") // Serialize (A) int precision = 3; SimTK::String filename = "BlockOnAString.ostates"; - StatesDocument docA(*model, trajA); + StatesDocument docA(*model, trajA, precision); docA.serialize(filename); // Deserialize (B) @@ -241,7 +260,7 @@ TEST_CASE("Serialization and Deserialization") docB.deserialize(*model, trajB); // Does A == B? - testEquality(*model, trajA, trajB); + testEquality(*model, trajA, trajB, precision); REQUIRE(1 == 1); From 82a8b216a2f007c444010aff1435639e70aa5a7d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 3 Jun 2024 16:36:23 -0500 Subject: [PATCH 17/56] Update testStatesDocument.cpp - Cleaned up the computation of max_eps. --- .../Simulation/Test/testStatesDocument.cpp | 45 ++++++++++--------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index a1d1f3e939..fa8318971b 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -47,19 +47,23 @@ customSquare(double x) } //_____________________________________________________________________________ -// Compute the maximum difference that can occur due to rounding a specified -// value at a specified precision. +/** +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. +*/ double -max_eps(int precision, double value) -{ +computeMaxRoundingError(double value, int precision) { if (value == 0) return 0.0; - double leastSig = trunc(log10(fabs(value))-precision); - double eps = 0.5*pow(10.0,leastSig); - cout << "least= " << leastSig << " eps= " << eps << endl; - return eps; + 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; } - //_____________________________________________________________________________ // Test for exact equality of two state trajectories. // If a state variable fails an equality test, it is likely that that @@ -90,7 +94,8 @@ testEquality(const Model& model, // Get time tA = stateA->getTime(); tB = stateB->getTime(); - CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, max_eps(precision,tA))); + double max_eps = computeMaxRoundingError(tA, precision); + CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, max_eps)); // Check the number of subsystesm int nsubA = stateA->getNumSubsystems(); @@ -98,34 +103,30 @@ testEquality(const Model& model, REQUIRE(nsubA == nsubB); // Q - double eps; const Vector& qA = stateA->getQ(); const Vector& qB = stateB->getQ(); - double tol = 1.0e-16; - Vector diff = (qB - qA)/tol; + Vector diff = qB - qA; int nq = qA.size(); std::cout << "diff= " << diff << std::endl; for (int i = 0; i < nq; ++i) { - eps = max_eps(precision, qA[i]); - double diff = qB[i] - qA[i]; - cout << "diff=" << diff << endl; - CHECK_THAT(qB[i], Catch::Matchers::WithinAbs(qA[i], eps)); + max_eps = computeMaxRoundingError(qA[i], precision); + CHECK_THAT(qB[i], Catch::Matchers::WithinAbs(qA[i], max_eps)); } // U const Vector& uA = stateA->getU(); const Vector& uB = stateB->getU(); int nu = uA.size(); for (int i = 0; i < nu; ++i) { - eps = max_eps(precision, uA[i]); - CHECK_THAT(uB[i], Catch::Matchers::WithinAbs(uA[i], eps)); + max_eps = computeMaxRoundingError(uA[i], precision); + CHECK_THAT(uB[i], Catch::Matchers::WithinAbs(uA[i], max_eps)); } // Z const Vector& zA = stateA->getZ(); const Vector& zB = stateB->getZ(); int nz = zA.size(); for (int i = 0; i < nz; ++i) { - eps = max_eps(precision, zA[i]); - CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], eps)); + max_eps = computeMaxRoundingError(zA[i], precision); + CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], max_eps)); } } @@ -249,7 +250,7 @@ TEST_CASE("Serialization and Deserialization") Array_& trajA = simulate(model); // Serialize (A) - int precision = 3; + int precision = 6; SimTK::String filename = "BlockOnAString.ostates"; StatesDocument docA(*model, trajA, precision); docA.serialize(filename); From b71cfa586bc101ad01317c93b7b9f02459db0fb6 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 5 Jun 2024 19:02:16 -0500 Subject: [PATCH 18/56] Update testStatesDocument.cpp - Added equality comparisons for discrete variables and modeling options. --- .../Simulation/Test/testStatesDocument.cpp | 230 +++++++++++++++--- 1 file changed, 193 insertions(+), 37 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index fa8318971b..c763fad91c 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -38,6 +38,9 @@ using std::endl; namespace { +// Constant used to determine equality tolerances +const double padFactor = 1.0 + SimTK::SignificantReal; + //_____________________________________________________________________________ // Sample internal method double @@ -53,6 +56,7 @@ 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) { @@ -65,25 +69,39 @@ computeMaxRoundingError(double value, int precision) { } //_____________________________________________________________________________ -// Test for exact 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. -bool -testEquality(const Model& model, +/** +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) { - // Check the array size - REQUIRE(trajA.size() == trajB.size()); - - // Continuous Variables - // Continuous variables gathered efficiently without using any + // 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(); - // Time + + // Loop over time for(int iTime=0; stateA!=trajA.cend(); ++iTime, ++stateA, ++stateB) { // Check subsystem consistency @@ -94,8 +112,8 @@ testEquality(const Model& model, // Get time tA = stateA->getTime(); tB = stateB->getTime(); - double max_eps = computeMaxRoundingError(tA, precision); - CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, max_eps)); + tol = padFactor * computeRoundingError(tA, precision); + CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, tol)); // Check the number of subsystesm int nsubA = stateA->getNumSubsystems(); @@ -103,51 +121,189 @@ testEquality(const Model& model, REQUIRE(nsubA == nsubB); // Q + double diff; const Vector& qA = stateA->getQ(); const Vector& qB = stateB->getQ(); - Vector diff = qB - qA; int nq = qA.size(); - std::cout << "diff= " << diff << std::endl; for (int i = 0; i < nq; ++i) { - max_eps = computeMaxRoundingError(qA[i], precision); - CHECK_THAT(qB[i], Catch::Matchers::WithinAbs(qA[i], max_eps)); + 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) { - max_eps = computeMaxRoundingError(uA[i], precision); - CHECK_THAT(uB[i], Catch::Matchers::WithinAbs(uA[i], max_eps)); + 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) { - max_eps = computeMaxRoundingError(zA[i], precision); - CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], max_eps)); + tol = padFactor * computeRoundingError(zA[i], precision); + CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], tol)); } } +} - // Discrete Variables - // The SimTK API does not allow an exhaustive, low-level comparison - // 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. +//_____________________________________________________________________________ +// 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 (int i = 0; i < 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 (int i = 0; i < a.size(); ++i) { + for (int j = 0; j < a[i].size(); ++j) { + tol = padFactor*computeRoundingError(a[i][j], precision); + CHECK_THAT(b[i][j], Catch::Matchers::WithinAbs(a[i][j], tol)); + } + } +} - // Modeling Options - // The SimTK API does not allow an exhaustive, low-level comparison - // across all subsystems. - // The comparision is done only for the modeling options registered - // in the OpenSim Component heirarchy. - // Note that on the SimTK side, modeling options are just a special - // kind of discrete variable. +//_____________________________________________________________________________ +// Test for equality of the discrete variables in two state trajectories. +// +// 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) +{ + double tol; + + // 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 (int j = 0; j < 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 (int j = 0; j < 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()); + } + } +} - return(true); +//_____________________________________________________________________________ +// Test for equality of the modeling options in two state trajectories. +// +// 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 +testEqualityForModelingOptions(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + double tol; + + // 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 (int j = 0; j < 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 +testEquality(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + REQUIRE(trajA.size() == trajB.size()); + testEqualityForContinuousVariables(model, trajA, trajB, precision); + testEqualityForDiscreteVariables(model, trajA, trajB, precision); + testEqualityForModelingOptions(model, trajA, trajB, precision); } //_____________________________________________________________________________ From 902d8f293702e275bede5a00601cba76e0362bcf Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 11 Jun 2024 15:09:03 -0500 Subject: [PATCH 19/56] Update testStatesDocument.cpp - Added ExtendedPointToPointSpring. --- .../Simulation/Test/testStatesDocument.cpp | 125 ++++++++++++++++-- 1 file changed, 112 insertions(+), 13 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index c763fad91c..80c257fb81 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -41,6 +41,99 @@ namespace // Constant used to determine equality tolerances const double padFactor = 1.0 + SimTK::SignificantReal; + +//----------------------------------------------------------------------------- +// Create a Component class for the purpose of adding discrete variables of +// all supported types (bool, int, double, Vec2, Vec3, Vec4, Vec5, Vec6. +class ExtendedPointToPointSpring : public PointToPointSpring { + OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, + PointToPointSpring); + +public: + // No new properties + + // Constructor + ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, + const PhysicalFrame& body2, SimTK::Vec3 point2, + double stiffness, double restlength) {} + + void + extendConnectToModel(OpenSim::Model& model) { + Super::extendConnectToModel(model); + + /* + // Find the OpenSim::Body + const string& bodyName = getBodyName(); + if (getModel().hasComponent(bodyName)) + _body = &(getModel().getComponent(bodyName)); + else + _body = &(getModel().getComponent( + "./bodyset/" + bodyName)); + */ + } + + void + extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + + /* + // Construct the SimTK::ExponentialContact object + SimTK::GeneralForceSubsystem& forces = _model->updForceSubsystem(); + const SimTK::Transform& XContactPlane = get_contact_plane_transform(); + const SimTK::Vec3& station = get_body_station(); + SimTK::ExponentialSpringForce* spr = + new SimTK::ExponentialSpringForce(forces, XContactPlane, + _body->getMobilizedBody(), station, getParameters()); + + // Get the subsystem index so we can access the SimTK::Force later. + ExponentialContact* mutableThis = + const_cast(this); + mutableThis->_spr = spr; + mutableThis->_index = spr->getForceIndex(); + + // Expose the discrete states of ExponentialSpringForce in OpenSim + bool allocate = false; + std::string name = getMuStaticDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getMuKineticDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getSlidingDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getAnchorPointDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + */ + } + + void + extendRealizeTopology(SimTK::State& state) const { + Super::extendRealizeTopology(state); + + /* + const SimTK::Subsystem* subsys = getSubsystem(); + SimTK::DiscreteVariableIndex index; + std::string name; + + name = getMuStaticDiscreteStateName(); + index = _spr->getMuStaticStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getMuKineticDiscreteStateName(); + index = _spr->getMuKineticStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getSlidingDiscreteStateName(); + index = _spr->getSlidingStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getAnchorPointDiscreteStateName(); + index = _spr->getAnchorPointStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + */ + } + +}; // End of class ExtendedPointToPointSpring + + //_____________________________________________________________________________ // Sample internal method double @@ -178,7 +271,7 @@ checkVector(const Array_& a, const Array_& b, int precision) } //_____________________________________________________________________________ -// Test for equality of the discrete variables in two state trajectories. +// 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. @@ -191,8 +284,6 @@ void testEqualityForDiscreteVariables(const Model& model, const Array_& trajA, const Array_& trajB, int precision) { - double tol; - // Loop over the named variables OpenSim::Array paths = model.getDiscreteVariableNames(); int nPaths = paths.getSize(); @@ -265,15 +356,15 @@ testEqualityForDiscreteVariables(const Model& model, } //_____________________________________________________________________________ -// Test for equality of the modeling options in two state trajectories. +// Test the equality of the modeling options. // // The SimTK API does not allow an exhaustive, low-level comparison of -// discrete variables on the SimTK side. +// modeling options 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. +// 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) @@ -300,7 +391,6 @@ void testEquality(const Model& model, const Array_& trajA, const Array_& trajB, int precision) { - REQUIRE(trajA.size() == trajB.size()); testEqualityForContinuousVariables(model, trajA, trajB, precision); testEqualityForDiscreteVariables(model, trajA, trajB, precision); testEqualityForModelingOptions(model, trajA, trajB, precision); @@ -344,8 +434,8 @@ buildModel() { double restlength = 0.0; Vec3 origin(0.0); Vec3 insertion(0.1, 0.1, 0.025); - PointToPointSpring* spring = new PointToPointSpring(ground, origin, - *block, insertion, kp, restlength); + ExtendedPointToPointSpring* spring = new ExtendedPointToPointSpring( + ground, origin, *block, insertion, kp, restlength); model->addForce(spring); return model; @@ -403,7 +493,7 @@ TEST_CASE("Serialization and Deserialization") // to worry about the reporter (or any other object) going out of scope // or being deleted. Model *model = buildModel(); - Array_& trajA = simulate(model); + Array_ trajA = simulate(model); // Serialize (A) int precision = 6; @@ -416,6 +506,15 @@ TEST_CASE("Serialization and Deserialization") Array_ trajB; docB.deserialize(*model, trajB); + // Check size + REQUIRE(trajA.size() == trajB.size()); + + // Realize both state trajectories to Stage::Report + for (int i = 0; i < trajA.size(); ++i) { + model->getSystem().realize(trajA[i], Stage::Report); + model->getSystem().realize(trajB[i], Stage::Report); + } + // Does A == B? testEquality(*model, trajA, trajB, precision); From a5f7cdc3e8819536b96790f1255a5e0e7b3a351e Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 26 Jun 2024 09:36:36 -0500 Subject: [PATCH 20/56] Removed trailing whitespace from testStatesTrajectory.cpp --- OpenSim/Simulation/Test/testStatesTrajectory.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index 649eb310bf..8a97699581 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,7 +570,7 @@ 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]; @@ -676,7 +676,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, From 15bc707d4137a814b9152f61e09c03c43ed9adc0 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 26 Jun 2024 09:48:43 -0500 Subject: [PATCH 21/56] Altered testBoundsCheck() Since the trajectory was changed from std::vector to SimTK::Array_, bounds checking behavior has changed. SimTK::Array_ only checks bounds in DEBUG. In other words, an IndexOutOfRange exception will not be thrown in a release build. To account for this, I inserted an #ifdef to skip bounds checking unless in DEBUG. --- .../Simulation/Test/testStatesTrajectory.cpp | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index 8a97699581..45e459879c 100644 --- a/OpenSim/Simulation/Test/testStatesTrajectory.cpp +++ b/OpenSim/Simulation/Test/testStatesTrajectory.cpp @@ -571,15 +571,19 @@ void testBoundsCheck() { 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() { From 2233470a798b9fd5613cd6ab2bc6e356af067be6 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 27 Jun 2024 14:55:50 -0500 Subject: [PATCH 22/56] Minor: Added a missing space. --- OpenSim/Simulation/StatesDocument.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 6be7bae6d1..ca74adf9f0 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -245,7 +245,7 @@ formDiscreteElement(const Model& model, const Array_& traj) { 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]); + model.getDiscreteVariableAbstractValue(traj[0], paths[i]); // Append the vector according to type if (SimTK::Value::isA(v)) { From aa45b9d21c0f611d1f136c96642e040e1a7f19e5 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 27 Jun 2024 14:57:26 -0500 Subject: [PATCH 23/56] Added discrete variables to the test model. Added the following types: - bool - int - double - Vec2 - Vec3 - Vec4 - Vec5 - Vec6 All tests are passing, and the .ostates file looks right. --- .../Simulation/Test/testStatesDocument.cpp | 143 ++++++++++-------- 1 file changed, 79 insertions(+), 64 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 80c257fb81..c9104efd19 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -32,6 +32,7 @@ using namespace SimTK; using namespace OpenSim; using std::cout; using std::endl; +using std::string; // Internal static methods and classes. @@ -49,91 +50,105 @@ class ExtendedPointToPointSpring : public PointToPointSpring { OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, PointToPointSpring); +private: + // Names of discrete variables + string dvNameBool{"dvBool"}; + string dvNameInt{"dvInt"}; + string dvNameDbl{"dvDbl"}; + string dvNameVec2{"dvVec2"}; + string dvNameVec3{"dvVec3"}; + string dvNameVec4{"dvVec4"}; + string dvNameVec5{"dvVec5"}; + string dvNameVec6{"dvVec6"}; + public: // No new properties // Constructor ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, const PhysicalFrame& body2, SimTK::Vec3 point2, - double stiffness, double restlength) {} - - void - extendConnectToModel(OpenSim::Model& model) { - Super::extendConnectToModel(model); - - /* - // Find the OpenSim::Body - const string& bodyName = getBodyName(); - if (getModel().hasComponent(bodyName)) - _body = &(getModel().getComponent(bodyName)); - else - _body = &(getModel().getComponent( - "./bodyset/" + bodyName)); - */ - } + double stiffness, double restlength) : + PointToPointSpring (body1, point1, body2, point2, + stiffness, restlength) {} void extendAddToSystem(SimTK::MultibodySystem& system) const { Super::extendAddToSystem(system); - /* - // Construct the SimTK::ExponentialContact object - SimTK::GeneralForceSubsystem& forces = _model->updForceSubsystem(); - const SimTK::Transform& XContactPlane = get_contact_plane_transform(); - const SimTK::Vec3& station = get_body_station(); - SimTK::ExponentialSpringForce* spr = - new SimTK::ExponentialSpringForce(forces, XContactPlane, - _body->getMobilizedBody(), station, getParameters()); - - // Get the subsystem index so we can access the SimTK::Force later. - ExponentialContact* mutableThis = - const_cast(this); - mutableThis->_spr = spr; - mutableThis->_index = spr->getForceIndex(); - - // Expose the discrete states of ExponentialSpringForce in OpenSim bool allocate = false; - std::string name = getMuStaticDiscreteStateName(); - addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); - name = getMuKineticDiscreteStateName(); - addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); - name = getSlidingDiscreteStateName(); - addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); - name = getAnchorPointDiscreteStateName(); - addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); - */ + addDiscreteVariable(dvNameBool, Stage::Position, allocate); + addDiscreteVariable(dvNameInt, Stage::Position, allocate); + addDiscreteVariable(dvNameDbl, Stage::Position, allocate); + addDiscreteVariable(dvNameVec2, Stage::Position, allocate); + addDiscreteVariable(dvNameVec3, Stage::Position, allocate); + addDiscreteVariable(dvNameVec4, Stage::Position, allocate); + addDiscreteVariable(dvNameVec5, Stage::Position, allocate); + addDiscreteVariable(dvNameVec6, Stage::Position, allocate); } void - extendRealizeTopology(SimTK::State& state) const { + extendRealizeTopology(SimTK::State& state) const override { Super::extendRealizeTopology(state); - /* - const SimTK::Subsystem* subsys = getSubsystem(); - SimTK::DiscreteVariableIndex index; - std::string name; - - name = getMuStaticDiscreteStateName(); - index = _spr->getMuStaticStateIndex(); - updDiscreteVariableIndex(name, index, subsys); - - name = getMuKineticDiscreteStateName(); - index = _spr->getMuKineticStateIndex(); - updDiscreteVariableIndex(name, index, subsys); - - name = getSlidingDiscreteStateName(); - index = _spr->getSlidingStateIndex(); - updDiscreteVariableIndex(name, index, subsys); - - name = getAnchorPointDiscreteStateName(); - index = _spr->getAnchorPointStateIndex(); - updDiscreteVariableIndex(name, index, subsys); - */ + const DefaultSystemSubsystem& sub = getModel().getDefaultSubsystem(); + SimTK::SubsystemIndex ssIndex = sub.getMySubsystemIndex(); + SimTK::DiscreteVariableIndex dvIndex; + + // Bool + bool dvBool{false}; + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvBool)); + initializeDiscreteVariableIndexes(dvNameBool, ssIndex, dvIndex); + + // Int + int dvInt{0}; + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvInt)); + initializeDiscreteVariableIndexes(dvNameInt, ssIndex, dvIndex); + + // Dbl + double dvDbl{0.0}; + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvDbl)); + initializeDiscreteVariableIndexes(dvNameDbl, ssIndex, dvIndex); + + // Vec2 + Vec2 dvVec2(0.1, 0.2); + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvVec2)); + initializeDiscreteVariableIndexes(dvNameVec2, ssIndex, dvIndex); + + // Vec3 + Vec3 dvVec3(0.1, 0.2, 0.3); + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvVec3)); + initializeDiscreteVariableIndexes(dvNameVec3, ssIndex, dvIndex); + + // Vec4 + Vec4 dvVec4(0.1, 0.2, 0.3, 0.4); + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvVec4)); + initializeDiscreteVariableIndexes(dvNameVec4, ssIndex, dvIndex); + + // Vec5 + Vec5 dvVec5(0.1, 0.2, 0.3, 0.4, 0.5); + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvVec5)); + initializeDiscreteVariableIndexes(dvNameVec5, ssIndex, dvIndex); + + // Vec6 + Vec6 dvVec6(0.1, 0.2, 0.3, 0.4, 0.5, 0.6); + dvIndex = sub.allocateDiscreteVariable(state, + Stage::Position, new Value(dvVec6)); + initializeDiscreteVariableIndexes(dvNameVec6, ssIndex, dvIndex); } }; // End of class ExtendedPointToPointSpring +//----------------------------------------------------------------------------- +// Other Local Static Methods +//----------------------------------------------------------------------------- //_____________________________________________________________________________ // Sample internal method double @@ -497,7 +512,7 @@ TEST_CASE("Serialization and Deserialization") // Serialize (A) int precision = 6; - SimTK::String filename = "BlockOnAString.ostates"; + SimTK::String filename = "BlockOnASpring.ostates"; StatesDocument docA(*model, trajA, precision); docA.serialize(filename); From 850e1c53b94e14788ce72346e2d230fee7629fd8 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 1 Jul 2024 04:32:05 -0500 Subject: [PATCH 24/56] Revamped ExtendedPointToPointSpring So that discrete variables can be updated during a simulation, class ExtendedPointToPointSpring is now built on top of class ExtendedTwoPointLinearSpring. ExtendedTwoPointLinearSpring allocates auto-update discrete variables in its own `realizeTopology()` and those discrete variables are updated in its own `realizePosition()`. --- .../Simulation/Test/testStatesDocument.cpp | 355 ++++++++++++++---- 1 file changed, 287 insertions(+), 68 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index c9104efd19..d7a658543c 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -20,6 +20,8 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ +#include +#include #include #include #include @@ -42,107 +44,324 @@ namespace // Constant used to determine equality tolerances const double padFactor = 1.0 + SimTK::SignificantReal; - -//----------------------------------------------------------------------------- -// Create a Component class for the purpose of adding discrete variables of -// all supported types (bool, int, double, Vec2, Vec3, Vec4, Vec5, Vec6. -class ExtendedPointToPointSpring : public PointToPointSpring { - OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, - PointToPointSpring); - -private: - // Names of discrete variables - string dvNameBool{"dvBool"}; - string dvNameInt{"dvInt"}; - string dvNameDbl{"dvDbl"}; - string dvNameVec2{"dvVec2"}; - string dvNameVec3{"dvVec3"}; - string dvNameVec4{"dvVec4"}; - string dvNameVec5{"dvVec5"}; - string dvNameVec6{"dvVec6"}; +//============================================================================= +// Extension of the underlying Simbody TwoPointLinearSpring force class. +// Discrete variables of all types supported by OpenSim are added to the +// SimTK::State and given values in an overriding SimTK::relalizePosition() +// implementation. +class ExtendedTwoPointLinearSpring : public SimTK::Force::TwoPointLinearSpring { public: - // No new properties + // 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"}; // Constructor - ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, - const PhysicalFrame& body2, SimTK::Vec3 point2, + ExtendedTwoPointLinearSpring(SimTK::GeneralForceSubsystem& subsystem, + const MobilizedBody& body1, const Vec3 point1, + const MobilizedBody& body2, const Vec3 point2, double stiffness, double restlength) : - PointToPointSpring (body1, point1, body2, point2, - stiffness, restlength) {} - - void - extendAddToSystem(SimTK::MultibodySystem& system) const { - Super::extendAddToSystem(system); + TwoPointLinearSpring(subsystem, body1, point1, body2, point2, + stiffness, restlength) {} - bool allocate = false; - addDiscreteVariable(dvNameBool, Stage::Position, allocate); - addDiscreteVariable(dvNameInt, Stage::Position, allocate); - addDiscreteVariable(dvNameDbl, Stage::Position, allocate); - addDiscreteVariable(dvNameVec2, Stage::Position, allocate); - addDiscreteVariable(dvNameVec3, Stage::Position, allocate); - addDiscreteVariable(dvNameVec4, Stage::Position, allocate); - addDiscreteVariable(dvNameVec5, Stage::Position, allocate); - addDiscreteVariable(dvNameVec6, Stage::Position, allocate); - } + // Allocate the discrete variables + void realizeTopology(SimTK::State& state) const { + // Create a mutableThis + ExtendedTwoPointLinearSpring* mutableThis = + const_cast(this); - void - extendRealizeTopology(SimTK::State& state) const override { - Super::extendRealizeTopology(state); - - const DefaultSystemSubsystem& sub = getModel().getDefaultSubsystem(); - SimTK::SubsystemIndex ssIndex = sub.getMySubsystemIndex(); - SimTK::DiscreteVariableIndex dvIndex; + // Get the GeneralForceSubsystem + const GeneralForceSubsystem& fsub = getForceSubsystem(); + mutableThis->indexSS = fsub.getMySubsystemIndex(); // Bool bool dvBool{false}; - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvBool)); - initializeDiscreteVariableIndexes(dvNameBool, ssIndex, dvIndex); + mutableThis->indexBool = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvBool), Stage::Position); // Int int dvInt{0}; - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvInt)); - initializeDiscreteVariableIndexes(dvNameInt, ssIndex, dvIndex); + mutableThis->indexInt = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvInt), Stage::Position); // Dbl double dvDbl{0.0}; - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvDbl)); - initializeDiscreteVariableIndexes(dvNameDbl, ssIndex, dvIndex); + mutableThis->indexDbl = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvDbl), Stage::Position); // Vec2 Vec2 dvVec2(0.1, 0.2); - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvVec2)); - initializeDiscreteVariableIndexes(dvNameVec2, ssIndex, dvIndex); + mutableThis->indexVec2 = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvVec2), Stage::Position); // Vec3 Vec3 dvVec3(0.1, 0.2, 0.3); - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvVec3)); - initializeDiscreteVariableIndexes(dvNameVec3, ssIndex, dvIndex); + mutableThis->indexVec3 = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvVec3), Stage::Position); // Vec4 Vec4 dvVec4(0.1, 0.2, 0.3, 0.4); - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvVec4)); - initializeDiscreteVariableIndexes(dvNameVec4, ssIndex, dvIndex); + mutableThis->indexVec4 = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvVec4), Stage::Position); // Vec5 Vec5 dvVec5(0.1, 0.2, 0.3, 0.4, 0.5); - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvVec5)); - initializeDiscreteVariableIndexes(dvNameVec5, ssIndex, dvIndex); + mutableThis->indexVec5 = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvVec5), Stage::Position); // Vec6 Vec6 dvVec6(0.1, 0.2, 0.3, 0.4, 0.5, 0.6); - dvIndex = sub.allocateDiscreteVariable(state, - Stage::Position, new Value(dvVec6)); - initializeDiscreteVariableIndexes(dvNameVec6, ssIndex, dvIndex); + mutableThis->indexVec6 = fsub.allocateAutoUpdateDiscreteVariable(state, + Stage::Position, new Value(dvVec6), Stage::Position); + } + + // Set values of the discrete variables + // These values don't mean anything. They just provided a means of + // validating that the discrete variables are changing during a simulation + // and are being de/serialized correctly. + void realizePosition(const State& state) const { + + SimTK::GeneralForceSubsystem& fsub = SimTK::GeneralForceSubsystem(); + const SimTK::Vector& u = state.getU(); + + // Bool + SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexBool)) = u[0]; + fsub.markDiscreteVarUpdateValueRealized(state, indexBool); + + // Int + SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexInt)) = u[0]; + fsub.markDiscreteVarUpdateValueRealized(state, indexInt); + + // Dbl + SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexDbl)) = u[0]; + fsub.markDiscreteVarUpdateValueRealized(state, indexDbl); + + // Vec2 + Vec2& v2 = SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexVec2)); + v2[0] = u[0]; + v2[1] = u[1]; + fsub.markDiscreteVarUpdateValueRealized(state, indexVec2); + + // Vec3 + Vec3& v3 = SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexVec3)); + v3[0] = u[0]; + v3[1] = u[1]; + v3[2] = u[2]; + fsub.markDiscreteVarUpdateValueRealized(state, indexVec3); + + // Vec4 + Vec4& v4 = SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexVec4)); + v4[0] = u[0]; + v4[1] = u[1]; + v4[2] = u[2]; + v4[3] = u[3]; + fsub.markDiscreteVarUpdateValueRealized(state, indexVec4); + + // Vec5 + Vec5& v5 = SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexVec5)); + v5[0] = u[0]; + v5[1] = u[1]; + v5[2] = u[2]; + v5[3] = u[3]; + v5[4] = u[4]; + fsub.markDiscreteVarUpdateValueRealized(state, indexVec5); + + // Vec6 + Vec6& v6 = SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, 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]; + fsub.markDiscreteVarUpdateValueRealized(state, indexVec6); + } + +}; + + +//----------------------------------------------------------------------------- +// Create a Component class for the purpose of adding discrete variables of +// all supported types (bool, int, double, Vec2, Vec3, Vec4, Vec5, Vec6. +class ExtendedPointToPointSpring : public OpenSim::Force { + OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, OpenSim::Force); + +public: + + ExtendedTwoPointLinearSpring* sbSpring{nullptr}; + + // Properties + OpenSim_DECLARE_PROPERTY(point1, SimTK::Vec3, + "Spring attachment point on body1."); + OpenSim_DECLARE_PROPERTY(point2, SimTK::Vec3, + "Spring attachment point on body2."); + OpenSim_DECLARE_PROPERTY(stiffness, double, + "Spring stiffness (N/m)."); + OpenSim_DECLARE_PROPERTY(rest_length, double, + "Spring resting length (m)."); + + // Sockets + OpenSim_DECLARE_SOCKET(body1, PhysicalFrame, + "A frame on the first body that this spring connects to."); + OpenSim_DECLARE_SOCKET(body2, PhysicalFrame, + "A frame on the second body that this spring connects to."); + + // Constructor + ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, + const PhysicalFrame& body2, SimTK::Vec3 point2, + double stiffness, double restlength) + { + setNull(); + constructProperties(); + + // Set properties to the passed-in values. + setBody1(body1); + setBody2(body2); + + setPoint1(point1); + setPoint2(point2); + + setStiffness(stiffness); + setRestlength(restlength); + + } + + // Body accessors + void setBody1(const PhysicalFrame& body) { + connectSocket_body1(body); } + void setBody2(const PhysicalFrame& body) { + connectSocket_body2(body); } + const PhysicalFrame& getBody1() const { + return getConnectee("body1"); } + const PhysicalFrame& getBody2() const { + return getConnectee("body2"); } + // Point accessors + void setPoint1(SimTK::Vec3 aPosition) { set_point1(aPosition); } + const SimTK::Vec3& getPoint1() const { return get_point1(); } + void setPoint2(SimTK::Vec3 aPosition) { set_point2(aPosition); } + const SimTK::Vec3& getPoint2() const { return get_point2(); } + // Consitutive accessors + void setStiffness(double stiffness) {set_stiffness(stiffness);} + double getStiffness() const {return get_stiffness();} + void setRestlength(double restLength) {set_rest_length(restLength);} + double getRestlength() const {return get_rest_length();} + + + void + extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + + + } + + + void + extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const { + Super::extendAddToSystemAfterSubcomponents(system); + + const PhysicalFrame& body1 = getBody1(); + const PhysicalFrame& body2 = getBody2(); + + // Get underlying mobilized bodies + const SimTK::MobilizedBody& b1 = body1.getMobilizedBody(); + const SimTK::MobilizedBody& b2 = body2.getMobilizedBody(); + + // Now create a Simbody Force::ExtendedTwoPointLinearSpring + ExtendedPointToPointSpring* mutableThis = + const_cast(this); + mutableThis->sbSpring = new ExtendedTwoPointLinearSpring( + _model->updForceSubsystem(), + b1, getPoint1(), b2, getPoint2(), + getStiffness(), getRestlength()); + + // Beyond the const Component get the index so we can access the + // SimTK::Force later. + mutableThis->_index = mutableThis->sbSpring->getForceIndex(); + + // Initialize the indexes + bool allocate = false; + addDiscreteVariable(sbSpring->nameBool, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameInt, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameDbl, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameVec2, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameVec3, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameVec4, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameVec5, Stage::Position, allocate); + addDiscreteVariable(sbSpring->nameVec6, Stage::Position, allocate); + } + + void extendConnectToModel(Model& model) override { + // validate that the spring is attached to two different base + // frames; otherwise, unusual simulation behavior may occur + // (#3485) + auto const& pf1 = getConnectee("body1"); + auto const& pf2 = getConnectee("body2"); + OpenSim::Frame const& pf1Base = pf1.findBaseFrame(); + + if (&pf1Base == &pf2.findBaseFrame()) { + std::stringstream ss; + ss << " body1 (" << pf1.getAbsolutePathString() << ") and body2 (" << pf2.getAbsolutePathString() << ") have the same base frame (" << pf1Base.getAbsolutePathString() << "), this is not permitted."; + OPENSIM_THROW_FRMOBJ(OpenSim::Exception, std::move(ss).str()); + } } + void + extendRealizeTopology(SimTK::State& state) const override { + Super::extendRealizeTopology(state); + initializeDiscreteVariableIndexes(sbSpring->nameBool, + sbSpring->indexSS, sbSpring->indexBool); + initializeDiscreteVariableIndexes(sbSpring->nameInt, + sbSpring->indexSS, sbSpring->indexInt); + initializeDiscreteVariableIndexes(sbSpring->nameDbl, + sbSpring->indexSS, sbSpring->indexDbl); + initializeDiscreteVariableIndexes(sbSpring->nameVec2, + sbSpring->indexSS, sbSpring->indexVec2); + initializeDiscreteVariableIndexes(sbSpring->nameVec3, + sbSpring->indexSS, sbSpring->indexVec3); + initializeDiscreteVariableIndexes(sbSpring->nameVec4, + sbSpring->indexSS, sbSpring->indexVec4); + initializeDiscreteVariableIndexes(sbSpring->nameVec5, + sbSpring->indexSS, sbSpring->indexVec5); + initializeDiscreteVariableIndexes(sbSpring->nameVec6, + sbSpring->indexSS, sbSpring->indexVec6); + } + +private: + void setNull() { } + void constructProperties() { + const SimTK::Vec3 bodyOrigin(0.0, 0.0, 0.0); + constructProperty_point1(bodyOrigin); + constructProperty_point2(bodyOrigin); + + constructProperty_stiffness(1.0); + constructProperty_rest_length(0.0); + }; + }; // End of class ExtendedPointToPointSpring From 820c04c00feb294ae55426842cfb6444624e3d01 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 1 Jul 2024 14:40:04 -0500 Subject: [PATCH 25/56] Reverted to a simpler ExtendedPointToPointSpring class. The discrete variables can be added in extendRealizeTopology(). And, they can be changed in computeForce(). --- .../Simulation/Test/testStatesDocument.cpp | 247 +++++------------- 1 file changed, 64 insertions(+), 183 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index d7a658543c..62fedde71c 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -22,6 +22,7 @@ * -------------------------------------------------------------------------- */ #include #include + #include #include #include @@ -44,12 +45,15 @@ namespace // Constant used to determine equality tolerances const double padFactor = 1.0 + SimTK::SignificantReal; -//============================================================================= -// Extension of the underlying Simbody TwoPointLinearSpring force class. -// Discrete variables of all types supported by OpenSim are added to the -// SimTK::State and given values in an overriding SimTK::relalizePosition() -// implementation. -class ExtendedTwoPointLinearSpring : public SimTK::Force::TwoPointLinearSpring { + +//----------------------------------------------------------------------------- +// Create a force derived from PointToPointSpring that adds on a discrete +// variables of each supported type (bool, int, double, Vec2, Vec3, Vec4, +// Vec5, Vec6. +class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring +{ + OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, + OpenSim::PointToPointSpring); public: // Subsystem index @@ -74,21 +78,40 @@ class ExtendedTwoPointLinearSpring : public SimTK::Force::TwoPointLinearSpring { string nameVec6{"dvVec6"}; // Constructor - ExtendedTwoPointLinearSpring(SimTK::GeneralForceSubsystem& subsystem, - const MobilizedBody& body1, const Vec3 point1, - const MobilizedBody& body2, const Vec3 point2, + ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, + const PhysicalFrame& body2, SimTK::Vec3 point2, double stiffness, double restlength) : - TwoPointLinearSpring(subsystem, body1, point1, body2, point2, - stiffness, restlength) {} + PointToPointSpring(body1, point1, body2, point2, stiffness, restlength) + { } + + void + extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const override + { + Super::extendAddToSystemAfterSubcomponents(system); + + // Add the discrete variables to the list of OpenSim Components + bool allocate = false; + addDiscreteVariable(nameBool, Stage::Position, allocate); + addDiscreteVariable(nameInt, Stage::Position, allocate); + addDiscreteVariable(nameDbl, Stage::Position, allocate); + addDiscreteVariable(nameVec2, Stage::Position, allocate); + addDiscreteVariable(nameVec3, Stage::Position, allocate); + addDiscreteVariable(nameVec4, Stage::Position, allocate); + addDiscreteVariable(nameVec5, Stage::Position, allocate); + addDiscreteVariable(nameVec6, Stage::Position, allocate); + } + + void + extendRealizeTopology(SimTK::State& state) const override + { + Super::extendRealizeTopology(state); - // Allocate the discrete variables - void realizeTopology(SimTK::State& state) const { // Create a mutableThis - ExtendedTwoPointLinearSpring* mutableThis = - const_cast(this); + ExtendedPointToPointSpring* mutableThis = + const_cast(this); // Get the GeneralForceSubsystem - const GeneralForceSubsystem& fsub = getForceSubsystem(); + const GeneralForceSubsystem& fsub = getModel().getForceSubsystem(); mutableThis->indexSS = fsub.getMySubsystemIndex(); // Bool @@ -130,20 +153,38 @@ class ExtendedTwoPointLinearSpring : public SimTK::Force::TwoPointLinearSpring { Vec6 dvVec6(0.1, 0.2, 0.3, 0.4, 0.5, 0.6); mutableThis->indexVec6 = fsub.allocateAutoUpdateDiscreteVariable(state, Stage::Position, new Value(dvVec6), Stage::Position); + + // Initialize discrete variable indexes + initializeDiscreteVariableIndexes(nameBool, indexSS, indexBool); + initializeDiscreteVariableIndexes(nameInt, indexSS, indexInt); + initializeDiscreteVariableIndexes(nameDbl, indexSS, indexDbl); + initializeDiscreteVariableIndexes(nameVec2, indexSS, indexVec2); + initializeDiscreteVariableIndexes(nameVec3, indexSS, indexVec3); + initializeDiscreteVariableIndexes(nameVec4, indexSS, indexVec4); + initializeDiscreteVariableIndexes(nameVec5, indexSS, indexVec5); + initializeDiscreteVariableIndexes(nameVec6, indexSS, indexVec6); } - // Set values of the discrete variables - // These values don't mean anything. They just provided a means of - // validating that the discrete variables are changing during a simulation - // and are being de/serialized correctly. - void realizePosition(const State& state) const { + // Set the values of the discrete variables. + // The force calculation is done in SimTK::TwoPointLinearSpring. + // This method just provided a means of setting the added discrete + // variables, validating that they are changing during a simulation + // and being de/serialized correctly. + virtual void computeForce(const SimTK::State& state, + SimTK::Vector_& bodyForces, + SimTK::Vector& generalizedForces) const override + { + Super::computeForce(state, bodyForces, generalizedForces); SimTK::GeneralForceSubsystem& fsub = SimTK::GeneralForceSubsystem(); const SimTK::Vector& u = state.getU(); + return; + // Bool - SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexBool)) = u[0]; + bool& vBool = SimTK::Value::downcast( + fsub.updDiscreteVarUpdateValue(state, indexBool)); + vBool = u[0]; fsub.markDiscreteVarUpdateValueRealized(state, indexBool); // Int @@ -202,166 +243,6 @@ class ExtendedTwoPointLinearSpring : public SimTK::Force::TwoPointLinearSpring { fsub.markDiscreteVarUpdateValueRealized(state, indexVec6); } -}; - - -//----------------------------------------------------------------------------- -// Create a Component class for the purpose of adding discrete variables of -// all supported types (bool, int, double, Vec2, Vec3, Vec4, Vec5, Vec6. -class ExtendedPointToPointSpring : public OpenSim::Force { - OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, OpenSim::Force); - -public: - - ExtendedTwoPointLinearSpring* sbSpring{nullptr}; - - // Properties - OpenSim_DECLARE_PROPERTY(point1, SimTK::Vec3, - "Spring attachment point on body1."); - OpenSim_DECLARE_PROPERTY(point2, SimTK::Vec3, - "Spring attachment point on body2."); - OpenSim_DECLARE_PROPERTY(stiffness, double, - "Spring stiffness (N/m)."); - OpenSim_DECLARE_PROPERTY(rest_length, double, - "Spring resting length (m)."); - - // Sockets - OpenSim_DECLARE_SOCKET(body1, PhysicalFrame, - "A frame on the first body that this spring connects to."); - OpenSim_DECLARE_SOCKET(body2, PhysicalFrame, - "A frame on the second body that this spring connects to."); - - // Constructor - ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, - const PhysicalFrame& body2, SimTK::Vec3 point2, - double stiffness, double restlength) - { - setNull(); - constructProperties(); - - // Set properties to the passed-in values. - setBody1(body1); - setBody2(body2); - - setPoint1(point1); - setPoint2(point2); - - setStiffness(stiffness); - setRestlength(restlength); - - } - - // Body accessors - void setBody1(const PhysicalFrame& body) { - connectSocket_body1(body); } - void setBody2(const PhysicalFrame& body) { - connectSocket_body2(body); } - const PhysicalFrame& getBody1() const { - return getConnectee("body1"); } - const PhysicalFrame& getBody2() const { - return getConnectee("body2"); } - // Point accessors - void setPoint1(SimTK::Vec3 aPosition) { set_point1(aPosition); } - const SimTK::Vec3& getPoint1() const { return get_point1(); } - void setPoint2(SimTK::Vec3 aPosition) { set_point2(aPosition); } - const SimTK::Vec3& getPoint2() const { return get_point2(); } - // Consitutive accessors - void setStiffness(double stiffness) {set_stiffness(stiffness);} - double getStiffness() const {return get_stiffness();} - void setRestlength(double restLength) {set_rest_length(restLength);} - double getRestlength() const {return get_rest_length();} - - - void - extendAddToSystem(SimTK::MultibodySystem& system) const { - Super::extendAddToSystem(system); - - - } - - - void - extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const { - Super::extendAddToSystemAfterSubcomponents(system); - - const PhysicalFrame& body1 = getBody1(); - const PhysicalFrame& body2 = getBody2(); - - // Get underlying mobilized bodies - const SimTK::MobilizedBody& b1 = body1.getMobilizedBody(); - const SimTK::MobilizedBody& b2 = body2.getMobilizedBody(); - - // Now create a Simbody Force::ExtendedTwoPointLinearSpring - ExtendedPointToPointSpring* mutableThis = - const_cast(this); - mutableThis->sbSpring = new ExtendedTwoPointLinearSpring( - _model->updForceSubsystem(), - b1, getPoint1(), b2, getPoint2(), - getStiffness(), getRestlength()); - - // Beyond the const Component get the index so we can access the - // SimTK::Force later. - mutableThis->_index = mutableThis->sbSpring->getForceIndex(); - - // Initialize the indexes - bool allocate = false; - addDiscreteVariable(sbSpring->nameBool, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameInt, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameDbl, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameVec2, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameVec3, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameVec4, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameVec5, Stage::Position, allocate); - addDiscreteVariable(sbSpring->nameVec6, Stage::Position, allocate); - } - - void extendConnectToModel(Model& model) override { - // validate that the spring is attached to two different base - // frames; otherwise, unusual simulation behavior may occur - // (#3485) - auto const& pf1 = getConnectee("body1"); - auto const& pf2 = getConnectee("body2"); - OpenSim::Frame const& pf1Base = pf1.findBaseFrame(); - - if (&pf1Base == &pf2.findBaseFrame()) { - std::stringstream ss; - ss << " body1 (" << pf1.getAbsolutePathString() << ") and body2 (" << pf2.getAbsolutePathString() << ") have the same base frame (" << pf1Base.getAbsolutePathString() << "), this is not permitted."; - OPENSIM_THROW_FRMOBJ(OpenSim::Exception, std::move(ss).str()); - } - } - - void - extendRealizeTopology(SimTK::State& state) const override { - Super::extendRealizeTopology(state); - initializeDiscreteVariableIndexes(sbSpring->nameBool, - sbSpring->indexSS, sbSpring->indexBool); - initializeDiscreteVariableIndexes(sbSpring->nameInt, - sbSpring->indexSS, sbSpring->indexInt); - initializeDiscreteVariableIndexes(sbSpring->nameDbl, - sbSpring->indexSS, sbSpring->indexDbl); - initializeDiscreteVariableIndexes(sbSpring->nameVec2, - sbSpring->indexSS, sbSpring->indexVec2); - initializeDiscreteVariableIndexes(sbSpring->nameVec3, - sbSpring->indexSS, sbSpring->indexVec3); - initializeDiscreteVariableIndexes(sbSpring->nameVec4, - sbSpring->indexSS, sbSpring->indexVec4); - initializeDiscreteVariableIndexes(sbSpring->nameVec5, - sbSpring->indexSS, sbSpring->indexVec5); - initializeDiscreteVariableIndexes(sbSpring->nameVec6, - sbSpring->indexSS, sbSpring->indexVec6); - } - -private: - void setNull() { } - void constructProperties() { - const SimTK::Vec3 bodyOrigin(0.0, 0.0, 0.0); - constructProperty_point1(bodyOrigin); - constructProperty_point2(bodyOrigin); - - constructProperty_stiffness(1.0); - constructProperty_rest_length(0.0); - }; - }; // End of class ExtendedPointToPointSpring From 9e0424fd07fcb42f2e672fa977c64e50875ff1ec Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 2 Jul 2024 09:00:10 -0500 Subject: [PATCH 26/56] Discrete variables are now being altered during simulation - Changed the allocation for discrete variables to the DefaultSystemSubsystem. - Primarily using the subsystem index instead of the pointer to the subsystem. - Discrete variables are being changed during simulation in ExtendedPointToPointSpring::computeForce(). All checks are now passing! --- .../Simulation/Test/testStatesDocument.cpp | 92 +++++++++---------- 1 file changed, 45 insertions(+), 47 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 62fedde71c..532c686a2e 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -47,15 +47,15 @@ const double padFactor = 1.0 + SimTK::SignificantReal; //----------------------------------------------------------------------------- -// Create a force derived from PointToPointSpring that adds on a discrete -// variables of each supported type (bool, int, double, Vec2, Vec3, Vec4, -// Vec5, Vec6. +// Create a force component derived from PointToPointSpring that adds on a +// discrete variables of each supported type (bool, int, double, Vec2, Vec3, +// Vec4, Vec5, Vec6). class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring { OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, OpenSim::PointToPointSpring); -public: +private: // Subsystem index SubsystemIndex indexSS; // Indexes of discrete variables @@ -77,6 +77,8 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring string nameVec5{"dvVec5"}; string nameVec6{"dvVec6"}; +public: + // Constructor ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, const PhysicalFrame& body2, SimTK::Vec3 point2, @@ -102,57 +104,57 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring } void - extendRealizeTopology(SimTK::State& state) const override + extendRealizeTopology(SimTK::State& s) const override { - Super::extendRealizeTopology(state); + Super::extendRealizeTopology(s); // Create a mutableThis ExtendedPointToPointSpring* mutableThis = const_cast(this); - // Get the GeneralForceSubsystem - const GeneralForceSubsystem& fsub = getModel().getForceSubsystem(); + // Get the Subsystem + const DefaultSystemSubsystem& fsub = getModel().getDefaultSubsystem(); mutableThis->indexSS = fsub.getMySubsystemIndex(); // Bool bool dvBool{false}; - mutableThis->indexBool = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvBool), Stage::Position); + mutableThis->indexBool = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvBool), Stage::Dynamics); // Int int dvInt{0}; - mutableThis->indexInt = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvInt), Stage::Position); + mutableThis->indexInt = s.allocateAutoUpdateDiscreteVariable( + indexSS, Stage::Velocity, new Value(dvInt), Stage::Dynamics); // Dbl double dvDbl{0.0}; - mutableThis->indexDbl = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvDbl), Stage::Position); + mutableThis->indexDbl = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvDbl), Stage::Dynamics); // Vec2 Vec2 dvVec2(0.1, 0.2); - mutableThis->indexVec2 = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvVec2), Stage::Position); + mutableThis->indexVec2 = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec2), Stage::Dynamics); // Vec3 Vec3 dvVec3(0.1, 0.2, 0.3); - mutableThis->indexVec3 = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvVec3), Stage::Position); + mutableThis->indexVec3 = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec3), Stage::Dynamics); // Vec4 Vec4 dvVec4(0.1, 0.2, 0.3, 0.4); - mutableThis->indexVec4 = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvVec4), Stage::Position); + mutableThis->indexVec4 = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec4), Stage::Dynamics); // Vec5 Vec5 dvVec5(0.1, 0.2, 0.3, 0.4, 0.5); - mutableThis->indexVec5 = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvVec5), Stage::Position); + mutableThis->indexVec5 = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec5), Stage::Dynamics); // Vec6 Vec6 dvVec6(0.1, 0.2, 0.3, 0.4, 0.5, 0.6); - mutableThis->indexVec6 = fsub.allocateAutoUpdateDiscreteVariable(state, - Stage::Position, new Value(dvVec6), Stage::Position); + mutableThis->indexVec6 = s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec6), Stage::Dynamics); // Initialize discrete variable indexes initializeDiscreteVariableIndexes(nameBool, indexSS, indexBool); @@ -166,81 +168,77 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring } // Set the values of the discrete variables. - // The force calculation is done in SimTK::TwoPointLinearSpring. - // This method just provided a means of setting the added discrete - // variables, validating that they are changing during a simulation - // and being de/serialized correctly. + // 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 a simulation. virtual void computeForce(const SimTK::State& state, SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const override { Super::computeForce(state, bodyForces, generalizedForces); - SimTK::GeneralForceSubsystem& fsub = SimTK::GeneralForceSubsystem(); const SimTK::Vector& u = state.getU(); - return; - // Bool bool& vBool = SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexBool)); + state.updDiscreteVarUpdateValue(indexSS, indexBool)); vBool = u[0]; - fsub.markDiscreteVarUpdateValueRealized(state, indexBool); + state.markDiscreteVarUpdateValueRealized(indexSS, indexBool); // Int SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexInt)) = u[0]; - fsub.markDiscreteVarUpdateValueRealized(state, indexInt); + state.updDiscreteVarUpdateValue(indexSS, indexInt)) = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexInt); // Dbl SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexDbl)) = u[0]; - fsub.markDiscreteVarUpdateValueRealized(state, indexDbl); + state.updDiscreteVarUpdateValue(indexSS, indexDbl)) = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexDbl); // Vec2 Vec2& v2 = SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexVec2)); + state.updDiscreteVarUpdateValue(indexSS, indexVec2)); v2[0] = u[0]; v2[1] = u[1]; - fsub.markDiscreteVarUpdateValueRealized(state, indexVec2); + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec2); // Vec3 Vec3& v3 = SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexVec3)); + state.updDiscreteVarUpdateValue(indexSS, indexVec3)); v3[0] = u[0]; v3[1] = u[1]; v3[2] = u[2]; - fsub.markDiscreteVarUpdateValueRealized(state, indexVec3); + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec3); // Vec4 Vec4& v4 = SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexVec4)); + state.updDiscreteVarUpdateValue(indexSS, indexVec4)); v4[0] = u[0]; v4[1] = u[1]; v4[2] = u[2]; v4[3] = u[3]; - fsub.markDiscreteVarUpdateValueRealized(state, indexVec4); + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec4); // Vec5 Vec5& v5 = SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexVec5)); + state.updDiscreteVarUpdateValue(indexSS, indexVec5)); v5[0] = u[0]; v5[1] = u[1]; v5[2] = u[2]; v5[3] = u[3]; v5[4] = u[4]; - fsub.markDiscreteVarUpdateValueRealized(state, indexVec5); + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec5); // Vec6 Vec6& v6 = SimTK::Value::downcast( - fsub.updDiscreteVarUpdateValue(state, indexVec6)); + 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]; - fsub.markDiscreteVarUpdateValueRealized(state, indexVec6); + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec6); } }; // End of class ExtendedPointToPointSpring From a60bf8d61ee613b1eabc69e27bf2c3dcafd950ad Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 7 Jul 2024 17:47:14 -0500 Subject: [PATCH 27/56] Added ability to set a note for a states document. The note is serialized and deserialized. All checks are passing. --- OpenSim/Simulation/StatesDocument.cpp | 62 +++++++++++++++++-- OpenSim/Simulation/StatesDocument.h | 27 ++++++-- OpenSim/Simulation/StatesTrajectory.h | 3 +- .../Simulation/Test/testStatesDocument.cpp | 11 +++- 4 files changed, 88 insertions(+), 15 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index ca74adf9f0..2e27000f12 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -136,8 +136,11 @@ struct SDocUtil { //----------------------------------------------------------------------------- //_____________________________________________________________________________ StatesDocument:: -StatesDocument(const Model& model, const Array_& trajectory, int p) { - precision = clamp(1, p, SimTK::LosslessNumDigitsReal); +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); } @@ -147,8 +150,15 @@ StatesDocument(const Model& model, const Array_& trajectory, int p) { //_____________________________________________________________________________ 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); @@ -163,7 +173,7 @@ formRootElement(const Model& model, const Array_& traj) { Element rootElt = doc.getRootElement(); // Insert a comment at the top level, just before the root node. - string info = "OpenSim States Document (Version "; + string info = "OpenSim States Document (Version: "; info += std::to_string(model.getDocumentFileVersion()); info += ")"; Xml::Comment comment(info); @@ -175,9 +185,7 @@ formRootElement(const Model& model, const Array_& traj) { const char *localeName = "C"; std::locale::global(std::locale(localeName)); char buf[64]; - strftime(buf, sizeof buf, "%a %b %e %Y %H:%M:%S", std::localtime(&now)); - //SimTK::String datetime = buf; - //cout << "ostates datetime = " << datetime << endl; + 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()); @@ -188,6 +196,15 @@ formRootElement(const Model& model, const Array_& traj) { //_____________________________________________________________________________ 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"); @@ -395,6 +412,39 @@ prepareStatesTrajectory(const Model& model, Array_& traj) { //_____________________________________________________________________________ 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 1st 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(); diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index 4a1bd6a963..ba10b891bb 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -499,10 +499,12 @@ class OSIMSIMULATION_API StatesDocument { validity of the XML file is not tested until StatesDocument::deserialize() is called. - @param filename The name of the file, which may include the file system + @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) { doc.readFromFile(filename); + initializeNote(); + initializePrecision(); } /** Construct a StatesDocument instance from a states trajectory in @@ -516,14 +518,25 @@ class OSIMSIMULATION_API StatesDocument { @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 near lossless + 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 //------------------------------------------------------------------------- @@ -532,9 +545,7 @@ class OSIMSIMULATION_API StatesDocument { @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) { - doc.writeToFile(filename); - } + void serialize(const SimTK::String& filename); //------------------------------------------------------------------------- // Deserialization @@ -563,6 +574,8 @@ class OSIMSIMULATION_API StatesDocument { 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, @@ -577,6 +590,8 @@ class OSIMSIMULATION_API StatesDocument { void checkDocConsistencyWithModel(const Model& model) const; 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); @@ -592,6 +607,8 @@ class OSIMSIMULATION_API StatesDocument { // Member Variables int precision{SimTK::LosslessNumDigitsReal}; SimTK::Xml::Document doc; + SimTK::String note{""}; + }; // END of class StatesDocument diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index fbd8099196..c0f55f2c0a 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -332,9 +332,10 @@ class OSIMSIMULATION_API StatesTrajectory { */ OpenSim::StatesDocument exportToStatesDocument(const OpenSim::Model& model, + const SimTK::String& note = "", int precision = SimTK::LosslessNumDigitsReal) const { - return OpenSim::StatesDocument(model, m_states, precision); + return OpenSim::StatesDocument(model, m_states, note, precision); } /** Get a read-only reference to the underlying state array. */ diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 532c686a2e..79f116ab67 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -170,7 +170,7 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring // 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 a simulation. + // variables so that they change during the course of a simulation. virtual void computeForce(const SimTK::State& state, SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const override @@ -611,7 +611,8 @@ TEST_CASE("Serialization and Deserialization") // Serialize (A) int precision = 6; SimTK::String filename = "BlockOnASpring.ostates"; - StatesDocument docA(*model, trajA, precision); + SimTK::String note = "Output from `testStatesDocument.cpp` to validate state serialization and deserialization."; + StatesDocument docA(*model, trajA, note, precision); docA.serialize(filename); // Deserialize (B) @@ -619,6 +620,10 @@ TEST_CASE("Serialization and Deserialization") Array_ trajB; docB.deserialize(*model, trajB); + // Check the note and the precision. + CHECK(docB.getNote() == docA.getNote()); + CHECK(docB.getPrecision() == docB.getPrecision()); + // Check size REQUIRE(trajA.size() == trajB.size()); @@ -628,7 +633,7 @@ TEST_CASE("Serialization and Deserialization") model->getSystem().realize(trajB[i], Stage::Report); } - // Does A == B? + // Are state trajectories A and B the same? testEquality(*model, trajA, trajB, precision); REQUIRE(1 == 1); From 493d73d8b205069a97dca05b7ad26b4119093360 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 30 Aug 2024 08:08:25 -0500 Subject: [PATCH 28/56] Update testStatesDocument.cpp 1. Now looping over over output precision from precision = 2 to precision = 22. 2. Added the ability to change the name of a discrete state for the purpose of testing exceptions when a discrete state is not found. --- .../Simulation/Test/testStatesDocument.cpp | 107 ++++++++++++++++-- 1 file changed, 97 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 79f116ab67..f71753662a 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -48,7 +48,7 @@ const double padFactor = 1.0 + SimTK::SignificantReal; //----------------------------------------------------------------------------- // Create a force component derived from PointToPointSpring that adds on a -// discrete variables of each supported type (bool, int, double, Vec2, Vec3, +// discrete variable of each supported type (bool, int, double, Vec2, Vec3, // Vec4, Vec5, Vec6). class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring { @@ -82,9 +82,37 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring // Constructor ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, const PhysicalFrame& body2, SimTK::Vec3 point2, - double stiffness, double restlength) : + double stiffness, double restlength, int which, const string& suffix) : PointToPointSpring(body1, point1, body2, point2, stiffness, restlength) - { } + { + 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 @@ -171,6 +199,7 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring // 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 @@ -512,7 +541,7 @@ testEquality(const Model& model, //_____________________________________________________________________________ // Build the model Model* -buildModel() { +buildModel(int whichDiscreteState, const string& discreteStateSuffix) { // Create an empty model Model* model = new Model(); @@ -548,7 +577,8 @@ buildModel() { Vec3 origin(0.0); Vec3 insertion(0.1, 0.1, 0.025); ExtendedPointToPointSpring* spring = new ExtendedPointToPointSpring( - ground, origin, *block, insertion, kp, restlength); + ground, origin, *block, insertion, kp, restlength, + whichDiscreteState, discreteStateSuffix); model->addForce(spring); return model; @@ -600,12 +630,71 @@ TEST_CASE("Getting Started") TEST_CASE("Serialization and Deserialization") { - // Build the model and run a simulation + // Specify which discrete state gets a suffix. The suffix is used + // to change the name of a discrete variable so that it will not be found. + // When whichDiscreteState == 1, the suffix is not appended to any state. + int whichDiscreteState{-1}; + string suffix{"_ShouldNotBeFound"}; + + // 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(whichDiscreteState, suffix); + Array_ trajA = simulate(model); + + // Serialize (A) + int p; + for (p = 2; p < 22; ++p) { + cout << "Testing for precision = " << p << endl; + + SimTK::String filename = "BlockOnASpring.ostates"; + SimTK::String note = "Output from `testStatesDocument.cpp` to validate state serialization and deserialization."; + StatesDocument docA(*model, trajA, note, p); + docA.serialize(filename); + + // Deserialize (B) + StatesDocument docB(filename); + Array_ trajB; + docB.deserialize(*model, trajB); + + // Check the note and the precision. + CHECK(docB.getNote() == docA.getNote()); + CHECK(docB.getPrecision() == docB.getPrecision()); + + // Check size + REQUIRE(trajA.size() == trajB.size()); + + // Realize both state trajectories to Stage::Report + for (int i = 0; i < trajA.size(); ++i) { + model->getSystem().realize(trajA[i], Stage::Report); + model->getSystem().realize(trajB[i], Stage::Report); + } + + // Are state trajectories A and B the same? + testEquality(*model, trajA, trajB, p); + } + + delete model; +} + +TEST_CASE("Exception: Discrete State Not Found") +{ + // Specify which discrete state gets a suffix. The suffix is used + // to change the name of a discrete variable so that it will not be found. + // When whichDiscreteState == 1, the suffix is not appended to any state. + // When whichDiscreteState is between 0 and 7, the suffix will be + // appended to that discrete state. + int whichDiscreteState{-1}; + string suffix{"_ShouldNotBeFound"}; + + // Build a 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(); + Model *model = buildModel(whichDiscreteState, suffix); Array_ trajA = simulate(model); // Serialize (A) @@ -636,7 +725,5 @@ TEST_CASE("Serialization and Deserialization") // Are state trajectories A and B the same? testEquality(*model, trajA, trajB, precision); - REQUIRE(1 == 1); - delete model; -} +} \ No newline at end of file From 1489842712833797214681496f624d2b33cdf22c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 31 Aug 2024 11:02:01 -0500 Subject: [PATCH 29/56] Update testStatesDocument.cpp - Cleaned up the code a little bit. - Added code to deserialize with a model that has one discrete variable with a changed name. This was done to check exception handling. --- .../Simulation/Test/testStatesDocument.cpp | 103 +++++++++--------- 1 file changed, 54 insertions(+), 49 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index f71753662a..3f616c86d5 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -530,7 +530,7 @@ testEqualityForModelingOptions(const Model& model, // variable has not been added to OpenSim's Component heirarchy and therefore // has not been serialized. void -testEquality(const Model& model, +testStateEquality(const Model& model, const Array_& trajA, const Array_& trajB, int precision) { testEqualityForContinuousVariables(model, trajA, trajB, precision); @@ -541,7 +541,8 @@ testEquality(const Model& model, //_____________________________________________________________________________ // Build the model Model* -buildModel(int whichDiscreteState, const string& discreteStateSuffix) { +buildModel(int whichDiscreteState = -1, + const string& discreteStateSuffix = "") { // Create an empty model Model* model = new Model(); @@ -630,27 +631,21 @@ TEST_CASE("Getting Started") TEST_CASE("Serialization and Deserialization") { - // Specify which discrete state gets a suffix. The suffix is used - // to change the name of a discrete variable so that it will not be found. - // When whichDiscreteState == 1, the suffix is not appended to any state. - int whichDiscreteState{-1}; - string suffix{"_ShouldNotBeFound"}; - // 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(whichDiscreteState, suffix); + Model *model = buildModel(); Array_ trajA = simulate(model); // Serialize (A) - int p; - for (p = 2; p < 22; ++p) { + SimTK::String filename = "BlockOnASpring.ostates"; + SimTK::String note = "Output from `testStatesDocument.cpp` "; + note += "to validate state de/serialization."; + for (int p = 1; p < 22; ++p) { cout << "Testing for precision = " << p << endl; - SimTK::String filename = "BlockOnASpring.ostates"; - SimTK::String note = "Output from `testStatesDocument.cpp` to validate state serialization and deserialization."; StatesDocument docA(*model, trajA, note, p); docA.serialize(filename); @@ -673,7 +668,7 @@ TEST_CASE("Serialization and Deserialization") } // Are state trajectories A and B the same? - testEquality(*model, trajA, trajB, p); + testStateEquality(*model, trajA, trajB, p); } delete model; @@ -681,49 +676,59 @@ TEST_CASE("Serialization and Deserialization") TEST_CASE("Exception: Discrete State Not Found") { - // Specify which discrete state gets a suffix. The suffix is used - // to change the name of a discrete variable so that it will not be found. - // When whichDiscreteState == 1, the suffix is not appended to any state. - // When whichDiscreteState is between 0 and 7, the suffix will be - // appended to that discrete state. - int whichDiscreteState{-1}; - string suffix{"_ShouldNotBeFound"}; - - // Build a 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(whichDiscreteState, suffix); - Array_ trajA = simulate(model); + // (A) Build the default model and run a simulation + Model *modelA = buildModel(); + Array_ trajA = simulate(modelA); // Serialize (A) - int precision = 6; SimTK::String filename = "BlockOnASpring.ostates"; - SimTK::String note = "Output from `testStatesDocument.cpp` to validate state serialization and deserialization."; - StatesDocument docA(*model, trajA, note, precision); + SimTK::String note = "Output from `testStatesDocument.cpp` "; + note += "to validate state de/serialization."; + int precision = 6; + StatesDocument docA(*modelA, trajA, note, precision); docA.serialize(filename); - // Deserialize (B) - StatesDocument docB(filename); - Array_ trajB; - docB.deserialize(*model, trajB); - // Check the note and the precision. - CHECK(docB.getNote() == docA.getNote()); - CHECK(docB.getPrecision() == docB.getPrecision()); + // Deserialize using a series of models (B) + // In each modelB, the name of one discrete state is changed. + string suffix{"_ShouldNotBeFound"}; + for (int which = 0; which < 8; ++which) { + + // (B) Build a model that is different only with respect to one name of a + // specified discrete state. + // Specify which discrete state gets the suffix. The suffix is used to + // change the name of a discrete state so that it will not be found. + // When whichDiscreteState == 1, the suffix is not appended. + // When whichDiscreteState is between 0 and 7, the suffix will be + // appended to that discrete state. + Model *modelB = buildModel(which, suffix); + Array_ trajDoNotNeed = simulate(modelB); + + // Deserialize using modelB + // This should fail if the name of a discrete state has been changed. + StatesDocument docB(filename); + Array_ trajB; + //CHECK_THROWS(, "discrete state should not be found"); + docB.deserialize(*modelB, trajB); - // Check size - REQUIRE(trajA.size() == trajB.size()); + // Check the note and the precision. + CHECK(docB.getNote() == docA.getNote()); + CHECK(docB.getPrecision() == docB.getPrecision()); - // Realize both state trajectories to Stage::Report - for (int i = 0; i < trajA.size(); ++i) { - model->getSystem().realize(trajA[i], Stage::Report); - model->getSystem().realize(trajB[i], Stage::Report); - } + // Check size + REQUIRE(trajA.size() == trajB.size()); + + // Realize both state trajectories to Stage::Report + for (int i = 0; i < trajA.size(); ++i) { + modelB->getSystem().realize(trajA[i], Stage::Report); + modelB->getSystem().realize(trajB[i], Stage::Report); + } - // Are state trajectories A and B the same? - testEquality(*model, trajA, trajB, precision); + // Are state trajectories A and B the same? + testStateEquality(*modelB, trajA, trajB, precision); - delete model; + delete modelB; + } + + delete modelA; } \ No newline at end of file From 467d910ad0feccb1fefc1a0571b2a3b51a4c18b1 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 1 Sep 2024 05:32:10 -0500 Subject: [PATCH 30/56] Check that model names match --- OpenSim/Simulation/StatesDocument.cpp | 35 +++++++++++++++++++++------ OpenSim/Simulation/StatesDocument.h | 6 ++--- 2 files changed, 30 insertions(+), 11 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 2e27000f12..ac742a4a89 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -382,7 +382,25 @@ parseDoc(const Model& model, Array_& traj) { //_____________________________________________________________________________ void StatesDocument:: -checkDocConsistencyWithModel(const Model& model) const { +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); + 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); + } } //_____________________________________________________________________________ @@ -423,7 +441,7 @@ initializeNote() { } else if (noteElts.size() > 1) { cout << "StatesDocument: More than 1 `note` element found; "; - cout << "using just the 1st one." << endl; + cout << "using just the first one." << endl; } // Get the value @@ -452,7 +470,7 @@ initializeTime(Array_& traj) { // Check the number of time elements found. Should be 1. SimTK_ASSERT1_ALWAYS(timeElts.size() == 1, - "%d time elements found. Should only be 1.", timeElts.size()); + "%d time elements found. Only 1 should be found.", timeElts.size()); // Get the values Array_ timeArr; @@ -489,7 +507,7 @@ initializeContinuousVariables(const Model& model, SimTK::Array_& traj) { int n = varElts.size(); int m = varNames.size(); SimTK_ASSERT2_ALWAYS(n == m, - "Found %d variable elements. Should be %d.", n, m); + "Found %d continuous variable elements. Should be %d.", n, m); // Loop over the variable elements SimTK::Array_ varArr; @@ -521,7 +539,7 @@ 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'. Should only be 1.", + "Found %d elements with tag 'discrete'. Only 1 should be found.", discElts.size()); // Find all the child 'variable' elements @@ -533,7 +551,7 @@ initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { int n = varElts.size(); int m = varNames.size(); SimTK_ASSERT2_ALWAYS(n == m, - "Found %d variable elements. Should be %d.", n, m); + "Found %d discrete variable elements. Should be %d.", n, m); // Loop over the variable elements for (int i = 0; i < n; ++i) { @@ -597,7 +615,8 @@ initializeModelingOptions(const Model& model, SimTK::Array_& traj) { Element rootElt = doc.getRootElement(); Array_ modlElts = rootElt.getAllElements("modeling"); SimTK_ASSERT1_ALWAYS(modlElts.size() == 1, - "%d modeling elements found. Should only be 1.", modlElts.size()); + "%d modeling elements found. Only 1 should be found.", + modlElts.size()); Element modlElt = modlElts[0]; // Find all the child 'variable' elements. @@ -610,7 +629,7 @@ initializeModelingOptions(const Model& model, SimTK::Array_& traj) { int n = varElts.size(); int m = varNames.size(); SimTK_ASSERT2_ALWAYS(n == m, - "Found %d variable elements. Should be %d.", n, m); + "Found %d modeling option elements. Should be %d.", n, m); // Loop over the modeling options SimTK::Array_ varArr; diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index ba10b891bb..362140adc1 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -501,7 +501,7 @@ class OSIMSIMULATION_API StatesDocument { @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) { + StatesDocument(const SimTK::String& filename) : filename(filename) { doc.readFromFile(filename); initializeNote(); initializePrecision(); @@ -587,7 +587,7 @@ class OSIMSIMULATION_API StatesDocument { // Deserialization Helpers. void parseDoc(const Model& m, SimTK::Array_& t); - void checkDocConsistencyWithModel(const Model& model) const; + void checkDocConsistencyWithModel(const Model& model); void prepareStatesTrajectory(const Model& model, SimTK::Array_ &traj); void initializeNote(); @@ -607,9 +607,9 @@ class OSIMSIMULATION_API StatesDocument { // Member Variables int precision{SimTK::LosslessNumDigitsReal}; SimTK::Xml::Document doc; + SimTK::String filename{""}; SimTK::String note{""}; - }; // END of class StatesDocument } // end of namespace OpenSim From a7eb16bce82e5623963a78b5ff76134e005aeb3d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 1 Sep 2024 05:35:09 -0500 Subject: [PATCH 31/56] Update testStatesDocument.cpp Cleaned up the code. The following checks are now performed: - Serialization / deserialization for precision 1 to 21. - Check that exception is thrown when model names don't match. - Check that exception is thrown when a discrete state is not found. --- .../Simulation/Test/testStatesDocument.cpp | 95 ++++++++----------- 1 file changed, 40 insertions(+), 55 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 3f616c86d5..d2e152f16d 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -637,98 +637,83 @@ TEST_CASE("Serialization and Deserialization") // to worry about the reporter (or any other object) going out of scope // or being deleted. Model *model = buildModel(); - Array_ trajA = simulate(model); + Array_ traj = simulate(model); - // Serialize (A) + // Serialize SimTK::String filename = "BlockOnASpring.ostates"; - SimTK::String note = "Output from `testStatesDocument.cpp` "; - note += "to validate state de/serialization."; + SimTK::String note = "Output from `testStatesDocument.cpp`."; for (int p = 1; p < 22; ++p) { cout << "Testing for precision = " << p << endl; - StatesDocument docA(*model, trajA, note, p); - docA.serialize(filename); + StatesDocument doc(*model, traj, note, p); + doc.serialize(filename); - // Deserialize (B) - StatesDocument docB(filename); - Array_ trajB; - docB.deserialize(*model, trajB); + // (A) Deserialize + StatesDocument docA(filename); + Array_ trajA; + docA.deserialize(*model, trajA); // Check the note and the precision. - CHECK(docB.getNote() == docA.getNote()); - CHECK(docB.getPrecision() == docB.getPrecision()); + CHECK(docA.getNote() == doc.getNote()); + CHECK(docA.getPrecision() == doc.getPrecision()); // Check size - REQUIRE(trajA.size() == trajB.size()); + REQUIRE(traj.size() == traj.size()); // Realize both state trajectories to Stage::Report for (int i = 0; i < trajA.size(); ++i) { + model->getSystem().realize(traj[i], Stage::Report); model->getSystem().realize(trajA[i], Stage::Report); - model->getSystem().realize(trajB[i], Stage::Report); } // Are state trajectories A and B the same? - testStateEquality(*model, trajA, trajB, p); + testStateEquality(*model, traj, trajA, p); } delete model; } -TEST_CASE("Exception: Discrete State Not Found") +TEST_CASE("Exceptions") { - // (A) Build the default model and run a simulation - Model *modelA = buildModel(); - Array_ trajA = simulate(modelA); + // Build the default model and run a simulation + Model *model = buildModel(); + Array_ traj = simulate(model); - // Serialize (A) + // Serialize the default model SimTK::String filename = "BlockOnASpring.ostates"; - SimTK::String note = "Output from `testStatesDocument.cpp` "; - note += "to validate state de/serialization."; + SimTK::String note = "Output from `testStatesDocument.cpp`."; int precision = 6; - StatesDocument docA(*modelA, trajA, note, precision); - docA.serialize(filename); - - - // Deserialize using a series of models (B) - // In each modelB, the name of one discrete state is changed. + 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 + // In each model, the name of one discrete state is changed. string suffix{"_ShouldNotBeFound"}; for (int which = 0; which < 8; ++which) { - // (B) Build a model that is different only with respect to one name of a + // Build a model that is different only with respect to one name of a // specified discrete state. - // Specify which discrete state gets the suffix. The suffix is used to - // change the name of a discrete state so that it will not be found. - // When whichDiscreteState == 1, the suffix is not appended. - // When whichDiscreteState is between 0 and 7, the suffix will be - // appended to that discrete state. - Model *modelB = buildModel(which, suffix); + Model* modelB = buildModel(which, suffix); Array_ trajDoNotNeed = simulate(modelB); // Deserialize using modelB - // This should fail if the name of a discrete state has been changed. + // This should fail when name of a discrete state has been changed. StatesDocument docB(filename); Array_ trajB; - //CHECK_THROWS(, "discrete state should not be found"); - docB.deserialize(*modelB, trajB); - - // Check the note and the precision. - CHECK(docB.getNote() == docA.getNote()); - CHECK(docB.getPrecision() == docB.getPrecision()); - - // Check size - REQUIRE(trajA.size() == trajB.size()); - - // Realize both state trajectories to Stage::Report - for (int i = 0; i < trajA.size(); ++i) { - modelB->getSystem().realize(trajA[i], Stage::Report); - modelB->getSystem().realize(trajB[i], Stage::Report); - } - - // Are state trajectories A and B the same? - testStateEquality(*modelB, trajA, trajB, precision); + CHECK_THROWS(docB.deserialize(*modelB, trajB), + "Discrete state should not be found"); delete modelB; } - delete modelA; + delete model; } \ No newline at end of file From bc461444989adfbdd9d5520d251cd4ea94eaddc2 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 2 Sep 2024 14:34:55 -0500 Subject: [PATCH 32/56] Update testStatesDocument.cpp - code cleanup - added the ability to omit a discrete state When a discrete state is omitted, an exception should be thrown but it is not. Need to look into this. Previous checks are still running properly. --- .../Simulation/Test/testStatesDocument.cpp | 343 ++++++++++-------- 1 file changed, 196 insertions(+), 147 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index d2e152f16d..32f6fe3c94 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -47,8 +47,8 @@ const double padFactor = 1.0 + SimTK::SignificantReal; //----------------------------------------------------------------------------- -// Create a force component derived from PointToPointSpring that adds on a -// discrete variable of each supported type (bool, int, double, Vec2, Vec3, +// 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 { @@ -76,13 +76,20 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring 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, const string& suffix) : + double stiffness, double restlength, + int which = -1, const string& suffix = "", int omit = -1) : omit(omit), PointToPointSpring(body1, point1, body2, point2, stiffness, restlength) { switch (which) { @@ -111,7 +118,6 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring nameVec6 += suffix; break; } - } void @@ -119,16 +125,18 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring { Super::extendAddToSystemAfterSubcomponents(system); - // Add the discrete variables to the list of OpenSim Components + // 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; - addDiscreteVariable(nameBool, Stage::Position, allocate); - addDiscreteVariable(nameInt, Stage::Position, allocate); - addDiscreteVariable(nameDbl, Stage::Position, allocate); - addDiscreteVariable(nameVec2, Stage::Position, allocate); - addDiscreteVariable(nameVec3, Stage::Position, allocate); - addDiscreteVariable(nameVec4, Stage::Position, allocate); - addDiscreteVariable(nameVec5, Stage::Position, allocate); - addDiscreteVariable(nameVec6, Stage::Position, allocate); + 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 @@ -144,55 +152,77 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring const DefaultSystemSubsystem& fsub = getModel().getDefaultSubsystem(); mutableThis->indexSS = fsub.getMySubsystemIndex(); - // Bool - bool dvBool{false}; - mutableThis->indexBool = s.allocateAutoUpdateDiscreteVariable(indexSS, - Stage::Velocity, new Value(dvBool), Stage::Dynamics); - - // Int - int dvInt{0}; - mutableThis->indexInt = s.allocateAutoUpdateDiscreteVariable( - indexSS, Stage::Velocity, new Value(dvInt), Stage::Dynamics); - - // Dbl - double dvDbl{0.0}; - mutableThis->indexDbl = s.allocateAutoUpdateDiscreteVariable(indexSS, - Stage::Velocity, new Value(dvDbl), Stage::Dynamics); - - // Vec2 - Vec2 dvVec2(0.1, 0.2); - mutableThis->indexVec2 = s.allocateAutoUpdateDiscreteVariable(indexSS, - Stage::Velocity, new Value(dvVec2), Stage::Dynamics); - - // Vec3 - Vec3 dvVec3(0.1, 0.2, 0.3); - mutableThis->indexVec3 = s.allocateAutoUpdateDiscreteVariable(indexSS, - Stage::Velocity, new Value(dvVec3), Stage::Dynamics); - - // Vec4 - Vec4 dvVec4(0.1, 0.2, 0.3, 0.4); - mutableThis->indexVec4 = s.allocateAutoUpdateDiscreteVariable(indexSS, - Stage::Velocity, new Value(dvVec4), Stage::Dynamics); - - // Vec5 - Vec5 dvVec5(0.1, 0.2, 0.3, 0.4, 0.5); - mutableThis->indexVec5 = s.allocateAutoUpdateDiscreteVariable(indexSS, - Stage::Velocity, new Value(dvVec5), Stage::Dynamics); - - // Vec6 - 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); - - // Initialize discrete variable indexes - initializeDiscreteVariableIndexes(nameBool, indexSS, indexBool); - initializeDiscreteVariableIndexes(nameInt, indexSS, indexInt); - initializeDiscreteVariableIndexes(nameDbl, indexSS, indexDbl); - initializeDiscreteVariableIndexes(nameVec2, indexSS, indexVec2); - initializeDiscreteVariableIndexes(nameVec3, indexSS, indexVec3); - initializeDiscreteVariableIndexes(nameVec4, indexSS, indexVec4); - initializeDiscreteVariableIndexes(nameVec5, indexSS, indexVec5); - initializeDiscreteVariableIndexes(nameVec6, indexSS, indexVec6); + // 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. @@ -208,66 +238,82 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring const SimTK::Vector& u = state.getU(); - // Bool - bool& vBool = SimTK::Value::downcast( - state.updDiscreteVarUpdateValue(indexSS, indexBool)); - vBool = u[0]; - state.markDiscreteVarUpdateValueRealized(indexSS, indexBool); - - // Int - SimTK::Value::downcast( - state.updDiscreteVarUpdateValue(indexSS, indexInt)) = u[0]; - state.markDiscreteVarUpdateValueRealized(indexSS, indexInt); - - // Dbl - SimTK::Value::downcast( - state.updDiscreteVarUpdateValue(indexSS, indexDbl)) = u[0]; - state.markDiscreteVarUpdateValueRealized(indexSS, indexDbl); - - // Vec2 - Vec2& v2 = SimTK::Value::downcast( - state.updDiscreteVarUpdateValue(indexSS, indexVec2)); - v2[0] = u[0]; - v2[1] = u[1]; - state.markDiscreteVarUpdateValueRealized(indexSS, indexVec2); - - // Vec3 - 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); - - // Vec4 - 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); - - // Vec5 - 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); - - // Vec6 - 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); + // 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 @@ -276,14 +322,6 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring //----------------------------------------------------------------------------- // Other Local Static Methods //----------------------------------------------------------------------------- -//_____________________________________________________________________________ -// Sample internal method -double -customSquare(double x) -{ - return(x*x); -} - //_____________________________________________________________________________ /** Compute the maximum error that can result from rounding a value at a @@ -511,8 +549,6 @@ void testEqualityForModelingOptions(const Model& model, const Array_& trajA, const Array_& trajB, int precision) { - double tol; - // Loop over the named variables OpenSim::Array paths = model.getModelingOptionNames(); int nPaths = paths.getSize(); @@ -541,14 +577,14 @@ testStateEquality(const Model& model, //_____________________________________________________________________________ // Build the model Model* -buildModel(int whichDiscreteState = -1, - const string& discreteStateSuffix = "") { +buildModelFree(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("BlockOnASpring"); + model->setName("BlockOnASpringFreeJoint"); // Add bodies and joints OpenSim::Ground& ground = model->updGround(); @@ -621,14 +657,6 @@ simulate(Model* model) { } // End anonymous namespace -TEST_CASE("Getting Started") -{ - double x = 2.0; - double square = customSquare(x); - REQUIRE(square == x*x); -} - - TEST_CASE("Serialization and Deserialization") { // Build the model and run a simulation. @@ -636,7 +664,7 @@ TEST_CASE("Serialization and Deserialization") // 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(); + Model *model = buildModelFree(); Array_ traj = simulate(model); // Serialize @@ -676,7 +704,7 @@ TEST_CASE("Serialization and Deserialization") TEST_CASE("Exceptions") { // Build the default model and run a simulation - Model *model = buildModel(); + Model *model = buildModelFree(); Array_ traj = simulate(model); // Serialize the default model @@ -695,14 +723,15 @@ TEST_CASE("Exceptions") "Model names should not match."); model->setName(name); // return the original name - // (B) A discrete state is not found + // (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); + Model* modelB = buildModelFree(which, suffix); Array_ trajDoNotNeed = simulate(modelB); // Deserialize using modelB @@ -715,5 +744,25 @@ TEST_CASE("Exceptions") delete modelB; } + // (C) A discrete state is not found because the state doesn't exist. + // A 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 = buildModelFree(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"); + docC.deserialize(*modelC, trajC); + + delete modelC; + } + delete model; } \ No newline at end of file From 231e27dd0866158e72df05c359d345039b838573 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 3 Sep 2024 23:36:34 -0500 Subject: [PATCH 33/56] All checks passing. --- OpenSim/Simulation/StatesDocument.cpp | 2 +- .../Simulation/Test/testStatesDocument.cpp | 20 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index ac742a4a89..265d845cc1 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -546,7 +546,7 @@ initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { SimTK::String childTag = "variable"; Array_ varElts = discElts[0].getAllElements(childTag); - // Check that the number matches the number of discrete variables. + // Check that # children matches the number of discrete variables. OpenSim::Array varNames = model.getDiscreteVariableNames(); int n = varElts.size(); int m = varNames.size(); diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 32f6fe3c94..3f018c304c 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -577,7 +577,7 @@ testStateEquality(const Model& model, //_____________________________________________________________________________ // Build the model Model* -buildModelFree(int whichDiscreteState = -1, +buildModel(int whichDiscreteState = -1, const string& discreteStateSuffix = "", int omit = -1) { // Create an empty model @@ -615,7 +615,7 @@ buildModelFree(int whichDiscreteState = -1, Vec3 insertion(0.1, 0.1, 0.025); ExtendedPointToPointSpring* spring = new ExtendedPointToPointSpring( ground, origin, *block, insertion, kp, restlength, - whichDiscreteState, discreteStateSuffix); + whichDiscreteState, discreteStateSuffix, omit); model->addForce(spring); return model; @@ -664,7 +664,7 @@ TEST_CASE("Serialization and Deserialization") // 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 = buildModelFree(); + Model *model = buildModel(); Array_ traj = simulate(model); // Serialize @@ -704,7 +704,7 @@ TEST_CASE("Serialization and Deserialization") TEST_CASE("Exceptions") { // Build the default model and run a simulation - Model *model = buildModelFree(); + Model *model = buildModel(); Array_ traj = simulate(model); // Serialize the default model @@ -731,7 +731,7 @@ TEST_CASE("Exceptions") // Build a model that is different only with respect to one name of a // specified discrete state. - Model* modelB = buildModelFree(which, suffix); + Model* modelB = buildModel(which, suffix); Array_ trajDoNotNeed = simulate(modelB); // Deserialize using modelB @@ -745,21 +745,21 @@ TEST_CASE("Exceptions") } // (C) A discrete state is not found because the state doesn't exist. - // A exception should be thrown because the number of states don't match. + // 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 = buildModelFree(which, suffix, omit); + 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"); - docC.deserialize(*modelC, trajC); + CHECK_THROWS(docC.deserialize(*modelC, trajC), + "Expected number of discrete states should be wrong"); + //docC.deserialize(*modelC, trajC); delete modelC; } From c8e9133573efd412596c7f4543224e7c2d8e15eb Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 3 Sep 2024 23:40:46 -0500 Subject: [PATCH 34/56] Removed old code --- OpenSim/Simulation/StatesDocument.cpp | 107 ------------------ OpenSim/Simulation/StatesDocument.h | 3 - .../Simulation/Test/testStatesDocument.cpp | 1 - 3 files changed, 111 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 265d845cc1..e503afb85c 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -654,110 +654,3 @@ initializeModelingOptions(const Model& model, SimTK::Array_& traj) { } } } - - -//----------------------------------------------------------------------------- -// Testing -//----------------------------------------------------------------------------- -//_____________________________________________________________________________ -void -StatesDocument:: -test() { - // Make up some data and elements. - prototype(); - - // Write to String - SimTK::String docStr; - doc.writeToString(docStr); - cout << endl << "Prototype StatesDocument -----" << endl; - cout << docStr << endl; - - // Write to File - doc.writeToFile( - "C:/Users/fcand/Documents/GitHub/Work/Testing/OpenSim/test.ostates"); -} - -//_____________________________________________________________________________ -void -StatesDocument:: -prototype() { - - // Set the tag of the root element and get an iterator to it. - doc.setRootTag("ostates"); - Xml::Element& root = doc.getRootElement(); - Xml::node_iterator root_it = doc.node_begin(Xml::ElementNode); - - // Insert a comment at the top level, just before the root node. - string info = "Developing class StatesDocument. Version 0.0.1."; - Xml::Comment comment(info); - doc.insertTopLevelNodeBefore(root_it, comment); - - // Add attributes to the root node - root.setAttributeValue("model", "BouncingBlocks"); - - // Add time and state category elements - Xml::Element timeElt("time"); - root.appendNode(timeElt); - Xml::Element continuousElt("continuous"); - root.appendNode(continuousElt); - Xml::Element discreteElt("discrete"); - root.appendNode(discreteElt); - Xml::Element modelingElt("modeling"); - root.appendNode(modelingElt); - - // Number of State% Objects - int i; - int num = 11; - std::string numStr = std::to_string(num); - root.setAttributeValue("numStateObjects", numStr); - - // Time - SimTK::Vector_ time(num); - for (i = 0; i < num; ++i) { time[i] = 0.1 * SimTK::Pi * (double)i; } - timeElt.setValueAs>(time, precision); - //timeElt.setValueAs>(time); - - // Experiment with output precision - cout.unsetf(std::ios::floatfield); - cout << setprecision(precision); - cout << endl << time << endl << endl; - - // Hip Flexion - SimTK::Vector_ q(num); - for (i = 0; i < num; ++i) { q[i] = 1.0e-10 * SimTK::Pi * (double)i; } - Xml::Element hipElt("variable"); - hipElt.setAttributeValue("path", "/jointset/hip/flexion/value"); - hipElt.setValueAs>(q, precision); - //hipElt.setValueAs>(q); - continuousElt.appendNode(hipElt); - - // Elastic Anchor Point - SimTK::Array_ anchor(num); - for (i = 0; i < num; ++i) { - Vec3 val(0.0, 1.10000000001, 1.200000000000002); - anchor[i] = ((double)i) * val; - } - Xml::Element anchorElt("variable"); - anchorElt.setAttributeValue("path", "/forceset/EC0/anchor"); - anchorElt.setValueAs>(anchor, precision); - //anchorElt.setValueAs>(anchor); - discreteElt.appendNode(anchorElt); - - // Now -- Getting Vectors out! - // Time - Vector_ timeOut; - timeElt.getValueAs>(timeOut); - cout << endl << "timeOut: " << timeOut << endl; - // Hip Flexion - Vector_ qOut; - hipElt.getValueAs>(qOut); - cout << endl << "hipOut: " << qOut << endl; - // Anchor - Array_ anchorOut; - anchorElt.getValueAs>(anchorOut); - cout << endl << "anchorOut: " << anchorOut << endl; - - // Asserts - SimTK_ASSERT_ALWAYS(anchor[0] == anchorOut[0], - "Deserialized value not equal to original value."); -} diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index 362140adc1..f90859c91f 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -600,9 +600,6 @@ class OSIMSIMULATION_API StatesDocument { void initializeModelingOptions(const Model& model, SimTK::Array_ &traj); - // Testing - void prototype(); - private: // Member Variables int precision{SimTK::LosslessNumDigitsReal}; diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 3f018c304c..499aad5d7c 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -759,7 +759,6 @@ TEST_CASE("Exceptions") Array_ trajC; CHECK_THROWS(docC.deserialize(*modelC, trajC), "Expected number of discrete states should be wrong"); - //docC.deserialize(*modelC, trajC); delete modelC; } From 867725653433b31ced5d75defae0fa14004a8c91 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 10:26:03 -0500 Subject: [PATCH 35/56] Added read-only accessor for low-level state array const SimTK::Array_& StatesTrajectoryReporter::getStateArray(); --- OpenSim/Simulation/StatesTrajectoryReporter.cpp | 5 +++++ OpenSim/Simulation/StatesTrajectoryReporter.h | 8 +++++--- 2 files changed, 10 insertions(+), 3 deletions(-) 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: From 3c87d7781c07c1b21cb29c2fe4af50cd04f2ee6b Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 10:30:12 -0500 Subject: [PATCH 36/56] Code cleanup and refinement - Removed StatesDocument::parseDoc(). No reason to have it. - Simplified the name of StatesTrajectory::getUnderlyingStateArray() to getStateArray(). - Refined documentation comments and checked them for accuracy. --- OpenSim/Simulation/StatesDocument.cpp | 6 -- OpenSim/Simulation/StatesDocument.h | 75 +++++++++---------- OpenSim/Simulation/StatesTrajectory.h | 2 +- .../Simulation/Test/testStatesDocument.cpp | 4 +- 4 files changed, 38 insertions(+), 49 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index e503afb85c..ff0ad4046f 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -366,12 +366,6 @@ formModelingElement(const Model& model, const Array_& traj) { void StatesDocument:: deserialize(const Model& model, Array_& traj) { - parseDoc(model, traj); -} -//_____________________________________________________________________________ -void -StatesDocument:: -parseDoc(const Model& model, Array_& traj) { checkDocConsistencyWithModel(model); prepareStatesTrajectory(model, traj); initializeTime(traj); diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index f90859c91f..b3c90bdb8f 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -66,20 +66,20 @@ 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, -these output variables are implemented as a specialized kind of discrete -variable called an Auto-Update Discrete Variable. +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 the System or whether or not to apply a +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 or unlocked. When a Modeling Options is -changed, low-level aspects of the System must be reconstituted or, in SimTK +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 StatesDocument, only Continuous -Variables (i.e., OpenSim::StateVariable%s) were routinely and systematically -serialized, most commonly via the OpenSim::Manager as an OpenSim::Storage file +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 @@ -134,8 +134,8 @@ 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 -frequently encounter the term 'trajectory'. In these contexts, the term -connotes a time-ordered sequence, or a time-history, of values. +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 @@ -157,21 +157,21 @@ the OpenSim::Component class. A few examples follow. ``` template Component::getDiscreteVariableTrajectory( - const std::string& pathName, + 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 model -hierarchy based on the specifed path (`pathName`). 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.). +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& pathName, + const std::string& path, const SimTK::Array_& input, SimTK::Array_& output) const ``` @@ -261,15 +261,19 @@ 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 values recorded for each `variable` and each + 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. - 3) All `variable` and `option` paths must be found in the model + 4) All `variable` and `option` paths must be found in the model OpenSim::Component heirarchy. - 4) The type must be supported. As of January 2024, the following types are - supported: + 5) The type must be supported. As of September 2024, the following types + are supported: SimTK::State Category | Supported Type(s) ------------------------ | ----------------------- @@ -340,18 +344,16 @@ deserialize those same states and use them to accomplish a few basic things. // 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 does not provide direct access to this - // private array, but it does know how to export a StatesDocument based on - // those states. This "export" functionality is what is used below. + // 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, if the reporter did provide direct access to the - // underlying array of states (i.e., the Array_) instead of the - // StatesTrajectory object, the StatesDocument would be created by doing - // the following: - const SimTK::Array_& trajectory = reporter->getStates(); - StatesDocument doc(model, trajectory); + // 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 @@ -396,7 +398,7 @@ deserialize those same states and use them to accomplish a few basic things. // 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; // "traj" is short for "trajectory" + SimTK::Array_ traj; doc.deserialize(model, traj); // Below are some things that can be done once a deserialized state @@ -466,7 +468,7 @@ deserialize those same states and use them to accomplish a few basic things. ``` ### A Final Note -Because Storage files (*.sto) and TimeSeriesTable files (*.??) typically +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 @@ -562,12 +564,6 @@ class OSIMSIMULATION_API StatesDocument { void deserialize(const OpenSim::Model& model, SimTK::Array_& trajectory); - //------------------------------------------------------------------------- - // Testing - //------------------------------------------------------------------------- - /** An entry point for running basic tests on this class. */ - void test(); - protected: // Serialization Helpers. void formDoc(const Model& model, @@ -586,7 +582,6 @@ class OSIMSIMULATION_API StatesDocument { const SimTK::Array_& traj); // Deserialization Helpers. - void parseDoc(const Model& m, SimTK::Array_& t); void checkDocConsistencyWithModel(const Model& model); void prepareStatesTrajectory(const Model& model, SimTK::Array_ &traj); diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index c0f55f2c0a..0945f90580 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -339,7 +339,7 @@ class OSIMSIMULATION_API StatesTrajectory { } /** Get a read-only reference to the underlying state array. */ - const SimTK::Array_& getUnderlyingStateArray() const { + const SimTK::Array_& getStateArray() const { return m_states; } diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 499aad5d7c..859d9bceb8 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -650,8 +650,8 @@ simulate(Model* model) { manager.initialize(state); state = manager.integrate(tf); - // Return a copy of the state trajectory - return reporter->getStates().getUnderlyingStateArray(); + // Return a copy of the underlying state array + return reporter->getStateArray(); } } // End anonymous namespace From 439db745a6c57db7618349121ac0773f40f234e0 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 13:43:36 -0500 Subject: [PATCH 37/56] Minor code cleanup - Deleted unused code. - Updated copyright dates. --- OpenSim/Simulation/StatesDocument.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index ff0ad4046f..7d94a34c36 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -7,7 +7,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2022-20232 Stanford University and the Authors * + * 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 * @@ -52,7 +52,6 @@ struct SDocUtil { // Append the variable element varElt.setValueAs>(valArr, precision); - //varElt.setValueAs>(valArr); parent.appendNode(varElt); } //_________________________________________________________________________ @@ -219,7 +218,6 @@ formTimeElement(const Model& model, const Array_& traj) { } // Set the text value on the element - //timeElt.setValueAs>(time); timeElt.setValueAs>(time, precision); } //_____________________________________________________________________________ From 7c2b595433519c0f0c1597c48a92661091c655cc Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 13:54:03 -0500 Subject: [PATCH 38/56] Update StatesTrajectory.h - Improved documentation comments. --- OpenSim/Simulation/StatesTrajectory.h | 33 ++++++++++++--------------- 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index 0945f90580..7c781cf105 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -294,25 +294,22 @@ class OSIMSIMULATION_API StatesTrajectory { * 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 the following: - * - * StatesDocument::serializeToFile() - * StatesDocument::serializeToSring() + * calling `StatesDocument::serialize()`. * * Once the states have been serialized, they can be deserialized by - * constructing a new StatesDocument from a file or document string: - * - * StatesDocument(const SimTK::String& filename) - * StatesDocument(const char* xmlDocument) - * - * and then calling the following: - * + * 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 precision - * sufficient to restore a time-history of SimTK::State%s with virtually - * no trunction or rounding error. + * ``` + * + * 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 @@ -323,11 +320,11 @@ class OSIMSIMULATION_API StatesTrajectory { * 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 generally include only the - * continuous states (i.e., OpenSim::StateVariable%s). + * 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 source as + * complete StatesTrajectory, a StatesDocument is the preferred means as * it will include continuous, discrete, and modeling states. */ OpenSim::StatesDocument From 4f994e02cbd4e2e7790265b631ac080d3388978c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 15:08:31 -0500 Subject: [PATCH 39/56] Fixed typo in StatesDocument.h An error was being issued for Ubuntu and Mac builds because "OPENSIM_STATES_DOCUMENT_H_" was not commented out on line 609. I added the comment token. --- OpenSim/Simulation/StatesDocument.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index b3c90bdb8f..0bb821ef1f 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -606,4 +606,4 @@ class OSIMSIMULATION_API StatesDocument { } // end of namespace OpenSim -#endif OPENSIM_STATES_DOCUMENT_H_ \ No newline at end of file +#endif // OPENSIM_STATES_DOCUMENT_H_ From 054908ba31cc7832cad61a312396a4766115a0cb Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 15:31:51 -0500 Subject: [PATCH 40/56] Corrected some type compatibilities in StatesDocument.cpp In a number of instances, I changed the type from `int` to `size_t`. --- OpenSim/Simulation/StatesDocument.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 7d94a34c36..ec1596fe43 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -59,14 +59,14 @@ struct SDocUtil { inline static void - getEltValue(const string& path, int expectedSize, + getEltValue(const string& path, size_t expectedSize, Element& varElt, Array_& vArr) { // Interpret the element value varElt.getValueAs>(vArr); // Check the size - int n = vArr.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, expectedSize); @@ -114,7 +114,7 @@ struct SDocUtil { varElt.getValueAs>(vArr); // Check the sizes. - int n = vArr.size(); + size_t n = vArr.size(); SimTK_ASSERT2_ALWAYS(n == traj.size(), "Found %d values. Should match nTime = %d values.", n, traj.size()); @@ -469,7 +469,7 @@ initializeTime(Array_& traj) { timeElts[0].getValueAs>(timeArr); // Check the size of the time array. - int n = traj.size(); + size_t n = traj.size(); SimTK_ASSERT2_ALWAYS(n == traj.size(), "Found %d time values. Should match numStateObjects = %d", n, traj.size()); From 12d7f099d40eeff78dd2af35206d9990e7aa9e81 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 15:59:26 -0500 Subject: [PATCH 41/56] Using std::string::c_str() for strings in SimTK_ASSERT macros Windows can handle a std::string, but Mac and Ubuntu builds are failing. --- OpenSim/Simulation/StatesDocument.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index ec1596fe43..0a044da330 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -69,7 +69,7 @@ struct SDocUtil { 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, expectedSize); + n, path.c_str(), expectedSize); } //_________________________________________________________________________ template @@ -384,14 +384,14 @@ checkDocConsistencyWithModel(const Model& model) { Attribute modelNameAttr = rootElt.getOptionalAttribute("model"); SimTK_ASSERT1(modelNameAttr.isValid(), "The 'model' attribute of the root element was not found in file %s.", - filename); + 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); + SimTK_ASSERT_ALWAYS(false, msg.c_str()); } } @@ -475,7 +475,7 @@ initializeTime(Array_& traj) { n, traj.size()); // Initialize the State objects - for (int i = 0; i < n; ++i) traj[i].setTime(timeArr[i]); + for (size_t i = 0; i < n; ++i) traj[i].setTime(timeArr[i]); } //_____________________________________________________________________________ void From 4ce36dd1989f9359e260d5be543ce1fbb836b822 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 16:43:15 -0500 Subject: [PATCH 42/56] Update testStatesDocument.cpp - Addressed some type warnings. - Removed the method `ComputeMaxRoundingError()`. --- .../Simulation/Test/testStatesDocument.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 859d9bceb8..5bd3ffbad0 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -77,7 +77,7 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring string nameVec5{"dvVec5"}; string nameVec6{"dvVec6"}; // Omit a discrete state altogether - int omit; + int omit{-1}; public: @@ -330,7 +330,7 @@ specified precision. This method assumes a base-10 representation of the value. @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; @@ -340,6 +340,7 @@ computeMaxRoundingError(double value, int precision) { if(max_eps < SimTK::LeastPositiveReal) return SimTK::LeastPositiveReal; return max_eps; } +*/ // No longer used, but might be useful elsewhere, so saving. //_____________________________________________________________________________ /** @@ -429,7 +430,7 @@ checkScalar(const Array_& a, const Array_& b, int precision) { double tol; Array_ dvA, dvB; - for (int i = 0; i < dvA.size(); ++i) { + for (size_t i = 0; i < dvA.size(); ++i) { tol = padFactor*computeRoundingError(a[i], precision); CHECK_THAT(b[i], Catch::Matchers::WithinAbs(a[i], tol)); } @@ -442,8 +443,8 @@ void checkVector(const Array_& a, const Array_& b, int precision) { double tol; - for (int i = 0; i < a.size(); ++i) { - for (int j = 0; j < a[i].size(); ++j) { + for (size_t i = 0; i < a.size(); ++i) { + for (size_t j = 0; j < a[i].size(); ++j) { tol = padFactor*computeRoundingError(a[i][j], precision); CHECK_THAT(b[i][j], Catch::Matchers::WithinAbs(a[i][j], tol)); } @@ -478,13 +479,13 @@ testEqualityForDiscreteVariables(const Model& model, Array_ dvA, dvB; model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); - for (int j = 0; j < dvA.size(); ++j) CHECK(dvB[j] == dvA[j]); + for (size_t j = 0; j < 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 (int j = 0; j < dvA.size(); ++j) CHECK(dvB[j] == dvA[j]); + for (size_t j = 0; j < dvA.size(); ++j) CHECK(dvB[j] == dvA[j]); } else if (SimTK::Value::isA(abstractVal)) { Array_ dvA, dvB; @@ -556,7 +557,7 @@ testEqualityForModelingOptions(const Model& model, Array_ moA, moB; model.getModelingOptionTrajectory(paths[i], trajA, moA); model.getModelingOptionTrajectory(paths[i], trajB, moB); - for (int j = 0; j < moA.size(); ++j) CHECK(moB[j] == moA[j]); + for (size_t j = 0; j < moA.size(); ++j) CHECK(moB[j] == moA[j]); } } @@ -689,7 +690,7 @@ TEST_CASE("Serialization and Deserialization") REQUIRE(traj.size() == traj.size()); // Realize both state trajectories to Stage::Report - for (int i = 0; i < trajA.size(); ++i) { + for (size_t i = 0; i < trajA.size(); ++i) { model->getSystem().realize(traj[i], Stage::Report); model->getSystem().realize(trajA[i], Stage::Report); } From 5c2ba5f2838056fa10bb3806fb9fad38a71c3057 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 4 Sep 2024 16:51:46 -0500 Subject: [PATCH 43/56] Preventing auto generation of a default constructor --- OpenSim/Simulation/StatesDocument.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index 0bb821ef1f..54fc76b851 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -489,6 +489,9 @@ class OSIMSIMULATION_API StatesDocument { //------------------------------------------------------------------------- // Construction //------------------------------------------------------------------------- + /** No valid default constructor. */ + StatesDocument() = delete; + /** 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 From f6662d67107b6eb1b6efefd255a4ad1d8bfb69de Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 25 Sep 2024 11:25:37 -0500 Subject: [PATCH 44/56] Update StatesDocument.h I temporarily added a default constructor to see if that would resolve some errors that are occurring in the Continuous Integration. --- OpenSim/Simulation/StatesDocument.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index 54fc76b851..1bdac7570c 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -489,8 +489,9 @@ class OSIMSIMULATION_API StatesDocument { //------------------------------------------------------------------------- // Construction //------------------------------------------------------------------------- - /** No valid default constructor. */ - StatesDocument() = delete; + /** 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, From e2a2337a450a1bee80805a3456e6ca8488cfb5e4 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 25 Sep 2024 13:06:55 -0500 Subject: [PATCH 45/56] Update testStatesDocument.cpp It looks like different return types of size() methods are assumed across operating systems. I inserted a (size_t) cast to address compiler errors on Ubuntu --- .../Simulation/Test/testStatesDocument.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 5bd3ffbad0..933375b9e4 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -77,7 +77,7 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring string nameVec5{"dvVec5"}; string nameVec6{"dvVec6"}; // Omit a discrete state altogether - int omit{-1}; + int omit; public: @@ -89,8 +89,9 @@ class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring 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) : omit(omit), - PointToPointSpring(body1, point1, body2, point2, stiffness, restlength) + int which = -1, const string& suffix = "", int omit = -1) : + PointToPointSpring(body1, point1, body2, point2, stiffness, restlength), + omit(omit) { switch (which) { case(0) : @@ -430,7 +431,7 @@ checkScalar(const Array_& a, const Array_& b, int precision) { double tol; Array_ dvA, dvB; - for (size_t i = 0; i < dvA.size(); ++i) { + 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)); } @@ -443,8 +444,8 @@ void checkVector(const Array_& a, const Array_& b, int precision) { double tol; - for (size_t i = 0; i < a.size(); ++i) { - for (size_t j = 0; j < a[i].size(); ++j) { + 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)); } @@ -479,13 +480,15 @@ testEqualityForDiscreteVariables(const Model& model, Array_ dvA, dvB; model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); - for (size_t j = 0; j < dvA.size(); ++j) CHECK(dvB[j] == dvA[j]); + 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 < dvA.size(); ++j) CHECK(dvB[j] == dvA[j]); + 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; @@ -557,7 +560,7 @@ testEqualityForModelingOptions(const Model& model, Array_ moA, moB; model.getModelingOptionTrajectory(paths[i], trajA, moA); model.getModelingOptionTrajectory(paths[i], trajB, moB); - for (size_t j = 0; j < moA.size(); ++j) CHECK(moB[j] == moA[j]); + for (size_t j = 0; j<(size_t)moA.size(); ++j) CHECK(moB[j] == moA[j]); } } @@ -690,7 +693,7 @@ TEST_CASE("Serialization and Deserialization") REQUIRE(traj.size() == traj.size()); // Realize both state trajectories to Stage::Report - for (size_t i = 0; i < trajA.size(); ++i) { + 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); } From eb534f331e73c5f54c1079f360839b4b72f431e2 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Mon, 30 Sep 2024 14:21:57 -0700 Subject: [PATCH 46/56] Add bindings for StatesDocument class --- Bindings/OpenSimHeaders_simulation.h | 1 + Bindings/simulation.i | 1 + 2 files changed, 2 insertions(+) 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) From bd764f61da1ff58967ca7e2393e80e299c39f1e7 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 12 Oct 2024 09:32:57 -0500 Subject: [PATCH 47/56] Reverted StatesTrajectory to use std::vector(SimTK::State) For reasons of performance and compatibility with Simbody, OpenSim::StatesDocument uses SimTK::Array_<>, instead of std::vector<>, as the container for states trajectories. However, some user-facing classes, like OpenSim::StatesTrajectory, have opted to use std::vector<> as the container. I migrated OpenSim::StatesTrajectory to use SimTK::Array_<>. Unfortunately, this caused errors when bindings for Python and perhaps Java and Matlab are generated. To maintain compatibility with existing code and simplify binding generation, this commit reverts to using std::vector as the container for state trajectories. In addition, several new interface methods were introduced to the StatesDocument API to allow either SimTK::Array_<> or std::vector<> to be used as the container. Internally, however, all state trajectory operations are mediated by SimTK::Array_<>. This flexibility is gained at almost no computational cost by using a shallow copy constructor to convert std::vector objects to SimTK::Array_ objects. --- OpenSim/Simulation/StatesDocument.cpp | 132 ++++++++++++++---- OpenSim/Simulation/StatesDocument.h | 46 +++++- OpenSim/Simulation/StatesTrajectory.h | 10 +- .../Simulation/StatesTrajectoryReporter.cpp | 2 +- OpenSim/Simulation/StatesTrajectoryReporter.h | 2 +- .../Simulation/Test/testStatesDocument.cpp | 8 +- 6 files changed, 160 insertions(+), 40 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 0a044da330..b8c0726839 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -136,12 +136,35 @@ struct SDocUtil { //_____________________________________________________________________________ StatesDocument:: StatesDocument(const Model& model, const Array_& trajectory, - const String& note, int p) + const String& note, int p) : + note(note), precision(clamp(1, p, SimTK::LosslessNumDigitsReal)) { - 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) : + note(note), 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 @@ -365,6 +388,9 @@ void StatesDocument:: deserialize(const Model& model, Array_& traj) { checkDocConsistencyWithModel(model); + initializeNumberOfStateObjects(); + initializePrecision(); + initializeNote(); prepareStatesTrajectory(model, traj); initializeTime(traj); initializeContinuousVariables(model, traj); @@ -374,6 +400,31 @@ deserialize(const Model& model, Array_& 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 @@ -398,26 +449,30 @@ checkDocConsistencyWithModel(const Model& model) { //_____________________________________________________________________________ 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(). +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"); - int nTime; - bool success = nTimeAttr.getValue().tryConvertTo(nTime); + bool success = nTimeAttr.getValue().tryConvertTo(nStateObjects); 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); + 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 @@ -440,17 +495,36 @@ initializeNote() { 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:: -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); +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 @@ -469,7 +543,7 @@ initializeTime(Array_& traj) { timeElts[0].getValueAs>(timeArr); // Check the size of the time array. - size_t n = traj.size(); + size_t n = timeArr.size(); SimTK_ASSERT2_ALWAYS(n == traj.size(), "Found %d time values. Should match numStateObjects = %d", n, traj.size()); diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index 1bdac7570c..87f5b18d36 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -535,6 +535,28 @@ class OSIMSIMULATION_API StatesDocument { 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 //------------------------------------------------------------------------- @@ -558,8 +580,10 @@ class OSIMSIMULATION_API StatesDocument { //------------------------------------------------------------------------- /** 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. + 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 @@ -568,6 +592,20 @@ class OSIMSIMULATION_API StatesDocument { 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, @@ -587,8 +625,11 @@ class OSIMSIMULATION_API StatesDocument { // 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); @@ -602,6 +643,7 @@ class OSIMSIMULATION_API StatesDocument { private: // Member Variables int precision{SimTK::LosslessNumDigitsReal}; + int nStateObjects{0}; SimTK::Xml::Document doc; SimTK::String filename{""}; SimTK::String note{""}; diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index 7c781cf105..133aecab9d 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -189,7 +189,7 @@ class OSIMSIMULATION_API StatesTrajectory { /** Iterator type that does not allow modifying the trajectory. * Most users do not need to understand what this is. */ - typedef SimTK::Array_::const_iterator const_iterator; + typedef std::vector::const_iterator const_iterator; /** A helper type to allow using range for loops over a subset of the * trajectory. */ @@ -303,8 +303,8 @@ class OSIMSIMULATION_API StatesTrajectory { * ``` * and then calling: * ``` - * StatesDocument::deserialize(const OpenSim::Model& model, - * SimTK::Array_& trajectory) + * StatesDocument::deserialize(const OpenSim::Model& model, + * std::vector& trajectory) * ``` * * The .ostates format is plain-text XML (see SimTK::Xml) with a @@ -336,13 +336,13 @@ class OSIMSIMULATION_API StatesTrajectory { } /** Get a read-only reference to the underlying state array. */ - const SimTK::Array_& getStateArray() const { + const std::vector& getStateArray() const { return m_states; } private: - SimTK::Array_ m_states; + std::vector m_states; public: diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.cpp b/OpenSim/Simulation/StatesTrajectoryReporter.cpp index 6e8e9d0bfc..4e1bdeb982 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.cpp +++ b/OpenSim/Simulation/StatesTrajectoryReporter.cpp @@ -34,7 +34,7 @@ const StatesTrajectory& StatesTrajectoryReporter::getStates() const { return m_states; } -const SimTK::Array_& +const std::vector& StatesTrajectoryReporter::getStateArray() const { return m_states.getStateArray(); } diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.h b/OpenSim/Simulation/StatesTrajectoryReporter.h index 2d61b7209b..d4eb15b380 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.h +++ b/OpenSim/Simulation/StatesTrajectoryReporter.h @@ -44,7 +44,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(StatesTrajectoryReporter, AbstractReporter); /** 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; + const std::vector& getStateArray() const; /** Clear the accumulated states. */ void clear(); diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 933375b9e4..19a78a882f 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -36,6 +36,7 @@ using namespace OpenSim; using std::cout; using std::endl; using std::string; +using std::vector; // Internal static methods and classes. @@ -654,8 +655,11 @@ simulate(Model* model) { manager.initialize(state); state = manager.integrate(tf); - // Return a copy of the underlying state array - return reporter->getStateArray(); + // Return a copy of the underlying state array, after repackaging it + // as a SimTK::Array_. + const vector& trajectory = reporter->getStateArray(); + const Array_ traj(trajectory); + return traj; } } // End anonymous namespace From a09c2b3dd2088e68703169b953cc4874b35b6af6 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 12 Oct 2024 10:07:38 -0500 Subject: [PATCH 48/56] Update StatesDocument.cpp Now settings for `note` and `precision` are updated in the body of the constructor instead of in the initializer list. --- OpenSim/Simulation/StatesDocument.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index b8c0726839..7aeec4b257 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -136,17 +136,20 @@ struct SDocUtil { //_____________________________________________________________________________ StatesDocument:: StatesDocument(const Model& model, const Array_& trajectory, - const String& note, int p) : - note(note), precision(clamp(1, p, SimTK::LosslessNumDigitsReal)) + 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) : - note(note), precision(clamp(1, p, SimTK::LosslessNumDigitsReal)) + 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. From 998e5ad7cf84a2c860592ac7e03d362a1e2f6ec4 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 13 Oct 2024 13:19:28 -0500 Subject: [PATCH 49/56] Restored commented-out code in testStatesTrajectory.cpp --- OpenSim/Simulation/Test/testStatesTrajectory.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index 45e459879c..aa150dc55a 100644 --- a/OpenSim/Simulation/Test/testStatesTrajectory.cpp +++ b/OpenSim/Simulation/Test/testStatesTrajectory.cpp @@ -571,12 +571,12 @@ void testBoundsCheck() { 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 + #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 From 4206e24f0c744c96415895b8a2ec8d864f2ad746 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 14 Oct 2024 15:12:18 -0500 Subject: [PATCH 50/56] typo corrected --- OpenSim/Simulation/StatesDocument.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 7aeec4b257..8111ab60f2 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -431,7 +431,7 @@ 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. + // 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(); From 515d3331581efd4c0ac4826a2e2c703a737202bc Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 14 Oct 2024 15:14:28 -0500 Subject: [PATCH 51/56] Update StatesDocument.h Updated the documentation to reflect the fact that std::vector is also an acceptable form of the state trajectory for interfacing with OpenSim. --- OpenSim/Simulation/StatesDocument.h | 31 +++++++++++++++++++---------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h index 87f5b18d36..2032212799 100644 --- a/OpenSim/Simulation/StatesDocument.h +++ b/OpenSim/Simulation/StatesDocument.h @@ -129,8 +129,9 @@ 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). +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 @@ -146,13 +147,21 @@ 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. +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. -This class relies heavily on a few trjectory-centric methods available in -the OpenSim::Component class. A few examples follow. +@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 @@ -194,7 +203,7 @@ 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. +unnecessary. ### Output Precision The precision with which numbers are serialized to a `.ostates` file can be @@ -311,7 +320,7 @@ deserialize those same states and use them to accomplish a few basic things. // ------------------------------- // Add a StatesTrajectory Reporter // ------------------------------- - // The reporter records the SimTK::State in a SimTK::Array_<> at a + // The reporter records the SimTK::State in an std::vector<> at a // specified time interval. OpenSim::StatesTrajectoryReporter* reporter = new StatesTrajectoryReporter(); @@ -343,7 +352,7 @@ deserialize those same states and use them to accomplish a few basic things. // ----------------------- // 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_). + // 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(); @@ -352,7 +361,7 @@ deserialize those same states and use them to accomplish a few basic things. // 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(); + const std::vector& traj = reporter->getVectorOfStateObjects(); StatesDocument doc(model, traj); // ---------------------------- @@ -398,7 +407,7 @@ deserialize those same states and use them to accomplish a few basic things. // 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; + SimTK::Array_ traj; // Or, std::vector traj; doc.deserialize(model, traj); // Below are some things that can be done once a deserialized state @@ -436,7 +445,7 @@ deserialize those same states and use them to accomplish a few basic things. // 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(); + Vec3 force = forces.getContactElement(0)->getForce(); } // ---------------------------------------------------- From 32009c2717998b2dadbc9b9ea00e07185ed6d8cb Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 14 Oct 2024 15:15:25 -0500 Subject: [PATCH 52/56] Update StatesTrajectoryReporter.cpp Migrated StatesTrajectoryReporter::getStateArray() to getVectorOfStateObjects() --- OpenSim/Simulation/StatesTrajectoryReporter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.cpp b/OpenSim/Simulation/StatesTrajectoryReporter.cpp index 4e1bdeb982..e738544231 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.cpp +++ b/OpenSim/Simulation/StatesTrajectoryReporter.cpp @@ -35,7 +35,7 @@ const StatesTrajectory& StatesTrajectoryReporter::getStates() const { } const std::vector& -StatesTrajectoryReporter::getStateArray() const { +StatesTrajectoryReporter::getVectorOfStateObjects() const { return m_states.getStateArray(); } From fa570c9280d339e8a63f591d3a80737b0b5b4030 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 14 Oct 2024 15:16:04 -0500 Subject: [PATCH 53/56] Update StatesTrajectoryReporter.h Migrated StatesTrajectoryReporter::getStateArray() to getVectorOfStateObjects(). --- OpenSim/Simulation/StatesTrajectoryReporter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.h b/OpenSim/Simulation/StatesTrajectoryReporter.h index d4eb15b380..a17e750413 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.h +++ b/OpenSim/Simulation/StatesTrajectoryReporter.h @@ -44,7 +44,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(StatesTrajectoryReporter, AbstractReporter); /** 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& getStateArray() const; + const std::vector& getVectorOfStateObjects() const; /** Clear the accumulated states. */ void clear(); From d093cb170fbcdba29b0d2a1e6a89dd154c5a380d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 14 Oct 2024 18:50:01 -0500 Subject: [PATCH 54/56] Update StatesDocument.cpp I added comments for developers should a demand additional supported variable types emerge. --- OpenSim/Simulation/StatesDocument.cpp | 44 +++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp index 8111ab60f2..c709976f81 100644 --- a/OpenSim/Simulation/StatesDocument.cpp +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -247,6 +247,14 @@ formTimeElement(const Model& model, const Array_& traj) { 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) { @@ -269,6 +277,12 @@ formContinuousElement(const Model& model, const Array_& traj) { } } //_____________________________________________________________________________ +// 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) { @@ -360,6 +374,14 @@ formDiscreteElement(const Model& model, const Array_& traj) { } //_____________________________________________________________________________ +// 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) { @@ -555,6 +577,14 @@ initializeTime(Array_& traj) { 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) { @@ -602,6 +632,12 @@ initializeContinuousVariables(const Model& model, SimTK::Array_& traj) { } } //_____________________________________________________________________________ +// 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) { @@ -677,6 +713,14 @@ initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { } } //_____________________________________________________________________________ +// 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) { From aba49bc632cef902ddf50fc0f4ff78fd83b2bfe2 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 14 Oct 2024 18:55:31 -0500 Subject: [PATCH 55/56] Update testStatesDocument.cpp Corrected a compile error. --- OpenSim/Simulation/Test/testStatesDocument.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp index 19a78a882f..698efccde3 100644 --- a/OpenSim/Simulation/Test/testStatesDocument.cpp +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -657,7 +657,7 @@ simulate(Model* model) { // Return a copy of the underlying state array, after repackaging it // as a SimTK::Array_. - const vector& trajectory = reporter->getStateArray(); + const vector& trajectory = reporter->getVectorOfStateObjects(); const Array_ traj(trajectory); return traj; } From 8501fdd9706802568b93e621c069f4f61e25d713 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 17 Oct 2024 20:54:42 -0500 Subject: [PATCH 56/56] Updated CHANGELOG.md to include the new StatesDocument class (#3902) --- CHANGELOG.md | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) 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 ======