Plug-in Gait KAD & KADMed variants

modules/body/include/openma/body/plugingait.h

@@ -51,7 +51,9 @@ namespace body
     OPENMA_DECLARE_NODEID(PluginGait, SkeletonHelper)
 
   public:
-    PluginGait(int region, int side, Node* parent = nullptr);
+    typedef enum {Basic = 0x00, KAD, KADMed} Variant;
+
+    PluginGait(int region, int side, Variant variant, Node* parent = nullptr);
 
     double markerDiameter() const _OPENMA_NOEXCEPT;
     void setMarkerDiameter(double value) _OPENMA_NOEXCEPT;
@@ -83,6 +85,18 @@ namespace body
     void setRightAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT;
     double leftAsisTrochanterAPDistance() const _OPENMA_NOEXCEPT;
     void setLeftAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT;
+    double rightTibialTorsionOffset() const _OPENMA_NOEXCEPT;
+    void setRightTibialTorsionOffset(double value) _OPENMA_NOEXCEPT;
+    double leftTibialTorsionOffset() const _OPENMA_NOEXCEPT;
+    void setLeftTibialTorsionOffset(double value) _OPENMA_NOEXCEPT;
+    double rightThighRotationOffset() const _OPENMA_NOEXCEPT;
+    void setRightThighRotationOffset(double value) _OPENMA_NOEXCEPT;
+    double leftThighRotationOffset() const _OPENMA_NOEXCEPT;
+    void setLeftThighRotationOffset(double value) _OPENMA_NOEXCEPT;
+    double rightShankRotationOffset() const _OPENMA_NOEXCEPT;
+    void setRightShankRotationOffset(double value) _OPENMA_NOEXCEPT;
+    double leftShankRotationOffset() const _OPENMA_NOEXCEPT;
+    void setLeftShankRotationOffset(double value) _OPENMA_NOEXCEPT;
     double rightKneeWidth() const _OPENMA_NOEXCEPT;
     void setRightKneeWidth(double value) _OPENMA_NOEXCEPT;
     double leftKneeWidth() const _OPENMA_NOEXCEPT;
@@ -100,6 +114,8 @@ namespace body
     double rightStaticRotationOffset() const _OPENMA_NOEXCEPT;
     double leftStaticPlantarFlexionOffset() const _OPENMA_NOEXCEPT;
     double leftStaticRotationOffset() const _OPENMA_NOEXCEPT;
+    double rightAnkleAbAddOffset() const _OPENMA_NOEXCEPT;
+    double leftAnkleAbAddOffset() const _OPENMA_NOEXCEPT;
 
     virtual bool calibrate(Node* trials, Subject* subject) override;
     virtual LandmarksTranslator* defaultLandmarksTranslator() override;

modules/body/include/openma/body/plugingait_p.h

@@ -47,6 +47,9 @@
 #include "openma/base/macros.h" // _OPENMA_CONSTEXPR
 #include "openma/math.h"
 
+#include <Eigen/Geometry>
+#include <Eigen_openma/Utils/sign.h>
+
 #include <unordered_map>
 
 namespace ma
@@ -84,6 +87,12 @@ namespace body
       Property<PluginGait, double, &PluginGait::leftLegLength,                   &PluginGait::setLeftLegLength>                 {"leftLegLength"},
       Property<PluginGait, double, &PluginGait::rightAsisTrochanterAPDistance,   &PluginGait::setRightAsisTrochanterAPDistance> {"rightAsisTrochanterAPDistance"},
       Property<PluginGait, double, &PluginGait::leftAsisTrochanterAPDistance,    &PluginGait::setLeftAsisTrochanterAPDistance>  {"leftAsisTrochanterAPDistance"},
+      Property<PluginGait, double, &PluginGait::rightTibialTorsionOffset,        &PluginGait::setRightTibialTorsionOffset>      {"rightTibialTorsionOffset"},
+      Property<PluginGait, double, &PluginGait::leftTibialTorsionOffset,         &PluginGait::setLeftTibialTorsionOffset>       {"leftTibialTorsionOffset"},
+      Property<PluginGait, double, &PluginGait::rightThighRotationOffset,        &PluginGait::setRightThighRotationOffset>      {"rightThighRotationOffset"},
+      Property<PluginGait, double, &PluginGait::leftThighRotationOffset,         &PluginGait::setLeftThighRotationOffset>       {"leftThighRotationOffset"},
+      Property<PluginGait, double, &PluginGait::rightShankRotationOffset,        &PluginGait::setRightShankRotationOffset>      {"rightShankRotationOffset"},
+      Property<PluginGait, double, &PluginGait::leftShankRotationOffset,         &PluginGait::setLeftShankRotationOffset>       {"leftShankRotationOffset"},
       Property<PluginGait, double, &PluginGait::rightKneeWidth,                  &PluginGait::setRightKneeWidth>                {"rightKneeWidth"},
       Property<PluginGait, double, &PluginGait::leftKneeWidth,                   &PluginGait::setLeftKneeWidth>                 {"leftKneeWidth"},
       Property<PluginGait, double, &PluginGait::rightAnkleWidth,                 &PluginGait::setRightAnkleWidth>               {"rightAnkleWidth"},
