Add TableUtilities and more CommonUtilities.

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["<model-file>"].asString());
         if (args["<states-file>"]) {
-            Storage states(args["<states-file>"].asString());
+            TimeSeriesTable states(args["<states-file>"].asString());
             VisualizerUtilities::showMotion(model, states);
         } else {
             VisualizerUtilities::showModel(model);

Bindings/OpenSimHeaders_common.h

@@ -57,6 +57,7 @@
 #include <OpenSim/Common/StorageInterface.h>
 #include <OpenSim/Common/TRCFileAdapter.h>
 #include <OpenSim/Common/TableSource.h>
+#include <OpenSim/Common/TableUtilities.h>
 #include <OpenSim/Common/TimeSeriesTable.h>
 #include <OpenSim/Common/Units.h>
 #include <OpenSim/Common/XYFunctionInterface.h>

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))

Bindings/common.i

@@ -337,6 +337,7 @@ DATATABLE_CLONE(double, SimTK::Rotation_<double>)
 %include <OpenSim/Common/AbstractDataTable.h>
 %include <OpenSim/Common/DataTable.h>
 %include <OpenSim/Common/TimeSeriesTable.h>
+%include <OpenSim/Common/TableUtilities.h>
 
 %template(DataTable)           OpenSim::DataTable_<double, double>;
 %template(DataTableVec3)       OpenSim::DataTable_<double, SimTK::Vec3>;

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`

OpenSim/Common/CommonUtilities.cpp

@@ -23,12 +23,17 @@
 
 #include "CommonUtilities.h"
 
+#include "PiecewiseLinearFunction.h"
+#include "STOFileAdapter.h"
+#include "TimeSeriesTable.h"
 #include <chrono>
 #include <ctime>
 #include <iomanip>
 #include <memory>
 #include <sstream>
 
+#include <SimTKcommon/internal/Pathname.h>
+
 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<SimTK::Real> 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<double> x_no_nans;
+    std::vector<double> 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<SimTK::Real(const SimTK::Real&)> 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<double>(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;
+}

OpenSim/Common/CommonUtilities.h

@@ -24,10 +24,21 @@
  * -------------------------------------------------------------------------- */
 
 #include "osimCommonDLL.h"
+#include <functional>
 #include <iostream>
+#include <memory>
+
+#include <SimTKcommon/internal/BigMatrix.h>
 
 namespace OpenSim {
 
+/// Since OpenSim does not require C++14 (which contains std::make_unique()),
+/// here is an implementation of make_unique().
+template <typename T, typename... Args>
+std::unique_ptr<T> make_unique(Args&&... args) {
+    return std::unique_ptr<T>(new T(std::forward<Args>(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<SimTK::Real> 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_<timestamp>.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<double(const double&)> calcResidual,
+        double left, double right, const double& tolerance = 1e-6,
+        int maxIterations = 1000);
+
 } // namespace OpenSim
 
 #endif // OPENSIM_COMMONUTILITIES_H_

OpenSim/Common/Exception.h

@@ -317,6 +317,11 @@ class ComponentNotFound : public Exception {
     }
 };
 
+class NonUniqueLabels : public OpenSim::Exception {
+public:
+    using Exception::Exception;
+};
+
 }; //namespace
 //=============================================================================
 //=============================================================================

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<std::string>& list)
 {
     std::vector<std::string>::iterator it = list.begin();

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<std::string>& list);
 //=============================================================================
 };  // END CLASS IO

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)