Add implementation for friction parameters

CMakeLists.txt

@@ -17,7 +17,7 @@ endif()
 # Main project
 project(
   YARP
-  VERSION 3.6.101
+  VERSION 3.6.102
   LANGUAGES C CXX
 )
 set(PROJECT_DESCRIPTION "YARP: A thin middleware for humanoid robots and more")

doc/release/master/friction_parameters.md

@@ -0,0 +1,17 @@
+Friction parameters {#master}
+---------------------------------
+
+New Features
+------------
+
+### Viscous and Coulomb Friction Parameters
+
+- Now ITorqueControl supports 4 new friction parameters:
+  - viscousPos
+  - viscousNeg
+  - coulombPos
+  - coulombNeg
+
+**Note:**
+These parameters are used to improve the way we model and compensate for the friction in the FW.
+

src/devices/ControlBoardWrapper/RPCMessagesParser.cpp

@@ -478,15 +478,19 @@ void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd,
                 break;
             }
 
-            if (b->size() != 4) {
-                yCError(CONTROLBOARD, "received a SET VOCAB_MOTOR_PARAMS command not understood, size!=4");
+            if (b->size() != 8) {
+                yCError(CONTROLBOARD, "received a SET VOCAB_MOTOR_PARAMS command not understood, size != 8");
                 break;
             }
 
             params.bemf = b->get(0).asFloat64();
             params.bemf_scale = b->get(1).asFloat64();
             params.ktau = b->get(2).asFloat64();
             params.ktau_scale = b->get(3).asFloat64();
+            params.viscousPos = b->get(4).asFloat64();
+            params.viscousNeg = b->get(5).asFloat64();
+            params.coulombPos = b->get(6).asFloat64();
+            params.coulombNeg = b->get(7).asFloat64();
 
             *ok = rpc_ITorque->setMotorTorqueParams(joint, params);
         } break;
@@ -557,6 +561,10 @@ void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd,
             b.addFloat64(params.bemf_scale);
             b.addFloat64(params.ktau);
             b.addFloat64(params.ktau_scale);
+            b.addFloat64(params.viscousPos);
+            b.addFloat64(params.viscousNeg);
+            b.addFloat64(params.coulombPos);
+            b.addFloat64(params.coulombNeg);
         } break;
         case VOCAB_RANGE: {
             *ok = rpc_ITorque->getTorqueRange(cmd.get(3).asInt32(), &dtmp, &dtmp2);

src/devices/RemoteControlBoard/RemoteControlBoard.cpp

@@ -2211,6 +2211,10 @@ bool RemoteControlBoard::setMotorTorqueParams(int j, const MotorTorqueParameters
     b.addFloat64(params.bemf_scale);
     b.addFloat64(params.ktau);
     b.addFloat64(params.ktau_scale);
+    b.addFloat64(params.viscousPos);
+    b.addFloat64(params.viscousNeg);
+    b.addFloat64(params.coulombPos);
+    b.addFloat64(params.coulombNeg);
     bool ok = rpc_p.write(cmd, response);
     return CHECK_FAIL(ok, response);
 }
@@ -2229,15 +2233,19 @@ bool RemoteControlBoard::getMotorTorqueParams(int j, MotorTorqueParameters *para
             return false;
         }
         Bottle& l = *lp;
-        if (l.size() != 4)
+        if (l.size() != 8)
         {
-            yCError(REMOTECONTROLBOARD, "getMotorTorqueParams return value not understood, size!=4");
+            yCError(REMOTECONTROLBOARD, "getMotorTorqueParams return value not understood, size != 8");
             return false;
         }
         params->bemf        = l.get(0).asFloat64();
         params->bemf_scale  = l.get(1).asFloat64();
         params->ktau        = l.get(2).asFloat64();
         params->ktau_scale  = l.get(3).asFloat64();
+        params->viscousPos   = l.get(4).asFloat64();
+        params->viscousNeg = l.get(5).asFloat64();
+        params->coulombPos   = l.get(6).asFloat64();
+        params->coulombNeg = l.get(7).asFloat64();
         return true;
     }
     return false;

src/devices/fakeMotionControl/fakeMotionControl.cpp

@@ -264,6 +264,10 @@ bool FakeMotionControl::alloc(int nj)
     _ktau_scale = allocAndCheck<int>(nj);
     _filterType=allocAndCheck<int>(nj);
     _last_position_move_time=allocAndCheck<double>(nj);