@@ -95,18 +104,22 @@ namespace body
       Property<PluginGait, double, &PluginGait::rightStaticPlantarFlexionOffset>                                                {"rightStaticPlantarFlexionOffset"},
       Property<PluginGait, double, &PluginGait::rightStaticRotationOffset>                                                      {"rightStaticRotationOffset"},
       Property<PluginGait, double, &PluginGait::leftStaticPlantarFlexionOffset>                                                 {"leftStaticPlantarFlexionOffset"},
-      Property<PluginGait, double, &PluginGait::leftStaticRotationOffset>                                                       {"leftStaticRotationOffset"}
+      Property<PluginGait, double, &PluginGait::leftStaticRotationOffset>                                                       {"leftStaticRotationOffset"},
+      Property<PluginGait, double, &PluginGait::rightAnkleAbAddOffset>                                                          {"rightAnkleAbAddOffset"},
+      Property<PluginGait, double, &PluginGait::leftAnkleAbAddOffset>                                                           {"leftAnkleAbAddOffset"}
     )
 
   public:
-    PluginGaitPrivate(PluginGait* pint, const std::string& name, int region, int side);
+    PluginGaitPrivate(PluginGait* pint, const std::string& name, int region, int side, int variant);
     ~PluginGaitPrivate() _OPENMA_NOEXCEPT;
 
