Merge pull request #2198 from opensim-org/express_C3D_forces_at_optional_location

Express c3d forces at optional location

Author: chrisdembia

Bindings/Java/tests/TestC3DFileAdapter.java

@@ -5,7 +5,8 @@ class TestC3DFileAdapter {
     public static void test_C3DFileAdapter() {
         C3DFileAdapter c3dAdapter = new C3DFileAdapter();
         StdMapStringTimeSeriesTableVec3 tables =
-            c3dAdapter.read("walking5.c3d");
+            c3dAdapter.read("walking5.c3d", 
+            C3DFileAdapter.ForceLocation.CenterOfPressure);
 
         // Marker data read from C3D.
         TimeSeriesTableVec3 markerTable = tables.get("markers");

Bindings/Python/tests/test_DataAdapter.py

@@ -36,7 +36,7 @@ def test_C3DFileAdapter(self):
         except AttributeError:
             # C3D support not available. OpenSim was not compiled with BTK.
             return
-        tables = adapter.read(os.path.join(test_dir, 'walking2.c3d'))
+        tables = adapter.read(os.path.join(test_dir, 'walking2.c3d'), 0)
         markers = tables['markers']
         forces = tables['forces']
 
@@ -45,7 +45,7 @@ def test_C3DFileAdapter(self):
         assert forces.getNumRows()     == 9992
         assert forces.getNumColumns()  == 6
 
-        tables = adapter.read(os.path.join(test_dir, 'walking5.c3d'))
+        tables = adapter.read(os.path.join(test_dir, 'walking5.c3d'), 1)
 
         # Marker data read from C3D.
         markers = tables['markers']

Bindings/common.i

@@ -382,6 +382,27 @@ namespace OpenSim {
 %include <OpenSim/Common/CSVFileAdapter.h>
 %include <OpenSim/Common/C3DFileAdapter.h>
 
+%extend OpenSim::C3DFileAdapter {
+    Tables read(const std::string& fileName, unsigned int wrt) {
+        C3DFileAdapter::ForceLocation location;
+        switch(wrt) {
+            case 0:
+                location = C3DFileAdapter::ForceLocation::OriginOfForcePlate;
+                break;
+            case 1:
+                location = C3DFileAdapter::ForceLocation::CenterOfPressure;
+                break;
+            case 2:
+                location = C3DFileAdapter::ForceLocation::PointOfWrenchApplication;
+                break;
+            default:
+                throw OpenSim::Exception{
+                    "An invalid C3DFileAdapter::ForceLocation was provided."};
+        }
+        return C3DFileAdapter::read(fileName, location);
+    };
+};
+
 namespace OpenSim {
     %ignore TableSource_::TableSource_(TableSource_ &&);
 }

OpenSim/Auxiliary/auxiliaryTestFunctions.h

@@ -33,24 +33,73 @@
 #include <fstream>
 #include <string>
 #include <regex>
+#include <type_traits>
 
-template <typename T>
-void ASSERT_EQUAL(T expected, 
+ /**
+ * ASSERT_EQUAL is a general utility for comparing two values and throwing
+ * an Exception with a caller defined message when values are not equivalent.
+ * Note, ASSERT_EQUAL is typically used to verify that some found value matches
+ * a given expected or standard value. If the expected value is NaN, 
+ * ASSERT_EQUAL will NOT throw if the found value is also NaN. This is
+ * particularly helpful for comparing motion capture data where missing data
+ * are denoted by NaN values. If NaNs are not acceptable for your test, then
+ * the expected value should not be NaN. In the case of floating point values
+ * (or containers of floating points) a tolerance of the same value type is
+ * required.
+ */
+template <typename T,
+   typename std::enable_if<std::is_floating_point<T>::value>::type* = nullptr >
+void ASSERT_EQUAL(T expected,
                   T found, 
                   T tolerance, 
                   std::string file = "", 
                   int line = -1, 
                   std::string message = "") {
+    // if both values are NaN treat them as being equivalent for the
+    // sake of comparing experimental data and results where NaNs are
+    // possible
+    if(SimTK::isNaN(found) && SimTK::isNaN(expected))
+        return;
+    if (found < expected - tolerance || found > expected + tolerance)
+        throw OpenSim::Exception(message, file, line);
+}
+
+template <typename T,
+    typename std::enable_if<std::is_integral<T>::value>::type* = nullptr >
+    void ASSERT_EQUAL(T expected,
+                      T found,
+                      std::string file = "",
+                      int line = -1,
+                      std::string message = "") {
+    if (found != expected)
+        throw OpenSim::Exception(message, file, line);
+}
+
+template <typename T,
+    typename std::enable_if<!std::is_arithmetic<T>::value>::type* = nullptr >
+    void ASSERT_EQUAL(T expected,
+                      T found,
+                      T tolerance,
+                      std::string file = "",
+                      int line = -1,
+                      std::string message = "") {
+    // if both values are NaN treat them as equivalent 
+    if (found.isNaN() && expected.isNaN() )
+        return;
     if (found < expected - tolerance || found > expected + tolerance)
         throw OpenSim::Exception(message, file, line);
 }
+
 template<int M, typename ELT, int STRIDE>
 void ASSERT_EQUAL(const SimTK::Vec<M, ELT, STRIDE>& vecA,
                   const SimTK::Vec<M, ELT, STRIDE>& vecB,
                   const std::string& file = "",
                   int line = -1,
                   const std::string& message = "") {
     try {
+        // if both values are NaN treat them as being equivalent
+        if (vecA.isNaN() && vecB.isNaN())
+            return;
         SimTK_TEST_EQ(vecA, vecB);
     } catch(const SimTK::Exception::Assert&) {
         throw OpenSim::Exception(message, file, line);
@@ -64,12 +113,37 @@ void ASSERT_EQUAL(const SimTK::Vec<M, ELT, STRIDE>& vecA,
                   int line = -1,
                   const std::string& message = "") {
     try {
+        // if both values are NaN treat them as being equivalent
+        if (vecA.isNaN() && vecB.isNaN())
+            return;
         SimTK_TEST_EQ_TOL(vecA, vecB, tolerance);
     } catch(const SimTK::Exception::Assert&) {
         throw OpenSim::Exception(message, file, line);
     }
 }
 
+template<typename Container, typename T>
+void ASSERT_EQUAL( const Container& vecA,
+                   const Container& vecB,
+                    T tolerance,
+                    std::string file = "",
+                    int line = -1,
+                    std::string message = "") {
+
+    if (vecA.size() != vecB.size()) {
+        throw OpenSim::Exception(message, file, line);
+    }
+    else {
+        for (auto i = 0; i < vecA.size(); ++i) {
+            // if both values are NaN treat them as being equivalent
+            if ( SimTK::isNaN(vecA[i]) && SimTK::isNaN(vecB[i]) )
+                continue;
+            if (vecA[i] < vecB[i] - tolerance || vecA[i] > vecB[i] + tolerance) {
+                throw OpenSim::Exception(message, file, line);
+            }
+        }
+    }
+}
 
 inline void ASSERT(bool cond, 
                    std::string file="", 

OpenSim/Common/C3DFileAdapter.cpp

@@ -44,8 +44,12 @@ C3DFileAdapter::clone() const {
 }
 
 C3DFileAdapter::Tables
-C3DFileAdapter::read(const std::string& fileName) {
-    auto abstables = C3DFileAdapter{}.extendRead(fileName);
+C3DFileAdapter::read(const std::string& fileName, ForceLocation wrt)
+{
+    C3DFileAdapter c3dreader{};
+    c3dreader.setLocationForForceExpression(wrt);
+
+    auto abstables = c3dreader.extendRead(fileName);
     auto marker_table = 
         std::static_pointer_cast<TimeSeriesTableVec3>(abstables.at(_markers));
     auto force_table = 
@@ -108,13 +112,23 @@ C3DFileAdapter::extendRead(const std::string& fileName) const {
 
         double time_step{1.0 / acquisition->GetPointFrequency()};
         for(int f = 0; f < marker_nrow; ++f) {
-            SimTK::RowVector_<SimTK::Vec3> row{marker_pts->GetItemNumber()};
+            SimTK::RowVector_<SimTK::Vec3> row{ marker_pts->GetItemNumber(), 
+                                                SimTK::Vec3(SimTK::NaN) };
             int m{0};
             for(auto it = marker_pts->Begin();  it != marker_pts->End(); ++it) {
                 auto pt = *it;
-                row[m++] = SimTK::Vec3{pt->GetValues().coeff(f, 0),
-                                       pt->GetValues().coeff(f, 1),
-                                       pt->GetValues().coeff(f, 2)};
+                // BTK reads empty values as zero, but sets a "residual" value
+                // to -1 and it is how it knows to export these values as 
+                // blank, instead of 0,  when exporting to .trc
+                // See: BTKCore/Code/IO/btkTRCFileIO.cpp#L359-L360
+                // Read in value if it is not zero or residual is not -1
+                if (!pt->GetValues().row(f).isZero() ||    //not precisely zero
+                    (pt->GetResiduals().coeff(f) != -1) ) {//residual is not -1
+                    row[m] = SimTK::Vec3{ pt->GetValues().coeff(f, 0),
+                                          pt->GetValues().coeff(f, 1),
+                                          pt->GetValues().coeff(f, 2) };
+                }
+                ++m;
             }
 
             marker_matrix.updRow(f) = row;
@@ -169,6 +183,8 @@ C3DFileAdapter::extendRead(const std::string& fileName) const {
         // Get ground reaction wrenches for the force platform.
         auto ground_reaction_wrench_filter = 
             btk::GroundReactionWrenchFilter::New();
+        ground_reaction_wrench_filter->setLocation(
+            btk::GroundReactionWrenchFilter::Location(getLocationForForceExpression()));
         ground_reaction_wrench_filter->SetInput(*platform);
         auto wrench_collection = ground_reaction_wrench_filter->GetOutput();
         ground_reaction_wrench_filter->Update();

OpenSim/Common/C3DFileAdapter.h

@@ -37,6 +37,52 @@ class OSIMCOMMON_API C3DFileAdapter : public FileAdapter {
     typedef std::vector<Event>                         EventTable; 
     typedef std::map<std::string, std::shared_ptr<TimeSeriesTableVec3>> Tables;
 
+    /** Enumerated list of locations in which read in forces are expressed.
+        Measurement from force plates can be expressed by the C3DFileAdapter
+        either at the OriginOfForcePlate (the default), CenterOfPressure, or
+        the PointOfWrenchApplication. It is an optional argument to 
+        C3DFileAdapter::read().
+
+        In the case of the CenterOfPressure (COP), the underlying assumptions
+        are that the ground plane (in which COP is defined) passes through the
+        lab origin (0,0,0) with the Z-axis as its normal vector.
+
+        The PointOfWrenchApplication (PWA) does not assume a plane of contact.
+        The PWA is an equivalent wrench in the lab frame and computed according
+        to Shimba 1984.
+        Takeshi Shimba, An estimation of center of gravity from force platform
+        data, Journal of Biomechanics, 17(1), pp53-60, 1984.
+
+        <b>C++ example</b>
+        \code{.cpp}
+        auto tables  =  C3DFileAdapter::read("myData.c3d", 
+                            C3DFileAdapter::ForceLocation::CenterOfPressure);
+        \endcode
+
+        <b>Python example</b>
+        \code{.py}
+        import opensim
+        tables = C3DFileAdapter.read("myData.c3d",
+                    opensim.C3DFileAdapter.ForceLocation_CenterOfPressure)
+        \endcode
+
+        <b>Java example</b>
+        \code{.java}
+        tables = C3DFileAdapter.read("myData.c3d",
+                    C3DFileAdapter.ForceLocation.CenterOfPressure);
+        \endcode
+
+        <b>MATLAB example</b>
+        \code{.m}
+        tables = C3DFileAdapter.read("myData.c3d", 1);
+        \endcode
+    */
+    enum class ForceLocation {
+        OriginOfForcePlate       = 0,   ///< 0 : the origin of the force-plate
+        CenterOfPressure         = 1,   ///< 1 : the center of pressure
+        PointOfWrenchApplication = 2    ///< 2 : PWA as defined by Shimba, 1984
+    };
+
     C3DFileAdapter()                                 = default;
     C3DFileAdapter(const C3DFileAdapter&)            = default;
     C3DFileAdapter(C3DFileAdapter&&)                 = default;
@@ -45,14 +91,27 @@ class OSIMCOMMON_API C3DFileAdapter : public FileAdapter {
     ~C3DFileAdapter()                                = default;
 
     C3DFileAdapter* clone() const override;
+
+    void setLocationForForceExpression(const ForceLocation location) {
+        _location = location;
+    }
+
+    const ForceLocation getLocationForForceExpression() const {
+        return _location;
+    }
 
     /** Read in a C3D file into separate markers and forces tables of type
         TimeSeriesTableVec3. The markers table has each column labeled by its
         corresponding marker name. For the forces table, the data are grouped
         by sensor (force-plate #) in force, point and moment order, with the
-        respective *f#*, *p#* and *m#* column labels. */
+        respective *f#*, *p#* and *m#* column labels. C3DFileAdpater provides
+        options for expressing the force-plate measurements either as the
+        net force and moments expressed at the ForcePlateOrigin, the 
+        CentereOfPressure, or the PointOfWrenchApplication (see above).
+        */
     static
-    Tables read(const std::string& fileName);
+    Tables read(const std::string& fileName, 
+                ForceLocation wrt = ForceLocation::OriginOfForcePlate);
 
     static
     void write(const Tables& markerTable, const std::string& fileName);
@@ -69,6 +128,8 @@ class OSIMCOMMON_API C3DFileAdapter : public FileAdapter {
 private:
     static const std::unordered_map<std::string, std::size_t> _unit_index;
 
+    ForceLocation _location{ ForceLocation::OriginOfForcePlate };
+
 };
 
 } // namespace OpenSim

OpenSim/Common/DataTable.h

@@ -782,7 +782,7 @@ class DataTable_ : public AbstractDataTable {
 
         if(index < getNumRows() - 1)
             for(size_t r = index; r < getNumRows() - 1; ++r)
-                _depData.updRow((int)index) = _depData.row((int)(index + 1));
+                _depData.updRow((int)r) = _depData.row((int)(r + 1));
 
         _depData.resizeKeep(_depData.nrow() - 1, _depData.ncol());
         _indData.erase(_indData.begin() + index);