diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp index 19b3ce4e1ba88b20c6e205e359dc0710f7f9140c..86764c9675b2c0aa47842ace863a94ed03c0ac52 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp +++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp @@ -1,6 +1,8 @@ #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; }