Skip to content

Commit

Permalink
LuGre friction model parameters moved from joint to motor structures.
Browse files Browse the repository at this point in the history
  • Loading branch information
@ale-git
ale-git committed Nov 14, 2024
1 parent db8dfcb commit e63f6e9
Show file tree
Hide file tree
Showing 5 changed files with 154 additions and 3 deletions.
2 changes: 1 addition & 1 deletion conf/iCubFindDependencies.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ checkandset_dependency(IPOPT)
checkandset_dependency(OpenCV)
checkandset_dependency(Qt5)

set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.39.1)
set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.41.0)

if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
if(icub_firmware_shared_VERSION VERSION_LESS ${MINIMUM_REQUIRED_icub_firmware_shared_VERSION})
Expand Down
24 changes: 23 additions & 1 deletion src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -216,6 +217,7 @@ embObjMotionControl::embObjMotionControl() :
_joint2set(0),
_timeouts(0),
_impedance_params(0),
_lugre_params(0),
_axesInfo(0),
_jointEncs(0),
_motorEncs(0),
Expand Down Expand Up @@ -951,6 +953,16 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
return false;
}

////// LUGRE PARAMETERS
if(! _mcparser->parseLugreGroup(config,_lugre_params))
{
yWarning() << "embObjMC " << getBoardInfo() << "LUGRE section: missing, sensorless torque estimation disabled";
static const double NOT_AVAILABLE = -1.0;
for(j=0; j<_njoints; j++)
{
_lugre_params[j].Km = NOT_AVAILABLE;
}
}

////// IMPEDANCE LIMITS DEFAULT VALUES (UNDER TESTING)
for(j=0; j<_njoints; j++)
Expand Down Expand Up @@ -1370,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;
Expand Down Expand Up @@ -1457,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);
Expand Down
3 changes: 2 additions & 1 deletion src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,7 +297,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;
Expand Down
109 changes: 109 additions & 0 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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].S0 = 0.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();
Expand Down
19 changes: 19 additions & 0 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -510,6 +510,24 @@ 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
double Kw; // Viscous friction constant
double S0; // LuGre bristle analogy eleastic constant
double S1; // LuGre bristle analogy viscous constant
double Vth; // Stribeck velocity threshold
double Fc_pos; // Coulomb friction constant (positive rotation)
double Fc_neg; // Coulomb friction constant (negative rotation)
double Fs_pos; // Stribeck friction constant (positive rotation)
double Fs_neg; // Stribeck friction constant (negative rotation)
double tbd0; // for future use?
double tbd1; // for future use?
double tbd2; // for future use?
} lugreParameters_t;

typedef struct
{
bool enabled;
Expand Down Expand Up @@ -648,6 +666,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);
};
Expand Down

0 comments on commit e63f6e9

Please sign in to comment.