Added LuGre friction model parameters to protocol

conf/iCubFindDependencies.cmake

@@ -64,7 +64,7 @@ checkandset_dependency(IPOPT)
 checkandset_dependency(OpenCV)
 checkandset_dependency(Qt5)
 
-set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.41.1)
+set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.41.2)
 
 if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
   if(icub_firmware_shared_VERSION VERSION_LESS ${MINIMUM_REQUIRED_icub_firmware_shared_VERSION})

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -125,6 +125,7 @@ bool embObjMotionControl::alloc(int nj)
     _joint2set.resize(nj);
     _timeouts.resize(nj);
     _impedance_params.resize(nj);
+    _lugre_params.resize(nj);
     _axesInfo.resize(nj);
     _jointEncs.resize(nj);
     _motorEncs.resize(nj);
@@ -141,7 +142,7 @@ bool embObjMotionControl::alloc(int nj)
         _temperatureExceededLimitWatchdog.at(i).setThreshold(txrate);
         _temperatureSensorErrorWatchdog.at(i).setThreshold(txrate);
     }
-
+    
     return true;
 }
 
@@ -216,6 +217,7 @@ embObjMotionControl::embObjMotionControl() :
     _joint2set(0),
     _timeouts(0),
     _impedance_params(0),
+    _lugre_params(0),
     _axesInfo(0),
     _jointEncs(0),
     _motorEncs(0),
@@ -953,6 +955,12 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
         return false;
     }
 
+    ////// LUGRE PARAMETERS
+    if(! _mcparser->parseLugreGroup(config,_lugre_params))
+    {
+        yError() << "embObjMC " << getBoardInfo() << "LUGRE section: error detected in parameters syntax";
+
+    }
 
     ////// IMPEDANCE LIMITS DEFAULT VALUES (UNDER TESTING)
     for(j=0; j<_njoints; j++)
@@ -1374,7 +1382,7 @@ bool embObjMotionControl::init()
         jconfig.impedance.damping   = (eOmeas_damping_t) _measureConverter->impN2S(_impedance_params[logico].damping, fisico);
         jconfig.impedance.stiffness = (eOmeas_stiffness_t) _measureConverter->impN2S(_impedance_params[logico].stiffness, fisico);
         jconfig.impedance.offset    = 0;
-
+        
         _cacheImpedance[logico].stiffness = jconfig.impedance.stiffness;
         _cacheImpedance[logico].damping   = jconfig.impedance.damping;
         _cacheImpedance[logico].offset    = jconfig.impedance.offset;
@@ -1461,6 +1469,16 @@ bool embObjMotionControl::init()
         motor_cfg.temperatureLimit = (eOmeas_temperature_t) S_16(_temperatureSensorsVector.at(logico)->convertTempCelsiusToRaw(_temperatureLimits.at(logico).hardwareTemperatureLimit)); //passing raw value not in degree
 	    motor_cfg.limitsofrotor.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico ));
         motor_cfg.limitsofrotor.min = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMin, fisico ));
+
+        motor_cfg.LuGre_params.Km     = _lugre_params[logico].Km;
+        motor_cfg.LuGre_params.Kw     = _lugre_params[logico].Kw;
+        motor_cfg.LuGre_params.S0     = _lugre_params[logico].S0;
+        motor_cfg.LuGre_params.S1     = _lugre_params[logico].S1;
+        motor_cfg.LuGre_params.Vth    = _lugre_params[logico].Vth;
+        motor_cfg.LuGre_params.Fc_pos = _lugre_params[logico].Fc_pos;
+        motor_cfg.LuGre_params.Fc_neg = _lugre_params[logico].Fc_neg;
+        motor_cfg.LuGre_params.Fs_pos = _lugre_params[logico].Fs_pos;
+        motor_cfg.LuGre_params.Fs_neg = _lugre_params[logico].Fs_neg;
 
         yarp::dev::Pid tmp;
         tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_CURRENT, _cur_pids[logico].pid, fisico);

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -301,7 +301,8 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     std::vector<eomc::timeouts_t>            _timeouts;
 
     std::vector<eomc::impedanceParameters_t> _impedance_params;   /** impedance parameters */ // TODO doubled!!! optimize using just one of the 2!!!
