Add implementation for friction parameters

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.viscousUp = b->get(4).asFloat64();
+            params.viscousDown = b->get(5).asFloat64();
+            params.coulombUp = b->get(6).asFloat64();
+            params.coulombDown = 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.viscousUp);
+            b.addFloat64(params.viscousDown);
+            b.addFloat64(params.coulombUp);
+            b.addFloat64(params.coulombDown);
         } 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.viscousUp);
+    b.addFloat64(params.viscousDown);
+    b.addFloat64(params.coulombUp);
+    b.addFloat64(params.coulombDown);
     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->viscousUp   = l.get(4).asFloat64();
+        params->viscousDown = l.get(5).asFloat64();
+        params->coulombUp   = l.get(6).asFloat64();
+        params->coulombDown = 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);
+    _viscousUp=allocAndCheck<double>(nj);
+    _viscousDown=allocAndCheck<double>(nj);
+    _coulombUp=allocAndCheck<double>(nj);
+    _coulombDown=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(_viscousUp);
+    checkAndDestroy(_viscousDown);
+    checkAndDestroy(_coulombUp);
+    checkAndDestroy(_coulombDown);
     checkAndDestroy(_posCtrl_references);
     checkAndDestroy(_posDir_references);
     checkAndDestroy(_command_speeds);
@@ -434,6 +442,10 @@ FakeMotionControl::FakeMotionControl() :
     _ktau                   (nullptr),
     _kbemf_scale            (nullptr),
     _ktau_scale             (nullptr),
