Add TableUtilities and more CommonUtilities.

Applications/opensim-cmd/opensim-cmd_viz.h

@@ -107,7 +107,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/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,22 @@ 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 the following utility functions:
+  - make_unique()
+  - createVector() (using a C++ initializer list)
+  - createVectorLinspace()
+  - interpolate()
+  - updateStateLabels40() to update column labels from v3.3 to v4.0 syntax.
+  - solveBisection()
+- Introduce the following utilities for TimeSeriesTable
+  - TableUtilities::checkNonUniqueLabels()
+  - TableUtilities::isInDegrees()
+  - TableUtilities::findStateLabelIndex()
+  - TableUtilities::filterLowpass()
+  - TableUtilities::pad()
+  - TableUtilities::resample()
+  - TableUtilities::resampleWithInterval()
+- StatesTrajectories can now be created from a TimeSeriesTable of states.
 
 
 v4.1

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,117 @@ 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::getAbsolutePathnameFromXMLDocument(
+        const std::string& documentFileName,
+        const std::string& pathnameRelativeToDocument) {
+    // 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, pathnameRelativeToDocument);
+}
+
+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 auto x = createVectorLinspace(1000, left, right);
+        TimeSeriesTable table;
+        table.setColumnLabels({"residual"});
+        SimTK::RowVector row(1);
+        for (int i = 0; i < x.nrow(); ++i) {
+            row[0] = calcResidual(x[i]);
+            table.appendRow(x[i], row);
+        }
+        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
+/// linearly spaced between start and end.
+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 getAbsolutePathnameFromXMLDocument(
+        const std::string& documentFileName,
+        const std::string& pathnameRelativeToDocument);
+
+/// 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 Logger::shouldLog(Logger::Level::Debug), 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,21 @@ Uppercase(const std::string &aStr)
     return result;
 }
 
+bool IO::StartsWith(const std::string& string, const std::string& start) {
+    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) {
+    if (string.length() >= ending.length()) {
+        return string.compare(string.length() - ending.length(),
+                              ending.length(), ending) == 0;
+    }
+    return false;
+}
+
 void IO::eraseEmptyElements(std::vector<std::string>& list)
 {
     std::vector<std::string>::iterator it = list.begin();

OpenSim/Common/IO.h

@@ -120,6 +120,12 @@ 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`.
+    /// https://stackoverflow.com/questions/874134/find-if-string-ends-with-another-string-in-c
+    static bool StartsWith(const std::string& string, const std::string& start);
+    /// Determine if `string` ends with the substring `ending`.
+    /// https://stackoverflow.com/questions/874134/find-if-string-ends-with-another-string-in-c
+    static bool EndsWith(const std::string& string, const std::string& ending);
     static void eraseEmptyElements(std::vector<std::string>& list);
 //=============================================================================
 };  // END CLASS IO

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<double> 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<double> Signal::
 Pad(int aPad,int aN,const double aSignal[])
 {
-    if(aPad<=0) return(NULL);
+    if (aPad == 0) return std::vector<double>(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<double> 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<aPad+aPad+aN;i++,j--)  s[i] = aSignal[j];
     for(i=aPad+aN;i<aPad+aPad+aN;i++)  s[i] = 2.0*aSignal[aN-1] - s[i];
 
-    return(s);
+    return s;
 }
 //_____________________________________________________________________________
 /**