Update eomcParser and embObjMotionControl to support the new velocity…

…Thresh param (#903)

CMakeLists.txt

@@ -19,7 +19,7 @@ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}"
 option(ICUB_COMPILE_BINDINGS "Compile optional language bindings"  FALSE)
 
 set(YCM_REQUIRED_VERSION 0.15.3)
-set(YARP_REQUIRED_VERSION 3.8.1)
+set(YARP_REQUIRED_VERSION 3.9.0)
 
 find_package(YCM ${YCM_REQUIRED_VERSION} REQUIRED)
 find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED)

conf/iCubFindDependencies.cmake

@@ -65,8 +65,8 @@ checkandset_dependency(OpenCV)
 checkandset_dependency(Qt5)
 
 if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
-  if(icub_firmware_shared_VERSION VERSION_LESS 1.37.0)
-    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.37.0 is required")
+  if(icub_firmware_shared_VERSION VERSION_LESS 1.37.1)
+    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.37.1 is required")
   endif()
 endif()
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -1346,6 +1346,7 @@ bool embObjMotionControl::init()
         jconfig.motor_params.friction.viscous_neg_val = _measureConverter->viscousNeg_user2raw(_trq_pids[logico].viscousNeg, fisico);
         jconfig.motor_params.friction.coulomb_pos_val = _measureConverter->coulombPos_user2raw(_trq_pids[logico].coulombPos, fisico);
         jconfig.motor_params.friction.coulomb_neg_val = _measureConverter->coulombNeg_user2raw(_trq_pids[logico].coulombNeg, fisico);
+        jconfig.motor_params.friction.velocityThres_val = _measureConverter->velocityThres_user2raw(_trq_pids[logico].velocityThres, fisico);
 
         jconfig.gearbox_E2J = _gearbox_E2J[logico];
 
@@ -3678,7 +3679,7 @@ bool embObjMotionControl::getRemoteVariableRaw(std::string key, yarp::os::Bottle
             MotorTorqueParameters params;
             getMotorTorqueParamsRaw(i, &params);
             char buff[1000];
-            snprintf(buff, 1000, "J %d : bemf %+3.3f bemf_scale %+3.3f ktau %+3.3f ktau_scale %+3.3f viscousPos %+3.3f viscousNeg %+3.3f coulombPos %+3.3f coulombNeg %+3.3f", i, params.bemf, params.bemf_scale, params.ktau, params.ktau_scale, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg);
+            snprintf(buff, 1000, "J %d : bemf %+3.3f bemf_scale %+3.3f ktau %+3.3f ktau_scale %+3.3f viscousPos %+3.3f viscousNeg %+3.3f coulombPos %+3.3f coulombNeg %+3.3f velocityThres %+3.3f", i, params.bemf, params.bemf_scale, params.ktau, params.ktau_scale, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg, params.velocityThres);
             r.addString(buff);
         }
         return true;
@@ -4102,8 +4103,9 @@ bool embObjMotionControl::getMotorTorqueParamsRaw(int j, MotorTorqueParameters *
     params->viscousNeg = eo_params.friction.viscous_neg_val ;
     params->coulombPos = eo_params.friction.coulomb_pos_val;
     params->coulombNeg = eo_params.friction.coulomb_neg_val;
+    params->velocityThres  = eo_params.friction.velocityThres_val;
 
-    //printf("debug getMotorTorqueParamsRaw %f %f %f %f %f %f %f %f\n",  params->bemf, params->bemf_scale, params->ktau,params->ktau_scale, params->viscousPos, params->viscousNeg, params->coulombPos, params->coulombNeg);
+    //printf("debug getMotorTorqueParamsRaw %f %f %f %f %f %f %f %f\n",  params->bemf, params->bemf_scale, params->ktau,params->ktau_scale, params->viscousPos, params->viscousNeg, params->coulombPos, params->coulombNeg, params->threshold);
 
     return true;
 }
