Skip to content

Commit

Permalink
Extend the friction parameters to support viscous and coulomb frictio…
Browse files Browse the repository at this point in the history 
…ns (#782)
  • Loading branch information
@sgiraz
sgiraz authored Mar 7, 2022
1 parent 47e71bb commit 420be39
Show file tree
Hide file tree
Showing 7 changed files with 68 additions and 11 deletions.
3 changes: 3 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,3 +24,6 @@ licenses-good.txt
.vs/
CMakeSettings.json
*.txt.user
.vscode/settings.json
.vscode/c_cpp_properties.json
.vscode/launch.json
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
option(ICUB_COMPILE_BINDINGS "Compile optional language bindings" FALSE)

set(YCM_REQUIRED_VERSION 0.13.0)
set(YARP_REQUIRED_VERSION 3.6.0)
set(YARP_REQUIRED_VERSION 3.6.102)

find_package(YCM ${YCM_REQUIRED_VERSION} REQUIRED)
find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED)
Expand Down
4 changes: 2 additions & 2 deletions conf/iCubFindDependencies.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,8 @@ checkandset_dependency(Qt5)

if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
# icub-firmware-shared 4.0.7 was actually a wrong version exported by icub-firmware-shared <= 1.15
if(icub_firmware_shared_VERSION VERSION_GREATER 4 OR icub_firmware_shared_VERSION VERSION_LESS 1.22.2)
message(FATAL_ERROR "An old version of icub-firmware-shared has been detected, but at least 1.22.2 is required")
if(icub_firmware_shared_VERSION VERSION_GREATER 4 OR icub_firmware_shared_VERSION VERSION_LESS 1.24.0)
message(FATAL_ERROR "An old version of icub-firmware-shared has been detected, but at least 1.24.0 is required")
endif()
endif()

Expand Down
24 changes: 19 additions & 5 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1296,7 +1296,7 @@ bool embObjMotionControl::init()
memset(&jconfig, 0, sizeof(eOmc_joint_config_t));
yarp::dev::Pid tmp;
tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_POSITION,_trj_pids[logico].pid, fisico);
copyPid_iCub2eo(&tmp, &jconfig.pidtrajectory);
copyPid_iCub2eo(&tmp, &jconfig.pidtrajectory);
//tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_DIRECT, _dir_pids[logico].pid, fisico);
//copyPid_iCub2eo(&tmp, &jconfig.piddirect);
tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_TORQUE, _trq_pids[logico].pid, fisico);
Expand Down Expand Up @@ -1329,6 +1329,10 @@ bool embObjMotionControl::init()
jconfig.motor_params.bemf_scale = 0;
jconfig.motor_params.ktau_value = _measureConverter->ktau_user2raw(_trq_pids[logico].ktau, fisico);
jconfig.motor_params.ktau_scale = 0;
jconfig.motor_params.friction.viscous_pos_val = _measureConverter->viscousPos_user2raw(_trq_pids[logico].viscousPos, fisico);
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.gearbox_E2J = _gearbox_E2J[logico];

Expand Down Expand Up @@ -1392,7 +1396,7 @@ bool embObjMotionControl::init()

if (false == res->setcheckRemoteValue(protid, &motor_cfg, 10, 0.010, 0.050))
{
yError() << "FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for motor config fisico #" << fisico << "in "<< getBoardInfo();
yError() << "FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for motor config fisico #" << fisico << "in "<< getBoardInfo();
return false;
}
else
Expand Down Expand Up @@ -3598,7 +3602,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 ", i, params.bemf, params.bemf_scale, params.ktau, params.ktau_scale);
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);
r.addString(buff);
}
return true;
Expand Down Expand Up @@ -4018,7 +4022,12 @@ bool embObjMotionControl::getMotorTorqueParamsRaw(int j, MotorTorqueParameters *
params->bemf_scale = eo_params.bemf_scale;
params->ktau = eo_params.ktau_value;
params->ktau_scale = eo_params.ktau_scale;
//printf("debug getMotorTorqueParamsRaw %f %f %f %f\n", params->bemf, params->bemf_scale, params->ktau,params->ktau_scale);
params->viscousPos = eo_params.friction.viscous_pos_val;
params->viscousNeg = eo_params.friction.viscous_neg_val ;
params->coulombPos = eo_params.friction.coulomb_pos_val;
params->coulombNeg = eo_params.friction.coulomb_neg_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);

return true;
}
Expand All @@ -4028,12 +4037,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\n",j, params.bemf, params.ktau);
//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);

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_neg_val = static_cast<float32_t>(params.viscousNeg);
eo_params.friction.coulomb_pos_val = static_cast<float32_t>(params.coulombPos);
eo_params.friction.coulomb_neg_val = static_cast<float32_t>(params.coulombNeg);


if(false == res->setRemoteValue(id32, &eo_params))
{
Expand Down
24 changes: 24 additions & 0 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,10 @@ yarp::dev::eomc::Parser::Parser(int numofjoints, string boardname)
_kbemf=allocAndCheck<double>(_njoints);
_ktau=allocAndCheck<double>(_njoints);
_filterType=allocAndCheck<int>(_njoints);
_viscousPos=allocAndCheck<double>(_njoints);
_viscousNeg=allocAndCheck<double>(_njoints);
_coulombPos=allocAndCheck<double>(_njoints);
_coulombNeg=allocAndCheck<double>(_njoints);

minjerkAlgoMap.clear();
//directAlgoMap.clear();
Expand All @@ -63,6 +67,10 @@ Parser::~Parser()
checkAndDestroy(_kbemf);
checkAndDestroy(_ktau);
checkAndDestroy(_filterType);
checkAndDestroy(_viscousPos);
checkAndDestroy(_viscousNeg);
checkAndDestroy(_coulombPos);
checkAndDestroy(_coulombNeg);
}