+    _viscousPos=allocAndCheck<double>(nj);
+    _viscousNeg=allocAndCheck<double>(nj);
+    _coulombPos=allocAndCheck<double>(nj);
+    _coulombNeg=allocAndCheck<double>(nj);
 
     // Reserve space for data stored locally. values are initialized to 0
     _posCtrl_references = allocAndCheck<double>(nj);
@@ -336,6 +340,10 @@ bool FakeMotionControl::dealloc()
     checkAndDestroy(_kbemf_scale);
     checkAndDestroy(_ktau_scale);
     checkAndDestroy(_filterType);
+    checkAndDestroy(_viscousPos);
+    checkAndDestroy(_viscousNeg);
+    checkAndDestroy(_coulombPos);
+    checkAndDestroy(_coulombNeg);
     checkAndDestroy(_posCtrl_references);
     checkAndDestroy(_posDir_references);
     checkAndDestroy(_command_speeds);
@@ -434,6 +442,10 @@ FakeMotionControl::FakeMotionControl() :
     _ktau                   (nullptr),
     _kbemf_scale            (nullptr),
     _ktau_scale             (nullptr),
+    _viscousPos             (nullptr),
+    _viscousNeg             (nullptr),
+    _coulombPos             (nullptr),
+    _coulombNeg             (nullptr),
     _filterType             (nullptr),
     _torqueSensorId         (nullptr),
     _torqueSensorChan       (nullptr),
@@ -732,7 +744,7 @@ bool FakeMotionControl::parsePositionPidsGroup(Bottle& pidsGroup, Pid myPid[])
     return true;
 }
 
-bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[])
+bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[])
 {
     int j=0;
     Bottle xtmp;
@@ -827,6 +839,34 @@ bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], dou
         filterType[j] = xtmp.get(j+1).asInt32();
     }
 
+    if (!extractGroup(pidsGroup, xtmp, "viscousPos", "viscous pos parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        viscousPos[j] = xtmp.get(j+1).asFloat64();
+    }
+
+    if (!extractGroup(pidsGroup, xtmp, "viscousNeg", "viscous neg parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        viscousNeg[j] = xtmp.get(j+1).asFloat64();
+    }
+
+    if (!extractGroup(pidsGroup, xtmp, "coulombPos", "coulomb pos parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        coulombPos[j] = xtmp.get(j+1).asFloat64();
+    }
+
+    if (!extractGroup(pidsGroup, xtmp, "coulombNeg", "coulomb neg parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        coulombNeg[j] = xtmp.get(j+1).asFloat64();
+    }
+
     //conversion from metric to machine units (if applicable)
 //     for (j=0; j<_njoints; j++)
 //     {
@@ -2861,7 +2901,18 @@ bool FakeMotionControl::getMotorTorqueParamsRaw(int j, MotorTorqueParameters *pa
     params->bemf_scale = _kbemf_scale[j];
     params->ktau = _ktau[j];
     params->ktau_scale = _ktau_scale[j];
-    yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf << params->bemf_scale << params->ktau << params->ktau_scale;
+    params->viscousPos = _viscousPos[j];
+    params->viscousNeg = _viscousNeg[j];
+    params->coulombPos = _coulombPos[j];
+    params->coulombNeg = _coulombNeg[j];
+    yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf
+                                                            << params->bemf_scale
+                                                            << params->ktau
+                                                            << params->ktau_scale
+                                                            << params->viscousPos
+                                                            << params->viscousNeg
+                                                            << params->coulombPos
+                                                            << params->coulombNeg;
     return true;
 }
 
@@ -2871,7 +2922,18 @@ bool FakeMotionControl::setMotorTorqueParamsRaw(int j, const MotorTorqueParamete
     _ktau[j] = params.ktau;
     _kbemf_scale[j] = params.bemf_scale;
     _ktau_scale[j] = params.ktau_scale;
-    yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf << params.bemf_scale << params.ktau << params.ktau_scale;
+    _viscousPos[j] = params.viscousPos;
+    _viscousNeg[j] = params.viscousNeg;
+    _coulombPos[j] = params.coulombPos;
+    _coulombNeg[j] = params.coulombNeg;
+    yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf
+                                                            << params.bemf_scale
+                                                            << params.ktau
+                                                            << params.ktau_scale
+                                                            << params.viscousPos
+                                                            << params.viscousNeg
+                                                            << params.coulombPos
+                                                            << params.coulombNeg;
     return true;
 }
 

src/devices/fakeMotionControl/fakeMotionControl.h

@@ -193,6 +193,10 @@ class FakeMotionControl :
     double *_ktau;                              /** motor torque constant */
     int *_kbemf_scale;                          /** back-emf compensation parameter */
     int *_ktau_scale;                           /** motor torque constant */
+    double *_viscousPos;                         /** viscous pos friction */
+    double *_viscousNeg;                       /** viscous neg friction */
+    double *_coulombPos;                         /** coulomb up friction */
+    double *_coulombNeg;                       /** coulomb neg friction */
     int * _filterType;                          /** the filter type (int value) used by the force control algorithm */
     int *_torqueSensorId;                       /** Id of associated Joint Torque Sensor */
     int *_torqueSensorChan;                     /** Channel of associated Joint Torque Sensor */
@@ -504,7 +508,8 @@ class FakeMotionControl :
     void cleanup();
     bool dealloc();
     bool parsePositionPidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[]);
-    bool parseTorquePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], double kbemf[], double ktau[], int filterType[]);
+    bool parseTorquePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[]);
+
     bool parseImpedanceGroup_NewFormat(yarp::os::Bottle& pidsGroup, ImpedanceParameters vals[]);
 
     bool extractGroup(yarp::os::Bottle &input, yarp::os::Bottle &out, const std::string &key1, const std::string &txt, int size);