-    void computeHipJointCenter(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT;
+    void computeHipJointCentre(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT;
     bool calibrateLowerLimb(int side, const math::Position* HJC, ummp* landmarks) _OPENMA_NOEXCEPT;
     bool reconstructUpperLimb(Model* model, Trial* trial, int side, const math::Vector* u_torso, const math::Vector* o_torso, ummp* landmarks, double sampleRate, double startTime) const _OPENMA_NOEXCEPT;
     bool reconstructLowerLimb(Model* model, Trial* trial, int side, const math::Vector* HJC, ummp* landmarks, double sampleRate, double startTime) const _OPENMA_NOEXCEPT;
 
+    int Variant;
+
     double MarkerDiameter;
 
     bool HeadOffsetEnabled;
@@ -125,6 +138,12 @@ namespace body
     double LeftLegLength;
     double RightAsisTrochanterAPDistance; 
     double LeftAsisTrochanterAPDistance;
+    double RightTibialTorsionOffset;
+    double LeftTibialTorsionOffset;
+    double RightThighRotationOffset;
+    double LeftThighRotationOffset;
+    double RightShankRotationOffset;
+    double LeftShankRotationOffset;
     double RightKneeWidth;
     double LeftKneeWidth;
     double RightAnkleWidth;
@@ -137,9 +156,15 @@ namespace body
     double RightStaticRotationOffset;
     double LeftStaticPlantarFlexionOffset;
     double LeftStaticRotationOffset;
+    double RightAnkleAbAddOffset;
+    double LeftAnkleAbAddOffset;
+
+    using CalibrateJointFuncPtr = bool (*)(math::Position* , math::Position*, std::vector<double*>& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* );
+    CalibrateJointFuncPtr CalibrateKneeJointCentre;
+    CalibrateJointFuncPtr CalibrateAnkleJointCentre;
   };
 
-  inline void PluginGaitPrivate::computeHipJointCenter(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT
+  inline void PluginGaitPrivate::computeHipJointCentre(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT
   {
     //const double theta = 0.49567350756639; // 28.4 * M_PI / 180.0;
     //const double beta = 0.314159265358979; // 18.0 * M_PI / 180.0;
@@ -344,10 +369,11 @@ namespace math
     typename Nested<XprTwo>::type m_Xpr2;
     typename Nested<XprThree>::type m_Xpr3;
     double Offset;
+    double Beta;
 
   public:
-    ChordOp(double offset, const XprBase<XprOne>& x1, const XprBase<XprTwo>& x2, const XprBase<XprThree>& x3)
-    : m_Xpr1(x1), m_Xpr2(x2), m_Xpr3(x3), Offset(offset)
+    ChordOp(double offset, const XprBase<XprOne>& x1, const XprBase<XprTwo>& x2, const XprBase<XprThree>& x3, double beta)
+    : m_Xpr1(x1), m_Xpr2(x2), m_Xpr3(x3), Offset(offset), Beta(beta)
     {
       assert(this->m_Xpr1.rows() == this->m_Xpr2.rows());
       assert(this->m_Xpr2.rows() == this->m_Xpr3.rows());
@@ -370,7 +396,7 @@ namespace math
       const Vector u = v.cross(K - I).normalized();
       const Pose local(u, v, u.cross(v), (J + I) / 2.0);
       // Compute the angle to project I along v
-      const auto d = (I - J).norm();
+      const Scalar d = (I - J).norm();
       const auto theta = (d.residuals() >= 0).select((this->Offset / d.values()).asin()*2.0, 0.0);
       // Do this projection in the local frame
       Vector t = local.inverse().transform(I);
@@ -380,7 +406,73 @@ namespace math
       t.values().middleCols<1>(1) = _tempX * theta.cos() - _tempY * theta.sin() ;
       t.values().rightCols<1>() = _tempX * theta.sin() + _tempY * theta.cos() ;
       // Transform back the projected point and return the result
-      return Eigen::internal::ChordOpValues(local.transform(t).values());
+      Vector P = local.transform(t);
+      // Is there a second condition that the projected point need to meet?
+      // In case the beta angle is not null, an iterative solution is used to
+      // Find the best projected point with the specified offset and the angle.
+      // NOTE: THE PART BELOW IS AN ADAPTION OF THE CODE PROPOSED IN THE PYCGA PROJECT WHICH TAKES IT FROM MORGAN SANGEUX
+      if (fabs(this->Beta) > std::numeric_limits<double>::epsilon())
+      {
+
+        // Next part is iterative and realized for each sample
+        const double eps = 1e-5;
+        for (Index i = 0, len = P.rows() ; i < len ; ++i)
+        {
+          if (P.residuals().coeff(i) < 0.0)
+            continue;
+          int count = 0;
+          double sAlpha = 0, iAlpha = this->Beta, dBeta = fabs(this->Beta);
+          // Angle 
+          double ca = cos(theta.coeff(i) / 2.0);
+          // Distance where should be the new project point
+          double r = this->Offset * ca;
+          // Compute the projection of K on IJ using the Pythagorean theorem
+          const Eigen::Matrix<double,1,3> O = J.values().row(i) - v.values().row(i) * sqrt(d.values().coeff(i) * d.values().coeff(i) - this->Offset * this->Offset * (1 + ca * ca));
+          // Compute the local frame used to find the new projected point satisfaying an angle close to beta
+          Eigen::Matrix<double,1,3> Pi = P.values().row(i);
+          const Eigen::Matrix<double,1,3> w2 = v.values().row(i);
+          const Eigen::Matrix<double,1,3> v2 = w2.cross(Pi-O).normalized();
+          const Eigen::Matrix<double,1,3> u2 = v2.cross(w2).normalized();
+          // const Pose local2(v,v2,v2.cross(v).normalized(),O);
+          while ((dBeta > eps) && (count < 1000))
+          {
+            sAlpha += iAlpha;
+            // Compute the new projected point
+            double c = r * cos(sAlpha);
+            double s = r * sin(sAlpha);
+            P.values().coeffRef(i,0) = u2.coeff(0) * c + v2.coeff(0) * s + O.coeff(0);
+            P.values().coeffRef(i,1) = u2.coeff(1) * c + v2.coeff(1) * s + O.coeff(1);
+            P.values().coeffRef(i,2) = u2.coeff(2) * c + v2.coeff(2) * s + O.coeff(2);
+            // Verify if the new point projected on the plane corresponds to the angle beta
+            Pi = P.values().row(i);
+            const Eigen::Matrix<double,1,3> Ii = I.values().row(i);
+            const Eigen::Matrix<double,1,3> Ji = J.values().row(i);
+            const Eigen::Matrix<double,1,3> Ki = K.values().row(i);
+            Eigen::Matrix<double,1,3> nBone = Ji - Pi;
+            Eigen::Matrix<double,1,3> projK = nBone.cross((Ki - Pi).cross(nBone));
+            Eigen::Matrix<double,1,3> projI = nBone.cross((Ii - Pi).cross(nBone));
+            double iBeta = sign((projK.cross(projI) * nBone.transpose()).coeff(0)) * acos((projK * projI.transpose() / projK.norm() / projI.norm()).coeff(0));
+            double odBeta = dBeta;
+            dBeta = fabs(this->Beta - iBeta);
+            // Look for the direction and magnitude used by the next iteration
+            if ((dBeta - odBeta) > 0.0)
+            {
+              if (count == 0)
+              {
+                sAlpha -= iAlpha;
+                iAlpha *= -1.0;
+              }
+              else
+              {
+                iAlpha /= -2.0; 
+              }
+            }
+            ++count;
+          }
+        }
+      }
+      // Pass the result to be assigned in the result of the operation chord.
+      return Eigen::internal::ChordOpValues(P.values());
     };
 
     auto residuals() const _OPENMA_NOEXCEPT -> decltype(generate_residuals((OPENMA_MATHS_DECLVAL_NESTED(XprOne).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprTwo).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprThree).residuals() >= 0.0)))
@@ -390,9 +482,9 @@ namespace math
   };
 
   template <typename XprOne, typename XprTwo, typename XprThree>
-  static inline ChordOp<XprOne,XprTwo,XprThree> compute_chord(double offset, const XprBase<XprOne>& I, const XprBase<XprTwo>& J, const XprBase<XprThree>& K)
+  static inline ChordOp<XprOne,XprTwo,XprThree> compute_chord(double offset, const XprBase<XprOne>& I, const XprBase<XprTwo>& J, const XprBase<XprThree>& K, double beta = 0.0)
   {
-    return ChordOp<XprOne,XprTwo,XprThree>(offset,I,J,K);
+    return ChordOp<XprOne,XprTwo,XprThree>(offset,I,J,K,beta);
   };
 };
 };