Expand Down Expand Up @@ -902,6 +910,18 @@ bool Parser::parsePidsGroupDeluxe(Bottle& pidsGroup, Pid myPid[])
if (!extractGroup(pidsGroup, xtmp, "filterType", "filterType param", _njoints)) return false;
for (int j = 0; j<_njoints; j++) _filterType[j] = xtmp.get(j + 1).asInt32();

if (!extractGroup(pidsGroup, xtmp, "viscousPos", "viscousPos parameter", _njoints)) return false;
for (int j = 0; j<_njoints; j++) _viscousPos[j] = xtmp.get(j + 1).asFloat64();

if (!extractGroup(pidsGroup, xtmp, "viscousNeg", "viscousNeg parameter", _njoints)) return false;
for (int j = 0; j<_njoints; j++) _viscousNeg[j] = xtmp.get(j + 1).asFloat64();

if (!extractGroup(pidsGroup, xtmp, "coulombPos", "coulombPos parameter", _njoints)) return false;
for (int j = 0; j<_njoints; j++) _coulombPos[j] = xtmp.get(j + 1).asFloat64();

if (!extractGroup(pidsGroup, xtmp, "coulombNeg", "coulombNeg parameter", _njoints)) return false;
for (int j = 0; j<_njoints; j++) _coulombNeg[j] = xtmp.get(j + 1).asFloat64();

return true;
}

Expand Down Expand Up @@ -1230,6 +1250,10 @@ bool Parser::getCorrectPidForEachJoint(PidInfo *ppids/*, PidInfo *vpids*/, TrqPi
tpids[i].enabled = true;
tpids[i].kbemf = _kbemf[i];
tpids[i].ktau = _ktau[i];
tpids[i].viscousPos = _viscousPos[i];
tpids[i].viscousNeg = _viscousNeg[i];
tpids[i].coulombPos = _coulombPos[i];
tpids[i].coulombNeg = _coulombNeg[i];
tpids[i].filterType = _filterType[i];
}
else
Expand Down
14 changes: 11 additions & 3 deletions src/libraries/icubmod/embObjMotionControl/eomcParser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,10 @@ class TrqPidInfo : public PidInfo
public:
double kbemf; /** back-emf compensation parameter */
double ktau; /** motor torque constant */
double viscousPos;
double viscousNeg;
double coulombPos;
double coulombNeg;
int filterType;
};

Expand Down Expand Up @@ -330,22 +334,26 @@ class Parser

double *_kbemf; /** back-emf compensation parameter */
double *_ktau; /** motor torque constant */
int * _filterType;
int *_filterType;
double *_viscousPos;
double *_viscousNeg;
double *_coulombPos;
double *_coulombNeg;





//PID parsing functions
bool parseControlsGroup(yarp::os::Searchable &config);

bool parseSelectedPositionControl(yarp::os::Searchable &config);
bool parseSelectedVelocityControl(yarp::os::Searchable &config);
bool parseSelectedMixedControl(yarp::os::Searchable &config);
//bool parseSelectedPosDirectControl(yarp::os::Searchable &config);
//bool parseSelectedVelDirectControl(yarp::os::Searchable &config);
bool parseSelectedTorqueControl(yarp::os::Searchable &config);

bool parseSelectedCurrentPid(yarp::os::Searchable &config, bool pidisMandatory, PidInfo *pids);
bool parseSelectedSpeedPid(yarp::os::Searchable &config, bool pidisMandatory, PidInfo *pids);

Expand Down
8 changes: 8 additions & 0 deletions src/simulators/iCubSimulation/wrapper/iCubSimulationControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2001,6 +2001,10 @@ bool iCubSimulationControl::getMotorTorqueParamsRaw(int axis, MotorTorqueParamet
params->bemf_scale=motor_torque_params[axis].bemf_scale;
params->ktau=motor_torque_params[axis].ktau;
params->ktau_scale=motor_torque_params[axis].ktau_scale;
params->viscousPos=motor_torque_params[axis].viscousPos;
params->viscousNeg=motor_torque_params[axis].viscousNeg;
params->coulombPos=motor_torque_params[axis].coulombPos;
params->coulombNeg=motor_torque_params[axis].coulombNeg;
}
NOT_YET_IMPLEMENTED("getMotorTorqueParamsRaw");
return true; //fake
Expand All @@ -2014,6 +2018,10 @@ bool iCubSimulationControl::setMotorTorqueParamsRaw(int axis, const MotorTorqueP
motor_torque_params[axis].bemf_scale=params.bemf_scale;
motor_torque_params[axis].ktau=params.ktau;
motor_torque_params[axis].ktau_scale=params.ktau_scale;
motor_torque_params[axis].viscousPos=params.viscousPos;
motor_torque_params[axis].viscousNeg=params.viscousNeg;
motor_torque_params[axis].coulombPos=params.coulombPos;
motor_torque_params[axis].coulombNeg=params.coulombNeg;
}
NOT_YET_IMPLEMENTED("setMotorTorqueParamsRaw");
return true; //fake
Expand Down

0 comments on commit 420be39

Please sign in to comment.