src/libYARP_dev/src/yarp/dev/ControlBoardHelper.cpp

@@ -40,6 +40,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
     double *dutycycleToPWMs;
     double *bemfToRaws;
     double *ktauToRaws;
+    double *viscousPosToRaws;
+    double *viscousNegToRaws;
+    double *coulombPosToRaws;
+    double *coulombNegToRaws;
 
     PidUnits* PosPid_units;
     PidUnits* VelPid_units;
@@ -60,6 +64,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
         dutycycleToPWMs(nullptr),
         bemfToRaws(nullptr),
         ktauToRaws(nullptr),
+        viscousPosToRaws(nullptr),
+        viscousNegToRaws(nullptr),
+        coulombPosToRaws(nullptr),
+        coulombNegToRaws(nullptr),
         PosPid_units(nullptr),
         VelPid_units(nullptr),
         CurPid_units(nullptr),
@@ -75,6 +83,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
         std::fill_n(dutycycleToPWMs, size, 1.0);
         std::fill_n(bemfToRaws, size, 1.0);
         std::fill_n(ktauToRaws, size, 1.0);
+        std::fill_n(viscousPosToRaws, size, 1.0);
+        std::fill_n(viscousNegToRaws, size, 1.0);
+        std::fill_n(coulombPosToRaws, size, 1.0);
+        std::fill_n(coulombNegToRaws, size, 1.0);
     }
 
     ~PrivateUnitsHandler()
@@ -94,6 +106,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
         checkAndDestroy<double>(dutycycleToPWMs);
         checkAndDestroy<double>(bemfToRaws);
         checkAndDestroy<double>(ktauToRaws);
+        checkAndDestroy<double>(viscousPosToRaws);
+        checkAndDestroy<double>(viscousNegToRaws);
+        checkAndDestroy<double>(coulombPosToRaws);
+        checkAndDestroy<double>(coulombNegToRaws);
     }
 
     void alloc(int n)
@@ -114,6 +130,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
         CurPid_units = new PidUnits[nj];
         bemfToRaws = new double[nj];
         ktauToRaws = new double[nj];
+        viscousPosToRaws = new double[nj];
+        viscousNegToRaws = new double[nj];
+        coulombPosToRaws = new double[nj];
+        coulombNegToRaws = new double[nj];
 
         yAssert(position_zeros != nullptr);
         yAssert(helper_ones != nullptr);
@@ -130,6 +150,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
         yAssert(CurPid_units != nullptr);
         yAssert(bemfToRaws != nullptr);
         yAssert(ktauToRaws != nullptr);
+        yAssert(viscousPosToRaws != nullptr);
+        yAssert(viscousNegToRaws != nullptr);
+        yAssert(coulombPosToRaws != nullptr);
+        yAssert(coulombNegToRaws != nullptr);
 
         pid_units[VOCAB_PIDTYPE_POSITION] = PosPid_units;
         pid_units[VOCAB_PIDTYPE_VELOCITY] = VelPid_units;
