Merge pull request #297 from pariterre/dev

Added Kistler FPCOPPOLY tag
pyomeca · Oct 10, 2023 · 03cafd1 · 03cafd1
2 parents c58e2c6 + 44adee2
commit 03cafd1
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 13 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(ezc3d VERSION 1.5.4)
+project(ezc3d VERSION 1.5.5)
 
 option(BUILD_SHARED_LIBS "Choose if build should be a dynamic or static library" ON)
 # Option to allow checking of size for matrix getter (slower but safer if activated)

diff --git a/external/gtest b/external/gtest
diff --git a/include/ezc3d/modules/ForcePlatforms.h b/include/ezc3d/modules/ForcePlatforms.h
@@ -88,6 +88,8 @@ class EZC3D_API ezc3d::Modules::ForcePlatform{
     std::vector<ezc3d::Vector3d> _CoP;  ///< Center of Pressure vectors for all instants (including subframes) in global reference frame
     std::vector<ezc3d::Vector3d> _Tz;  ///< Moment [0, 0, Tz] vectors for all instants (including subframes) expressed at the CoP
 
+    std::vector<double> _type3copPoly; ///< The polynomial coefficients of the Kistler platforms
+
 public:
     ///
     /// \brief Returns the number of frame recorded

diff --git a/src/modules/ForcePlatforms.cpp b/src/modules/ForcePlatforms.cpp
@@ -7,10 +7,13 @@
 /// \date March 25th, 2020
 ///
 
+#include <cmath>
+
 #include "ezc3d/modules/ForcePlatforms.h"
 #include "ezc3d/ezc3d_all.h"
 #include <stdexcept>
 
+
 ezc3d::Modules::ForcePlatform::ForcePlatform()
 {
 
@@ -153,11 +156,12 @@ void ezc3d::Modules::ForcePlatform::extractType(
     else if (_type == 2 || _type == 4){
 
     }
-    else if (_type == 3 || _type == 7){
-        if (_type == 7){
-            throw std::runtime_error("Type 7 is not supported yet, "
-                                     "please open an Issue on github for "
-                                     "support");
+    else if (_type == 3){
+        const auto& copPoly = c3d.parameters().group("FORCE_PLATFORM").parameter("FPCOPPOLY").valuesAsDouble();
+        if (copPoly.size() != 0){
+            _type3copPoly = std::vector<double>(copPoly.begin() + idx * 12, copPoly.begin() + (idx + 1) * 12 + 1);
+        } else {
+            _type3copPoly = std::vector<double>(12);
         }
     }
     else if (_type == 5){
@@ -168,6 +172,10 @@ void ezc3d::Modules::ForcePlatform::extractType(
         throw std::runtime_error("Type 6 is not supported yet, please "
                                  "open an Issue on github for support");
     }
+    else if (_type == 7){
+        throw std::runtime_error("Type 7 is not supported yet, please "
+                                 "open an Issue on github for support");
+    }
     else if (_type == 11 || _type == 12){
         throw std::runtime_error("Kistler Split Belt Treadmill is not "
                                  "supported for ForcePlatform analysis");
@@ -428,6 +436,19 @@ void ezc3d::Modules::ForcePlatform::extractData(
                             -moment_raw(1)/force_raw(2),
                             moment_raw(0)/force_raw(2),
                             0);
+                if (_type == 3){
+                    // The following is based on https://nbviewer.org/github/BMClab/BMC/blob/master/notebooks/KistlerForcePlateCalculation.ipynb
+                    // With th difference that offset are added after being computed, which correspond to the actual documentation provided by 
+                    // Kistler, as discussed in here: https://github.com/pyomeca/ezc3d/issues/253 
+                    double xOffset = 
+                        (_type3copPoly[0] * std::pow(CoP_raw.y(), 4) + _type3copPoly[ 1] * std::pow(CoP_raw.y(), 2) + _type3copPoly[ 2]) * std::pow(CoP_raw.x(), 3) + 
+                        (_type3copPoly[3] * std::pow(CoP_raw.y(), 4) + _type3copPoly[ 4] * std::pow(CoP_raw.y(), 2) + _type3copPoly[ 5]) * CoP_raw.x();
+                    double yOffset = 
+                        (_type3copPoly[6] * std::pow(CoP_raw.x(), 4) + _type3copPoly[ 7] * std::pow(CoP_raw.x(), 2) + _type3copPoly[ 8]) * std::pow(CoP_raw.y(), 3) + 
+                        (_type3copPoly[9] * std::pow(CoP_raw.x(), 4) + _type3copPoly[10] * std::pow(CoP_raw.x(), 2) + _type3copPoly[11]) * CoP_raw.y();
+                    CoP_raw(0) -= xOffset;
+                    CoP_raw(1) -= yOffset;
+                }
                 _CoP[cmp] = _refFrame * CoP_raw + _meanCorners;
                 _Tz[cmp] = _refFrame * static_cast<Vector3d>(
                             moment_raw - force_raw.cross(-1*CoP_raw));

diff --git a/test/test_modules.cpp b/test/test_modules.cpp
@@ -196,13 +196,13 @@ TEST(ForcePlatForm, Type3){
     EXPECT_DOUBLE_EQ(moments[0](1), -7249.1753702378046);
     EXPECT_DOUBLE_EQ(moments[0](2), -29.186897835585174);
 
-    EXPECT_DOUBLE_EQ(cop[0](0), 447.0933749250247);
-    EXPECT_DOUBLE_EQ(cop[0](1), 291.11710822600031);
-    EXPECT_DOUBLE_EQ(cop[0](2), -5.7480063534872716);
+    EXPECT_DOUBLE_EQ(cop[0](0), 446.65133312146855);
+    EXPECT_DOUBLE_EQ(cop[0](1), 291.14651198373156);
+    EXPECT_DOUBLE_EQ(cop[0](2), -5.7485786342715768);
 
-    EXPECT_DOUBLE_EQ(Tz[0](0), -0.001218477848857239);
-    EXPECT_DOUBLE_EQ(Tz[0](1), -0.0013630671086886962);
-    EXPECT_DOUBLE_EQ(Tz[0](2), 0.87114378914262325);
+    EXPECT_DOUBLE_EQ(Tz[0](0), -26.062938932447089);
+    EXPECT_DOUBLE_EQ(Tz[0](1), -391.8241595747582);
+    EXPECT_DOUBLE_EQ(Tz[0](2), -0.33922107657657574);
 
     // CAL_MATRIX
     for (size_t i=0; i<2; ++i){