fix torque estimation for gripper joint (type 16)

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp

 #include "TorqueEstimation.h"
 #include <math.h>
+#include <ArmarXCore/core/logging/Logging.h>
 
+/* --------- !!! Do not change the following function for motor model "motor16" !!! ----------- */
 float MotorTorqueEstimator::linearModel_dcx16(int32_t nI, int32_t pwm)
 {
     // 4096: ticks per turn
@@ -37,6 +39,7 @@ float MotorTorqueEstimator::linearModel_dcx16(int32_t nI, int32_t pwm)
     return T;
 }
 
+/* --------- !!! the following function are not used anymore, instead use the neural network for "motor22" !!! ----------- */
 float MotorTorqueEstimator::linearModel_dcx22(float nI, int32_t pwm)
 {
     float n = nI * 60.f / M_PI / 2.f * 231.f; //rad/s -> turns/min
@@ -57,45 +60,31 @@ float MotorTorqueEstimator::linearModel_dcx22(float nI, int32_t pwm)
 
 void MotorTorqueEstimator::estimateTorque(float vel, int32_t pwm)//, int motorModel, bool linearModelOnly = false)
 {
-    // calculate the torque based on the linear model
-    //    std::cout << "model type: " << motorModel << std::endl;
-    if (motorModel == "Motor16")
-    {
-        torque = linearModel_dcx16(vel, pwm);
-    }
-    else if (motorModel == "Motor22")
-    {
-        torque = linearModel_dcx22(vel, pwm);
-    }
-    else
-    {
-        //std::cout << "motor model should be set to 'Motor16' or 'Motor22'" << std::endl;
-        torque = 0.0;
-    }
-
+    torque = 0.0f;
     // calculate the torque induced by non-linear model
     if (!linearModelOnly)
     {
         float vel_input = (float)vel / vel_factor;
         float pwm_input = (float)pwm / pwm_factor;
-        static float pwmXvel_old = 0;
+        //        static float pwmXvel_old = 0;
         float pwmXvel = vel_input * pwm_input;
-        if (pwmXvel < 0)
-        {
-            pwmXvel = -1;
-            pwmXvel_old = pwmXvel;
-        }
-        else if (pwmXvel > 0)
-        {
-            pwmXvel = 1;
-            pwmXvel_old = pwmXvel;
-        }
-        else
-        {
-            pwmXvel = pwmXvel_old;
-        }
+        //        if (pwmXvel < 0)
+        //        {
+        //            pwmXvel = -1;
+        //            pwmXvel_old = pwmXvel;
+        //        }
+        //        else if (pwmXvel > 0)
+        //        {
+        //            pwmXvel = 1;
+        //            pwmXvel_old = pwmXvel;
+        //        }
+        //        else
+        //        {
+        //            pwmXvel = pwmXvel_old;
+        //        }
 
         torque += network->feedforward(vel_input, pwm_input, pwmXvel);
+        //        torque += network->feedforward(vel_input, pwm_input, 0.0f);
     }
 
     /*// calculating the torque induced by motor inertia
@@ -109,5 +98,14 @@ void MotorTorqueEstimator::estimateTorque(float vel, int32_t pwm)//, int motorMo
     float acc = (float) 0.001 * (angVel - vel_old) / deltaT;
     vel_old = angVel;
     float inertiaTorque = acc * inertia;*/
+
+    /* --------- !!! Do not change the following if-code for motor model "motor16" !!! ----------- */
+    if (motorModel == "Motor16")
+    {
+        //        ARMARX_INFO << deactivateSpam(0.01) << "motor 16, torque: " << torque;
+        torque = linearModel_dcx16(vel, pwm);
+    }
+
     torqueWithInertia = torque;// + inertiaTorque;
+    //    ARMARX_INFO << deactivateSpam(0.01) << "motor 16, torqueWithInertia: " << torqueWithInertia;
 }