-    eomc::impedanceLimits_t *                _impedance_limits;  /** impedancel imits */
+    std::vector<eomc::lugreParameters_t>     _lugre_params;   /** LuGre friction model parameters */
+    eomc::impedanceLimits_t *                _impedance_limits;  /** impedance limits */
 
 
     eomc::PidInfo    *                      _trj_pids;

src/libraries/icubmod/embObjMotionControl/eomcParser.cpp

@@ -2457,6 +2457,115 @@ bool Parser::parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impeda
 
 }
 
+/*
+typedef struct
+{
+    double Km;
+    double Kw;
+    double S0; 
+    double S1;
+    double Vth;
+    double Fc_pos;
+    double Fc_neg;
+    double Fs_pos;
+    double Fs_neg;
+    double tbd0;
+    double tbd1;
+    double tbd2;
+} lugreParameters_t;
+*/
+
+bool Parser::parseLugreGroup(yarp::os::Searchable &config,std::vector<lugreParameters_t> &lugre)
+{
+    Bottle lugreGroup;
+    lugreGroup=config.findGroup("LUGRE","LUGRE parameters");
+
+    if(lugreGroup.isNull())
+    {
+        for (int j = 0; j<_njoints; ++j)
+        {
+            lugre[j].Km = -1.0;
+        }
+
+        yWarning() <<"embObjMC BOARD " << _boardname << "fromConfig(): Warning: no LUGRE group found in config file, returning";
+        return true;
+    }
+
+
+
+    if(_verbosewhenok)
+    {
+        yDebug()  << "embObjMC BOARD " << _boardname << ":fromConfig() detected that LUGRE parameters section is found";
+    }
+
+    if(!checkAndSetVectorSize(lugre, _njoints, "parseLugreGroup"))
+        return false;
+
+
+    int j=0;
+    Bottle xtmp;
+    if (!extractGroup(lugreGroup, xtmp, "Km", "torque constant parameter", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Km = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "Kw", "viscous friction parameter", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Kw = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "S0", "hysteresis position parameter", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].S0 = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "S1", "hysteresis velocity parameter", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].S1 = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "Vth", "velocity threshold parameter", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Vth = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "Fc_pos", "Coulomb force parameter (forward)", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Fc_pos = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "Fc_neg", "Coulomb force parameter (backward)", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Fc_neg = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "Fs_pos", "Stribeck force parameter (forward)", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Fs_pos = xtmp.get(j+1).asFloat64();
+
+    if (!extractGroup(lugreGroup, xtmp, "Fs_neg", "Stribeck force parameter (backward)", _njoints))
+        return false;
+
+    for (j=0; j<_njoints; j++)
+        lugre[j].Fs_neg = xtmp.get(j+1).asFloat64();
+
+    if(_verbosewhenok)
+    {
+        yInfo() << "embObjMC BOARD " << _boardname << "LUGRE section: parameters successfully loaded";
+    }
+    return true;
+
+}
+
 bool Parser::convert(std::string const &fromstring, eOmc_jsetconstraint_t &jsetconstraint, bool& formaterror)
 {
     const char *t = fromstring.c_str();

src/libraries/icubmod/embObjMotionControl/eomcParser.h

@@ -510,6 +510,21 @@ typedef struct
     impedanceLimits_t limits;
 } impedanceParameters_t;
 
+
+// LuGre friction model parameters
+typedef struct
+{
+    double Km;     // Motor torque constant (not strictly LuGre) but required to compute the net torque [Nm/A]
+    double Kw;     // Viscous friction constant [Nm/(rad/s)]
+    double S0;     // LuGre bristle analogy eleastic constant [Nm/rad]
+    double S1;     // LuGre bristle analogy viscous constant [Nm/(rad/s)]
+    double Vth;    // Stribeck velocity threshold [rad/s]
+    double Fc_pos; // Coulomb friction constant (positive rotation) [Nm]
+    double Fc_neg; // Coulomb friction constant (negative rotation) [Nm]
+    double Fs_pos; // Stribeck friction constant (positive rotation) [Nm]
+    double Fs_neg; // Stribeck friction constant (negative rotation) [Nm]
+} lugreParameters_t;
+
 typedef struct
 {
     bool enabled;
@@ -648,6 +663,7 @@ class Parser
     bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[]);
     bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[]);
     bool parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance);
+    bool parseLugreGroup(yarp::os::Searchable &config,std::vector<lugreParameters_t> &lugre);
     bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found);
     bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams);
 };