@@ -155,6 +179,10 @@ class  yarp::dev::ControlBoardHelper::PrivateUnitsHandler
         memcpy(this->CurPid_units, other.CurPid_units, sizeof(*other.CurPid_units)*nj);
         memcpy(this->bemfToRaws, other.bemfToRaws, sizeof(*other.bemfToRaws)*nj);
         memcpy(this->ktauToRaws, other.ktauToRaws, sizeof(*other.ktauToRaws)*nj);
+        memcpy(this->viscousPosToRaws, other.viscousPosToRaws, sizeof(*other.viscousPosToRaws)*nj);
+        memcpy(this->viscousNegToRaws, other.viscousNegToRaws, sizeof(*other.viscousNegToRaws)*nj);
+        memcpy(this->coulombPosToRaws, other.coulombPosToRaws, sizeof(*other.coulombPosToRaws)*nj);
+        memcpy(this->coulombNegToRaws, other.coulombNegToRaws, sizeof(*other.coulombNegToRaws)*nj);
     }
 };
 
@@ -754,6 +782,30 @@ void ControlBoardHelper::ktau_user2raw(double ktau_user, int j, double &ktau_raw
     k = toHw(j);
 }
 
+void ControlBoardHelper::viscousPos_user2raw(double viscousPos_user, int j, double &viscousPos_raw, int &k)
+{
+    viscousPos_raw = viscousPos_user * mPriv->viscousPosToRaws[j];
+    k = toHw(j);
+}
+
+void ControlBoardHelper::viscousNeg_user2raw(double viscousNeg_user, int j, double &viscousNeg_raw, int &k)
+{
+    viscousNeg_raw = viscousNeg_user * mPriv->viscousNegToRaws[j];
+    k = toHw(j);
+}
+
+void ControlBoardHelper::coulombPos_user2raw(double coulombPos_user, int j, double &coulombPos_raw, int &k)
+{
+    coulombPos_raw = coulombPos_user * mPriv->coulombPosToRaws[j];
+    k = toHw(j);
+}
+
+void ControlBoardHelper::coulombNeg_user2raw(double coulombNeg_user, int j, double &coulombNeg_raw, int &k)
+{
+    coulombNeg_raw = coulombNeg_user * mPriv->coulombNegToRaws[j];
+    k = toHw(j);
+}
+
 void ControlBoardHelper::bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user)
 {
     j_user = toUser(k_raw);
@@ -776,6 +828,51 @@ double  ControlBoardHelper::ktau_user2raw(double ktau_user, int j)
     return ktau_user * mPriv->ktauToRaws[j];
 }
 
+double  ControlBoardHelper::viscousPos_user2raw(double viscousPos_user, int j)
+{
+    return viscousPos_user * mPriv->viscousPosToRaws[j];
+}
+
+double ControlBoardHelper::viscousNeg_user2raw(double viscousNeg_user, int j)
+{
+    return viscousNeg_user * mPriv->viscousNegToRaws[j];
+}
+
+double ControlBoardHelper::coulombPos_user2raw(double coulombPos_user, int j)
+{
+    return coulombPos_user * mPriv->coulombPosToRaws[j];
+}
+
+double ControlBoardHelper::coulombNeg_user2raw(double coulombNeg_user, int j)
+{
+    return coulombNeg_user * mPriv->coulombNegToRaws[j];
+}
+
+void ControlBoardHelper::viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user)
+{
+    j_user = toUser(k_raw);
+    viscousPos_user = viscousPos_raw / mPriv->viscousPosToRaws[j_user];
+}
+
+void ControlBoardHelper::viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user)
+{
+    j_user = toUser(k_raw);
+    viscousNeg_user = viscousNeg_raw / mPriv->viscousNegToRaws[j_user];
+}
+
+void ControlBoardHelper::coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user)
+{
+    j_user = toUser(k_raw);
+    coulombPos_user = coulombPos_raw / mPriv->coulombPosToRaws[j_user];
+}
+
+void ControlBoardHelper::coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user)
+{
+    j_user = toUser(k_raw);
+    coulombNeg_user = coulombNeg_raw / mPriv->coulombNegToRaws[j_user];
+}
+
+
 // *******************************************//
 
 int ControlBoardHelper::axes()