diff --git a/Applications/opensim-cmd/opensim-cmd_viz.h b/Applications/opensim-cmd/opensim-cmd_viz.h index e87e2dbabc..6102172daa 100644 --- a/Applications/opensim-cmd/opensim-cmd_viz.h +++ b/Applications/opensim-cmd/opensim-cmd_viz.h @@ -106,7 +106,7 @@ int viz(int argc, const char** argv) { if (args["model"].asBool()) { Model model(args[""].asString()); if (args[""]) { - Storage states(args[""].asString()); + TimeSeriesTable states(args[""].asString()); VisualizerUtilities::showMotion(model, states); } else { VisualizerUtilities::showModel(model); diff --git a/Bindings/OpenSimHeaders_common.h b/Bindings/OpenSimHeaders_common.h index c80b8d47bf..a44709c3c6 100644 --- a/Bindings/OpenSimHeaders_common.h +++ b/Bindings/OpenSimHeaders_common.h @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include diff --git a/Bindings/Python/examples/posthoc_StatesTrajectory_example.py b/Bindings/Python/examples/posthoc_StatesTrajectory_example.py index 73251242d0..b631d44fd4 100644 --- a/Bindings/Python/examples/posthoc_StatesTrajectory_example.py +++ b/Bindings/Python/examples/posthoc_StatesTrajectory_example.py @@ -61,9 +61,7 @@ # Analyze the simulation. # ----------------------- -# Retrieve the StatesTrajectory from the reporter. Alternately, we could load a -# StatesTrajectory from a STO file using -# StatesTrajectory.createFromStatesStorage(). +# Retrieve the StatesTrajectory from the reporter. statesTraj = reporter.getStates() for itime in range(statesTraj.getSize()): @@ -84,3 +82,14 @@ # abstractOutput = joint.getOutput('reaction_on_parent') # output = osim.OutputSpatialVec.safeDownCast(abstractOutput) # outputValue = output.getValue(s) + +# Alternately, we could load a StatesTrajectory from a TimeSeriesTable file +# using StatesTrajectory.createFromStatesTable(). +statesTable = manager.getStatesTable() +statesTraj2 = osim.StatesTrajectory.createFromStatesTable(model, statesTable) + +for itime in range(statesTraj2.getSize()): + state = statesTraj2[itime] + time = state.getTime() + u = model.getStateVariableValue(state, 'joint/q/speed') + print('time: %f s. u: %f rad/s.' % (time, u)) diff --git a/Bindings/common.i b/Bindings/common.i index 480249253d..dbe440e3a8 100644 --- a/Bindings/common.i +++ b/Bindings/common.i @@ -337,6 +337,7 @@ DATATABLE_CLONE(double, SimTK::Rotation_) %include %include %include +%include %template(DataTable) OpenSim::DataTable_; %template(DataTableVec3) OpenSim::DataTable_; diff --git a/CHANGELOG.md b/CHANGELOG.md index 615cfbbe48..b2070ca16d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,9 @@ v4.2 - Fix a segfault that occurs when using OpenSim's Python Package with Anaconda's Python on a Mac. - Expose PropertyHelper class to python bindings to allow editing of objects using the properties interface (useful for editing objects defined in plugins) in python (consistent with Java/Matlab). - Whitespace is trimmed when reading table metadata for STO, MOT, and CSV files. +- Introduce utilities for creating SimTK::Vectors, linear interpolation, updating table column labels from v3.3 to v4.0 syntax, solving for a function's root using bisection (OpenSim/Common/CommonUtilities.h) ([PR #2808](https://github.com/opensim-org/opensim-core/pull/2808)). +- Introduce utilities for querying, filtering, and resampling TimeSeriesTables (OpenSim/Common/TableUtilities.h) ([PR #2808](https://github.com/opensim-org/opensim-core/pull/2808)). +- StatesTrajectories can now be created from a TimeSeriesTable of states. - Minor performance improvements (5-10 %) for controller-heavy models (PR #2806) - `Controller::isEnabled` will now only return whether the particular controller is enabled - Previously, it would return `false` if its parent `Model`'s `Model::getAllControllersEnabled` returned `false` diff --git a/OpenSim/Common/CommonUtilities.cpp b/OpenSim/Common/CommonUtilities.cpp index 41006e6268..4bdc4adf03 100644 --- a/OpenSim/Common/CommonUtilities.cpp +++ b/OpenSim/Common/CommonUtilities.cpp @@ -23,12 +23,17 @@ #include "CommonUtilities.h" +#include "PiecewiseLinearFunction.h" +#include "STOFileAdapter.h" +#include "TimeSeriesTable.h" #include #include #include #include #include +#include + std::string OpenSim::getFormattedDateTime( bool appendMicroseconds, std::string format) { using namespace std::chrono; @@ -63,3 +68,118 @@ std::string OpenSim::getFormattedDateTime( } return ss.str(); } + +SimTK::Vector OpenSim::createVectorLinspace( + int length, double start, double end) { + SimTK::Vector v(length); + for (int i = 0; i < length; ++i) { + v[i] = start + i * (end - start) / (length - 1); + } + return v; +} + +SimTK::Vector OpenSim::createVector( + std::initializer_list elements) { + return SimTK::Vector((int)elements.size(), elements.begin()); +} + +SimTK::Vector OpenSim::interpolate(const SimTK::Vector& x, + const SimTK::Vector& y, const SimTK::Vector& newX, + const bool ignoreNaNs) { + + OPENSIM_THROW_IF(x.size() != y.size(), Exception, + "Expected size of x to equal size of y, but size of x " + "is {} and size of y is {}.", + x.size(), y.size()); + + // Create vectors of non-NaN values if user set 'ignoreNaNs' argument to + // 'true', otherwise throw an exception. If no NaN's are present in the + // provided data vectors, the '*_no_nans' variables below will contain + // the original data vector values. + std::vector x_no_nans; + std::vector y_no_nans; + for (int i = 0; i < x.size(); ++i) { + + bool shouldNotPushBack = + (SimTK::isNaN(x[i]) || SimTK::isNaN(y[i])) && ignoreNaNs; + if (!shouldNotPushBack) { + x_no_nans.push_back(x[i]); + y_no_nans.push_back(y[i]); + } + } + + OPENSIM_THROW_IF(x_no_nans.empty(), Exception, + "Input vectors are empty (perhaps after removing NaNs)."); + + PiecewiseLinearFunction function( + (int)x_no_nans.size(), &x_no_nans[0], &y_no_nans[0]); + SimTK::Vector newY(newX.size(), SimTK::NaN); + for (int i = 0; i < newX.size(); ++i) { + const auto& newXi = newX[i]; + if (x_no_nans[0] <= newXi && newXi <= x_no_nans[x_no_nans.size() - 1]) + newY[i] = function.calcValue(SimTK::Vector(1, newXi)); + } + return newY; +} + +std::string OpenSim::convertRelativeFilePathToAbsoluteFromXMLDocument( + const std::string& documentFileName, + const std::string& filePathRelativeToDocument) { + // Get the directory containing the XML file. + std::string directory; + bool dontApplySearchPath; + std::string fileName, extension; + SimTK::Pathname::deconstructPathname(documentFileName, dontApplySearchPath, + directory, fileName, extension); + return SimTK::Pathname::getAbsolutePathnameUsingSpecifiedWorkingDirectory( + directory, filePathRelativeToDocument); +} + +SimTK::Real OpenSim::solveBisection( + std::function calcResidual, + SimTK::Real left, SimTK::Real right, const SimTK::Real& tolerance, + int maxIterations) { + SimTK::Real midpoint = left; + + OPENSIM_THROW_IF(maxIterations < 0, Exception, + "Expected maxIterations to be positive, but got {}.", + maxIterations); + + const bool sameSign = calcResidual(left) * calcResidual(right) >= 0; + if (sameSign && Logger::shouldLog(Logger::Level::Debug)) { + const int numRows = 1000; + const auto x = createVectorLinspace(numRows, left, right); + SimTK::Matrix residual(numRows, 1); + for (int i = 0; i < numRows; ++i) { + residual(i, 0) = calcResidual(x[i]); + } + TimeSeriesTable table(std::vector(x.getContiguousScalarData(), + x.getContiguousScalarData() + x.size()), + residual, {"residual"}); + STOFileAdapter::write( + table, fmt::format("solveBisection_residual_{}.sto", + getFormattedDateTime())); + } + OPENSIM_THROW_IF(sameSign, Exception, + "Function has same sign at bounds of {} and {}.", left, right); + + SimTK::Real residualMidpoint; + SimTK::Real residualLeft = calcResidual(left); + int iterCount = 0; + while (iterCount < maxIterations && (right - left) > tolerance) { + midpoint = 0.5 * (left + right); + residualMidpoint = calcResidual(midpoint); + if (residualMidpoint * residualLeft < 0) { + // The solution is to the left of the current midpoint. + right = midpoint; + } else { + left = midpoint; + residualLeft = calcResidual(left); + } + ++iterCount; + } + if (iterCount == maxIterations) { + log_warn("Bisection reached max iterations at x = {}.", midpoint); + } + return midpoint; +} diff --git a/OpenSim/Common/CommonUtilities.h b/OpenSim/Common/CommonUtilities.h index 9f0446f21c..cac317f396 100644 --- a/OpenSim/Common/CommonUtilities.h +++ b/OpenSim/Common/CommonUtilities.h @@ -24,10 +24,21 @@ * -------------------------------------------------------------------------- */ #include "osimCommonDLL.h" +#include #include +#include + +#include namespace OpenSim { +/// Since OpenSim does not require C++14 (which contains std::make_unique()), +/// here is an implementation of make_unique(). +template +std::unique_ptr make_unique(Args&&... args) { + return std::unique_ptr(new T(std::forward(args)...)); +} + /// Get a string with the current date and time formatted as %Y-%m-%dT%H%M%S /// (year, month, day, "T", hour, minute, second). You can change the datetime /// format via the `format` parameter. @@ -57,6 +68,51 @@ class OSIMCOMMON_API FileRemover { std::string m_filepath; }; +/// Create a SimTK::Vector with the provided length whose elements are +/// uniformly spaced between start and end (same as Matlab's linspace()). +OSIMCOMMON_API +SimTK::Vector createVectorLinspace(int length, double start, double end); + +#ifndef SWIG +/// Create a SimTK::Vector using modern C++ syntax. +OSIMCOMMON_API +SimTK::Vector createVector(std::initializer_list elements); +#endif + +/// Linearly interpolate y(x) at new values of x. The optional 'ignoreNaNs' +/// argument will ignore any NaN values contained in the input vectors and +/// create the interpolant from the non-NaN values only. Note that this option +/// does not necessarily prevent NaN values from being returned in 'newX', which +/// will have NaN for any values of newX outside of the range of x. +/// @throws Exception if x and y are different sizes, or x or y is empty. +OSIMCOMMON_API +SimTK::Vector interpolate(const SimTK::Vector& x, const SimTK::Vector& y, + const SimTK::Vector& newX, const bool ignoreNaNs = false); + +/// An OpenSim XML file may contain file paths that are relative to the +/// directory containing the XML file; use this function to convert that +/// relative path into an absolute path. +OSIMCOMMON_API +std::string convertRelativeFilePathToAbsoluteFromXMLDocument( + const std::string& documentFileName, + const std::string& filePathRelativeToDirectoryContainingDocument); + +/// Solve for the root of a scalar function using the bisection method. +/// If the values of calcResidual(left) and calcResidual(right) have the same +/// sign and the logger level is Debug (or more verbose), then this function +/// writes a file `solveBisection_residual_.sto` containing the +/// residual function. +/// @param calcResidual a function that computes the error +/// @param left lower bound on the root +/// @param right upper bound on the root +/// @param tolerance convergence requires that the bisection's "left" and +/// "right" are less than tolerance apart. +/// @param maxIterations abort after this many iterations. +OSIMCOMMON_API +SimTK::Real solveBisection(std::function calcResidual, + double left, double right, const double& tolerance = 1e-6, + int maxIterations = 1000); + } // namespace OpenSim #endif // OPENSIM_COMMONUTILITIES_H_ diff --git a/OpenSim/Common/Exception.h b/OpenSim/Common/Exception.h index 17d06c3b68..1241247092 100644 --- a/OpenSim/Common/Exception.h +++ b/OpenSim/Common/Exception.h @@ -317,6 +317,11 @@ class ComponentNotFound : public Exception { } }; +class NonUniqueLabels : public OpenSim::Exception { +public: + using Exception::Exception; +}; + }; //namespace //============================================================================= //============================================================================= diff --git a/OpenSim/Common/IO.cpp b/OpenSim/Common/IO.cpp index a140fb7f4e..48350da8e2 100644 --- a/OpenSim/Common/IO.cpp +++ b/OpenSim/Common/IO.cpp @@ -653,6 +653,33 @@ Uppercase(const std::string &aStr) return result; } +bool IO::StartsWith(const std::string& string, const std::string& start) { + // https://stackoverflow.com/questions/874134/find-if-string-ends-with-another-string-in-c + if (string.length() >= start.length()) { + return string.compare(0, start.length(), start) == 0; + } + return false; +} + +bool IO::EndsWith(const std::string& string, const std::string& ending) { + // https://stackoverflow.com/questions/874134/find-if-string-ends-with-another-string-in-c + if (string.length() >= ending.length()) { + return string.compare(string.length() - ending.length(), + ending.length(), ending) == 0; + } + return false; +} + +bool IO::StartsWithIgnoringCase( + const std::string& string, const std::string& start) { + return StartsWith(IO::Lowercase(string), IO::Lowercase(start)); +} + +bool IO::EndsWithIgnoringCase( + const std::string& string, const std::string& ending) { + return EndsWith(IO::Lowercase(string), IO::Lowercase(ending)); +} + void IO::eraseEmptyElements(std::vector& list) { std::vector::iterator it = list.begin(); diff --git a/OpenSim/Common/IO.h b/OpenSim/Common/IO.h index dbc3992fc3..e9686b115c 100644 --- a/OpenSim/Common/IO.h +++ b/OpenSim/Common/IO.h @@ -120,6 +120,18 @@ class OSIMCOMMON_API IO { static void TrimWhitespace(std::string &rStr) { TrimLeadingWhitespace(rStr); TrimTrailingWhitespace(rStr); } static std::string Lowercase(const std::string &aStr); static std::string Uppercase(const std::string &aStr); + /// Determine if `string` starts with the substring `start`. + static bool StartsWith(const std::string& string, const std::string& start); + /// Determine if `string` ends with the substring `ending`. + static bool EndsWith(const std::string& string, const std::string& ending); + /// Same as StartsWith() except both arguments are first converted to + /// lowercase before performing the check. + static bool StartsWithIgnoringCase( + const std::string& string, const std::string& start); + /// Same as EndsWith() except both arguments are first converted to + /// lowercase before performing the check. + static bool EndsWithIgnoringCase( + const std::string& string, const std::string& ending); static void eraseEmptyElements(std::vector& list); //============================================================================= }; // END CLASS IO diff --git a/OpenSim/Common/PiecewiseLinearFunction.cpp b/OpenSim/Common/PiecewiseLinearFunction.cpp index 502fea46db..1f293bf722 100644 --- a/OpenSim/Common/PiecewiseLinearFunction.cpp +++ b/OpenSim/Common/PiecewiseLinearFunction.cpp @@ -75,19 +75,20 @@ PiecewiseLinearFunction::PiecewiseLinearFunction(int aN,const double *aX,const d setName(aName); // NUMBER OF DATA POINTS - if(aN < 2) - { - log_error("PiecewiseLinearFunction: there must be 2 or more data " - "points."); - return; - } + OPENSIM_THROW_IF_FRMOBJ(aN < 2, Exception, + "PiecewiseLinearFunction: there must be 2 or more data " + "points, but got {} data points.", + aN); // CHECK DATA - if((aX==NULL)||(aY==NULL)) - { - log_error("PiecewiseLinearFunction: NULL arrays for data points " - "encountered."); - return; + OPENSIM_THROW_IF_FRMOBJ(aX == nullptr || aY == nullptr, Exception, + "x and/or y data is null."); + + for (int i = 1; i < aN; ++i) { + OPENSIM_THROW_IF_FRMOBJ(aX[i] < aX[i - 1], Exception, + "Expected independent variable to be non-decreasing, but x[{}] " + "= {} is less than x[{}] = {}", + i, aX[i], i - 1, aX[i - 1]); } // INDEPENDENT VALUES (KNOT SEQUENCE) diff --git a/OpenSim/Common/Signal.cpp b/OpenSim/Common/Signal.cpp index 3790c6ba76..592a4a28e3 100644 --- a/OpenSim/Common/Signal.cpp +++ b/OpenSim/Common/Signal.cpp @@ -158,7 +158,7 @@ SmoothSpline(int degree,double T,double fc,int N,double *times,double *sig,doubl * @return 0 on success, and -1 on failure. */ int Signal:: -LowpassIIR(double T,double fc,int N,double *sig,double *sigf) +LowpassIIR(double T,double fc,int N,const double *sig,double *sigf) { int i,j; double fs/*,ws*/,wc,wa,wa2,wa3; @@ -275,8 +275,7 @@ LowpassFIR(int M,double T,double f,int N,double *sig,double *sigf) } // PAD THE SIGNAL SO FILTERING CAN BEGIN AT THE FIRST DATA POINT - double *s = Pad(M,N,sig); - if(s==NULL) return(-1); + std::vector s = Pad(M,N,sig); // CALCULATE THE ANGULAR CUTOFF FREQUENCY w = 2.0*SimTK_PI*f; @@ -313,10 +312,7 @@ LowpassFIR(int M,double T,double f,int N,double *sig,double *sigf) // sigf[n] = sigf[n] / sum_coef; //} - // CLEANUP - delete[] s; - - return(0); + return 0; } @@ -398,42 +394,23 @@ double *s; return(0); } -//_____________________________________________________________________________ -/** - * Pad a signal with a specified number of data points. - * - * The signal is prepended and appended with a reflected and negated - * portion of the signal of the appropriate size so as to preserve the value - * and slope of the signal. - * - * PARAMETERS - * @param aPad Size of the pad-- number of points to prepend and append. - * @param aN Number of data points in the signal. - * @param aSignal Signal to be padded. - * @return Padded signal. The size is aN + 2*aPad. NULL is returned - * on an error. The caller is responsible for deleting the returned - * array. - */ -double* Signal:: +std::vector Signal:: Pad(int aPad,int aN,const double aSignal[]) { - if(aPad<=0) return(NULL); + if (aPad == 0) return std::vector(aSignal, aSignal + aN); + OPENSIM_THROW_IF(aPad < 0, Exception, + "Expected aPad to be non-negative, but got {}.", + aPad); // COMPUTE FINAL SIZE int size = aN + 2*aPad; - if(aPad>aN) { - log_error("Signal.Pad: requested pad size ({}) is greater than the " - "number of points ({}).", - aPad, aN); - return(NULL); - } + OPENSIM_THROW_IF(aPad > aN, Exception, + "Signal.Pad: requested pad size ({}) is greater than the " + "number of points ({}).", + aPad, aN); // ALLOCATE - double *s = new double[size]; - if (s == NULL) { - log_error("Signal.Pad: Failed to allocate memory."); - return(NULL); - } + std::vector s(size); // PREPEND int i,j; @@ -447,7 +424,7 @@ Pad(int aPad,int aN,const double aSignal[]) for(i=aPad+aN,j=aN-2;i &rSignal) { - if(aPad<=0) return; - - // COMPUTE NEW SIZE - int size = rSignal.getSize(); - int newSize = size + 2*aPad; - - // HANDLE PAD GREATER THAN SIZE - if(aPad>=size) { - int pad = size - 1; - while(size=size) pad = size - 1; - } - return; - } - - // ALLOCATE - Array s(0.0,newSize); - - // PREPEND - int i,j; - for(i=0,j=aPad;i padded = Pad(aPad, rSignal.getSize(), rSignal.get()); + rSignal.setSize((int)padded.size()); + std::copy_n(padded.data(), padded.size(), rSignal.get()); } diff --git a/OpenSim/Common/Signal.h b/OpenSim/Common/Signal.h index d5a947c53a..a25c224cea 100644 --- a/OpenSim/Common/Signal.h +++ b/OpenSim/Common/Signal.h @@ -27,9 +27,8 @@ * Author: Frank C. Anderson */ - #include "osimCommonDLL.h" - +#include namespace OpenSim { @@ -64,7 +63,7 @@ class OSIMCOMMON_API Signal int aN,double *aTimes,double *aSignal,double *rFilteredSignal); static int LowpassIIR(double aDeltaT,double aCutOffFrequency, - int aN,double *aSignal,double *rFilteredSignal); + int aN,const double *aSignal,double *rFilteredSignal); static int LowpassFIR(int aOrder,double aDeltaT,double aCutoffFrequency, int aN,double *aSignal,double *rFilteredSignal); @@ -76,10 +75,18 @@ class OSIMCOMMON_API Signal //-------------------------------------------------------------------------- // PADDING //-------------------------------------------------------------------------- - static double* - Pad(int aPad,int aN,const double aSignal[]); - static void - Pad(int aPad,OpenSim::Array &aSignal); + /// Pad a signal with a specified number of data points. + /// + /// The signal is prepended and appended with a reflected and negated + /// portion of the signal of the appropriate size so as to preserve the + /// value and slope of the signal. + /// + /// @param aPad Size of the pad-- number of points to prepend and append. + /// @param aN Number of data points in the signal. + /// @param aSignal Signal to be padded. + /// @return Padded signal. The size is aN + 2*aPad. + static std::vector Pad(int aPad, int aN, const double aSignal[]); + static void Pad(int aPad, OpenSim::Array& aSignal); //-------------------------------------------------------------------------- // POINT REDUCTION diff --git a/OpenSim/Common/Storage.cpp b/OpenSim/Common/Storage.cpp index 3e493d4282..ca6b929d4d 100644 --- a/OpenSim/Common/Storage.cpp +++ b/OpenSim/Common/Storage.cpp @@ -29,6 +29,7 @@ // INCLUDES #include "Storage.h" +#include "CommonUtilities.h" #include "GCVSpline.h" #include "GCVSplineSet.h" #include "IO.h" @@ -38,6 +39,7 @@ #include "SimTKcommon.h" #include "SimmMacros.h" #include "StateVector.h" +#include "TableUtilities.h" #include "TimeSeriesTable.h" #include @@ -522,71 +524,11 @@ getHeaderToken() const int Storage:: getStateIndex(const std::string &aColumnName, int startIndex) const { - int thisColumnIndex = -1; - - // This uses the `do while(false)` idiom to run common code if one of a - // number of conditions succeeds (much like what a goto would be used for). - do { - thisColumnIndex = _columnLabels.findIndex(aColumnName); - if (thisColumnIndex != -1) break; - - // 4.0 and its beta versions differ slightly in the absolute path but - // the //value (or speed) will be common to both. - // Likewise, for muscle states /activation (or fiber_length) - // must be common to the state variable (path) name and column label. - std::string shortPath = aColumnName; - std::string::size_type front = shortPath.find("/"); - while (thisColumnIndex < 0 && front < std::string::npos) { - shortPath = shortPath.substr(front + 1, aColumnName.length()); - thisColumnIndex = _columnLabels.findIndex(shortPath); - front = shortPath.find("/"); - } - if (thisColumnIndex != -1) break; - - // Assume column labels follow pre-v4.0 state variable labeling. - // Redo search with what the pre-v4.0 label might have been. - - // First, try just the last element of the path. - std::string::size_type back = aColumnName.rfind("/"); - std::string prefix = aColumnName.substr(0, back); - std::string shortName = aColumnName.substr(back + 1, - aColumnName.length() - back); - thisColumnIndex = _columnLabels.findIndex(shortName); - if (thisColumnIndex != -1) break; - - // If that didn't work, specifically check for coordinate state names - // (/value and /speed) and muscle state names - // (/activation /fiber_length). - if (shortName == "value") { - // pre-v4.0 did not have "/value" so remove it if here - back = prefix.rfind("/"); - shortName = prefix.substr(back + 1, prefix.length()); - thisColumnIndex = _columnLabels.findIndex(shortName); - } - else if (shortName == "speed") { - // replace "/speed" (the v4.0 labeling for speeds) with "_u" - back = prefix.rfind("/"); - shortName = - prefix.substr(back + 1, prefix.length() - back) + "_u"; - thisColumnIndex = _columnLabels.findIndex(shortName); - } - else if (back < aColumnName.length()) { - // try replacing the '/' with '.' in the last segment - shortName = aColumnName; - shortName.replace(back, 1, "."); - back = shortName.rfind("/"); - shortName = shortName.substr(back + 1, - shortName.length() - back); - thisColumnIndex = _columnLabels.findIndex(shortName); - } - if (thisColumnIndex != -1) break; - - // If all of the above checks failed, return -1. + int thisColumnIndex = + TableUtilities::findStateLabelIndex(_columnLabels, aColumnName); + if (thisColumnIndex == -1) { return -1; - - } while (false); - - // If we get here, we successfully found a column. + } // Subtract 1 because time is included in the labels but not // in the "state vector". return thisColumnIndex - 1; @@ -1399,8 +1341,10 @@ TimeSeriesTable Storage::exportToTable() const { // Exclude the first column label. It is 'time'. Time is a separate column // in TimeSeriesTable and column label is optional. - table.setColumnLabels(_columnLabels.get() + 1, - _columnLabels.get() + _columnLabels.getSize()); + if (_columnLabels.size() > 1) { + table.setColumnLabels(_columnLabels.get() + 1, + _columnLabels.get() + _columnLabels.getSize()); + } for(int i = 0; i < _storage.getSize(); ++i) { const auto& row = getStateVector(i)->getData(); diff --git a/OpenSim/Common/TableUtilities.cpp b/OpenSim/Common/TableUtilities.cpp new file mode 100644 index 0000000000..a0466c66c6 --- /dev/null +++ b/OpenSim/Common/TableUtilities.cpp @@ -0,0 +1,318 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: TableUtilities.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-2020 Stanford University and the Authors * + * Author(s): Christopher Dembia * + * * + * 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 "TableUtilities.h" + +#include "CommonUtilities.h" +#include "FunctionSet.h" +#include "GCVSplineSet.h" +#include "PiecewiseLinearFunction.h" +#include "Signal.h" +#include "Storage.h" + +using namespace OpenSim; + +void TableUtilities::checkNonUniqueLabels(std::vector labels) { + std::sort(labels.begin(), labels.end()); + auto it = std::adjacent_find(labels.begin(), labels.end()); + OPENSIM_THROW_IF(it != labels.end(), NonUniqueLabels, + "Label '{}' appears more than once.", *it); +} + +bool TableUtilities::isInDegrees(const TimeSeriesTable& table) { + OPENSIM_THROW_IF(!table.hasTableMetaDataKey("inDegrees"), Exception, + "Table does not have 'inDegrees' metadata."); + const std::string inDegrees = + table.getTableMetaData("inDegrees"); + OPENSIM_THROW_IF(inDegrees != "yes" && inDegrees != "no", Exception, + "Expected table's 'inDegrees' metadata to be 'yes' or 'no', " + "but got '{}'.", + inDegrees); + return inDegrees == "yes"; +} + +int TableUtilities::findStateLabelIndex( + const Array& labels, const std::string& desired) { + return findStateLabelIndexInternal( + labels.get(), labels.get() + labels.getSize(), desired); +} + +int TableUtilities::findStateLabelIndex( + const std::vector& labels, const std::string& desired) { + return findStateLabelIndexInternal( + labels.data(), labels.data() + labels.size(), desired); +} + +int TableUtilities::findStateLabelIndexInternal(const std::string* begin, + const std::string* end, const std::string& desired) { + + auto found = std::find(begin, end, desired); + if (found != end) return (int)std::distance(begin, found); + + // 4.0 and its beta versions differ slightly in the absolute path but + // the //value (or speed) will be common to both. + // Likewise, for muscle states /activation (or fiber_length) + // must be common to the state variable (path) name and column label. + std::string shortPath = desired; + std::string::size_type front = shortPath.find('/'); + while (found == end && front < std::string::npos) { + shortPath = shortPath.substr(front + 1, desired.length()); + found = std::find(begin, end, shortPath); + front = shortPath.find('/'); + } + if (found != end) return (int)std::distance(begin, found); + + // Assume column labels follow pre-v4.0 state variable labeling. + // Redo search with what the pre-v4.0 label might have been. + + // First, try just the last element of the path. + std::string::size_type back = desired.rfind('/'); + std::string prefix = desired.substr(0, back); + std::string shortName = desired.substr(back + 1, desired.length() - back); + found = std::find(begin, end, shortName); + if (found != end) return (int)std::distance(begin, found); + + // If that didn't work, specifically check for coordinate state names + // (/value and /speed) and muscle state names + // (/activation /fiber_length). + if (shortName == "value") { + // pre-v4.0 did not have "/value" so remove it if here + back = prefix.rfind('/'); + shortName = prefix.substr(back + 1, prefix.length()); + found = std::find(begin, end, shortName); + } else if (shortName == "speed") { + // replace "/speed" (the v4.0 labeling for speeds) with "_u" + back = prefix.rfind('/'); + shortName = prefix.substr(back + 1, prefix.length() - back) + "_u"; + found = std::find(begin, end, shortName); + } else if (back < desired.length()) { + // try replacing the '/' with '.' in the last segment + shortName = desired; + shortName.replace(back, 1, "."); + back = shortName.rfind('/'); + shortName = shortName.substr(back + 1, shortName.length() - back); + found = std::find(begin, end, shortName); + } + if (found != end) return (int)std::distance(begin, found); + + // If all of the above checks failed, return -1. + return -1; +} + +void TableUtilities::filterLowpass( + TimeSeriesTable& table, double cutoffFreq, bool padData) { + OPENSIM_THROW_IF(cutoffFreq < 0, Exception, + "Cutoff frequency must be non-negative; got {}.", cutoffFreq); + + if (padData) { pad(table, (int)table.getNumRows() / 2); } + + const int numRows = (int)table.getNumRows(); + OPENSIM_THROW_IF(numRows < 4, Exception, + "Expected at least 4 rows to filter, but got {} rows.", numRows); + + const auto& time = table.getIndependentColumn(); + + double dtMin = SimTK::Infinity; + for (int irow = 1; irow < numRows; ++irow) { + double dt = time[irow] - time[irow - 1]; + if (dt < dtMin) dtMin = dt; + } + OPENSIM_THROW_IF( + dtMin < SimTK::Eps, Exception, "Storage cannot be resampled."); + + double dtAvg = (time.back() - time.front()) / (numRows - 1); + + // Resample if the sampling interval is not uniform. + if (dtAvg - dtMin > SimTK::Eps) { + table = resampleWithInterval(table, dtMin); + } + + SimTK::Vector filtered(numRows); + for (int icol = 0; icol < (int)table.getNumColumns(); ++icol) { + SimTK::VectorView column = table.getDependentColumnAtIndex(icol); + Signal::LowpassIIR(dtMin, cutoffFreq, numRows, + column.getContiguousScalarData(), + filtered.updContiguousScalarData()); + table.updDependentColumnAtIndex(icol) = filtered; + } +} + +void TableUtilities::pad( + TimeSeriesTable& table, int numRowsToPrependAndAppend) { + if (numRowsToPrependAndAppend == 0) return; + + OPENSIM_THROW_IF(numRowsToPrependAndAppend < 0, Exception, + "Expected numRowsToPrependAndAppend to be non-negative; but " + "got {}.", + numRowsToPrependAndAppend); + + table._indData = Signal::Pad(numRowsToPrependAndAppend, + (int)table._indData.size(), table._indData.data()); + + size_t numColumns = table.getNumColumns(); + + // _indData.size() is now the number of rows after padding. + SimTK::Matrix newMatrix((int)table._indData.size(), (int)numColumns); + for (size_t icol = 0; icol < numColumns; ++icol) { + SimTK::Vector column = table.getDependentColumnAtIndex(icol); + const std::vector newColumn = + Signal::Pad(numRowsToPrependAndAppend, column.size(), + column.getContiguousScalarData()); + newMatrix.updCol((int)icol) = + SimTK::Vector((int)newColumn.size(), newColumn.data(), true); + } + table.updMatrix() = newMatrix; +} + +namespace { +template +std::unique_ptr createFunctionSet(const TimeSeriesTable& table) { + auto set = make_unique(); + const auto& time = table.getIndependentColumn(); + const auto numRows = (int)table.getNumRows(); + for (int icol = 0; icol < (int)table.getNumColumns(); ++icol) { + const double* y = + table.getDependentColumnAtIndex(icol).getContiguousScalarData(); + set->adoptAndAppend(new FunctionType(numRows, time.data(), y)); + } + return set; +} + +template <> +inline std::unique_ptr createFunctionSet( + const TimeSeriesTable& table) { + const auto& time = table.getIndependentColumn(); + return OpenSim::make_unique(table, std::vector{}, + std::min((int)time.size() - 1, 5)); +} +} // namespace + +/// Resample (interpolate) the table at the provided times. In general, a +/// 5th-order GCVSpline is used as the interpolant; a lower order is used if the +/// table has too few points for a 5th-order spline. Alternatively, you can +/// provide a different function type as a template argument (e.g., +/// PiecewiseLinearFunction). +/// @throws Exception if new times are +/// not within existing initial and final times, if the new times are +/// decreasing, or if getNumTimes() < 2. +/// @ingroup moconumutil +template +TimeSeriesTable TableUtilities::resample( + const TimeSeriesTable& in, const TimeVector& newTime) { + + const auto& time = in.getIndependentColumn(); + + OPENSIM_THROW_IF(time.size() < 2, Exception, + "Cannot resample if number of times is 0 or 1."); + OPENSIM_THROW_IF(newTime[0] < time[0], Exception, + "New initial time ({}) cannot be less than existing initial time " + "({})", + newTime[0], time[0]); + OPENSIM_THROW_IF(newTime[newTime.size() - 1] > time[time.size() - 1], + Exception, + "New final time ({}) cannot be greater than existing final time " + "({})", + newTime[newTime.size() - 1], time[time.size() - 1]); + for (int itime = 1; itime < (int)newTime.size(); ++itime) { + OPENSIM_THROW_IF(newTime[itime] < newTime[itime - 1], Exception, + "New times must be non-decreasing, but " + "time[{}] < time[{}] ({} < {}).", + itime, itime - 1, newTime[itime], newTime[itime - 1]); + } + + // Copy over metadata. + TimeSeriesTable out = in; + for (int irow = (int)out.getNumRows() - 1; irow >= 0; --irow) { + out.removeRowAtIndex(irow); + } + + std::unique_ptr functions = + createFunctionSet(in); + SimTK::Vector curTime(1); + SimTK::RowVector row(functions->getSize()); + for (int itime = 0; itime < (int)newTime.size(); ++itime) { + curTime[0] = newTime[itime]; + for (int icol = 0; icol < functions->getSize(); ++icol) { + row(icol) = functions->get(icol).calcValue(curTime); + } + // Not efficient! + out.appendRow(curTime[0], row); + } + return out; +} + +template +TimeSeriesTable TableUtilities::resampleWithInterval( + const TimeSeriesTable& in, double interval) { + std::vector time; + double t = in.getIndependentColumn().front(); + double finalTime = in.getIndependentColumn().back(); + while (t <= finalTime) { + time.push_back(t); + t += interval; + } + return resample, FunctionType>(in, time); +} + +template +TimeSeriesTable TableUtilities::resampleWithIntervalBounded( + const TimeSeriesTable& in, double interval) { + const auto& time = in.getIndependentColumn(); + double duration = time.back() - time.front(); + if (duration / interval > Storage::MAX_RESAMPLE_SIZE) { + double newInterval = duration / Storage::MAX_RESAMPLE_SIZE; + log_warn("Requested resampling time interval {} leads to more than {} " + "sampling points; using larger time interval {}.", + interval, Storage::MAX_RESAMPLE_SIZE, newInterval); + interval = newInterval; + } + return resampleWithInterval(in, interval); +} + +// Explicit template instantiations. +namespace OpenSim { +template OSIMCOMMON_API TimeSeriesTable TableUtilities::resample( + const TimeSeriesTable&, const SimTK::Vector&); +template OSIMCOMMON_API TimeSeriesTable +TableUtilities::resample( + const TimeSeriesTable&, const SimTK::Vector&); + +template OSIMCOMMON_API TimeSeriesTable +TableUtilities::resample, GCVSpline>( + const TimeSeriesTable&, const std::vector&); +template OSIMCOMMON_API TimeSeriesTable +TableUtilities::resample, PiecewiseLinearFunction>( + const TimeSeriesTable&, const std::vector&); + +template OSIMCOMMON_API TimeSeriesTable TableUtilities::resampleWithInterval( + const TimeSeriesTable&, double); +template OSIMCOMMON_API TimeSeriesTable +TableUtilities::resampleWithInterval( + const TimeSeriesTable&, double); + +template OSIMCOMMON_API TimeSeriesTable TableUtilities::resampleWithIntervalBounded( + const TimeSeriesTable&, double); +template OSIMCOMMON_API TimeSeriesTable +TableUtilities::resampleWithIntervalBounded( + const TimeSeriesTable&, double); +} // namespace OpenSim diff --git a/OpenSim/Common/TableUtilities.h b/OpenSim/Common/TableUtilities.h new file mode 100644 index 0000000000..ddeda22f4e --- /dev/null +++ b/OpenSim/Common/TableUtilities.h @@ -0,0 +1,109 @@ +#ifndef OPENSIM_TABLEUTILITIES_H_ +#define OPENSIM_TABLEUTILITIES_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: TableUtilities.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) 2005-2020 Stanford University and the Authors * + * Author(s): Christopher Dembia * + * * + * 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 "Array.h" +#include "GCVSpline.h" +#include "TableUtilities.h" +#include "TimeSeriesTable.h" +#include "osimCommonDLL.h" + +namespace OpenSim { + +class OSIMCOMMON_API TableUtilities { +public: + /// Throws an exception if the same label appears more than once in the list + /// of labels. + /// @throws NonUniqueLabels + static void checkNonUniqueLabels(std::vector labels); + + /// Returns true if the table contains 'inDegrees' metadata set to 'yes', + /// and returns false if the table contains 'inDegrees' metadata set to + /// 'no'. + /// @throws Exception if table does not have 'inDegrees' table metadata. + /// @throws Exception if the 'inDegrees' metadata is neither 'yes' or 'no'. + static bool isInDegrees(const TimeSeriesTable& table); + + /// Get the index in the provided array of labels that corresponds to the + /// desired label. This function attempts to handle the change in + /// state variable names that occurred in OpenSim version 4.0; for example, + /// if you search for `/speed` and it is not found, then this + /// function looks for `_u`. If you search for + /// `/activation` and it is not found, then this function looks for + /// `.activation`. This function returns -1 if the desired label is + /// not found. + static int findStateLabelIndex( + const Array& labels, const std::string& desired); + + /// @copydoc findStateLabelIndex() + static int findStateLabelIndex( + const std::vector& labels, const std::string& desired); + + /// Lowpass filter the data in a TimeSeriesTable at a provided cutoff + /// frequency. If padData is true, then the data is first padded with pad() + /// using numRowsToPrependAndAppend = table.getNumRows() / 2. + /// The filtering is performed with Signal::LowpassIIR() + static void filterLowpass(TimeSeriesTable& table, + double cutoffFreq, bool padData = false); + + /// Pad each column by the number of rows specified. The padded data is + /// obtained by reflecting and negating the data in the table. + /// Postcondition: the number of rows is table.getNumRows() + 2 * + /// numRowsToPrependAndAppend. + static void pad(TimeSeriesTable& table, int numRowsToPrependAndAppend); + + /// Resample (interpolate) the table at the provided times. In general, a + /// 5th-order GCVSpline is used as the interpolant; a lower order is used if + /// the table has too few points for a 5th-order spline. Alternatively, you + /// can provide a different function type as a template argument; currently, + /// the only other supported function is PiecewiseLinearFunction. + /// @throws Exception if new times are + /// not within existing initial and final times, if the new times are + /// decreasing, or if getNumTimes() < 2. + template + static TimeSeriesTable resample( + const TimeSeriesTable& in, const TimeVector& newTime); + + /// Resample the table using the given time interval (using resample()). + /// The new final time is not guaranteed to match the original final + /// time. + template + static TimeSeriesTable resampleWithInterval( + const TimeSeriesTable& in, double interval); + + /// Same as resampleWithInterval() but the interval may be reduced + /// to ensure the number of sampling points does not exceed + /// Storage::MAX_RESAMPLE_SIZE. + template + static TimeSeriesTable resampleWithIntervalBounded( + const TimeSeriesTable& in, double interval); + +private: + static int findStateLabelIndexInternal(const std::string* begin, + const std::string* end, const std::string& desired); +}; + +} // namespace OpenSim + +#endif // OPENSIM_TABLEUTILITIES_H_ diff --git a/OpenSim/Common/Test/testDataTable.cpp b/OpenSim/Common/Test/testDataTable.cpp index 307c20dcf9..6504f5aefa 100644 --- a/OpenSim/Common/Test/testDataTable.cpp +++ b/OpenSim/Common/Test/testDataTable.cpp @@ -20,14 +20,22 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ +#include + #include +#define CATCH_CONFIG_MAIN +#include +#include +#include +#include +#include #include -#include -int main() { - using namespace SimTK; - using namespace OpenSim; - using OpenSim::Exception; +using namespace SimTK; +using namespace OpenSim; +using OpenSim::Exception; + +TEST_CASE("DataTable") { // Default construct, add metadata to columns, append rows one at a time. @@ -68,13 +76,13 @@ int main() { const auto& labels = table.getColumnLabels(); for(size_t i = 0; i < labels.size(); ++i) if(labels.at(i) != std::to_string(i)) - throw Exception{"Test failed: labels.at(i) != " - "std::to_string(i)"}; + throw OpenSim::Exception{"Test failed: labels.at(i) != " + "std::to_string(i)"}; for(size_t i = 0; i < labels.size(); ++i) if(table.getColumnIndex(labels.at(i)) != i) - throw Exception{"Test failed: " - "table.getColumnIndex(labels.at(i)) != i"}; + throw OpenSim::Exception{ + "Test failed: table.getColumnIndex(labels.at(i)) != i"}; } // Test exceptions (table should be empty here). @@ -114,7 +122,7 @@ int main() { const auto& avgRow = table.averageRow(0.2, 0.8); for(int i = 0; i < avgRow.ncol(); ++i) OPENSIM_THROW_IF(std::abs(avgRow[i] - 2) > 1e-8/*epsilon*/, - Exception, + OpenSim::Exception, "Test failed: averageRow() failed."); const auto& nearRow = table.getNearestRow(0.55); @@ -143,44 +151,32 @@ int main() { std::cout << table.toString({-1, -2}, {"1", "4"}); // Retrieve added metadata and rows to check. - if(table.getNumRows() != unsigned{5}) - throw Exception{"Test Failed: table.getNumRows() != unsigned{5}"}; + REQUIRE(table.getNumRows() == unsigned{5}); - if(table.getNumColumns() != unsigned{5}) - throw Exception{"Test Failed: table.getNumColumns() != unsigned{5}"}; + REQUIRE(table.getNumColumns() == unsigned{5}); const auto& dep_metadata_ref = table.getDependentsMetaData(); const auto& labels_ref = dep_metadata_ref.getValueArrayForKey("labels"); for(unsigned i = 0; i < 5; ++i) - if(labels_ref[i].getValue() != std::to_string(i + 1)) - throw Exception{"Test failed: labels_ref[i].getValue()" - " != std::to_string(i + 1)"}; + CHECK(labels_ref[i].getValue() == std::to_string(i + 1)); { - const auto& labels = table.getColumnLabels(); - for(unsigned i = 0; i < 5; ++i) - if(labels.at(i) != std::to_string(i + 1)) - throw Exception{"Test failed: labels[i].getValue()" - " != std::to_string(i + 1)"}; + const auto& labels = table.getColumnLabels(); + for(unsigned i = 0; i < 5; ++i) + CHECK(labels.at(i) == std::to_string(i + 1)); } const auto& col_index_ref = dep_metadata_ref.getValueArrayForKey("column-index"); for(unsigned i = 0; i < 5; ++i) - if(col_index_ref[i].getValue() != i + 1) - throw Exception{"Test failed: col_index_ref[i].getValue()" - " != i + 1"}; + CHECK(col_index_ref[i].getValue() == i + 1); const auto& ind_metadata_ref = table.getIndependentMetaData(); - if(ind_metadata_ref.getValueForKey("labels").getValue() - != std::string{"0"}) - throw Exception{"Test failed: ind_metadata_ref.getValueForKey" - "(\"labels\").getValue() != std::string{\"0\"}"}; - if(ind_metadata_ref.getValueForKey("column-index").getValue() - != unsigned{0}) - throw Exception{"Test failed: ind_metadata_ref.getValueForKey" - "(\"column-index\").getValue() != unsigned{0}"}; + CHECK(ind_metadata_ref.getValueForKey("labels").getValue() == + std::string{"0"}); + CHECK(ind_metadata_ref.getValueForKey("column-index") + .getValue() == unsigned{0}); table.updDependentColumnAtIndex(0) += 2; table.updDependentColumnAtIndex(2) += 2; @@ -190,17 +186,13 @@ int main() { for(unsigned i = 0; i < 5; ++i) { for(unsigned j = 0; j < 5; ++j) { const auto row_i_1 = table.getRowAtIndex(i); - if(row_i_1[j] != (row + i)[j]) - throw Exception{"Test failed: row_i_1[j] != (row + i)[j]"}; + CHECK(row_i_1[j] == (row + i)[j]); const auto row_i_2 = table.getRow(0 + 0.25 * i); - if(row_i_2[j] != (row + i)[j]) - throw Exception{"Test failed: row_i_2[j] != (row + i)[j]"}; + CHECK(row_i_2[j] == (row + i)[j]); const auto col_i = table.getDependentColumnAtIndex(i); - if(col_i[j] != j) - throw Exception{"Test failed: table.getDependentColumnAtIndex" - "(i)[j] != j"}; + CHECK(col_i[j] == j); } } @@ -212,15 +204,9 @@ int main() { // ASSERT(table.getNumRows() == 5 && table.getNumColumns() == 7); const auto& tab_metadata_ref = table.getTableMetaData(); - if(tab_metadata_ref.getValueForKey("DataRate").getValue() - != 600) - throw Exception{"Test failed: tab_metadata_ref.getValueForKey" - "(\"DataRate\").getValue() != 600"}; - if(tab_metadata_ref.getValueForKey("Filename").getValue() - != std::string{"/path/to/file"}) - throw Exception{"Test failed: tab_metadata_ref.getValueForKey" - "(\"Filename\").getValue() != std::string" - "{\"/path/to/file\"}"}; + CHECK(tab_metadata_ref.getValueForKey("DataRate").getValue() == 600); + CHECK(tab_metadata_ref.getValueForKey("Filename").getValue() == + std::string{"/path/to/file"}); { std::cout << "Test feeding rows of one table to another [double]." @@ -396,7 +382,7 @@ int main() { const auto& avgRowVec3 = tableVec3.averageRow(0.1, 0.2); for(int i = 0; i < 3; ++i) OPENSIM_THROW_IF(std::abs(avgRowVec3[0][i] - 2) > 1e-8/*epsilon*/, - Exception, + OpenSim::Exception, "Test failed: averageRow() failed."); const auto& nearRowVec3 = tableVec3.getNearestRow(0.29); @@ -463,7 +449,7 @@ int main() { const auto& avgRowQuat = tableQuat.averageRow(0.1, 0.2); for(int i = 0; i < 4; ++i) { OPENSIM_THROW_IF(std::abs(avgRowQuat[0][i] - 0.5) > 1e-8/*epsilon*/, - Exception, + OpenSim::Exception, "Test failed: averageRow() failed."); } @@ -514,7 +500,7 @@ int main() { const auto& avgRowSVec = tableSpatialVec.averageRow(0.1, 0.2); for(int i = 0; i < 3; ++i) { OPENSIM_THROW_IF(std::abs(avgRowSVec[0][0][i] - 2) > 1e-8/*eps*/, - Exception, + OpenSim::Exception, "Test failed: averageRow() failed."); } @@ -776,7 +762,7 @@ int main() { } for (int r = 0; r < nr; ++r) indColumn[r] = 0.001*r; - + TimeSeriesTable table{ indColumn, huge, labels }; std::clock_t t0 = std::clock(); @@ -784,7 +770,7 @@ int main() { table.removeColumnAtIndex(1); double dTc = 1.e3*(std::clock() - t0) / CLOCKS_PER_SEC; - + std::cout << "\tRemoving columns took:" << dTc << "ms" << std::endl; TimeSeriesTable table2{ indColumn, huge, labels }; @@ -797,6 +783,134 @@ int main() { std::cout << "\tRemoving rows took:" << dTr << "ms" << std::endl; } +} + +TEST_CASE("TableUtilities::checkNonUniqueLabels") { + CHECK_THROWS_AS(TableUtilities::checkNonUniqueLabels({"a", "a"}), + NonUniqueLabels); +} + +TEST_CASE("TableUtilities::isInDegrees") { + SECTION("no inDegrees metadata") { + CHECK_THROWS_WITH(TableUtilities::isInDegrees(TimeSeriesTable()), + Catch::Contains("Table does not have 'inDegrees' metadata.")); + } + SECTION("inDegrees=invalid") { + TimeSeriesTable table; + table.addTableMetaData("inDegrees", std::string("invalid")); + CHECK_THROWS_WITH(!TableUtilities::isInDegrees(table), + Catch::Contains("Expected table's 'inDegrees' metadata")); + } + Storage sto; + SECTION("inDegrees=yes") { + sto.setInDegrees(true); + TimeSeriesTable table = sto.exportToTable(); + CHECK(TableUtilities::isInDegrees(table)); + } + SECTION("inDegrees=no") { + sto.setInDegrees(false); + TimeSeriesTable table = sto.exportToTable(); + CHECK(!TableUtilities::isInDegrees(table)); + } +} + +TEST_CASE("TableUtilities::findStateLabelIndex") { + CHECK(TableUtilities::findStateLabelIndex( + std::vector{"hip_flexion"}, + "/jointset/hip/hip_flexion/value") == 0); + CHECK(TableUtilities::findStateLabelIndex( + std::vector{"hip_flexion_u"}, + "/jointset/hip/hip_flexion/speed") == 0); + CHECK(TableUtilities::findStateLabelIndex( + std::vector{"vasti.activation"}, + "/forceset/vasti/activation") == 0); + CHECK(TableUtilities::findStateLabelIndex( + std::vector{"vasti.fiber_length"}, + "/forceset/vasti/fiber_length") == 0); + + CHECK(TableUtilities::findStateLabelIndex( + std::vector{"hip_flexin"}, + "/jointset/hip/hip_flexion/value") == -1); +} - return 0; +TEST_CASE("TableUtilities::filterLowpass") { + const int numRows = 100; + + // Create a table with random values, and write the table to a file. + std::vector time(numRows); + SimTK::Vector timeVec = createVectorLinspace(numRows, 0, 1); + std::copy_n(timeVec.getContiguousScalarData(), numRows, time.data()); + TimeSeriesTable table(time); + table.appendColumn("a", SimTK::Test::randVector(numRows)); + STOFileAdapter::write(table, "testFilterLowpass.sto"); + + // Filter the table. + TableUtilities::filterLowpass(table, 6.0); + + // Load the original table as a storage, and filter the storage. + Storage sto("testFilterLowpass.sto"); + sto.lowpassIIR(6.0); + + // The filtered table and storage should match. + SimTK::Vector filteredStorageColumn(numRows); + double* columnData = filteredStorageColumn.updContiguousScalarData(); + sto.getDataColumn("a", columnData); + const auto& filteredTableColumn = table.getDependentColumnAtIndex(0); + for (int i = 0; i < numRows; ++i) { + CHECK(filteredStorageColumn[i] == Approx(filteredTableColumn[i])); + } +} + +TEST_CASE("TableUtilities::pad") { + Storage sto("test.sto"); + TimeSeriesTable paddedTable = sto.exportToTable(); + TableUtilities::pad(paddedTable, 1); + + sto.pad(1); + TimeSeriesTable paddedSto = sto.exportToTable(); + + CHECK(paddedTable.getIndependentColumn() == + paddedSto.getIndependentColumn()); + REQUIRE(paddedTable.getNumRows() == paddedSto.getNumRows()); + REQUIRE(paddedTable.getNumColumns() == paddedSto.getNumColumns()); + for (int irow = 0; irow < (int)paddedTable.getNumRows(); ++irow) { + for (int icol = 0; icol < (int)paddedTable.getNumColumns(); ++icol) { + CHECK(paddedTable.getMatrix().getElt(irow, icol) == + paddedSto.getMatrix().getElt(irow, icol)); + } + } +} + +TEST_CASE("TableUtilities::resample") { + TimeSeriesTable table(std::vector{0.0, 1, 2}); + table.appendColumn("a", {1.0, 0.5, 0.0}); + { + TimeSeriesTable resampled = + TableUtilities::resample(table, createVector({0.5})); + REQUIRE(resampled.getNumRows() == 1); + REQUIRE(resampled.getNumColumns() == 1); + CHECK(resampled.getIndependentColumn()[0] == 0.5); + CHECK(resampled.getDependentColumnAtIndex(0)[0] == Approx(0.75)); + } + { + TimeSeriesTable resampled = TableUtilities::resampleWithInterval(table, + 0.4); + REQUIRE(resampled.getNumRows() == 6); + REQUIRE(resampled.getNumColumns() == 1); + const auto& time = resampled.getIndependentColumn(); + CHECK(time[0] == Approx(0).margin(1e-10)); + CHECK(time[1] == Approx(0.4)); + CHECK(time[2] == Approx(0.8)); + CHECK(time[3] == Approx(1.2)); + CHECK(time[4] == Approx(1.6)); + CHECK(time[5] == Approx(2.0)); + + const auto& column = resampled.getDependentColumnAtIndex(0); + CHECK(column[0] == Approx(1.0)); + CHECK(column[1] == Approx(0.8)); + CHECK(column[2] == Approx(0.6)); + CHECK(column[3] == Approx(0.4)); + CHECK(column[4] == Approx(0.2)); + CHECK(column[5] == Approx(0.0).margin(1e-10)); + } } diff --git a/OpenSim/Common/Test/testFunctions.cpp b/OpenSim/Common/Test/testFunctions.cpp index 4c57b1de15..8c2a279a61 100644 --- a/OpenSim/Common/Test/testFunctions.cpp +++ b/OpenSim/Common/Test/testFunctions.cpp @@ -21,12 +21,13 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -#include -#include -#include - #include "ComponentsForTesting.h" +#include +#include +#include +#include + #define CATCH_CONFIG_MAIN #include @@ -76,3 +77,38 @@ TEST_CASE("SignalGenerator") { Approx(amplitude * std::sin(omega * time + phase) + offset)); } } + +TEST_CASE("Interpolate using PiecewiseLinearFunction") { + SimTK::Vector x = createVector({0, 1}); + SimTK::Vector y = createVector({1, 0}); + SimTK::Vector newX = createVector({-1, 0.25, 0.75, 1.5}); + SimTK::Vector newY = OpenSim::interpolate(x, y, newX); + + SimTK_TEST(SimTK::isNaN(newY[0])); + SimTK_TEST_EQ(newY[1], 0.75); + SimTK_TEST_EQ(newY[2], 0.25); + SimTK_TEST(SimTK::isNaN(newY[3])); +} + +TEST_CASE("solveBisection()") { + + auto calcResidual = [](const SimTK::Real& x) { return x - 3.78; }; + { + const auto root = solveBisection(calcResidual, -5, 5, 1e-6); + SimTK_TEST_EQ_TOL(root, 3.78, 1e-6); + // Make sure the tolerance has an effect. + SimTK_TEST_NOTEQ_TOL(root, 3.78, 1e-10); + } + { + const auto root = solveBisection(calcResidual, -5, 5, 1e-10); + SimTK_TEST_EQ_TOL(root, 3.78, 1e-10); + } + + // Multiple roots. + { + auto parabola = [](const SimTK::Real& x) { + return SimTK::square(x - 2.5); + }; + REQUIRE_THROWS_AS(solveBisection(parabola, -5, 5), OpenSim::Exception); + } +} diff --git a/OpenSim/Common/TimeSeriesTable.h b/OpenSim/Common/TimeSeriesTable.h index 14decd929e..fa561c515b 100644 --- a/OpenSim/Common/TimeSeriesTable.h +++ b/OpenSim/Common/TimeSeriesTable.h @@ -493,7 +493,7 @@ class TimeSeriesTable_ : public DataTable_ { DT::_indData[rowIndex + 1]); } } - /** trim table to rows ebtween start_index and last_index incluslively + /** trim table to rows between start_index and last_index inclusively */ void trimToIndices(const size_t& start_index, const size_t& last_index) { // This uses the rather invasive but efficient mechanism to copy a @@ -511,6 +511,8 @@ class TimeSeriesTable_ : public DataTable_ { this->_indData = newIndependentVector; } + friend class TableUtilities; + }; // TimeSeriesTable_ /** See TimeSeriesTable_ for details on the interface. */ diff --git a/OpenSim/Common/osimCommon.h b/OpenSim/Common/osimCommon.h index 81351f3a6b..814cbf77c9 100644 --- a/OpenSim/Common/osimCommon.h +++ b/OpenSim/Common/osimCommon.h @@ -42,7 +42,6 @@ #include "PiecewiseConstantFunction.h" #include "PiecewiseLinearFunction.h" #include "PolynomialFunction.h" -#include "RegisterTypes_osimCommon.h" #include "RegisterTypes_osimCommon.h" // to expose RegisterTypes_osimCommon #include "Reporter.h" #include "Scale.h" @@ -55,6 +54,7 @@ #include "Stopwatch.h" #include "StorageInterface.h" #include "TableSource.h" +#include "TableUtilities.h" #include "TimeSeriesTable.h" #endif // OPENSIM_OSIMCOMMON_H_ diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 944f6683ca..a251cd03db 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -28,6 +28,8 @@ #include +#include + using namespace OpenSim; SimTK::State OpenSim::simulate(Model& model, @@ -94,6 +96,19 @@ SimTK::State OpenSim::simulate(Model& model, return state; } +void OpenSim::updateStateLabels40(const Model& model, + std::vector& labels) { + + TableUtilities::checkNonUniqueLabels(labels); + + const Array stateNames = model.getStateVariableNames(); + for (int isv = 0; isv < stateNames.size(); ++isv) { + int i = TableUtilities::findStateLabelIndex(labels, stateNames[isv]); + if (i == -1) continue; + labels[i] = stateNames[isv]; + } +} + std::unique_ptr OpenSim::updatePre40KinematicsStorageFor40MotionType(const Model& pre40Model, const Storage &kinematics) diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 1a6f800aa0..448145b281 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -47,6 +47,21 @@ OSIMSIMULATION_API SimTK::State simulate(Model& model, bool saveStatesFile = false); /// @} +/// Update a vector of state labels (in place) to use post-4.0 state paths +/// instead of pre-4.0 state names. For example, this converts labels as +/// follows: +/// - `pelvis_tilt` -> `/jointset/ground_pelvis/pelvis_tilt/value` +/// - `pelvis_tilt_u` -> `/jointset/ground_pelvis/pelvis_tilt/speed` +/// - `soleus.activation` -> `/forceset/soleus/activation` +/// - `soleus.fiber_length` -> `/forceset/soleus/fiber_length` +/// This can also be used to update the column labels of an Inverse +/// Kinematics Tool solution MOT file so that the data can be used as +/// states. If a label does not identify a state in the model, the column +/// label is not changed. +/// @throws Exception if labels are not unique. +OSIMSIMULATION_API void updateStateLabels40( + const Model& model, std::vector& labels); + #ifndef SWIG /** Not available through scripting. @returns nullptr if no update is necessary. */ diff --git a/OpenSim/Simulation/StatesTrajectory.cpp b/OpenSim/Simulation/StatesTrajectory.cpp index 34acfe6424..2676f81bcd 100644 --- a/OpenSim/Simulation/StatesTrajectory.cpp +++ b/OpenSim/Simulation/StatesTrajectory.cpp @@ -22,7 +22,10 @@ * -------------------------------------------------------------------------- */ #include "StatesTrajectory.h" + +#include #include +#include #include using namespace OpenSim; @@ -39,13 +42,13 @@ void StatesTrajectory::append(const SimTK::State& state) { if (!m_states.empty()) { SimTK_APIARGCHECK2_ALWAYS(m_states.back().getTime() <= state.getTime(), - "StatesTrajectory", "append", + "StatesTrajectory", "append", "New state's time (%f) must be equal to or greater than the " "time for the last state in the trajectory (%f).", state.getTime(), m_states.back().getTime() ); - // We assume the trajectory (before appending) is already consistent, + // We assume the trajectory (before appending) is already consistent, // so we only need to check consistency with a single state in the // trajectory. OPENSIM_THROW_IF(!m_states.back().isConsistent(state), @@ -131,11 +134,11 @@ TimeSeriesTable StatesTrajectory::exportToTable(const Model& model, // Set the column labels as metadata. std::vector stateVars = requestedStateVars.empty() ? - createVector(model.getStateVariableNames()) : + ::createVector(model.getStateVariableNames()) : requestedStateVars; table.setColumnLabels(stateVars); size_t numDepColumns = stateVars.size(); - + // Fill up the table with the data. for (size_t itime = 0; itime < getSize(); ++itime) { const auto& state = get(itime); @@ -147,7 +150,7 @@ TimeSeriesTable StatesTrajectory::exportToTable(const Model& model, row = model.getStateVariableValues(state).transpose(); } else { for (unsigned icol = 0; icol < numDepColumns; ++icol) { - row[static_cast(icol)] = + row[static_cast(icol)] = model.getStateVariableValue(state, stateVars[icol]); } } @@ -164,6 +167,16 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( bool allowMissingColumns, bool allowExtraColumns, bool assemble) { + return createFromStatesTable(model, sto.exportToTable(), + allowMissingColumns, allowExtraColumns, assemble); +} + +StatesTrajectory StatesTrajectory::createFromStatesTable( + const Model& model, + const TimeSeriesTable& table, + bool allowMissingColumns, + bool allowExtraColumns, + bool assemble) { // Assemble the required objects. // ============================== @@ -173,31 +186,24 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( // Make a copy of the model so that we can get a corresponding state. Model localModel(model); - + // We'll keep editing this state as we loop through time. auto state = localModel.initSystem(); // The labels of the columns in the storage file. - const auto& stoLabels = sto.getColumnLabels(); - int numDependentColumns = stoLabels.getSize() - 1; + const auto& tableLabels = table.getColumnLabels(); + int numDependentColumns = (int)table.getNumColumns(); // Error checking. // =============== // Angular quantities must be expressed in radians. // TODO we could also manually convert the necessary coords/speeds to // radians. - OPENSIM_THROW_IF(sto.isInDegrees(), StatesStorageIsInDegrees); + OPENSIM_THROW_IF(TableUtilities::isInDegrees(table), DataIsInDegrees); - // If column labels aren't unique, It's unclear which column the user + // If column labels aren't unique, it's unclear which column the user // wanted to use for the related state variable. - OPENSIM_THROW_IF(!sto.storageLabelsAreUnique(), - NonUniqueColumnsInStatesStorage); - - // To be safe, we don't process storages in which individual rows are - // missing values. - OPENSIM_THROW_IF(numDependentColumns != sto.getSmallestNumberOfStates(), - VaryingNumberOfStatesPerRow, - numDependentColumns, sto.getSmallestNumberOfStates()); + TableUtilities::checkNonUniqueLabels(tableLabels); // Check if states are missing from the Storage. // --------------------------------------------- @@ -206,9 +212,13 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( // Also, assemble the indices of the states that we will actually set in the // trajectory. std::map statesToFillUp; + for (const auto& label : tableLabels) { + log_info("DEBUG {}", label); + } for (int is = 0; is < modelStateNames.getSize(); ++is) { // getStateIndex() will check for pre-4.0 column names. - const int stateIndex = sto.getStateIndex(modelStateNames[is]); + const int stateIndex = TableUtilities::findStateLabelIndex( + tableLabels, modelStateNames[is]); if (stateIndex == -1) { missingColumnNames.push_back(modelStateNames[is]); } else { @@ -216,7 +226,7 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( } } OPENSIM_THROW_IF(!allowMissingColumns && !missingColumnNames.empty(), - MissingColumnsInStatesStorage, + MissingColumns, localModel.getName(), missingColumnNames); // Check if the Storage has columns that are not states in the Model. @@ -226,26 +236,22 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( std::vector extraColumnNames; // We want the actual column names, not the state names; the two // might be different b/c the state names changed in v4.0. - for (int ic = 1; ic < stoLabels.getSize(); ++ic) { + for (int ic = 1; ic < (int)tableLabels.size(); ++ic) { // Has this label been marked as a model state? - // stateIndex = columnIndex - 1 (b/c of the time column). - if (statesToFillUp.count(ic - 1) == 0) { - extraColumnNames.push_back(stoLabels[ic]); + if (statesToFillUp.count(ic) == 0) { + extraColumnNames.push_back(tableLabels[ic]); } } - OPENSIM_THROW(ExtraColumnsInStatesStorage, localModel.getName(), + OPENSIM_THROW(ExtraColumns, localModel.getName(), extraColumnNames); } } - + // Fill up trajectory. // =================== // Reserve the memory we'll need to fit all the states. - states.m_states.reserve(sto.getSize()); - - // Working memory for Storage. - SimTK::Vector dependentValues(numDependentColumns); + states.m_states.reserve(table.getNumRows()); // Working memory for state. Initialize so that missing columns end up as // NaN. @@ -255,18 +261,16 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( state.updY().setToNaN(); // Loop through all rows of the Storage. - for (int itime = 0; itime < sto.getSize(); ++itime) { - - // Extract data from Storage into the working Vector. - sto.getData(itime, numDependentColumns, dependentValues); + for (int itime = 0; itime < (int)table.getNumRows(); ++itime) { + const auto& row = table.getRowAtIndex(itime); // Set the correct time in the state. - sto.getTime(itime, state.updTime()); + state.setTime(table.getIndependentColumn()[itime]); // Fill up current State with the data for the current time. for (const auto& kv : statesToFillUp) { // 'first': index for Storage; 'second': index for Model. - statesValues[kv.second] = dependentValues[kv.first]; + statesValues[kv.second] = row[kv.first]; } localModel.setStateVariableValues(state, statesValues); if (assemble) { @@ -283,7 +287,7 @@ StatesTrajectory StatesTrajectory::createFromStatesStorage( StatesTrajectory StatesTrajectory::createFromStatesStorage( const Model& model, const std::string& filepath) { - return createFromStatesStorage(model, Storage(filepath)); + return createFromStatesTable(model, TimeSeriesTable(filepath)); } StatesTrajectory::IncompatibleModel::IncompatibleModel( diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index 82331704ac..69195660b5 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -318,19 +318,19 @@ class OSIMSIMULATION_API StatesTrajectory { }; #endif - /** Thrown when trying to create a StatesTrajectory from a states Storage, - * and the Storage does not contain a column for every continuous state + /** Thrown when trying to create a StatesTrajectory from states data, + * and the data does not contain a column for every continuous state * variable. */ - class MissingColumnsInStatesStorage : public OpenSim::Exception { + class MissingColumns : public OpenSim::Exception { public: - MissingColumnsInStatesStorage(const std::string& file, size_t line, + MissingColumns(const std::string& file, size_t line, const std::string& func, const std::string& modelName, std::vector missingStates) : OpenSim::Exception(file, line, func) { std::string msg = "The following "; msg += std::to_string(missingStates.size()) + " states from Model '"; - msg += modelName + "' are missing from the states Storage:\n"; + msg += modelName + "' are missing from the data:\n"; for (unsigned i = 0; i < (missingStates.size() - 1); ++i) { msg += " " + missingStates[i] + "\n"; } @@ -340,13 +340,12 @@ class OSIMSIMULATION_API StatesTrajectory { } }; - /** Thrown when trying to create a StatesTrajectory from a states Storage, and - * the Storage contains columns that do not correspond to continuous state - * variables. - * */ - class ExtraColumnsInStatesStorage : public OpenSim::Exception { + /** Thrown when trying to create a StatesTrajectory from states data, and + * the data contains columns that do not correspond to continuous state + * variables. */ + class ExtraColumns : public OpenSim::Exception { public: - ExtraColumnsInStatesStorage( + ExtraColumns( const std::string& file, size_t line, const std::string& func, const std::string& modelName, @@ -363,20 +362,11 @@ class OSIMSIMULATION_API StatesTrajectory { addMessage(msg); } }; - - /** Thrown by createFromStatesStorage(). */ - class NonUniqueColumnsInStatesStorage : public OpenSim::Exception { - public: - NonUniqueColumnsInStatesStorage(const std::string& file, size_t line, - const std::string& func) : OpenSim::Exception(file, line, func) { - addMessage("States Storage column labels are not unique."); - } - }; - + /** Thrown by createFromStatesStorage(). */ - class StatesStorageIsInDegrees : public OpenSim::Exception { + class DataIsInDegrees : public OpenSim::Exception { public: - StatesStorageIsInDegrees(const std::string& file, size_t line, + DataIsInDegrees(const std::string& file, size_t line, const std::string& func) : OpenSim::Exception(file, line, func) { addMessage("States Storage is in degrees, but this is inappropriate " "for creating a StatesTrajectory. Edit the Storage so that " @@ -384,29 +374,33 @@ class OSIMSIMULATION_API StatesTrajectory { "no in the header."); } }; - - /** Thrown by createFromStatesStorage(). */ - class VaryingNumberOfStatesPerRow : public OpenSim::Exception { - public: - VaryingNumberOfStatesPerRow(const std::string& file, size_t line, - const std::string& func, - int numDepColumns, int smallestNumStates) : - OpenSim::Exception(file, line, func) { - std::string msg = "States Storage has varying number of entries "; - msg += "per row (from " + std::to_string(smallestNumStates) + " to "; - msg += std::to_string(numDepColumns) + "). You must provide a "; - msg += "States Storage that has the same number "; - msg += "of entries in every row."; - addMessage(msg); - } - }; - /// @name Create partial trajectory from a states Storage + /// @name Create partial trajectory from a states table /// @{ - /** Create a partial trajectory of States from a states Storage - * object. The resulting StatesTrajectory will restore continuous state + + /** + * This function is identical to createFromStatesTable() except that this + * function accepts a Storage instead of a TimeSeriesTable. + */ + // TODO When OSTATES support is complete, add the following blurb to + // the doxygen block above. + /* #### History + * Before OpenSim 4.0, the only way to save states to a file was as + * a Storage file, typically called a states storage file and named + * `*_states.sto`. You can use this function to create a StatesTrajectory + * from such a Storage file. OpenSim 4.0 introduced the ability to save and + * read a complete StatesTrajectory to/from an OSTATES file, and so this + * function should only be used when you are stuck with pre-4.0 files. */ + static StatesTrajectory createFromStatesStorage(const Model& model, + const Storage& sto, + bool allowMissingColumns = false, + bool allowExtraColumns = false, + bool assemble = false); + + /** Create a partial trajectory of States from a states table. + * The resulting StatesTrajectory will restore continuous state * variable values, but not discrete state variable values, modeling - * option values, etc. Also, keep in mind that states Storage files usually + * option values, etc. Also, keep in mind that states files usually * do not contain the state values to full precision, and thus cannot * exactly reproduce results from the initial state trajectory. Lastly, * this function optionally modifies each state to obey any constraints in @@ -421,19 +415,19 @@ class OSIMSIMULATION_API StatesTrajectory { * @note The naming convention for state variables changed in OpenSim v4.0; * `ankle_r/ankle_angle_r/speed` used to be `ankle_angle_r_u`, * `soleus_r/activation` used to be `soleus_r.activation`, etc. This - * function can handle %Storage column labels that use the pre-v4.0 naming + * function can handle column labels that use the pre-v4.0 naming * convention. - * + * * @param model The Model to which the states belong. A Model is necessary - * since the storage file lists the state variables by name. - * @param sto The Storage object containing state values. + * because the data file lists the state variables by name. + * @param table The table containing state values. * @param allowMissingColumns If false, throws exception if there are * continuous state variables in the Model for which there is no - * column in the Storage. If true, no exception is thrown but such + * column in the table. If true, no exception is thrown but such * state variables are set to NaN. * @param allowExtraColumns If false, throws exception if there are - * columns in the Storage that do not correspond to continuous state - * variables in the Model. If true, such columns of the Storage are + * columns in the table that do not correspond to continuous state + * variables in the Model. If true, such columns of the table are * ignored. * @param assemble Modify state variable values to satisfy * kinematic constraints (by calling Model::assemble()). @@ -452,39 +446,27 @@ class OSIMSIMULATION_API StatesTrajectory { * @code{.py} * import opensim * model = opensim.Model("subject01.osim") - * sto = opensim.Storage("subject01_states.sto") - * states = opensim.StatesTrajectory.createFromStatesStorage(model, sto) + * table = opensim.TimeSeriesTable("subject01_states.sto") + * states = opensim.StatesTrajectory.createFromStatesTable(model, table) * print(states[0].getTime()) * print(model.getStateVariableValue(states[0], "knee/flexion/value")) * @endcode - * - * @throws MissingColumnsInStatesStorage See the description of the + * + * @throws MissingColumns See the description of the * `allowMissingColumns` argument. - * - * @throws ExtraColumnsInStatesStorage See the description of the + * + * @throws ExtraColumns See the description of the * `allowExtraColumns` argument. * - * @throws NonUniqueColumnsInStatesStorage Thrown if multiple columns in - * the Storage have the same name. - * - * @throws StatesStorageIsInDegrees Thrown if the Storage is in degrees + * @throws NonUniqueLabels Thrown if multiple columns in + * the table have the same name. + * + * @throws DataIsInDegrees Thrown if the table is in degrees * (inDegrees=yes); angular quantities must use radians to properly * create the trajectory. - * - * @throws VaryingNumberOfStatesPerRow Thrown if the rows of the storage - * don't all have the same number of entries. */ - // TODO When OSTATES support is complete, add the following blurb to - // the doxygen block above. - /* #### History - * Before OpenSim 4.0, the only way to save states to a file was as - * a Storage file, typically called a states storage file and named - * `*_states.sto`. You can use this function to create a StatesTrajectory - * from such a Storage file. OpenSim 4.0 introduced the ability to save and - * read a complete StatesTrajectory to/from an OSTATES file, and so this - * function should only be used when you are stuck with pre-4.0 files. */ - static StatesTrajectory createFromStatesStorage(const Model& model, - const Storage& sto, + static StatesTrajectory createFromStatesTable(const Model& model, + const TimeSeriesTable& table, bool allowMissingColumns = false, bool allowExtraColumns = false, bool assemble = false); diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index d676b94baa..df197d0593 100644 --- a/OpenSim/Simulation/Test/testStatesTrajectory.cpp +++ b/OpenSim/Simulation/Test/testStatesTrajectory.cpp @@ -326,18 +326,18 @@ void testFromStatesStorageInconsistentModel(const std::string &stoFilepath) { // Test that an exception is thrown. SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, stoMissingCols), - StatesTrajectory::MissingColumnsInStatesStorage + StatesTrajectory::MissingColumns ); // Check some other similar calls. SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, stoMissingCols, false, true), - StatesTrajectory::MissingColumnsInStatesStorage + StatesTrajectory::MissingColumns ); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, stoMissingCols, false, false), - StatesTrajectory::MissingColumnsInStatesStorage + StatesTrajectory::MissingColumns ); // No exception if allowing missing columns. @@ -375,17 +375,17 @@ void testFromStatesStorageInconsistentModel(const std::string &stoFilepath) { model.initSystem(); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto), - StatesTrajectory::ExtraColumnsInStatesStorage + StatesTrajectory::ExtraColumns ); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto, true, false), - StatesTrajectory::ExtraColumnsInStatesStorage + StatesTrajectory::ExtraColumns ); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto, false, false), - StatesTrajectory::ExtraColumnsInStatesStorage + StatesTrajectory::ExtraColumns ); // No exception if allowing extra columns, and behavior is @@ -403,23 +403,23 @@ void testFromStatesStorageUniqueColumnLabels() { // Edit column labels so that they are not unique. auto labels = sto.getColumnLabels(); labels[10] = labels[7]; - sto.setColumnLabels(labels); - + sto.setColumnLabels(labels); + SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto), - StatesTrajectory::NonUniqueColumnsInStatesStorage); + NonUniqueLabels); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto, true, true), - StatesTrajectory::NonUniqueColumnsInStatesStorage); + NonUniqueLabels); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto, true, false), - StatesTrajectory::NonUniqueColumnsInStatesStorage); + NonUniqueLabels); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto, false, true), - StatesTrajectory::NonUniqueColumnsInStatesStorage); + NonUniqueLabels); SimTK_TEST_MUST_THROW_EXC( StatesTrajectory::createFromStatesStorage(model, sto, false, false), - StatesTrajectory::NonUniqueColumnsInStatesStorage); + NonUniqueLabels); // TODO unique even considering old and new formats for state variable // names (/value and /speed) in the same file. @@ -511,24 +511,6 @@ void testFromStatesStoragePre40CorrectStates() { } } -void testFromStatesStorageAllRowsHaveSameLength() { - // Missing data in individual rows leads to an incorrect StatesTrajectory - // and could really confuse users. - Model model("gait2354_simbody.osim"); - model.initSystem(); - - const auto stateNames = model.getStateVariableNames(); - Storage sto(statesStoFname); - // Append a too-short state vector. - SimTK::Vector_ v(model.getNumStateVariables() - 10, 1.0); - StateVector sv{25.0, v}; - sto.append(sv); - - SimTK_TEST_MUST_THROW_EXC( - StatesTrajectory::createFromStatesStorage(model, sto), - StatesTrajectory::VaryingNumberOfStatesPerRow); -} - void testCopying() { Model model("gait2354_simbody.osim"); @@ -817,7 +799,6 @@ int main() { SimTK_SUBTEST(testFromStatesStorageGivesCorrectStates); SimTK_SUBTEST1(testFromStatesStorageInconsistentModel, statesStoFname); SimTK_SUBTEST(testFromStatesStorageUniqueColumnLabels); - SimTK_SUBTEST(testFromStatesStorageAllRowsHaveSameLength); // Export to data table. SimTK_SUBTEST(testExport); diff --git a/OpenSim/Simulation/VisualizerUtilities.cpp b/OpenSim/Simulation/VisualizerUtilities.cpp index a9995af106..0d5f96d419 100644 --- a/OpenSim/Simulation/VisualizerUtilities.cpp +++ b/OpenSim/Simulation/VisualizerUtilities.cpp @@ -26,14 +26,14 @@ //============================================================================= #include "VisualizerUtilities.h" -#include #include -#include -#include +#include +#include +#include #include +#include #include - using namespace std; using namespace OpenSim; using namespace SimTK; @@ -41,10 +41,11 @@ using namespace SimTK; void VisualizerUtilities::showModel(Model model) { model.setUseVisualizer(true); - // Avoid excessive display of Frames for all Bodies, Ground and additional Frames + // Avoid excessive display of Frames for all Bodies, Ground and additional + // Frames SimTK::State& si = model.initSystem(); auto silo = &model.updVisualizer().updInputSilo(); - + SimTK::DecorativeText help("(hit Esc. to exit)"); help.setIsScreenText(true); auto simtkVisualizer = model.updVisualizer().updSimbodyVisualizer(); @@ -66,13 +67,13 @@ void VisualizerUtilities::showModel(Model model) { return; } } - } +} // Based on code from simtk.org/projects/predictivesim SimbiconExample/main.cpp. -void VisualizerUtilities::showMotion(Model model, Storage statesSto) { +void VisualizerUtilities::showMotion(Model model, TimeSeriesTable statesTable) { - const SimTK::Real initialTime = statesSto.getFirstTime(); - const SimTK::Real finalTime = statesSto.getLastTime(); + const SimTK::Real initialTime = statesTable.getIndependentColumn().front(); + const SimTK::Real finalTime = statesTable.getIndependentColumn().back(); const SimTK::Real duration = finalTime - initialTime; // A data rate of 300 Hz means we can maintain 30 fps down to @@ -84,14 +85,15 @@ void VisualizerUtilities::showMotion(Model model, Storage statesSto) { // Prepare data. // ------------- - statesSto.resample(1.0 / dataRate, 4 /* degree */); - if (statesSto.isInDegrees()) { + statesTable = + TableUtilities::resampleWithInterval(statesTable, 1.0 / dataRate); + if (TableUtilities::isInDegrees(statesTable)) { model.setUseVisualizer(false); model.initSystem(); - model.getSimbodyEngine().convertDegreesToRadians(statesSto); + model.getSimbodyEngine().convertDegreesToRadians(statesTable); } - auto statesTraj = StatesTrajectory::createFromStatesStorage( - model, statesSto, true, true, false); + auto statesTraj = StatesTrajectory::createFromStatesTable( + model, statesTable, true, true, false); const int numStates = (int)statesTraj.getSize(); // Must setUseVisualizer(true) *after* createFromStatesStorage(), otherwise @@ -112,8 +114,6 @@ void VisualizerUtilities::showMotion(Model model, Storage statesSto) { std::string modelName = model.getName().empty() ? "" : model.getName(); std::string title = "Visualizing model '" + modelName + "'"; - if (!statesSto.getName().empty() && statesSto.getName() != "UNKNOWN") - title += " with motion '" + statesSto.getName() + "'"; title += " (" + getFormattedDateTime(false, "ISO") + ")"; viz.setWindowTitle(title); viz.setMode(SimTK::Visualizer::RealTime); @@ -235,8 +235,10 @@ void VisualizerUtilities::showMarkerData( TimeSeriesTableVec3 markerTimeSeriesInMeters(markerTimeSeries); // if units are in mm, convert to meters first - if (markerTimeSeriesInMeters.getTableMetaData().hasKey("Units")){ - auto unitsString = markerTimeSeriesInMeters.getTableMetaData().getValueAsString("Units"); + if (markerTimeSeriesInMeters.getTableMetaData().hasKey("Units")) { + auto unitsString = + markerTimeSeriesInMeters.getTableMetaData().getValueAsString( + "Units"); if (unitsString == "mm") { for (auto column : markerTimeSeriesInMeters.getColumnLabels()) markerTimeSeriesInMeters.updDependentColumn(column) /= 1000; @@ -274,7 +276,8 @@ void VisualizerUtilities::showMarkerData( const SimTK::Real initialTime = times.front(); const SimTK::Real finalTime = times.back(); - previewWorld.updVisualizer().updSimbodyVisualizer().setBackgroundType(SimTK::Visualizer::SolidColor); + previewWorld.updVisualizer().updSimbodyVisualizer().setBackgroundType( + SimTK::Visualizer::SolidColor); previewWorld.updVisualizer().updSimbodyVisualizer().setBackgroundColor( SimTK::Black); previewWorld.realizePosition(state); @@ -291,13 +294,13 @@ void VisualizerUtilities::showMarkerData( } void VisualizerUtilities::showOrientationData( - const TimeSeriesTableQuaternion& quatTable, std::string layoutString, - const Model* modelForPose) { + const TimeSeriesTableQuaternion& quatTable, std::string layoutString, + const Model* modelForPose) { Model world; std::map mapOfLayouts = { {"line", 0}, {"circle", 1}, {"model", 2}}; - int layout = 0; + int layout = 0; if (mapOfLayouts.find(layoutString) == mapOfLayouts.end()) { cout << "Warning: layout option " << layoutString << " not found, ignoring and assuming line layout.." << endl; @@ -345,12 +348,12 @@ void VisualizerUtilities::showOrientationData( for (int i = 0; i < numJoints; i++) { joints[i] ->updCoordinate(FreeJoint::Coord::TranslationY) - .set_default_value( - 1 + sin((double)i / (numJoints-1) * SimTK::Pi)); + .set_default_value(1 + sin((double)i / (numJoints - 1) * + SimTK::Pi)); joints[i] ->updCoordinate(FreeJoint::Coord::TranslationZ) - .set_default_value( - 1 + cos((double)i / (numJoints-1) * SimTK::Pi)); + .set_default_value(1 + cos((double)i / (numJoints - 1) * + SimTK::Pi)); } break; case 2: @@ -385,7 +388,8 @@ void VisualizerUtilities::showOrientationData( if (!nameFound) { cout << "No body with name " << nameFromData << " was found in model." - << " Data for this orientation will be displayed at origin"; + << " Data for this orientation will be displayed at " + "origin"; } } break; @@ -454,7 +458,7 @@ void VisualizerUtilities::showOrientationData( if (sliderIndex == timeSliderIndex) { auto desiredIndex = (sliderValue - initialTime) / (finalTime - initialTime); - frameNumber = static_cast(desiredIndex*times.size()); + frameNumber = static_cast(desiredIndex * times.size()); applyFrame(frameNumber); } else { std::cout << "Internal error: unrecognized slider." @@ -478,29 +482,29 @@ void VisualizerUtilities::showOrientationData( pause = !pause; auto& text = static_cast( simbodyVisualizer.updDecoration(pausedIndex)); - text.setText( - pause ? "Paused (hit Space to resume)" - : "(hit Esc. to exit, Space to pause)"); + text.setText(pause ? "Paused (hit Space to resume)" + : "(hit Esc. to exit, Space to pause)"); simbodyVisualizer.drawFrameNow(state); } } if (pause) { std::this_thread::sleep_for(std::chrono::milliseconds(5)); - if (frameNumber>0) frameNumber--; + if (frameNumber > 0) frameNumber--; } - simbodyVisualizer.setSliderValue(timeSliderIndex, times[frameNumber]); + simbodyVisualizer.setSliderValue( + timeSliderIndex, times[frameNumber]); } } } -void VisualizerUtilities::addVisualizerControls(ModelVisualizer& vizualizer, - double initialTime, double finalTime) { +void VisualizerUtilities::addVisualizerControls( + ModelVisualizer& vizualizer, double initialTime, double finalTime) { auto& simbodyViz = vizualizer.updSimbodyVisualizer(); - //simbodyViz.setMode(SimTK::Visualizer::RealTime); + // simbodyViz.setMode(SimTK::Visualizer::RealTime); simbodyViz.setDesiredBufferLengthInSec(0); simbodyViz.setDesiredFrameRate(30); simbodyViz.setShowSimTime(true); - //viz.setBackgroundType(viz.SolidColor); + // viz.setBackgroundType(viz.SolidColor); // viz.setBackgroundColor(SimTK::White); // viz.setShowFrameRate(true); // viz.setShowFrameNumber(true); @@ -512,7 +516,7 @@ void VisualizerUtilities::addVisualizerControls(ModelVisualizer& vizualizer, // Add slider to control playback. const int realTimeScaleSliderIndex = 1; - //simbodyViz.addSlider("Speed", realTimeScaleSliderIndex, minRealTimeScale, + // simbodyViz.addSlider("Speed", realTimeScaleSliderIndex, minRealTimeScale, // maxRealTimeScale, realTimeScale); // TODO this slider results in choppy playback if not paused. @@ -531,6 +535,4 @@ void VisualizerUtilities::addVisualizerControls(ModelVisualizer& vizualizer, keyBindingsMenu.push_back(std::make_pair("Zoom to fit: R", 4)); keyBindingsMenu.push_back(std::make_pair("Quit: Esc", 5)); simbodyViz.addMenu("Key bindings", 1, keyBindingsMenu); - - } diff --git a/OpenSim/Simulation/VisualizerUtilities.h b/OpenSim/Simulation/VisualizerUtilities.h index 46831312f8..1897c8bcfe 100644 --- a/OpenSim/Simulation/VisualizerUtilities.h +++ b/OpenSim/Simulation/VisualizerUtilities.h @@ -33,12 +33,12 @@ class OSIMSIMULATION_API VisualizerUtilities { /// @name Visualize a motion of a model using the simbody-visualizer /// @{ - /// Play back an existing motion (from the Storage) in the + /// Play back an existing motion (from the table) in the /// simbody-visualizer. The Storage should contain all generalized /// coordinates. The visualizer window allows the user to control playback /// speed. This function blocks until the user exits the simbody-visualizer /// window. - static void showMotion(Model, Storage); + static void showMotion(Model, TimeSeriesTable); /// @} /// Visualize the passed in model in a simbody-visualizer window. diff --git a/doc/doxyfile_shared.in b/doc/doxyfile_shared.in index 0095ebd8b8..cf35fb23fd 100644 --- a/doc/doxyfile_shared.in +++ b/doc/doxyfile_shared.in @@ -204,7 +204,11 @@ TAB_SIZE = 4 # will result in a user-defined paragraph with heading "Side Effects:". # You can put \n's in the value part of an alias to insert newlines. -ALIASES = +ALIASES = "precondition=@par Precondition(s):^^" \ + "postcondition=@par Postcondition(s):^^" \ + "samplefile=^^Sample file:^^@verbatim^^" \ + "endsamplefile=@endverbatim" \ + "underdevelopment=@note This class is still under active development and should be used with caution." # This tag can be used to specify a number of word-keyword mappings (TCL only). # A mapping has the form "name=value". For example adding @@ -747,7 +751,8 @@ EXCLUDE_PATTERNS = *.svn \ */Matlab/* \ */Utilities/* \ */Applications/* \ - */Sandbox/* + */Sandbox/* \ + */Auxiliary/* # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names # (namespaces, classes, functions, etc.) that should be excluded from the