From 41d2133638ad82209d3d707d40881f72418cfc65 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 27 Sep 2023 16:46:52 -0400 Subject: [PATCH 1/2] Added Kistler FPCOPPOLY tag --- external/gtest | 2 +- include/ezc3d/modules/ForcePlatforms.h | 2 ++ src/modules/ForcePlatforms.cpp | 29 +++++++++++++++++++++----- test/test_modules.cpp | 12 +++++------ 4 files changed, 33 insertions(+), 12 deletions(-) diff --git a/external/gtest b/external/gtest index 3880b13e..c9461a9b 160000 --- a/external/gtest +++ b/external/gtest @@ -1 +1 @@ -Subproject commit 3880b13e4c0b04ca88f69b9c93da6058bd836c34 +Subproject commit c9461a9b55ba954df0489bab6420eb297bed846b diff --git a/include/ezc3d/modules/ForcePlatforms.h b/include/ezc3d/modules/ForcePlatforms.h index 7cbce4dd..c4a1de76 100644 --- a/include/ezc3d/modules/ForcePlatforms.h +++ b/include/ezc3d/modules/ForcePlatforms.h @@ -88,6 +88,8 @@ class EZC3D_API ezc3d::Modules::ForcePlatform{ std::vector _CoP; ///< Center of Pressure vectors for all instants (including subframes) in global reference frame std::vector _Tz; ///< Moment [0, 0, Tz] vectors for all instants (including subframes) expressed at the CoP + std::vector _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 index 486e5633..06379995 100644 --- a/src/modules/ForcePlatforms.cpp +++ b/src/modules/ForcePlatforms.cpp @@ -7,10 +7,13 @@ /// \date March 25th, 2020 /// +#include + #include "ezc3d/modules/ForcePlatforms.h" #include "ezc3d/ezc3d_all.h" #include + 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(copPoly.begin() + idx * 12, copPoly.begin() + (idx + 1) * 12 + 1); + } else { + _type3copPoly = std::vector(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,17 @@ 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 + // which surpringly update CoP_raw.x() before computing both values (meaning CoP_raw.y() depends on the corrected x instead of the measured one). + 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(); + CoP_raw(1) -= + (_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[cmp] = _refFrame * CoP_raw + _meanCorners; _Tz[cmp] = _refFrame * static_cast( moment_raw - force_raw.cross(-1*CoP_raw)); diff --git a/test/test_modules.cpp b/test/test_modules.cpp index 6180ba53..60494cf1 100644 --- 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){ From 44adee24fdec99a2fa1946274d584ca33e91cae3 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Mon, 2 Oct 2023 13:30:00 -0400 Subject: [PATCH 2/2] Corrected the kistler equation --- CMakeLists.txt | 2 +- src/modules/ForcePlatforms.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7a9ade2d..fd2a7a00 100644 --- 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/src/modules/ForcePlatforms.cpp b/src/modules/ForcePlatforms.cpp index 06379995..6a89f4da 100644 --- a/src/modules/ForcePlatforms.cpp +++ b/src/modules/ForcePlatforms.cpp @@ -438,14 +438,16 @@ void ezc3d::Modules::ForcePlatform::extractData( 0); if (_type == 3){ // The following is based on https://nbviewer.org/github/BMClab/BMC/blob/master/notebooks/KistlerForcePlateCalculation.ipynb - // which surpringly update CoP_raw.x() before computing both values (meaning CoP_raw.y() depends on the corrected x instead of the measured one). + // 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(); - CoP_raw(1) -= + 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(