+    _viscousUp              (nullptr),
+    _viscousDown            (nullptr),
+    _coulombUp              (nullptr),
+    _coulombDown            (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 viscousUp[], double viscousDown[], double coulombUp[], double coulombDown[])
 {
     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, "viscousUp", "viscous up parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        viscousUp[j] = xtmp.get(j+1).asFloat64();
+    }
+
+    if (!extractGroup(pidsGroup, xtmp, "viscousDown", "viscous down parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        viscousDown[j] = xtmp.get(j+1).asFloat64();
+    }
+
+    if (!extractGroup(pidsGroup, xtmp, "coulombUp", "coulomb up parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        coulombUp[j] = xtmp.get(j+1).asFloat64();
+    }
+
+    if (!extractGroup(pidsGroup, xtmp, "coulombDown", "coulomb up parameter", _njoints)) {
+        return false;
+    }
+    for (j=0; j<_njoints; j++) {
+        coulombDown[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->viscousUp = _viscousUp[j];
+    params->viscousDown = _viscousDown[j];
+    params->coulombUp = _coulombUp[j];
+    params->coulombDown = _coulombDown[j];
+    yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf
+                                                            << params->bemf_scale
+                                                            << params->ktau
+                                                            << params->ktau_scale
+                                                            << params->viscousUp
+                                                            << params->viscousDown
+                                                            << params->coulombUp
+                                                            << params->coulombDown;
     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;
+    _viscousUp[j] = params.viscousUp;
+    _viscousDown[j] = params.viscousDown;
+    _coulombUp[j] = params.coulombUp;
+    _coulombDown[j] = params.coulombDown;
+    yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf
+                                                            << params.bemf_scale
+                                                            << params.ktau
+                                                            << params.ktau_scale
+                                                            << params.viscousUp
+                                                            << params.viscousDown
+                                                            << params.coulombUp
+                                                            << params.coulombDown;
     return true;
 }
 

src/devices/fakeMotionControl/fakeMotionControl.h

@@ -191,6 +191,10 @@ class FakeMotionControl :
     int *_velocityTimeout;                      /** velocity shifts */
     double *_kbemf;                             /** back-emf compensation parameter */
     double *_ktau;                              /** motor torque constant */
+    double *_viscousUp;                         /** viscous up friction */
+    double *_viscousDown;                       /** viscous down friction */
+    double *_coulombUp;                         /** coulomb up friction */
+    double *_coulombDown;                       /** coulomb down friction */
     int *_kbemf_scale;                          /** back-emf compensation parameter */
     int *_ktau_scale;                           /** motor torque constant */
     int * _filterType;                          /** the filter type (int value) used by the force control algorithm */
@@ -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 viscousUp[], double viscousDown[], double coulombUp[], double coulombDown[]);
+
     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/ITorqueControl.h

@@ -22,7 +22,11 @@ class YARP_dev_API yarp::dev::MotorTorqueParameters
     double bemf_scale;
     double ktau;
     double ktau_scale;
-    MotorTorqueParameters() : bemf(0), bemf_scale(0), ktau(0), ktau_scale(0) {};
+    double viscousUp;
+    double viscousDown;
+    double coulombUp;
+    double coulombDown;
+    MotorTorqueParameters() : bemf(0), bemf_scale(0), ktau(0), ktau_scale(0), viscousUp(0), viscousDown(0), coulombUp(0), coulombDown(0) {};
 };
 
 /**

src/libYARP_dev/src/yarp/dev/ImplementTorqueControl.cpp

@@ -93,6 +93,10 @@ bool ImplementTorqueControl::setMotorTorqueParams(int j,  const yarp::dev::Motor
     castToMapper(helper)->ktau_user2raw(params.ktau, j, params_raw.ktau, k);
     params_raw.bemf_scale = params.bemf_scale;
     params_raw.ktau_scale = params.ktau_scale;
+    params_raw.viscousUp = params.viscousUp;
+    params_raw.viscousDown = params.viscousDown;
+    params_raw.coulombUp = params.coulombUp;
+    params_raw.coulombDown = params.coulombDown;
 
     return iTorqueRaw->setMotorTorqueParamsRaw(k, params_raw);
 }
@@ -113,6 +117,10 @@ bool ImplementTorqueControl::getMotorTorqueParams(int j,  yarp::dev::MotorTorque
         castToMapper(helper)->ktau_raw2user(params_raw.ktau, k, (*params).ktau, tmp_j);
         (*params).bemf_scale = params_raw.bemf_scale;
         (*params).ktau_scale = params_raw.ktau_scale;
+        (*params).viscousUp = params_raw.viscousUp;
+        (*params).viscousDown = params_raw.viscousDown;
+        (*params).coulombUp = params_raw.coulombUp;
+        (*params).coulombDown = params_raw.coulombDown;
     }
     return b;
 }

tests/libYARP_dev/CMakeLists.txt

@@ -25,6 +25,7 @@ target_sources(harness_dev
     frameGrabberCropperTest.cpp
     frameGrabber_nws_yarpTest.cpp
     grabberDualTest.cpp
+    TorqueControlTest.cpp
 )
 
 target_link_libraries(harness_dev

tests/libYARP_dev/TorqueControlTest.cpp

@@ -0,0 +1,89 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/dev/PolyDriver.h>
+
+#include <yarp/os/Network.h>
+#include <yarp/dev/ControlBoardInterfaces.h>
+#include <yarp/dev/IMultipleWrapper.h>
+
+#include <string>
+
+#include <catch.hpp>
+#include <harness.h>
+
+using namespace yarp::os;
+using namespace yarp::dev;
+
+TEST_CASE("dev::TorqueControl", "[yarp::dev]")
+{
+    YARP_REQUIRE_PLUGIN("group", "device");
+    YARP_REQUIRE_PLUGIN("fakeMotionControl", "device");
+    YARP_REQUIRE_PLUGIN("controlBoard_nws_yarp", "device");
+    YARP_REQUIRE_PLUGIN("remote_controlboard", "device");
+
+    Network::setLocalMode(true);
+
+    SECTION("test the controlBoard_nws_yarp device")
+    {
+        PolyDriver dd;
+        Property p;
+        p.put("device","controlBoard_nws_yarp");
+        p.put("subdevice","fakeMotionControl");
+        p.put("name","/motor");
+        auto& pg = p.addGroup("GENERAL");
+        pg.put("Joints", 1);
+        bool result;
+        result = dd.open(p);
+        REQUIRE(result); // controlboardwrapper open reported successful
+
+        // Check if IMultipleWrapper interface is correctly found
+        yarp::dev::IMultipleWrapper * i_mwrapper=nullptr;
+        result = dd.view(i_mwrapper);
+        REQUIRE(result); // IMultipleWrapper view reported successful
+        REQUIRE(i_mwrapper != nullptr); // IMultipleWrapper pointer not null
+
+        PolyDriver dd2;
+        Property p2;
+        p2.put("device","remote_controlboard");
+        p2.put("remote","/motor");
+        p2.put("local","/motor/client");
+        p2.put("carrier","tcp");
+        p2.put("ignoreProtocolCheck","true");
+        result = dd2.open(p2);
+        REQUIRE(result); // remote_controlboard open reported successful
+
+        ITorqueControl *trq = nullptr;
+        result = dd2.view(trq);
+        REQUIRE(result); // interface reported
+
+        yarp::dev::MotorTorqueParameters params;
+        yarp::dev::MotorTorqueParameters res;
+
+        //params.bemf = 0.1;
+        //params.bemf_scale = 0.2;
+        //params.ktau = 0.3;
+        //params.ktau_scale = 0.4;
+        params.viscousUp = 0.5;
+        params.viscousDown = 0.6;
+        params.coulombUp = 0.7;
+        params.coulombDown = 0.8;
+
+        trq->setMotorTorqueParams(0, params);
+        trq->getMotorTorqueParams(0, &res);
+
+        //CHECK(res.bemf == 0.1); // interface seems functional
+        //CHECK(res.bemf_scale == 0.2); // interface seems functional
+        //CHECK(res.ktau == 0.3); // interface seems functional
+        //CHECK(res.ktau_scale == 0.4); // interface seems functional
+        CHECK(res.viscousUp == 0.5); // interface seems functional
+        CHECK(res.viscousDown == 0.6); // interface seems functional
+        CHECK(res.coulombUp == 0.7); // interface seems functional
+        CHECK(res.coulombDown == 0.8); // interface seems functional
+        CHECK(dd.close()); // close dd reported successful
+        CHECK(dd2.close()); // close dd2 reported successful
+    }
+}