@@ -4113,16 +4115,17 @@ bool embObjMotionControl::setMotorTorqueParamsRaw(int j, const MotorTorqueParame
     eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
     eOmc_motor_params_t eo_params = {0};
 
-    //printf("setMotorTorqueParamsRaw for j %d(INPUT): benf=%f ktau=%f viscousPos=%f viscousNeg=%f coulombPos=%f coulombNeg=%f\n",j, params.bemf, params.ktau, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg);
+    //printf("setMotorTorqueParamsRaw for j %d(INPUT): benf=%f ktau=%f viscousPos=%f viscousNeg=%f coulombPos=%f coulombNeg=%f\n",j, params.bemf, params.ktau, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg, params.threshold);
 
     eo_params.bemf_value  = (float)   params.bemf;
     eo_params.bemf_scale  = (uint8_t) params.bemf_scale;
     eo_params.ktau_value  = (float)   params.ktau;
     eo_params.ktau_scale  = (uint8_t) params.ktau_scale;
-    eo_params.friction.viscous_pos_val   = static_cast<float32_t>(params.viscousPos);
+    eo_params.friction.viscous_pos_val = static_cast<float32_t>(params.viscousPos);
     eo_params.friction.viscous_neg_val = static_cast<float32_t>(params.viscousNeg);
-    eo_params.friction.coulomb_pos_val   = static_cast<float32_t>(params.coulombPos);
+    eo_params.friction.coulomb_pos_val = static_cast<float32_t>(params.coulombPos);
     eo_params.friction.coulomb_neg_val = static_cast<float32_t>(params.coulombNeg);
+    eo_params.friction.velocityThres_val = static_cast<float32_t>(params.velocityThres);
 
 
     if(false == res->setRemoteValue(id32, &eo_params))

src/libraries/icubmod/embObjMotionControl/eomcParser.cpp

@@ -56,6 +56,7 @@ yarp::dev::eomc::Parser::Parser(int numofjoints, string boardname)
     _viscousNeg=allocAndCheck<double>(_njoints);
     _coulombPos=allocAndCheck<double>(_njoints);
     _coulombNeg=allocAndCheck<double>(_njoints);
+    _velocityThres=allocAndCheck<double>(_njoints);
 
     minjerkAlgoMap.clear();
     //directAlgoMap.clear();
@@ -71,6 +72,7 @@ Parser::~Parser()
     checkAndDestroy(_viscousNeg);
     checkAndDestroy(_coulombPos);
     checkAndDestroy(_coulombNeg);
+    checkAndDestroy(_velocityThres);
 }
 
 
@@ -901,7 +903,7 @@ bool Parser::parsePidsGroupDeluxe(Bottle& pidsGroup, Pid myPid[])
 
     Bottle xtmp;
 
-    if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints)) return false; 
+    if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints, false)) return false; 
     for (int j = 0; j<_njoints; j++) _kbemf[j] = xtmp.get(j + 1).asFloat64();
 
     if (!extractGroup(pidsGroup, xtmp, "ktau", "ktau parameter", _njoints)) return false; 
@@ -947,6 +949,21 @@ bool Parser::parsePidsGroupDeluxe(Bottle& pidsGroup, Pid myPid[])
         for (int j = 0; j<_njoints; j++) _coulombNeg[j] = xtmp.get(j + 1).asFloat64();
     }
 
+    if (!extractGroup(pidsGroup, xtmp, "velocityThres", "velocity threshold parameter for torque control", _njoints, false))
+    {
+        for (int j = 0; j<_njoints; j++) _velocityThres[j] = 0;
+    }
+    else
+    {
+        for (int j = 0; j<_njoints; j++) 
+        {
+            _velocityThres[j] = xtmp.get(j + 1).asFloat64();
+            std::cout << "_velocityThres[" << j << "] = " << _velocityThres[j] << std::endl;
+        }
+
+    }
+
+
     return true;
 }
 
@@ -1279,6 +1296,7 @@ bool Parser::getCorrectPidForEachJoint(PidInfo *ppids/*, PidInfo *vpids*/, TrqPi
             tpids[i].viscousNeg = _viscousNeg[i];
             tpids[i].coulombPos = _coulombPos[i];
             tpids[i].coulombNeg = _coulombNeg[i];
+            tpids[i].velocityThres = _velocityThres[i];
             tpids[i].filterType = _filterType[i];
         }
         else

src/libraries/icubmod/embObjMotionControl/eomcParser.h

@@ -197,6 +197,7 @@ class TrqPidInfo : public PidInfo
     double viscousNeg;
     double coulombPos;
     double coulombNeg;
+    double velocityThres;
     int    filterType;
 };
 
@@ -349,6 +350,7 @@ class Parser
     double *_viscousNeg;
     double *_coulombPos;
     double *_coulombNeg;
+    double *_velocityThres;