From 2ff7cf32d6d4dce34594026e934cd6c3d32547cc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Sat, 28 Apr 2018 00:06:28 +0200
Subject: [PATCH 01/10] Provide forward compatibility with YARP 3.x (#180)

---
 .../CanBusControlboard/CanBusControlboard.hpp | 48 ++++++-----
 .../CanBusControlboard/ITorqueControlImpl.cpp | 72 ++++++++--------
 .../IVelocityControl2Impl.cpp                 |  6 +-
 .../YarpPlugins/CuiAbsolute/CuiAbsolute.hpp   | 10 ++-
 .../CuiAbsolute/ITorqueControlRawImpl.cpp     | 35 ++++----
 .../CuiAbsolute/IVelocityControl2RawImpl.cpp  |  2 +
 .../FakeControlboard/FakeControlboard.hpp     | 52 +++++------
 .../FakeControlboard/ITorqueControlImpl.cpp   | 20 +++--
 .../IVelocityControl2Impl.cpp                 |  2 +
 libraries/YarpPlugins/FakeJoint/FakeJoint.hpp | 10 ++-
 .../FakeJoint/ITorqueControlRawImpl.cpp       | 34 ++++----
 .../FakeJoint/IVelocityControl2RawImpl.cpp    |  2 +
 .../LacqueyFetch/ITorqueControlRawImpl.cpp    | 34 ++++----
 .../LacqueyFetch/IVelocityControl2RawImpl.cpp |  2 +
 .../YarpPlugins/LacqueyFetch/LacqueyFetch.hpp | 10 ++-
 .../TechnosoftIpos/ITorqueControlRawImpl.cpp  | 86 ++++++++++---------
 .../IVelocityControl2RawImpl.cpp              |  3 +-
 .../TechnosoftIpos/TechnosoftIpos.hpp         | 10 ++-
 .../TextilesHand/ITorqueControlRawImpl.cpp    | 34 ++++----
 .../TextilesHand/IVelocityControl2RawImpl.cpp |  2 +
 .../YarpPlugins/TextilesHand/TextilesHand.hpp | 10 ++-
 21 files changed, 269 insertions(+), 215 deletions(-)

diff --git a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
index 593762440..933b4e0b9 100644
--- a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
+++ b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
@@ -521,8 +521,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     virtual bool getTargetPositions(const int n_joint, const int *joints, double *refs);
 
-
-
     // ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------
 
     /** Set new position for a single axis.
@@ -581,27 +579,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     virtual bool setRefTorque(int j, double t);
 
-    /** Set the back-efm compensation gain for a given joint.
-     * @param j joint number
-     * @param bemf the returned bemf gain of joint j
-     * @return true/false on success/failure
-     */
-    virtual bool getBemfParam(int j, double *bemf);
-
-    /** Set the back-efm compensation gain for a given joint.
-     * @param j joint number
-     * @param bemf new value
-     * @return true/false on success/failure
-     */
-    virtual bool setBemfParam(int j, double bemf);
-
-    /** Set new pid value for a joint axis.
-    * @param j joint number
-    * @param pid new pid value
-    * @return true/false on success/failure
-    */
-    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
-
     /** Get the value of the torque on a given joint (this is the
      * feedback if you have a torque sensor).
      * @param j joint number
@@ -632,6 +609,28 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     virtual bool getTorqueRanges(double *min, double *max);
 
+#if YARP_VERSION_MAJOR != 3
+    /** Set the back-efm compensation gain for a given joint.
+     * @param j joint number
+     * @param bemf the returned bemf gain of joint j
+     * @return true/false on success/failure
+     */
+    virtual bool getBemfParam(int j, double *bemf);
+
+    /** Set the back-efm compensation gain for a given joint.
+     * @param j joint number
+     * @param bemf new value
+     * @return true/false on success/failure
+     */
+    virtual bool setBemfParam(int j, double bemf);
+
+    /** Set new pid value for a joint axis.
+    * @param j joint number
+    * @param pid new pid value
+    * @return true/false on success/failure
+    */
+    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
+
     /** Set new pid value on multiple axes.
      * @param pids pointer to a vector of pids
      * @return true/false upon success/failure
@@ -729,6 +728,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      * @return true/false on success/failure
      */
     virtual bool setTorqueOffset(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControl Declarations. Implementation in IVelocityControl2Impl.cpp ---------
 
@@ -805,6 +805,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     // virtual bool stop(const int n_joint, const int *joints);
 
+#if YARP_VERSION_MAJOR != 3
     /** Set new velocity pid value for a joint
      * @param j joint number
      * @param pid new pid value
@@ -830,6 +831,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      * @return success/failure
      */
     virtual bool getVelPids(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
     // -----------IInteracionMode Declarations. Implementation in IInteracionModeImpl.cpp --------------
     /**
diff --git a/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
index 3f152ccb2..723126fc5 100644
--- a/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
@@ -57,90 +57,91 @@ bool roboticslab::CanBusControlboard::setRefTorque(int j, double t)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::getBemfParam(int j, double *bemf)
+bool roboticslab::CanBusControlboard::getTorque(int j, double *t)
 {
-    CD_DEBUG("(%d)\n",j);
+    //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream.
 
     //-- Check index within range
     if ( ! this->indexWithinRange(j) ) return false;
 
-    return iTorqueControlRaw[j]->getBemfParamRaw( 0, bemf );;
+    return iTorqueControlRaw[j]->getTorqueRaw( 0, t );;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::setBemfParam(int j, double bemf)
+bool roboticslab::CanBusControlboard::getTorques(double *t)
 {
-    CD_DEBUG("(%d,%f)\n",j,bemf);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
+    CD_DEBUG("\n");
 
-    return iTorqueControlRaw[j]->setBemfParamRaw( 0, bemf );;
+    bool ok = true;
+    for(int j=0; j<nodes.size(); j++)
+    {
+        ok &= this->getTorque(j, &(t[j]));
+    }
+    return ok;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
+bool roboticslab::CanBusControlboard::getTorqueRange(int j, double *min, double *max)
 {
     CD_DEBUG("(%d)\n",j);
 
     //-- Check index within range
     if ( ! this->indexWithinRange(j) ) return false;
 
-    return iTorqueControlRaw[j]->setTorquePidRaw( 0, pid );;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorque(int j, double *t)
-{
-    //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream.
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->getTorqueRaw( 0, t );;
+    return iTorqueControlRaw[j]->getTorqueRangeRaw( 0, min, max );
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::getTorques(double *t)
+bool roboticslab::CanBusControlboard::getTorqueRanges(double *min, double *max)
 {
     CD_DEBUG("\n");
 
     bool ok = true;
     for(int j=0; j<nodes.size(); j++)
     {
-        ok &= this->getTorque(j, &(t[j]));
+        ok &= this->getTorqueRange(j, min, max);
     }
     return ok;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::getTorqueRange(int j, double *min, double *max)
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::CanBusControlboard::getBemfParam(int j, double *bemf)
 {
     CD_DEBUG("(%d)\n",j);
 
     //-- Check index within range
     if ( ! this->indexWithinRange(j) ) return false;
 
-    return iTorqueControlRaw[j]->getTorqueRangeRaw( 0, min, max );
+    return iTorqueControlRaw[j]->getBemfParamRaw( 0, bemf );;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::getTorqueRanges(double *min, double *max)
+bool roboticslab::CanBusControlboard::setBemfParam(int j, double bemf)
 {
-    CD_DEBUG("\n");
+    CD_DEBUG("(%d,%f)\n",j,bemf);
 
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->getTorqueRange(j, min, max);
-    }
-    return ok;
+    //-- Check index within range
+    if ( ! this->indexWithinRange(j) ) return false;
+
+    return iTorqueControlRaw[j]->setBemfParamRaw( 0, bemf );;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::CanBusControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
+{
+    CD_DEBUG("(%d)\n",j);
+
+    //-- Check index within range
+    if ( ! this->indexWithinRange(j) ) return false;
+
+    return iTorqueControlRaw[j]->setTorquePidRaw( 0, pid );;
 }
 
 // -----------------------------------------------------------------------------
@@ -336,3 +337,4 @@ bool roboticslab::CanBusControlboard::setTorqueOffset(int j, double v)
 }
 
 // -----------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp b/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp
index ee7d340d6..3a5fc7f4b 100644
--- a/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp
@@ -9,7 +9,7 @@ bool roboticslab::CanBusControlboard::velocityMove(int j, double sp)
     CD_DEBUG("(%d), (%f)\n",j , sp);
 
     //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;    
+    if ( ! this->indexWithinRange(j) ) return false;
 
     return iVelocityControl2Raw[j]->velocityMoveRaw( 0, sp );
 }
@@ -85,6 +85,7 @@ bool roboticslab::CanBusControlboard::getRefVelocities(const int n_joint, const
 
 // -----------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::CanBusControlboard::setVelPid(int j, const yarp::dev::Pid &pid)
 {
     return true;
@@ -110,3 +111,6 @@ bool roboticslab::CanBusControlboard::getVelPids(yarp::dev::Pid *pids)
 {
     return true;
 }
+
+// -----------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
index 3796de7d1..ccb341918 100644
--- a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
+++ b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
@@ -155,13 +155,14 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
     virtual bool getRefTorqueRaw(int j, double *t);
     virtual bool setRefTorquesRaw(const double *t);
     virtual bool setRefTorqueRaw(int j, double t);
-    virtual bool getBemfParamRaw(int j, double *bemf);
-    virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool getTorqueRaw(int j, double *t);
     virtual bool getTorquesRaw(double *t);
     virtual bool getTorqueRangeRaw(int j, double *min, double *max);
     virtual bool getTorqueRangesRaw(double *min, double *max);
+#if YARP_VERSION_MAJOR != 3
+    virtual bool getBemfParamRaw(int j, double *bemf);
+    virtual bool setBemfParamRaw(int j, double bemf);
+    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
     virtual bool setTorqueErrorLimitRaw(int j, double limit);
     virtual bool setTorqueErrorLimitsRaw(const double *limits);
@@ -177,6 +178,7 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
     virtual bool disableTorquePidRaw(int j);
     virtual bool enableTorquePidRaw(int j);
     virtual bool setTorqueOffsetRaw(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
     virtual bool velocityMoveRaw(int j, double sp);
@@ -190,10 +192,12 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
+#if YARP_VERSION_MAJOR != 3
     virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
     virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
     virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
 
diff --git a/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp
index b9c7b040f..384f295e9 100644
--- a/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp
@@ -26,46 +26,47 @@ bool roboticslab::CuiAbsolute::setRefTorqueRaw(int j, double t)
     return true;
 }
 
-bool roboticslab::CuiAbsolute::getBemfParamRaw(int j, double *bemf)
+bool roboticslab::CuiAbsolute::getTorqueRaw(int j, double *t)
 {
-    CD_INFO("\n");
+    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
     return true;
 }
 
-bool roboticslab::CuiAbsolute::setBemfParamRaw(int j, double bemf)
+bool roboticslab::CuiAbsolute::getTorquesRaw(double *t)
 {
-    CD_INFO("\n");
-    return true;
+    CD_ERROR("\n");
+    return false;
 }
 
-bool roboticslab::CuiAbsolute::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
+bool roboticslab::CuiAbsolute::getTorqueRangeRaw(int j, double *min, double *max)
 {
     CD_INFO("\n");
     return true;
 }
 
-bool roboticslab::CuiAbsolute::getTorqueRaw(int j, double *t)
+bool roboticslab::CuiAbsolute::getTorqueRangesRaw(double *min, double *max)
 {
-    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
-    return true;
+    CD_ERROR("\n");
+    return false;
 }
 
-bool roboticslab::CuiAbsolute::getTorquesRaw(double *t)
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::CuiAbsolute::getBemfParamRaw(int j, double *bemf)
 {
-    CD_ERROR("\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
-bool roboticslab::CuiAbsolute::getTorqueRangeRaw(int j, double *min, double *max)
+bool roboticslab::CuiAbsolute::setBemfParamRaw(int j, double bemf)
 {
     CD_INFO("\n");
     return true;
 }
 
-bool roboticslab::CuiAbsolute::getTorqueRangesRaw(double *min, double *max)
+bool roboticslab::CuiAbsolute::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
 {
-    CD_ERROR("\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 bool roboticslab::CuiAbsolute::setTorquePidsRaw(const yarp::dev::Pid *pids)
@@ -157,4 +158,4 @@ bool roboticslab::CuiAbsolute::setTorqueOffsetRaw(int j, double v)
     CD_INFO("\n");
     return true;
 }
-
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp
index d2c849724..85d30bd61 100644
--- a/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp
@@ -52,6 +52,7 @@ bool roboticslab::CuiAbsolute::getRefVelocitiesRaw(const int n_joint, const int
 
 // --------------------------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::CuiAbsolute::setVelPidRaw(int j, const yarp::dev::Pid &pid)
 {
     CD_ERROR("Missing implementation\n");
@@ -81,3 +82,4 @@ bool roboticslab::CuiAbsolute::getVelPidsRaw(yarp::dev::Pid *pids)
     CD_ERROR("Missing implementation\n");
     return false;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
index e4cf98018..e4aaf07a0 100644
--- a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
+++ b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
@@ -448,6 +448,7 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool getRefVelocities(const int n_joint, const int *joints, double *vels);
 
+#if YARP_VERSION_MAJOR != 3
     /**
      * Set new velocity pid value for a joint
      * @param j joint number
@@ -477,6 +478,7 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      * @return success/failure
      */
     virtual bool getVelPids(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
 //  --------- IControlLimits Declarations. Implementation in IControlLimitsImpl.cpp ---------
 
@@ -664,30 +666,6 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool setRefTorque(int j, double t);
 
-    /**
-     * Set the back-efm compensation gain for a given joint.
-     * @param j joint number
-     * @param bemf the returned bemf gain of joint j
-     * @return true/false on success/failure
-     */
-    virtual bool getBemfParam(int j, double *bemf);
-
-    /**
-     * Set the back-efm compensation gain for a given joint.
-     * @param j joint number
-     * @param bemf new value
-     * @return true/false on success/failure
-     */
-    virtual bool setBemfParam(int j, double bemf);
-
-    /**
-     * Set new pid value for a joint axis.
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
-
     /**
      * Get the value of the torque on a given joint (this is the
      * feedback if you have a torque sensor).
@@ -722,6 +700,31 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool getTorqueRanges(double *min, double *max);
 
+#if YARP_VERSION_MAJOR != 3
+    /**
+     * Set the back-efm compensation gain for a given joint.
+     * @param j joint number
+     * @param bemf the returned bemf gain of joint j
+     * @return true/false on success/failure
+     */
+    virtual bool getBemfParam(int j, double *bemf);
+
+    /**
+     * Set the back-efm compensation gain for a given joint.
+     * @param j joint number
+     * @param bemf new value
+     * @return true/false on success/failure
+     */
+    virtual bool setBemfParam(int j, double bemf);
+
+    /**
+     * Set new pid value for a joint axis.
+     * @param j joint number
+     * @param pid new pid value
+     * @return true/false on success/failure
+     */
+    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
+
     /**
      * Set new pid value on multiple axes.
      * @param pids pointer to a vector of pids
@@ -834,6 +837,7 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      * @return true/false on success/failure
      */
     virtual bool setTorqueOffset(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
 // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
 
diff --git a/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
index fed560899..9e4888b99 100644
--- a/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
@@ -35,51 +35,52 @@ bool roboticslab::FakeControlboard::setRefTorque(int j, double t)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::getBemfParam(int j, double *bemf)
+bool roboticslab::FakeControlboard::getTorque(int j, double *t)
 {
+    //CD_DEBUG("joint: %d.\n",j);  //-- Way too verbose
+    *t = 0;
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::setBemfParam(int j, double bemf)
+bool roboticslab::FakeControlboard::getTorques(double *t)
 {
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
+bool roboticslab::FakeControlboard::getTorqueRange(int j, double *min, double *max)
 {
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::getTorque(int j, double *t)
+bool roboticslab::FakeControlboard::getTorqueRanges(double *min, double *max)
 {
-    //CD_DEBUG("joint: %d.\n",j);  //-- Way too verbose
-    *t = 0;
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::getTorques(double *t)
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::FakeControlboard::getBemfParam(int j, double *bemf)
 {
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::getTorqueRange(int j, double *min, double *max)
+bool roboticslab::FakeControlboard::setBemfParam(int j, double bemf)
 {
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::getTorqueRanges(double *min, double *max)
+bool roboticslab::FakeControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
 {
     return true;
 }
@@ -190,3 +191,4 @@ bool roboticslab::FakeControlboard::setTorqueOffset(int j, double v)
 }
 
 // -----------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp b/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp
index a71d382c3..36937061b 100644
--- a/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp
@@ -39,6 +39,7 @@ bool roboticslab::FakeControlboard::getRefVelocities(const int n_joint, const in
 
 // -----------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::FakeControlboard::setVelPid(int j, const yarp::dev::Pid &pid)
 {
     CD_DEBUG("\n");
@@ -70,3 +71,4 @@ bool roboticslab::FakeControlboard::getVelPids(yarp::dev::Pid *pids)
 }
 
 // -----------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
index 9e1c02503..02a236154 100644
--- a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
+++ b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
@@ -148,13 +148,14 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
     virtual bool getRefTorqueRaw(int j, double *t);
     virtual bool setRefTorquesRaw(const double *t);
     virtual bool setRefTorqueRaw(int j, double t);
-    virtual bool getBemfParamRaw(int j, double *bemf);
-    virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool getTorqueRaw(int j, double *t);
     virtual bool getTorquesRaw(double *t);
     virtual bool getTorqueRangeRaw(int j, double *min, double *max);
     virtual bool getTorqueRangesRaw(double *min, double *max);
+#if YARP_VERSION_MAJOR != 3
+    virtual bool getBemfParamRaw(int j, double *bemf);
+    virtual bool setBemfParamRaw(int j, double bemf);
+    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
     virtual bool setTorqueErrorLimitRaw(int j, double limit);
     virtual bool setTorqueErrorLimitsRaw(const double *limits);
@@ -170,6 +171,7 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
     virtual bool disableTorquePidRaw(int j);
     virtual bool enableTorquePidRaw(int j);
     virtual bool setTorqueOffsetRaw(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControl Declarations. Implementation in IVelocityControlImpl.cpp ---------
     virtual bool velocityMoveRaw(int j, double sp);
@@ -183,10 +185,12 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
+#if YARP_VERSION_MAJOR != 3
     virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
     virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
     virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
 
diff --git a/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp
index 1f03cc393..cb9021f1c 100644
--- a/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp
@@ -36,23 +36,23 @@ bool roboticslab::FakeJoint::setRefTorqueRaw(int j, double t)
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::getBemfParamRaw(int j, double *bemf)
+bool roboticslab::FakeJoint::getTorqueRaw(int j, double *t)
 {
-    CD_INFO("\n");
+    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
     return true;
 }
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::setBemfParamRaw(int j, double bemf)
+bool roboticslab::FakeJoint::getTorquesRaw(double *t)
 {
-    CD_INFO("\n");
-    return true;
+    CD_ERROR("\n");
+    return false;
 }
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
+bool roboticslab::FakeJoint::getTorqueRangeRaw(int j, double *min, double *max)
 {
     CD_INFO("\n");
     return true;
@@ -60,23 +60,24 @@ bool roboticslab::FakeJoint::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::getTorqueRaw(int j, double *t)
+bool roboticslab::FakeJoint::getTorqueRangesRaw(double *min, double *max)
 {
-    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
-    return true;
+    CD_ERROR("\n");
+    return false;
 }
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::getTorquesRaw(double *t)
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::FakeJoint::getBemfParamRaw(int j, double *bemf)
 {
-    CD_ERROR("\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::getTorqueRangeRaw(int j, double *min, double *max)
+bool roboticslab::FakeJoint::setBemfParamRaw(int j, double bemf)
 {
     CD_INFO("\n");
     return true;
@@ -84,10 +85,10 @@ bool roboticslab::FakeJoint::getTorqueRangeRaw(int j, double *min, double *max)
 
 // ------------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::getTorqueRangesRaw(double *min, double *max)
+bool roboticslab::FakeJoint::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
 {
-    CD_ERROR("\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 // ------------------------------------------------------------------------------
@@ -209,3 +210,4 @@ bool roboticslab::FakeJoint::setTorqueOffsetRaw(int j, double v)
     CD_INFO("\n");
     return true;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp
index 6f02ac1c6..7d6f7e43f 100644
--- a/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp
@@ -52,6 +52,7 @@ bool roboticslab::FakeJoint::getRefVelocitiesRaw(const int n_joint, const int *j
 
 // --------------------------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::FakeJoint::setVelPidRaw(int j, const yarp::dev::Pid &pid)
 {
     CD_ERROR("Missing implementation\n");
@@ -81,3 +82,4 @@ bool roboticslab::FakeJoint::getVelPidsRaw(yarp::dev::Pid *pids)
     CD_ERROR("Missing implementation\n");
     return false;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp
index c14a9303c..0224177fe 100644
--- a/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp
@@ -36,23 +36,23 @@ bool roboticslab::LacqueyFetch::setRefTorqueRaw(int j, double t)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getBemfParamRaw(int j, double *bemf)
+bool roboticslab::LacqueyFetch::getTorqueRaw(int j, double *t)
 {
-    CD_INFO("\n");
+    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::setBemfParamRaw(int j, double bemf)
+bool roboticslab::LacqueyFetch::getTorquesRaw(double *t)
 {
-    CD_INFO("\n");
-    return true;
+    CD_ERROR("Missing implementation\n");
+    return false;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
+bool roboticslab::LacqueyFetch::getTorqueRangeRaw(int j, double *min, double *max)
 {
     CD_INFO("\n");
     return true;
@@ -60,23 +60,24 @@ bool roboticslab::LacqueyFetch::setTorquePidRaw(int j, const yarp::dev::Pid &pid
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getTorqueRaw(int j, double *t)
+bool roboticslab::LacqueyFetch::getTorqueRangesRaw(double *min, double *max)
 {
-    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
-    return true;
+    CD_ERROR("Missing implementation\n");
+    return false;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getTorquesRaw(double *t)
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::LacqueyFetch::getBemfParamRaw(int j, double *bemf)
 {
-    CD_ERROR("Missing implementation\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getTorqueRangeRaw(int j, double *min, double *max)
+bool roboticslab::LacqueyFetch::setBemfParamRaw(int j, double bemf)
 {
     CD_INFO("\n");
     return true;
@@ -84,10 +85,10 @@ bool roboticslab::LacqueyFetch::getTorqueRangeRaw(int j, double *min, double *ma
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::getTorqueRangesRaw(double *min, double *max)
+bool roboticslab::LacqueyFetch::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
 {
-    CD_ERROR("Missing implementation\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 // -----------------------------------------------------------------------------
@@ -209,3 +210,4 @@ bool roboticslab::LacqueyFetch::setTorqueOffsetRaw(int j, double v)
     CD_INFO("\n");
     return true;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp
index 4e0a22803..ae0e138ca 100644
--- a/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp
@@ -76,6 +76,7 @@ bool roboticslab::LacqueyFetch::stopRaw(const int n_joint, const int *joints)
 */
 // -----------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::LacqueyFetch::setVelPidRaw(int j, const yarp::dev::Pid &pid)
 {
     CD_ERROR("Missing implementation\n");
@@ -105,3 +106,4 @@ bool roboticslab::LacqueyFetch::getVelPidsRaw(yarp::dev::Pid *pids)
     CD_ERROR("Missing implementation\n");
     return false;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
index ecfd0a670..91df65433 100644
--- a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
+++ b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
@@ -149,13 +149,14 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool getRefTorqueRaw(int j, double *t);
     virtual bool setRefTorquesRaw(const double *t);
     virtual bool setRefTorqueRaw(int j, double t);
-    virtual bool getBemfParamRaw(int j, double *bemf);
-    virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool getTorqueRaw(int j, double *t);
     virtual bool getTorquesRaw(double *t);
     virtual bool getTorqueRangeRaw(int j, double *min, double *max);
     virtual bool getTorqueRangesRaw(double *min, double *max);
+#if YARP_VERSION_MAJOR != 3
+    virtual bool getBemfParamRaw(int j, double *bemf);
+    virtual bool setBemfParamRaw(int j, double bemf);
+    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
     virtual bool setTorqueErrorLimitRaw(int j, double limit);
     virtual bool setTorqueErrorLimitsRaw(const double *limits);
@@ -171,6 +172,7 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool disableTorquePidRaw(int j);
     virtual bool enableTorquePidRaw(int j);
     virtual bool setTorqueOffsetRaw(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
     virtual bool velocityMoveRaw(int j, double sp);
@@ -184,10 +186,12 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
+#if YARP_VERSION_MAJOR != 3
     virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
     virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
     virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
     virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode);
diff --git a/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
index 7b9da5680..9e69ea42b 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
@@ -66,48 +66,6 @@ bool roboticslab::TechnosoftIpos::setRefTorqueRaw(int j, double t)
 
 // -------------------------------------------------------------------------------------
 
-bool roboticslab::TechnosoftIpos::getBemfParamRaw(int j, double *bemf)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setBemfParamRaw(int j, double bemf)
-{
-    CD_INFO("(%d,%f)\n",j,bemf);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
 bool roboticslab::TechnosoftIpos::getTorqueRaw(int j, double *t)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream.
@@ -168,6 +126,49 @@ bool roboticslab::TechnosoftIpos::getTorqueRangesRaw(double *min, double *max)
 
 // -------------------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::TechnosoftIpos::getBemfParamRaw(int j, double *bemf)
+{
+    CD_INFO("(%d)\n",j);
+
+    //-- Check index within range
+    if ( j != 0 ) return false;
+
+    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
+
+    return true;
+}
+
+// -------------------------------------------------------------------------------------
+
+bool roboticslab::TechnosoftIpos::setBemfParamRaw(int j, double bemf)
+{
+    CD_INFO("(%d,%f)\n",j,bemf);
+
+    //-- Check index within range
+    if ( j != 0 ) return false;
+
+    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
+
+    return true;
+}
+
+// -------------------------------------------------------------------------------------
+
+bool roboticslab::TechnosoftIpos::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
+{
+    CD_INFO("(%d)\n",j);
+
+    //-- Check index within range
+    if ( j != 0 ) return false;
+
+    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
+
+    return true;
+}
+
+// -------------------------------------------------------------------------------------
+
 bool roboticslab::TechnosoftIpos::setTorquePidsRaw(const yarp::dev::Pid *pids)
 {
     CD_ERROR("Missing implementation\n");
@@ -341,3 +342,4 @@ bool roboticslab::TechnosoftIpos::setTorqueOffsetRaw(int j, double v)
 }
 
 // -------------------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp
index 63a0c034a..c12d98239 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp
@@ -132,6 +132,7 @@ bool roboticslab::TechnosoftIpos::stopRaw(const int n_joint, const int *joints)
 
 // -----------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::TechnosoftIpos::setVelPidRaw(int j, const yarp::dev::Pid &pid)
 {
     CD_WARNING("Missing implementation\n");
@@ -161,4 +162,4 @@ bool roboticslab::TechnosoftIpos::getVelPidsRaw(yarp::dev::Pid *pids)
     CD_WARNING("Missing implementation\n");
     return true;
 }
-
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
index e0b6094c0..72b51e13b 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
@@ -166,13 +166,14 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
     virtual bool getRefTorqueRaw(int j, double *t);
     virtual bool setRefTorquesRaw(const double *t);
     virtual bool setRefTorqueRaw(int j, double t);
-    virtual bool getBemfParamRaw(int j, double *bemf);
-    virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool getTorqueRaw(int j, double *t);
     virtual bool getTorquesRaw(double *t);
     virtual bool getTorqueRangeRaw(int j, double *min, double *max);
     virtual bool getTorqueRangesRaw(double *min, double *max);
+#if YARP_VERSION_MAJOR != 3
+    virtual bool getBemfParamRaw(int j, double *bemf);
+    virtual bool setBemfParamRaw(int j, double bemf);
+    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
     virtual bool setTorqueErrorLimitRaw(int j, double limit);
     virtual bool setTorqueErrorLimitsRaw(const double *limits);
@@ -188,6 +189,7 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
     virtual bool disableTorquePidRaw(int j);
     virtual bool enableTorquePidRaw(int j);
     virtual bool setTorqueOffsetRaw(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
     virtual bool velocityMoveRaw(int j, double sp);
@@ -204,10 +206,12 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
     // -- virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // ------------------- Just declareted in IPositionControl2Raw
     // -- virtual bool stopRaw(const int n_joint, const int *joints);
+#if YARP_VERSION_MAJOR != 3
     virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
     virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
     virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
     virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode);
diff --git a/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp
index c61ec6d1d..ca56d2d25 100644
--- a/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp
@@ -36,23 +36,23 @@ bool roboticslab::TextilesHand::setRefTorqueRaw(int j, double t)
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::getBemfParamRaw(int j, double *bemf)
+bool roboticslab::TextilesHand::getTorqueRaw(int j, double *t)
 {
-    CD_INFO("\n");
+    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
     return true;
 }
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::setBemfParamRaw(int j, double bemf)
+bool roboticslab::TextilesHand::getTorquesRaw(double *t)
 {
-    CD_INFO("\n");
-    return true;
+    CD_ERROR("\n");
+    return false;
 }
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
+bool roboticslab::TextilesHand::getTorqueRangeRaw(int j, double *min, double *max)
 {
     CD_INFO("\n");
     return true;
@@ -60,23 +60,24 @@ bool roboticslab::TextilesHand::setTorquePidRaw(int j, const yarp::dev::Pid &pid
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::getTorqueRaw(int j, double *t)
+bool roboticslab::TextilesHand::getTorqueRangesRaw(double *min, double *max)
 {
-    //CD_INFO("\n");  //-- Too verbose in controlboardwrapper2 stream.
-    return true;
+    CD_ERROR("\n");
+    return false;
 }
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::getTorquesRaw(double *t)
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::TextilesHand::getBemfParamRaw(int j, double *bemf)
 {
-    CD_ERROR("\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::getTorqueRangeRaw(int j, double *min, double *max)
+bool roboticslab::TextilesHand::setBemfParamRaw(int j, double bemf)
 {
     CD_INFO("\n");
     return true;
@@ -84,10 +85,10 @@ bool roboticslab::TextilesHand::getTorqueRangeRaw(int j, double *min, double *ma
 
 // -----------------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::getTorqueRangesRaw(double *min, double *max)
+bool roboticslab::TextilesHand::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
 {
-    CD_ERROR("\n");
-    return false;
+    CD_INFO("\n");
+    return true;
 }
 
 // -----------------------------------------------------------------------------------
@@ -209,3 +210,4 @@ bool roboticslab::TextilesHand::setTorqueOffsetRaw(int j, double v)
     CD_INFO("\n");
     return true;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp
index 44b4a1bef..b62959e65 100644
--- a/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp
@@ -52,6 +52,7 @@ bool roboticslab::TextilesHand::getRefVelocitiesRaw(const int n_joint, const int
 
 // --------------------------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::TextilesHand::setVelPidRaw(int j, const yarp::dev::Pid &pid)
 {
     CD_ERROR("Missing implementation\n");
@@ -81,3 +82,4 @@ bool roboticslab::TextilesHand::getVelPidsRaw(yarp::dev::Pid *pids)
     CD_ERROR("Missing implementation\n");
     return false;
 }
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
index 5c683813e..db39e9bef 100644
--- a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
+++ b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
@@ -159,13 +159,14 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool getRefTorqueRaw(int j, double *t);
     virtual bool setRefTorquesRaw(const double *t);
     virtual bool setRefTorqueRaw(int j, double t);
-    virtual bool getBemfParamRaw(int j, double *bemf);
-    virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool getTorqueRaw(int j, double *t);
     virtual bool getTorquesRaw(double *t);
     virtual bool getTorqueRangeRaw(int j, double *min, double *max);
     virtual bool getTorqueRangesRaw(double *min, double *max);
+#if YARP_VERSION_MAJOR != 3
+    virtual bool getBemfParamRaw(int j, double *bemf);
+    virtual bool setBemfParamRaw(int j, double bemf);
+    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
     virtual bool setTorqueErrorLimitRaw(int j, double limit);
     virtual bool setTorqueErrorLimitsRaw(const double *limits);
@@ -181,6 +182,7 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool disableTorquePidRaw(int j);
     virtual bool enableTorquePidRaw(int j);
     virtual bool setTorqueOffsetRaw(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
     virtual bool velocityMoveRaw(int j, double sp);
@@ -194,10 +196,12 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
+#if YARP_VERSION_MAJOR != 3
     virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
     virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
     virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
     virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
 

From f6b0486455271ed6b630b78268c4a30e634a9eae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 19:57:20 +0200
Subject: [PATCH 02/10] Add YARP version guards in AmorControlboard device

---
 .../AmorControlboard/AmorControlboard.hpp     | 46 ++++++++--------
 .../AmorControlboard/ITorqueControlImpl.cpp   | 54 ++++++++++---------
 .../IVelocityControl2Impl.cpp                 |  2 +
 3 files changed, 55 insertions(+), 47 deletions(-)

diff --git a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
index 203c79939..951339e06 100644
--- a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
+++ b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
@@ -405,6 +405,7 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool getRefVelocities(const int n_joint, const int *joints, double *vels);
 
+#if YARP_VERSION_MAJOR != 3
     /** Set new velocity pid value for a joint
      * @param j joint number
      * @param pid new pid value
@@ -430,6 +431,7 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      * @return success/failure
      */
     virtual bool getVelPids(yarp::dev::Pid *pids);
+#endif // YARP_VERSION_MAJOR != 3
 
 //  --------- IControlLimits declarations. Implementation in IControlLimitsImpl.cpp ---------
 
@@ -636,20 +638,6 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool setRefTorques(const int n_joint, const int *joints, const double *t);
 
-    /** Get the back-emf compensation gain for a given joint.
-     * @param j joint number
-     * @param bemf the returned bemf gain of joint j
-     * @return true/false on success/failure
-     */
-    virtual bool getBemfParam(int j, double *bemf);
-
-    /** Set the back-emf compensation gain for a given joint.
-     * @param j joint number
-     * @param bemf new value
-     * @return true/false on success/failure
-     */
-    virtual bool setBemfParam(int j, double bemf);
-
     /** Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
      * @param j joint number
      * @param params a struct containing the motor parameters to be retrieved
@@ -664,13 +652,6 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params);
 
-     /** Set new pid value for a joint axis.
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
-
     /** Get the value of the torque on a given joint (this is the
      * feedback if you have a torque sensor).
      * @param j joint number
@@ -701,6 +682,28 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool getTorqueRanges(double *min, double *max);
 
+#if YARP_VERSION_MAJOR != 3
+    /** Get the back-emf compensation gain for a given joint.
+     * @param j joint number
+     * @param bemf the returned bemf gain of joint j
+     * @return true/false on success/failure
+     */
+    virtual bool getBemfParam(int j, double *bemf);
+
+    /** Set the back-emf compensation gain for a given joint.
+     * @param j joint number
+     * @param bemf new value
+     * @return true/false on success/failure
+     */
+    virtual bool setBemfParam(int j, double bemf);
+
+    /** Set new pid value for a joint axis.
+     * @param j joint number
+     * @param pid new pid value
+     * @return true/false on success/failure
+     */
+    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
+
     /** Set new pid value on multiple axes.
      * @param pids pointer to a vector of pids
      * @return true/false upon success/failure
@@ -798,6 +801,7 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      * @return true/false on success/failure
      */
     virtual bool setTorqueOffset(int j, double v);
+#endif // YARP_VERSION_MAJOR != 3
 
 // -------- IInteractionMode declarations. Implementation in IInteractionModeImpl.cpp --------
 
diff --git a/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp
index 0a531a8f7..a9dca7203 100644
--- a/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp
@@ -62,7 +62,7 @@ bool roboticslab::AmorControlboard::setRefTorques(const int n_joint, const int *
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::getBemfParam(int j, double *bemf)
+bool roboticslab::AmorControlboard::getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params)
 {
     CD_DEBUG("(%d)\n", j);
 
@@ -76,9 +76,9 @@ bool roboticslab::AmorControlboard::getBemfParam(int j, double *bemf)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::setBemfParam(int j, double bemf)
+bool roboticslab::AmorControlboard::setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params)
 {
-    CD_DEBUG("(%d, %f)\n", j, bemf);
+    CD_DEBUG("(%d)\n", j);
 
     if (!indexWithinRange(j))
     {
@@ -90,9 +90,9 @@ bool roboticslab::AmorControlboard::setBemfParam(int j, double bemf)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params)
+bool roboticslab::AmorControlboard::getTorque(int j, double *t)
 {
-    CD_DEBUG("(%d)\n", j);
+    //CD_DEBUG("(%d)\n", j);
 
     if (!indexWithinRange(j))
     {
@@ -104,7 +104,15 @@ bool roboticslab::AmorControlboard::getMotorTorqueParams(int j, yarp::dev::Motor
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params)
+bool roboticslab::AmorControlboard::getTorques(double *t)
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getTorqueRange(int j, double *min, double *max)
 {
     CD_DEBUG("(%d)\n", j);
 
@@ -118,7 +126,16 @@ bool roboticslab::AmorControlboard::setMotorTorqueParams(int j, const yarp::dev:
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
+bool roboticslab::AmorControlboard::getTorqueRanges(double *min, double *max)
+{
+    CD_DEBUG("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+#if YARP_VERSION_MAJOR != 3
+bool roboticslab::AmorControlboard::getBemfParam(int j, double *bemf)
 {
     CD_DEBUG("(%d)\n", j);
 
@@ -132,9 +149,9 @@ bool roboticslab::AmorControlboard::setTorquePid(int j, const yarp::dev::Pid &pi
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::getTorque(int j, double *t)
+bool roboticslab::AmorControlboard::setBemfParam(int j, double bemf)
 {
-    //CD_DEBUG("(%d)\n", j);
+    CD_DEBUG("(%d, %f)\n", j, bemf);
 
     if (!indexWithinRange(j))
     {
@@ -146,15 +163,7 @@ bool roboticslab::AmorControlboard::getTorque(int j, double *t)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::getTorques(double *t)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorqueRange(int j, double *min, double *max)
+bool roboticslab::AmorControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
 {
     CD_DEBUG("(%d)\n", j);
 
@@ -168,14 +177,6 @@ bool roboticslab::AmorControlboard::getTorqueRange(int j, double *min, double *m
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::getTorqueRanges(double *min, double *max)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::AmorControlboard::setTorquePids(const yarp::dev::Pid *pids)
 {
     CD_DEBUG("\n");
@@ -349,3 +350,4 @@ bool roboticslab::AmorControlboard::setTorqueOffset(int j, double v)
 }
 
 // -----------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp b/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp
index a7b9abf7d..690f9d217 100644
--- a/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp
+++ b/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp
@@ -112,6 +112,7 @@ bool roboticslab::AmorControlboard::setVelPid(int j, const yarp::dev::Pid &pid)
 
 // -----------------------------------------------------------------------------
 
+#if YARP_VERSION_MAJOR != 3
 bool roboticslab::AmorControlboard::setVelPids(const yarp::dev::Pid *pids)
 {
     CD_ERROR("Not available.\n");
@@ -135,3 +136,4 @@ bool roboticslab::AmorControlboard::getVelPids(yarp::dev::Pid *pids)
 }
 
 // -----------------------------------------------------------------------------
+#endif // YARP_VERSION_MAJOR != 3

From f929b890d9dee7fe700032eda6a0e61f9a2df286 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 20:29:47 +0200
Subject: [PATCH 03/10] Require YARP 2.3.70

https://github.com/roboticslab-uc3m/yarp-devices/issues/180#issuecomment-389949702
---
 CMakeLists.txt                                  | 9 +--------
 libraries/OneCanBusOneWrapper/CMakeLists.txt    | 2 --
 libraries/TwoCanBusThreeWrappers/CMakeLists.txt | 2 --
 libraries/YarpPlugins/CMakeLists.txt            | 4 ----
 programs/checkCanBus/CMakeLists.txt             | 2 --
 programs/dumpCanBus/CMakeLists.txt              | 2 --
 programs/launchLocomotion/CMakeLists.txt        | 2 --
 programs/launchManipulation/CMakeLists.txt      | 2 --
 programs/oneCanBusOneWrapper/CMakeLists.txt     | 2 --
 tests/testCuiAbsolute/CMakeLists.txt            | 2 --
 tests/testTechnosoftIpos/CMakeLists.txt         | 2 --
 11 files changed, 1 insertion(+), 30 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7eb77b36d..6e2e84421 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -82,14 +82,7 @@ set_property(GLOBAL PROPERTY ROBOTICSLAB_YARP_DEVICES_TARGETS)
 include(CMakeDependentOption)
 
 # Find YARP (main dependency).
-find_package(YARP REQUIRED)
-
-# Load YARP modules.
-if(YARP_VERSION_SHORT VERSION_LESS 2.3.70)
-    list(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH})
-    include(YarpPlugin)
-    include(YarpInstallationHelpers)
-endif()
+find_package(YARP 2.3.70 REQUIRED)
 
 # Configure installation paths for YARP resources.
 yarp_configure_external_installation(roboticslab-yarp-devices WITH_PLUGINS)
diff --git a/libraries/OneCanBusOneWrapper/CMakeLists.txt b/libraries/OneCanBusOneWrapper/CMakeLists.txt
index e7e1275b7..0a69c7115 100644
--- a/libraries/OneCanBusOneWrapper/CMakeLists.txt
+++ b/libraries/OneCanBusOneWrapper/CMakeLists.txt
@@ -2,8 +2,6 @@ option(ENABLE_OneCanBusOneWrapper "Enable/disable OneCanBusOneWrapper library" O
 
 if(ENABLE_OneCanBusOneWrapper)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_library(OneCanBusOneWrapper OneCanBusOneWrapper.cpp
                                 OneCanBusOneWrapper.hpp)
 
diff --git a/libraries/TwoCanBusThreeWrappers/CMakeLists.txt b/libraries/TwoCanBusThreeWrappers/CMakeLists.txt
index 78064ca8f..a7c54dbd3 100644
--- a/libraries/TwoCanBusThreeWrappers/CMakeLists.txt
+++ b/libraries/TwoCanBusThreeWrappers/CMakeLists.txt
@@ -2,8 +2,6 @@ option(ENABLE_TwoCanBusThreeWrappers "Enable/disable TwoCanBusThreeWrappers libr
 
 if(ENABLE_TwoCanBusThreeWrappers)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_library(TwoCanBusThreeWrappers TwoCanBusThreeWrappers.cpp
                                    TwoCanBusThreeWrappers.hpp)
 
diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt
index ebe843d7a..3cc7b6c58 100644
--- a/libraries/YarpPlugins/CMakeLists.txt
+++ b/libraries/YarpPlugins/CMakeLists.txt
@@ -2,10 +2,6 @@
 # Authors: Juan G. Victores & Raul de Santos Rico
 # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 
-if(YARP_VERSION_SHORT VERSION_LESS 2.3.70)
-    include_directories(${YARP_INCLUDE_DIRS})
-endif()
-
 # Create device interface target.
 add_library(YarpDevicesInterfaces INTERFACE)
 
diff --git a/programs/checkCanBus/CMakeLists.txt b/programs/checkCanBus/CMakeLists.txt
index 7556e618a..97e0282f8 100644
--- a/programs/checkCanBus/CMakeLists.txt
+++ b/programs/checkCanBus/CMakeLists.txt
@@ -2,8 +2,6 @@ option(ENABLE_checkCanBus "Enable/disable checkCanBus program" ON)
 
 if(ENABLE_checkCanBus)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(checkCanBus main.cpp
                            CheckCanBus.cpp
                            CheckCanBus.hpp
diff --git a/programs/dumpCanBus/CMakeLists.txt b/programs/dumpCanBus/CMakeLists.txt
index 6c53a3d81..484ac50bf 100644
--- a/programs/dumpCanBus/CMakeLists.txt
+++ b/programs/dumpCanBus/CMakeLists.txt
@@ -2,8 +2,6 @@ option(ENABLE_dumpCanBus "Enable/disable dumpCanBus program" ON)
 
 if(ENABLE_dumpCanBus)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(dumpCanBus main.cpp
                           DumpCanBus.cpp
                           DumpCanBus.hpp
diff --git a/programs/launchLocomotion/CMakeLists.txt b/programs/launchLocomotion/CMakeLists.txt
index 5e12b8b5a..a80c06d6f 100644
--- a/programs/launchLocomotion/CMakeLists.txt
+++ b/programs/launchLocomotion/CMakeLists.txt
@@ -3,8 +3,6 @@ cmake_dependent_option(ENABLE_launchLocomotion "Enable/disable launchLocomotion
 
 if(ENABLE_launchLocomotion)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(launchLocomotion main.cpp)
 
 add_dependencies(launchLocomotion COLOR_DEBUG)
diff --git a/programs/launchManipulation/CMakeLists.txt b/programs/launchManipulation/CMakeLists.txt
index bb3e0d0bb..f992bf351 100644
--- a/programs/launchManipulation/CMakeLists.txt
+++ b/programs/launchManipulation/CMakeLists.txt
@@ -3,8 +3,6 @@ cmake_dependent_option(ENABLE_launchManipulation "Enable/disable launchManipulat
 
 if(ENABLE_launchManipulation)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(launchManipulation main.cpp)
 
 add_dependencies(launchManipulation COLOR_DEBUG)
diff --git a/programs/oneCanBusOneWrapper/CMakeLists.txt b/programs/oneCanBusOneWrapper/CMakeLists.txt
index bf05eb2c7..d3723c1a6 100644
--- a/programs/oneCanBusOneWrapper/CMakeLists.txt
+++ b/programs/oneCanBusOneWrapper/CMakeLists.txt
@@ -3,8 +3,6 @@ cmake_dependent_option(ENABLE_oneCanBusOneWrapper "Enable/disable oneCanBusOneWr
 
 if(ENABLE_oneCanBusOneWrapper)
 
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(oneCanBusOneWrapper main.cpp)
 
 add_dependencies(oneCanBusOneWrapper COLOR_DEBUG)
diff --git a/tests/testCuiAbsolute/CMakeLists.txt b/tests/testCuiAbsolute/CMakeLists.txt
index bea2db8a7..2f524f06f 100644
--- a/tests/testCuiAbsolute/CMakeLists.txt
+++ b/tests/testCuiAbsolute/CMakeLists.txt
@@ -1,5 +1,3 @@
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(testCuiAbsolute testCuiAbsolute.cpp)
 
 add_dependencies(testCuiAbsolute COLOR_DEBUG)
diff --git a/tests/testTechnosoftIpos/CMakeLists.txt b/tests/testTechnosoftIpos/CMakeLists.txt
index bab0c191f..ce215f5d2 100644
--- a/tests/testTechnosoftIpos/CMakeLists.txt
+++ b/tests/testTechnosoftIpos/CMakeLists.txt
@@ -1,5 +1,3 @@
-include_directories(${YARP_INCLUDE_DIRS}) # remove if YARP > v2.3.70
-
 add_executable(testTechnosoftIpos testTechnosoftIpos.cpp)
 
 add_dependencies(testTechnosoftIpos COLOR_DEBUG)

From 0e7b91c74d1aa2d13e7169375031493850599e33 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 20:38:05 +0200
Subject: [PATCH 04/10] Remove all occurrences of setOpenLoopMode(int)

Removed without prior deprecation at YARP 2.3.70.
---
 .../YarpPlugins/AmorControlboard/AmorControlboard.hpp  |  7 -------
 .../YarpPlugins/AmorControlboard/IControlModeImpl.cpp  | 10 ----------
 .../CanBusControlboard/CanBusControlboard.hpp          |  7 -------
 .../CanBusControlboard/IControlMode2Impl.cpp           |  8 --------
 libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp      |  1 -
 .../YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp   | 10 ----------
 .../YarpPlugins/FakeControlboard/FakeControlboard.hpp  |  7 -------
 .../YarpPlugins/FakeControlboard/IControlModeImpl.cpp  |  8 --------
 libraries/YarpPlugins/FakeJoint/FakeJoint.hpp          |  1 -
 .../YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp     | 10 ----------
 .../YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp  | 10 ----------
 libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp    |  1 -
 .../TechnosoftIpos/IControlMode2RawImpl.cpp            | 10 ----------
 .../YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp      |  1 -
 .../YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp  | 10 ----------
 libraries/YarpPlugins/TextilesHand/TextilesHand.hpp    |  1 -
 16 files changed, 102 deletions(-)

diff --git a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
index 951339e06..8b1da08d7 100644
--- a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
+++ b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
@@ -511,13 +511,6 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
     */
     virtual bool setImpedanceVelocityMode(int j);
 
-    /**
-    * Set open loop mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setOpenLoopMode(int j);
-
     /**
     * Get the current control mode.
     * @param j: joint number
diff --git a/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp b/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp
index 71f3c2e0b..54d165c2d 100644
--- a/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp
+++ b/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp
@@ -54,16 +54,6 @@ bool roboticslab::AmorControlboard::setImpedanceVelocityMode(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorControlboard::setOpenLoopMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    if (!indexWithinRange(j))
-        return false;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::AmorControlboard::getControlMode(int j, int *mode)
 {
     //CD_DEBUG("(%d)\n", j);  //-- Way too verbose.
diff --git a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
index 933b4e0b9..97472dee5 100644
--- a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
+++ b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
@@ -143,13 +143,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
     */
     virtual bool setImpedanceVelocityMode(int j);
 
-    /**
-    * Set open loop mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setOpenLoopMode(int j);
-
     /**
     * Get the current control mode.
     * @param j: joint number
diff --git a/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp b/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp
index bf18287aa..f2b3b9638 100644
--- a/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp
@@ -44,14 +44,6 @@ bool roboticslab::CanBusControlboard::setImpedanceVelocityMode(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CanBusControlboard::setOpenLoopMode(int j)
-{
-    CD_ERROR("(%d)\n",j);  //-- Removed in YARP 2.3.70
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::CanBusControlboard::getControlMode(int j, int *mode)
 {
     //CD_DEBUG("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
diff --git a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
index ccb341918..16f837d39 100644
--- a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
+++ b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
@@ -86,7 +86,6 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
     virtual bool setTorqueModeRaw(int j);
     virtual bool setImpedancePositionModeRaw(int j);
     virtual bool setImpedanceVelocityModeRaw(int j);
-    virtual bool setOpenLoopModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 
diff --git a/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp
index c6db13f87..c3aa1ce5d 100644
--- a/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp
@@ -64,14 +64,6 @@ bool roboticslab::CuiAbsolute::setImpedanceVelocityModeRaw(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CuiAbsolute::setOpenLoopModeRaw(int j)
-{
-    CD_ERROR("(%d)\n",j);  //-- Removed in YARP 2.3.70
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::CuiAbsolute::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -123,8 +115,6 @@ bool roboticslab::CuiAbsolute::setControlModeRaw(const int j, const int mode)
         return setImpedancePositionModeRaw(j);
     else if( mode == VOCAB_CM_IMPEDANCE_VEL )
         return setImpedanceVelocityModeRaw(j);
-    /*else if( mode == VOCAB_CM_OPENLOOP )
-        return setOpenLoopModeRaw(j);*/
 
     return false;
 }
diff --git a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
index e4aaf07a0..44ff732e8 100644
--- a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
+++ b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
@@ -559,13 +559,6 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool setImpedanceVelocityMode(int j);
 
-    /**
-     * Set open loop mode, single axis.
-     * @param j joint number
-     * @return true/false success failure.
-     */
-    virtual bool setOpenLoopMode(int j);
-
     /**
      * Get the current control mode.
      * @param j joint number
diff --git a/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp b/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp
index b3f85ad83..391c68067 100644
--- a/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp
@@ -61,14 +61,6 @@ bool roboticslab::FakeControlboard::setImpedanceVelocityMode(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeControlboard::setOpenLoopMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    return false;  // removed in YARP 2.3.70
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::FakeControlboard::getControlMode(int j, int *mode)
 {
     // CD_DEBUG("\n");  //-- Way too verbose.
diff --git a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
index 02a236154..69ccf91a4 100644
--- a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
+++ b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
@@ -79,7 +79,6 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
     virtual bool setTorqueModeRaw(int j);
     virtual bool setImpedancePositionModeRaw(int j);
     virtual bool setImpedanceVelocityModeRaw(int j);
-    virtual bool setOpenLoopModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 
diff --git a/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp
index 81a156401..b0447abe1 100644
--- a/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp
@@ -64,14 +64,6 @@ bool roboticslab::FakeJoint::setImpedanceVelocityModeRaw(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::FakeJoint::setOpenLoopModeRaw(int j)
-{
-    CD_ERROR("(%d)\n",j);  //-- Removed in YARP 2.3.70
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::FakeJoint::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -123,8 +115,6 @@ bool roboticslab::FakeJoint::setControlModeRaw(const int j, const int mode)
         return setImpedancePositionModeRaw(j);
     else if( mode == VOCAB_CM_IMPEDANCE_VEL )
         return setImpedanceVelocityModeRaw(j);
-    /*else if( mode == VOCAB_CM_OPENLOOP )
-        return setOpenLoopModeRaw(j);*/
 
     return false;
 }
diff --git a/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp
index 5bd560a84..ce36644af 100644
--- a/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp
@@ -64,14 +64,6 @@ bool roboticslab::LacqueyFetch::setImpedanceVelocityModeRaw(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::LacqueyFetch::setOpenLoopModeRaw(int j)
-{
-    CD_ERROR("(%d)\n",j);  //-- Removed in YARP 2.3.70
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::LacqueyFetch::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -123,8 +115,6 @@ bool roboticslab::LacqueyFetch::setControlModeRaw(const int j, const int mode)
         return setImpedancePositionModeRaw(j);
     else if( mode == VOCAB_CM_IMPEDANCE_VEL )
         return setImpedanceVelocityModeRaw(j);
-    /*else if( mode == VOCAB_CM_OPENLOOP )
-        return setOpenLoopModeRaw(j);*/
 
     return false;
 }
diff --git a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
index 91df65433..f0a43ff1b 100644
--- a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
+++ b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
@@ -80,7 +80,6 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool setTorqueModeRaw(int j);
     virtual bool setImpedancePositionModeRaw(int j);
     virtual bool setImpedanceVelocityModeRaw(int j);
-    virtual bool setOpenLoopModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp
index 7e22f52e5..5e386bcef 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp
@@ -145,14 +145,6 @@ bool roboticslab::TechnosoftIpos::setImpedanceVelocityModeRaw(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::TechnosoftIpos::setOpenLoopModeRaw(int j)
-{
-    CD_ERROR("(%d)\n",j);  //-- Removed in YARP 2.3.70
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::TechnosoftIpos::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -303,8 +295,6 @@ bool roboticslab::TechnosoftIpos::setControlModeRaw(const int j, const int mode)
         return setImpedanceVelocityModeRaw(j);
     else if( mode == VOCAB_CM_POSITION_DIRECT )
         return setPositionDirectModeRaw();
-    /*else if( mode == VOCAB_CM_OPENLOOP )
-        return setOpenLoopModeRaw(j);*/
 
     return false;
 }
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
index 72b51e13b..885405bbf 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
@@ -91,7 +91,6 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
 
     virtual bool setImpedancePositionModeRaw(int j);
     virtual bool setImpedanceVelocityModeRaw(int j);
-    virtual bool setOpenLoopModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     //-- Auxiliary functions (splitted) of getControlModeRaw
     bool getControlModeRaw1();
diff --git a/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp
index 659681ea9..ecad8f043 100644
--- a/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp
@@ -64,14 +64,6 @@ bool roboticslab::TextilesHand::setImpedanceVelocityModeRaw(int j)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::TextilesHand::setOpenLoopModeRaw(int j)
-{
-    CD_ERROR("(%d)\n",j);  //-- Removed in YARP 2.3.70
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::TextilesHand::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -123,8 +115,6 @@ bool roboticslab::TextilesHand::setControlModeRaw(const int j, const int mode)
         return setImpedancePositionModeRaw(j);
     else if( mode == VOCAB_CM_IMPEDANCE_VEL )
         return setImpedanceVelocityModeRaw(j);
-    /*else if( mode == VOCAB_CM_OPENLOOP )
-        return setOpenLoopModeRaw(j);*/
 
     return false;
 }
diff --git a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
index db39e9bef..a8a1e54cf 100644
--- a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
+++ b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
@@ -90,7 +90,6 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool setTorqueModeRaw(int j);
     virtual bool setImpedancePositionModeRaw(int j);
     virtual bool setImpedanceVelocityModeRaw(int j);
-    virtual bool setOpenLoopModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 

From 9f24199863837af248db9fdb93dfaf68f6d63041 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 21:24:37 +0200
Subject: [PATCH 05/10] Remove deprecated control board methods

---
 .../AmorControlboard/AmorControlboard.hpp     | 133 ------------
 .../AmorControlboard/ITorqueControlImpl.cpp   | 188 ----------------
 .../IVelocityControl2Impl.cpp                 |  34 ---
 .../CanBusControlboard/CanBusControlboard.hpp | 133 ------------
 .../CanBusControlboard/ITorqueControlImpl.cpp | 204 ------------------
 .../IVelocityControl2Impl.cpp                 |  30 ---
 .../YarpPlugins/CuiAbsolute/CuiAbsolute.hpp   |  22 --
 .../CuiAbsolute/ITorqueControlRawImpl.cpp     |  96 ---------
 .../CuiAbsolute/IVelocityControl2RawImpl.cpp  |  32 ---
 .../FakeControlboard/FakeControlboard.hpp     | 153 -------------
 .../FakeControlboard/ITorqueControlImpl.cpp   | 112 ----------
 .../IVelocityControl2Impl.cpp                 |  34 ---
 libraries/YarpPlugins/FakeJoint/FakeJoint.hpp |  22 --
 .../FakeJoint/ITorqueControlRawImpl.cpp       | 126 -----------
 .../FakeJoint/IVelocityControl2RawImpl.cpp    |  32 ---
 .../LacqueyFetch/ITorqueControlRawImpl.cpp    | 126 -----------
 .../LacqueyFetch/IVelocityControl2RawImpl.cpp |  32 ---
 .../YarpPlugins/LacqueyFetch/LacqueyFetch.hpp |  22 --
 .../TechnosoftIpos/ITorqueControlRawImpl.cpp  | 188 ----------------
 .../IVelocityControl2RawImpl.cpp              |  32 ---
 .../TechnosoftIpos/TechnosoftIpos.hpp         |  22 --
 .../TextilesHand/ITorqueControlRawImpl.cpp    | 126 -----------
 .../TextilesHand/IVelocityControl2RawImpl.cpp |  32 ---
 .../YarpPlugins/TextilesHand/TextilesHand.hpp |  22 --
 24 files changed, 1953 deletions(-)

diff --git a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
index 8b1da08d7..079344881 100644
--- a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
+++ b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
@@ -405,34 +405,6 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool getRefVelocities(const int n_joint, const int *joints, double *vels);
 
-#if YARP_VERSION_MAJOR != 3
-    /** Set new velocity pid value for a joint
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setVelPid(int j, const yarp::dev::Pid &pid);
-
-    /** Set new velocity pid value on multiple joints
-     * @param pids pointer to a vector of pids
-     * @return true/false upon success/failure
-     */
-    virtual bool setVelPids(const yarp::dev::Pid *pids);
-
-    /** Get current velocity pid value for a specific joint.
-     * @param j joint number
-     * @param pid pointer to storage for the return value.
-     * @return success/failure
-     */
-    virtual bool getVelPid(int j, yarp::dev::Pid *pid);
-
-    /** Get current velocity pid value for a specific subset of joints.
-     * @param pids vector that will store the values of the pids.
-     * @return success/failure
-     */
-    virtual bool getVelPids(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
-
 //  --------- IControlLimits declarations. Implementation in IControlLimitsImpl.cpp ---------
 
     /**
@@ -689,111 +661,6 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      * @return true/false on success/failure
      */
     virtual bool setBemfParam(int j, double bemf);
-
-    /** Set new pid value for a joint axis.
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
-
-    /** Set new pid value on multiple axes.
-     * @param pids pointer to a vector of pids
-     * @return true/false upon success/failure
-     */
-    virtual bool setTorquePids(const yarp::dev::Pid *pids);
-
-    /** Set the torque error limit for the controller on a specific joint
-     * @param j joint number
-     * @param limit limit value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueErrorLimit(int j, double limit);
-
-    /** Get the torque error limit for the controller on all joints.
-     * @param limits pointer to the vector with the new limits
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueErrorLimits(const double *limits);
-
-    /** Get the current torque error for a joint.
-     * @param j joint number
-     * @param err pointer to the storage for the return value
-     * @return true/false on success failure
-     */
-    virtual bool getTorqueError(int j, double *err);
-
-    /** Get the torque error of all joints.
-     * @param errs pointer to the vector that will store the errors
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrors(double *errs);
-
-    /** Get the output of the controller (e.g. pwm value)
-     * @param j joint number
-     * @param out pointer to storage for return value
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePidOutput(int j, double *out);
-
-    /** Get the output of the controllers (e.g. pwm value)
-     * @param outs pointer to the vector that will store the output values
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePidOutputs(double *outs);
-
-    /** Get current pid value for a specific joint.
-     * @param j joint number
-     * @param pid pointer to storage for the return value.
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePid(int j, yarp::dev::Pid *pid);
-
-    /** Get current pid value for a specific joint.
-     * @param pids vector that will store the values of the pids.
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePids(yarp::dev::Pid *pids);
-
-    /** Get the torque error limit for the controller on a specific joint
-     * @param j joint number
-     * @param limit pointer to the result value
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrorLimit(int j, double *limit);
-
-    /** Get the torque error limit for all controllers
-     * @param limits pointer to the array that will store the output
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrorLimits(double *limits);
-
-    /** Reset the controller of a given joint, usually sets the
-     * current position of the joint as the reference value for the PID, and resets
-     * the integrator.
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool resetTorquePid(int j);
-
-    /** Disable the pid computation for a joint
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool disableTorquePid(int j);
-
-    /** Enable the pid computation for a joint
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool enableTorquePid(int j);
-
-    /** Set offset value for a given pid
-     * @param j joint number
-     * @param v the new value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueOffset(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
 // -------- IInteractionMode declarations. Implementation in IInteractionModeImpl.cpp --------
diff --git a/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp
index a9dca7203..99170d65f 100644
--- a/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/AmorControlboard/ITorqueControlImpl.cpp
@@ -161,193 +161,5 @@ bool roboticslab::AmorControlboard::setBemfParam(int j, double bemf)
     return true;
 }
 
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setTorquePids(const yarp::dev::Pid *pids)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setTorqueErrorLimit(int j, double limit)
-{
-    CD_DEBUG("(%d, %f)\n", j, limit);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setTorqueErrorLimits(const double *limits)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorqueError(int j, double *err)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorqueErrors(double *errs)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorquePidOutput(int j, double *out)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorquePidOutputs(double *outs)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorquePid(int j, yarp::dev::Pid *pid)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorquePids(yarp::dev::Pid *pids)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorqueErrorLimit(int j, double *limit)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getTorqueErrorLimits(double *limits)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::resetTorquePid(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::disableTorquePid(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::enableTorquePid(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setTorqueOffset(int j, double v)
-{
-    CD_DEBUG("(%d, %f)\n", j, v);
-
-    if (!indexWithinRange(j))
-    {
-        return false;
-    }
-
-    return true;
-}
-
 // -----------------------------------------------------------------------------
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp b/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp
index 690f9d217..5fdce1385 100644
--- a/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp
+++ b/libraries/YarpPlugins/AmorControlboard/IVelocityControl2Impl.cpp
@@ -103,37 +103,3 @@ bool roboticslab::AmorControlboard::getRefVelocities(const int n_joint, const in
 }
 
 // -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setVelPid(int j, const yarp::dev::Pid &pid)
-{
-    CD_ERROR("Not available (%d).\n", j);
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::AmorControlboard::setVelPids(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Not available.\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getVelPid(int j, yarp::dev::Pid *pid)
-{
-    CD_ERROR("Not available (%d).\n", j);
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::getVelPids(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Not available.\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
index 97472dee5..4a63ace63 100644
--- a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
+++ b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
@@ -616,111 +616,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      * @return true/false on success/failure
      */
     virtual bool setBemfParam(int j, double bemf);
-
-    /** Set new pid value for a joint axis.
-    * @param j joint number
-    * @param pid new pid value
-    * @return true/false on success/failure
-    */
-    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
-
-    /** Set new pid value on multiple axes.
-     * @param pids pointer to a vector of pids
-     * @return true/false upon success/failure
-     */
-    virtual bool setTorquePids(const yarp::dev::Pid *pids);
-
-    /** Set the torque error limit for the controller on a specific joint
-     * @param j joint number
-     * @param limit limit value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueErrorLimit(int j, double limit);
-
-    /** Get the torque error limit for the controller on all joints.
-     * @param limits pointer to the vector with the new limits
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueErrorLimits(const double *limits);
-
-    /** Get the current torque error for a joint.
-     * @param j joint number
-     * @param err pointer to the storage for the return value
-     * @return true/false on success failure
-     */
-    virtual bool getTorqueError(int j, double *err);
-
-    /** Get the torque error of all joints.
-     * @param errs pointer to the vector that will store the errors
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrors(double *errs);
-
-    /** Get the output of the controller (e.g. pwm value)
-     * @param j joint number
-     * @param out pointer to storage for return value
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePidOutput(int j, double *out);
-
-    /** Get the output of the controllers (e.g. pwm value)
-     * @param outs pointer to the vector that will store the output values
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePidOutputs(double *outs);
-
-    /** Get current pid value for a specific joint.
-     * @param j joint number
-     * @param pid pointer to storage for the return value.
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePid(int j, yarp::dev::Pid *pid);
-
-    /** Get current pid value for a specific joint.
-     * @param pids vector that will store the values of the pids.
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePids(yarp::dev::Pid *pids);
-
-    /** Get the torque error limit for the controller on a specific joint
-     * @param j joint number
-     * @param limit pointer to the result value
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrorLimit(int j, double *limit);
-
-    /** Get the torque error limit for all controllers
-     * @param limits pointer to the array that will store the output
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrorLimits(double *limits);
-
-    /** Reset the controller of a given joint, usually sets the
-     * current position of the joint as the reference value for the PID, and resets
-     * the integrator.
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool resetTorquePid(int j);
-
-    /** Disable the pid computation for a joint
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool disableTorquePid(int j);
-
-    /** Enable the pid computation for a joint
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool enableTorquePid(int j);
-
-    /** Set offset value for a given pid
-     * @param j joint number
-     * @param v the new value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueOffset(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControl Declarations. Implementation in IVelocityControl2Impl.cpp ---------
@@ -798,34 +693,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     // virtual bool stop(const int n_joint, const int *joints);
 
-#if YARP_VERSION_MAJOR != 3
-    /** Set new velocity pid value for a joint
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setVelPid(int j, const yarp::dev::Pid &pid);
-
-    /** Set new velocity pid value on multiple joints
-     * @param pids pointer to a vector of pids
-     * @return true/false upon success/failure
-     */
-    virtual bool setVelPids(const yarp::dev::Pid *pids);
-
-    /** Get current velocity pid value for a specific joint.
-     * @param j joint number
-     * @param pid pointer to storage for the return value.
-     * @return success/failure
-     */
-    virtual bool getVelPid(int j, yarp::dev::Pid *pid);
-
-    /** Get current velocity pid value for a specific subset of joints.
-     * @param pids vector that will store the values of the pids.
-     * @return success/failure
-     */
-    virtual bool getVelPids(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
-
     // -----------IInteracionMode Declarations. Implementation in IInteracionModeImpl.cpp --------------
     /**
      * Get the current interaction mode of the robot, values can be stiff or compliant.
diff --git a/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
index 723126fc5..303e73a43 100644
--- a/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
@@ -132,209 +132,5 @@ bool roboticslab::CanBusControlboard::setBemfParam(int j, double bemf)
     return iTorqueControlRaw[j]->setBemfParamRaw( 0, bemf );;
 }
 
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->setTorquePidRaw( 0, pid );;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setTorquePids(const yarp::dev::Pid *pids)
-{
-    CD_DEBUG("\n");
-
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->setTorquePid(j, pids[j]);
-    }
-    return ok;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setTorqueErrorLimit(int j, double limit)
-{
-    CD_DEBUG("(%d,%f)\n",j,limit);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->setTorqueErrorLimitRaw( 0, limit );
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setTorqueErrorLimits(const double *limits)
-{
-    CD_DEBUG("\n");
-
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->setTorqueErrorLimit(j, limits[j]);
-    }
-    return ok;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorqueError(int j, double *err)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->getTorqueErrorRaw( 0, err );
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorqueErrors(double *errs)
-{
-    CD_DEBUG("\n");
-
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->getTorqueError(j, &(errs[j]));
-    }
-    return ok;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorquePidOutput(int j, double *out)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->getTorquePidOutputRaw( 0, out );
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorquePidOutputs(double *outs)
-{
-    CD_DEBUG("\n");
-
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->getTorquePidOutput(j, &(outs[j]));
-    }
-    return ok;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorquePid(int j, yarp::dev::Pid *pid)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->getTorquePidRaw( 0, pid );
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorquePids(yarp::dev::Pid *pids)
-{
-    CD_DEBUG("\n");
-
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->getTorquePid(j, &(pids[j]));
-    }
-    return ok;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorqueErrorLimit(int j, double *limit)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->getTorqueErrorLimitRaw( 0, limit );
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getTorqueErrorLimits(double *limits)
-{
-    CD_DEBUG("\n");
-
-    bool ok = true;
-    for(int j=0; j<nodes.size(); j++)
-    {
-        ok &= this->getTorqueErrorLimit(j, &(limits[j]));
-    }
-    return ok;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::resetTorquePid(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->resetTorquePidRaw(0);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::disableTorquePid(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->disableTorquePidRaw(0);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::enableTorquePid(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->enableTorquePidRaw(0);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setTorqueOffset(int j, double v)
-{
-    CD_DEBUG("(%d,%f)\n",j,v);
-
-    //-- Check index within range
-    if ( ! this->indexWithinRange(j) ) return false;
-
-    return iTorqueControlRaw[j]->setTorqueOffsetRaw( 0, v );
-}
-
 // -----------------------------------------------------------------------------
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp b/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp
index 3a5fc7f4b..722e36252 100644
--- a/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/IVelocityControl2Impl.cpp
@@ -84,33 +84,3 @@ bool roboticslab::CanBusControlboard::getRefVelocities(const int n_joint, const
 }
 
 // -----------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::CanBusControlboard::setVelPid(int j, const yarp::dev::Pid &pid)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setVelPids(const yarp::dev::Pid *pids)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getVelPid(int j, yarp::dev::Pid *pid)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::getVelPids(yarp::dev::Pid *pids)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
index 16f837d39..c815b4c33 100644
--- a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
+++ b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
@@ -161,22 +161,6 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
 #if YARP_VERSION_MAJOR != 3
     virtual bool getBemfParamRaw(int j, double *bemf);
     virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
-    virtual bool setTorqueErrorLimitRaw(int j, double limit);
-    virtual bool setTorqueErrorLimitsRaw(const double *limits);
-    virtual bool getTorqueErrorRaw(int j, double *err);
-    virtual bool getTorqueErrorsRaw(double *errs);
-    virtual bool getTorquePidOutputRaw(int j, double *out);
-    virtual bool getTorquePidOutputsRaw(double *outs);
-    virtual bool getTorquePidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getTorquePidsRaw(yarp::dev::Pid *pids);
-    virtual bool getTorqueErrorLimitRaw(int j, double *limit);
-    virtual bool getTorqueErrorLimitsRaw(double *limits);
-    virtual bool resetTorquePidRaw(int j);
-    virtual bool disableTorquePidRaw(int j);
-    virtual bool enableTorquePidRaw(int j);
-    virtual bool setTorqueOffsetRaw(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
@@ -191,12 +175,6 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
-#if YARP_VERSION_MAJOR != 3
-    virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
-    virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
 
diff --git a/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp
index 384f295e9..5c5f06818 100644
--- a/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/CuiAbsolute/ITorqueControlRawImpl.cpp
@@ -62,100 +62,4 @@ bool roboticslab::CuiAbsolute::setBemfParamRaw(int j, double bemf)
     CD_INFO("\n");
     return true;
 }
-
-bool roboticslab::CuiAbsolute::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::setTorquePidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-bool roboticslab::CuiAbsolute::setTorqueErrorLimitRaw(int j, double limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::setTorqueErrorLimitsRaw(const double *limits)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-bool roboticslab::CuiAbsolute::getTorqueErrorRaw(int j, double *err)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::getTorqueErrorsRaw(double *errs)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-bool roboticslab::CuiAbsolute::getTorquePidOutputRaw(int j, double *out)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::getTorquePidOutputsRaw(double *outs)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-bool roboticslab::CuiAbsolute::getTorquePidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::getTorquePidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-bool roboticslab::CuiAbsolute::getTorqueErrorLimitRaw(int j, double *limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::getTorqueErrorLimitsRaw(double *limits)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-bool roboticslab::CuiAbsolute::resetTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::disableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::enableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-bool roboticslab::CuiAbsolute::setTorqueOffsetRaw(int j, double v)
-{
-    CD_INFO("\n");
-    return true;
-}
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp
index 85d30bd61..8326b7495 100644
--- a/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/CuiAbsolute/IVelocityControl2RawImpl.cpp
@@ -51,35 +51,3 @@ bool roboticslab::CuiAbsolute::getRefVelocitiesRaw(const int n_joint, const int
 }
 
 // --------------------------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::CuiAbsolute::setVelPidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::setVelPidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::getVelPidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::getVelPidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
index 44ff732e8..af6a8142f 100644
--- a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
+++ b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
@@ -448,38 +448,6 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool getRefVelocities(const int n_joint, const int *joints, double *vels);
 
-#if YARP_VERSION_MAJOR != 3
-    /**
-     * Set new velocity pid value for a joint
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setVelPid(int j, const yarp::dev::Pid &pid);
-
-    /**
-     * Set new velocity pid value on multiple joints
-     * @param pids pointer to a vector of pids
-     * @return true/false upon success/failure
-     */
-    virtual bool setVelPids(const yarp::dev::Pid *pids);
-
-    /**
-     * Get current velocity pid value for a specific joint.
-     * @param j joint number
-     * @param pid pointer to storage for the return value.
-     * @return success/failure
-     */
-    virtual bool getVelPid(int j, yarp::dev::Pid *pid);
-
-    /**
-     * Get current velocity pid value for a specific subset of joints.
-     * @param pids vector that will store the values of the pids.
-     * @return success/failure
-     */
-    virtual bool getVelPids(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
-
 //  --------- IControlLimits Declarations. Implementation in IControlLimitsImpl.cpp ---------
 
     /**
@@ -709,127 +677,6 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      * @return true/false on success/failure
      */
     virtual bool setBemfParam(int j, double bemf);
-
-    /**
-     * Set new pid value for a joint axis.
-     * @param j joint number
-     * @param pid new pid value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorquePid(int j, const yarp::dev::Pid &pid);
-
-    /**
-     * Set new pid value on multiple axes.
-     * @param pids pointer to a vector of pids
-     * @return true/false upon success/failure
-     */
-    virtual bool setTorquePids(const yarp::dev::Pid *pids);
-
-    /**
-     * Set the torque error limit for the controller on a specific joint
-     * @param j joint number
-     * @param limit limit value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueErrorLimit(int j, double limit);
-
-    /**
-     * Get the torque error limit for the controller on all joints.
-     * @param limits pointer to the vector with the new limits
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueErrorLimits(const double *limits);
-
-    /**
-     * Get the current torque error for a joint.
-     * @param j joint number
-     * @param err pointer to the storage for the return value
-     * @return true/false on success failure
-     */
-    virtual bool getTorqueError(int j, double *err);
-
-    /**
-     * Get the torque error of all joints.
-     * @param errs pointer to the vector that will store the errors
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrors(double *errs);
-
-    /**
-     * Get the output of the controller (e.g. pwm value)
-     * @param j joint number
-     * @param out pointer to storage for return value
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePidOutput(int j, double *out);
-
-    /**
-     * Get the output of the controllers (e.g. pwm value)
-     * @param outs pointer to the vector that will store the output values
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePidOutputs(double *outs);
-
-    /**
-     * Get current pid value for a specific joint.
-     * @param j joint number
-     * @param pid pointer to storage for the return value.
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePid(int j, yarp::dev::Pid *pid);
-
-    /**
-     * Get current pid value for a specific joint.
-     * @param pids vector that will store the values of the pids.
-     * @return true/false on success/failure
-     */
-    virtual bool getTorquePids(yarp::dev::Pid *pids);
-
-    /**
-     * Get the torque error limit for the controller on a specific joint
-     * @param j joint number
-     * @param limit pointer to the result value
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrorLimit(int j, double *limit);
-
-    /**
-     * Get the torque error limit for all controllers
-     * @param limits pointer to the array that will store the output
-     * @return true/false on success/failure
-     */
-    virtual bool getTorqueErrorLimits(double *limits);
-
-    /**
-     * Reset the controller of a given joint, usually sets the
-     * current position of the joint as the reference value for the PID, and resets
-     * the integrator.
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool resetTorquePid(int j);
-
-    /**
-     * Disable the pid computation for a joint
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool disableTorquePid(int j);
-
-    /**
-     * Enable the pid computation for a joint
-     * @param j joint number
-     * @return true/false on success/failure
-     */
-    virtual bool enableTorquePid(int j);
-
-    /**
-     * Set offset value for a given pid
-     * @param j joint number
-     * @param v the new value
-     * @return true/false on success/failure
-     */
-    virtual bool setTorqueOffset(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
 // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
diff --git a/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
index 9e4888b99..91b1f2de2 100644
--- a/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
@@ -78,117 +78,5 @@ bool roboticslab::FakeControlboard::setBemfParam(int j, double bemf)
     return true;
 }
 
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setTorquePid(int j, const yarp::dev::Pid &pid)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setTorquePids(const yarp::dev::Pid *pids)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setTorqueErrorLimit(int j, double limit)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setTorqueErrorLimits(const double *limits)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorqueError(int j, double *err)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorqueErrors(double *errs)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorquePidOutput(int j, double *out)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorquePidOutputs(double *outs)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorquePid(int j, yarp::dev::Pid *pid)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorquePids(yarp::dev::Pid *pids)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorqueErrorLimit(int j, double *limit)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getTorqueErrorLimits(double *limits)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::resetTorquePid(int j)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::disableTorquePid(int j)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::enableTorquePid(int j)
-{
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setTorqueOffset(int j, double v)
-{
-    return true;
-}
-
 // -----------------------------------------------------------------------------
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp b/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp
index 36937061b..48b19d108 100644
--- a/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/IVelocityControl2Impl.cpp
@@ -38,37 +38,3 @@ bool roboticslab::FakeControlboard::getRefVelocities(const int n_joint, const in
 }
 
 // -----------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::FakeControlboard::setVelPid(int j, const yarp::dev::Pid &pid)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setVelPids(const yarp::dev::Pid *pids)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getVelPid(int j, yarp::dev::Pid *pid)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::getVelPids(yarp::dev::Pid *pids)
-{
-    CD_DEBUG("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
index 69ccf91a4..020f8741c 100644
--- a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
+++ b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
@@ -154,22 +154,6 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
 #if YARP_VERSION_MAJOR != 3
     virtual bool getBemfParamRaw(int j, double *bemf);
     virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
-    virtual bool setTorqueErrorLimitRaw(int j, double limit);
-    virtual bool setTorqueErrorLimitsRaw(const double *limits);
-    virtual bool getTorqueErrorRaw(int j, double *err);
-    virtual bool getTorqueErrorsRaw(double *errs);
-    virtual bool getTorquePidOutputRaw(int j, double *out);
-    virtual bool getTorquePidOutputsRaw(double *outs);
-    virtual bool getTorquePidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getTorquePidsRaw(yarp::dev::Pid *pids);
-    virtual bool getTorqueErrorLimitRaw(int j, double *limit);
-    virtual bool getTorqueErrorLimitsRaw(double *limits);
-    virtual bool resetTorquePidRaw(int j);
-    virtual bool disableTorquePidRaw(int j);
-    virtual bool enableTorquePidRaw(int j);
-    virtual bool setTorqueOffsetRaw(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControl Declarations. Implementation in IVelocityControlImpl.cpp ---------
@@ -184,12 +168,6 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
-#if YARP_VERSION_MAJOR != 3
-    virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
-    virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
 
diff --git a/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp
index cb9021f1c..b6ea38b4d 100644
--- a/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/FakeJoint/ITorqueControlRawImpl.cpp
@@ -84,130 +84,4 @@ bool roboticslab::FakeJoint::setBemfParamRaw(int j, double bemf)
 }
 
 // ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setTorquePidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setTorqueErrorLimitRaw(int j, double limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setTorqueErrorLimitsRaw(const double *limits)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorqueErrorRaw(int j, double *err)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorqueErrorsRaw(double *errs)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorquePidOutputRaw(int j, double *out)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorquePidOutputsRaw(double *outs)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorquePidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorquePidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorqueErrorLimitRaw(int j, double *limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getTorqueErrorLimitsRaw(double *limits)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::resetTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::disableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::enableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// ------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setTorqueOffsetRaw(int j, double v)
-{
-    CD_INFO("\n");
-    return true;
-}
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp
index 7d6f7e43f..0343cc7e2 100644
--- a/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/FakeJoint/IVelocityControl2RawImpl.cpp
@@ -51,35 +51,3 @@ bool roboticslab::FakeJoint::getRefVelocitiesRaw(const int n_joint, const int *j
 }
 
 // --------------------------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::FakeJoint::setVelPidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setVelPidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getVelPidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::getVelPidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp
index 0224177fe..fe31f7292 100644
--- a/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/LacqueyFetch/ITorqueControlRawImpl.cpp
@@ -84,130 +84,4 @@ bool roboticslab::LacqueyFetch::setBemfParamRaw(int j, double bemf)
 }
 
 // -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setTorquePidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setTorqueErrorLimitRaw(int j, double limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setTorqueErrorLimitsRaw(const double *limits)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorqueErrorRaw(int j, double *err)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorqueErrorsRaw(double *errs)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorquePidOutputRaw(int j, double *out)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorquePidOutputsRaw(double *outs)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorquePidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorquePidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorqueErrorLimitRaw(int j, double *limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getTorqueErrorLimitsRaw(double *limits)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::resetTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::disableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::enableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setTorqueOffsetRaw(int j, double v)
-{
-    CD_INFO("\n");
-    return true;
-}
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp
index ae0e138ca..a908be14d 100644
--- a/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/LacqueyFetch/IVelocityControl2RawImpl.cpp
@@ -75,35 +75,3 @@ bool roboticslab::LacqueyFetch::stopRaw(const int n_joint, const int *joints)
 }
 */
 // -----------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::LacqueyFetch::setVelPidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setVelPidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getVelPidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::getVelPidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
index f0a43ff1b..300a0a81d 100644
--- a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
+++ b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
@@ -155,22 +155,6 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
 #if YARP_VERSION_MAJOR != 3
     virtual bool getBemfParamRaw(int j, double *bemf);
     virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
-    virtual bool setTorqueErrorLimitRaw(int j, double limit);
-    virtual bool setTorqueErrorLimitsRaw(const double *limits);
-    virtual bool getTorqueErrorRaw(int j, double *err);
-    virtual bool getTorqueErrorsRaw(double *errs);
-    virtual bool getTorquePidOutputRaw(int j, double *out);
-    virtual bool getTorquePidOutputsRaw(double *outs);
-    virtual bool getTorquePidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getTorquePidsRaw(yarp::dev::Pid *pids);
-    virtual bool getTorqueErrorLimitRaw(int j, double *limit);
-    virtual bool getTorqueErrorLimitsRaw(double *limits);
-    virtual bool resetTorquePidRaw(int j);
-    virtual bool disableTorquePidRaw(int j);
-    virtual bool enableTorquePidRaw(int j);
-    virtual bool setTorqueOffsetRaw(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
@@ -185,12 +169,6 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
-#if YARP_VERSION_MAJOR != 3
-    virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
-    virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
     virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode);
diff --git a/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
index 9e69ea42b..ccc249c8a 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
@@ -153,193 +153,5 @@ bool roboticslab::TechnosoftIpos::setBemfParamRaw(int j, double bemf)
     return true;
 }
 
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setTorquePidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setTorqueErrorLimitRaw(int j, double limit)
-{
-    CD_INFO("(%d,%f)\n",j,limit);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setTorqueErrorLimitsRaw(const double *limits)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorqueErrorRaw(int j, double *err)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorqueErrorsRaw(double *errs)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorquePidOutputRaw(int j, double *out)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorquePidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorquePidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorquePidOutputsRaw(double *outs)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorqueErrorLimitRaw(int j, double *limit)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getTorqueErrorLimitsRaw(double *limits)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::resetTorquePidRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::disableTorquePidRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::enableTorquePidRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
-// -------------------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setTorqueOffsetRaw(int j, double v)
-{
-    CD_INFO("(%d,%f)\n",j,v);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
-
 // -------------------------------------------------------------------------------------
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp
index c12d98239..076608800 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/IVelocityControl2RawImpl.cpp
@@ -131,35 +131,3 @@ bool roboticslab::TechnosoftIpos::stopRaw(const int n_joint, const int *joints)
 }
 
 // -----------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::TechnosoftIpos::setVelPidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_WARNING("Missing implementation\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setVelPidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_WARNING("Missing implementation\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getVelPidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_WARNING("Missing implementation\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::getVelPidsRaw(yarp::dev::Pid *pids)
-{
-    CD_WARNING("Missing implementation\n");
-    return true;
-}
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
index 885405bbf..4720ebb36 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
@@ -172,22 +172,6 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
 #if YARP_VERSION_MAJOR != 3
     virtual bool getBemfParamRaw(int j, double *bemf);
     virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
-    virtual bool setTorqueErrorLimitRaw(int j, double limit);
-    virtual bool setTorqueErrorLimitsRaw(const double *limits);
-    virtual bool getTorqueErrorRaw(int j, double *err);
-    virtual bool getTorqueErrorsRaw(double *errs);
-    virtual bool getTorquePidOutputRaw(int j, double *out);
-    virtual bool getTorquePidOutputsRaw(double *outs);
-    virtual bool getTorquePidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getTorquePidsRaw(yarp::dev::Pid *pids);
-    virtual bool getTorqueErrorLimitRaw(int j, double *limit);
-    virtual bool getTorqueErrorLimitsRaw(double *limits);
-    virtual bool resetTorquePidRaw(int j);
-    virtual bool disableTorquePidRaw(int j);
-    virtual bool enableTorquePidRaw(int j);
-    virtual bool setTorqueOffsetRaw(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
@@ -205,12 +189,6 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
     // -- virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // ------------------- Just declareted in IPositionControl2Raw
     // -- virtual bool stopRaw(const int n_joint, const int *joints);
-#if YARP_VERSION_MAJOR != 3
-    virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
-    virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
     virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode);
diff --git a/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp
index ca56d2d25..76f044f55 100644
--- a/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp
+++ b/libraries/YarpPlugins/TextilesHand/ITorqueControlRawImpl.cpp
@@ -84,130 +84,4 @@ bool roboticslab::TextilesHand::setBemfParamRaw(int j, double bemf)
 }
 
 // -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setTorquePidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setTorquePidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setTorqueErrorLimitRaw(int j, double limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setTorqueErrorLimitsRaw(const double *limits)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorqueErrorRaw(int j, double *err)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorqueErrorsRaw(double *errs)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorquePidOutputRaw(int j, double *out)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorquePidOutputsRaw(double *outs)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorquePidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorquePidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorqueErrorLimitRaw(int j, double *limit)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getTorqueErrorLimitsRaw(double *limits)
-{
-    CD_ERROR("\n");
-    return false;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::resetTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::disableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::enableTorquePidRaw(int j)
-{
-    CD_INFO("\n");
-    return true;
-}
-
-// -----------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setTorqueOffsetRaw(int j, double v)
-{
-    CD_INFO("\n");
-    return true;
-}
 #endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp b/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp
index b62959e65..319e4e7fd 100644
--- a/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp
+++ b/libraries/YarpPlugins/TextilesHand/IVelocityControl2RawImpl.cpp
@@ -51,35 +51,3 @@ bool roboticslab::TextilesHand::getRefVelocitiesRaw(const int n_joint, const int
 }
 
 // --------------------------------------------------------------------------------------------
-
-#if YARP_VERSION_MAJOR != 3
-bool roboticslab::TextilesHand::setVelPidRaw(int j, const yarp::dev::Pid &pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setVelPidsRaw(const yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getVelPidRaw(int j, yarp::dev::Pid *pid)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-
-// --------------------------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::getVelPidsRaw(yarp::dev::Pid *pids)
-{
-    CD_ERROR("Missing implementation\n");
-    return false;
-}
-#endif // YARP_VERSION_MAJOR != 3
diff --git a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
index a8a1e54cf..dcbf48741 100644
--- a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
+++ b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
@@ -165,22 +165,6 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
 #if YARP_VERSION_MAJOR != 3
     virtual bool getBemfParamRaw(int j, double *bemf);
     virtual bool setBemfParamRaw(int j, double bemf);
-    virtual bool setTorquePidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setTorquePidsRaw(const yarp::dev::Pid *pids);
-    virtual bool setTorqueErrorLimitRaw(int j, double limit);
-    virtual bool setTorqueErrorLimitsRaw(const double *limits);
-    virtual bool getTorqueErrorRaw(int j, double *err);
-    virtual bool getTorqueErrorsRaw(double *errs);
-    virtual bool getTorquePidOutputRaw(int j, double *out);
-    virtual bool getTorquePidOutputsRaw(double *outs);
-    virtual bool getTorquePidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getTorquePidsRaw(yarp::dev::Pid *pids);
-    virtual bool getTorqueErrorLimitRaw(int j, double *limit);
-    virtual bool getTorqueErrorLimitsRaw(double *limits);
-    virtual bool resetTorquePidRaw(int j);
-    virtual bool disableTorquePidRaw(int j);
-    virtual bool enableTorquePidRaw(int j);
-    virtual bool setTorqueOffsetRaw(int j, double v);
 #endif // YARP_VERSION_MAJOR != 3
 
     //  --------- IVelocityControlRaw Declarations. Implementation in IVelocityControl2RawImpl.cpp ---------
@@ -195,12 +179,6 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     // -- (just defined in IInteractionModeRaw) - virtual bool setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs);
     // -- (just defined in IInteractionModeRaw) - virtual bool stopRaw(const int n_joint, const int *joints);
-#if YARP_VERSION_MAJOR != 3
-    virtual bool setVelPidRaw(int j, const yarp::dev::Pid &pid);
-    virtual bool setVelPidsRaw(const yarp::dev::Pid *pids);
-    virtual bool getVelPidRaw(int j, yarp::dev::Pid *pid);
-    virtual bool getVelPidsRaw(yarp::dev::Pid *pids);
-#endif // YARP_VERSION_MAJOR != 3
 
     // ------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp -------
 

From a74fe93c977818b1f7cb6f724eeba5f083df4671 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 22:19:36 +0200
Subject: [PATCH 06/10] Don't implement deprecated control mode setters

---
 .../AmorControlboard/AmorControlboard.hpp     | 35 -------
 .../AmorControlboard/IControlModeImpl.cpp     | 50 ----------
 .../CanBusControlboard/CanBusControlboard.hpp | 38 --------
 .../CanBusControlboard/IControlMode2Impl.cpp  | 40 --------
 .../YarpPlugins/CuiAbsolute/CuiAbsolute.hpp   |  5 -
 .../CuiAbsolute/IControlMode2RawImpl.cpp      | 73 +-------------
 .../FakeControlboard/CMakeLists.txt           |  1 +
 .../FakeControlboard/FakeControlboard.cpp     | 46 +++++++++
 .../FakeControlboard/FakeControlboard.hpp     | 39 +-------
 .../FakeControlboard/IControlMode2Impl.cpp    |  4 -
 .../FakeControlboard/IControlModeImpl.cpp     | 57 +----------
 libraries/YarpPlugins/FakeJoint/FakeJoint.hpp |  5 -
 .../FakeJoint/IControlMode2RawImpl.cpp        | 73 +-------------
 .../LacqueyFetch/IControlMode2RawImpl.cpp     | 73 +-------------
 .../YarpPlugins/LacqueyFetch/LacqueyFetch.hpp |  5 -
 .../TechnosoftIpos/IControlMode2RawImpl.cpp   | 94 ++++++++++++++-----
 .../TechnosoftIpos/TechnosoftIpos.cpp         | 78 ---------------
 .../TechnosoftIpos/TechnosoftIpos.hpp         | 13 +--
 .../TextilesHand/IControlMode2RawImpl.cpp     | 73 +-------------
 .../YarpPlugins/TextilesHand/TextilesHand.hpp |  5 -
 20 files changed, 132 insertions(+), 675 deletions(-)
 create mode 100644 libraries/YarpPlugins/FakeControlboard/FakeControlboard.cpp

diff --git a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
index 079344881..d79a6ef59 100644
--- a/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
+++ b/libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp
@@ -448,41 +448,6 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
 
 //  --------- IControlMode declarations. Implementation in IControlModeImpl.cpp ---------
 
-    /**
-    * Set position mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setPositionMode(int j);
-
-    /**
-    * Set velocity mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setVelocityMode(int j);
-
-    /**
-    * Set torque mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setTorqueMode(int j);
-
-    /**
-    * Set impedance position mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setImpedancePositionMode(int j);
-
-    /**
-    * Set impedance velocity mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setImpedanceVelocityMode(int j);
-
     /**
     * Get the current control mode.
     * @param j: joint number
diff --git a/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp b/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp
index 54d165c2d..0298f25c9 100644
--- a/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp
+++ b/libraries/YarpPlugins/AmorControlboard/IControlModeImpl.cpp
@@ -4,56 +4,6 @@
 
 // ------------------- IControlMode related ------------------------------------
 
-bool roboticslab::AmorControlboard::setPositionMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    if (!indexWithinRange(j))
-        return false;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setVelocityMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    if (!indexWithinRange(j))
-        return false;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setTorqueMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    if (!indexWithinRange(j))
-        return false;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setImpedancePositionMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    if (!indexWithinRange(j))
-        return false;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::AmorControlboard::setImpedanceVelocityMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    if (!indexWithinRange(j))
-        return false;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::AmorControlboard::getControlMode(int j, int *mode)
 {
     //CD_DEBUG("(%d)\n", j);  //-- Way too verbose.
diff --git a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
index 4a63ace63..43a18482a 100644
--- a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
+++ b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
@@ -108,41 +108,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
 
     //  --------- IControlMode Declarations. Implementation in IControlMode2Impl.cpp ---------
 
-    /**
-    * Set position mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setPositionMode(int j);
-
-    /**
-    * Set velocity mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setVelocityMode(int j);
-
-    /**
-    * Set torque mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setTorqueMode(int j);
-
-    /**
-    * Set impedance position mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setImpedancePositionMode(int j);
-
-    /**
-    * Set impedance velocity mode, single axis.
-    * @param j: joint number
-    * @return: true/false success failure.
-    */
-    virtual bool setImpedanceVelocityMode(int j);
-
     /**
     * Get the current control mode.
     * @param j: joint number
@@ -208,8 +173,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
     */
     virtual bool setControlModes(int *modes);
 
-
-
     //  ---------- IEncoders Declarations. Implementation in IEncodersImpl.cpp ----------
 
     /**
@@ -757,7 +720,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     virtual bool setInteractionModes(yarp::dev::InteractionModeEnum* modes);
 
-
     // -------- Thread declarations. Implementation in ThreadImpl.cpp --------
 
     /**
diff --git a/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp b/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp
index f2b3b9638..3b9498685 100644
--- a/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/IControlMode2Impl.cpp
@@ -4,46 +4,6 @@
 
 // ------------------- IControlMode Related ------------------------------------
 
-bool roboticslab::CanBusControlboard::setPositionMode(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-    return setControlMode(j, VOCAB_CM_POSITION);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setVelocityMode(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-    return setControlMode(j, VOCAB_CM_VELOCITY);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setTorqueMode(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-    return setControlMode(j, VOCAB_CM_TORQUE);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setImpedancePositionMode(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-    return setControlMode(j, VOCAB_CM_IMPEDANCE_POS);
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CanBusControlboard::setImpedanceVelocityMode(int j)
-{
-    CD_DEBUG("(%d)\n",j);
-    return setControlMode(j, VOCAB_CM_IMPEDANCE_VEL);
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::CanBusControlboard::getControlMode(int j, int *mode)
 {
     //CD_DEBUG("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
diff --git a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
index c815b4c33..fd00e272c 100644
--- a/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
+++ b/libraries/YarpPlugins/CuiAbsolute/CuiAbsolute.hpp
@@ -81,11 +81,6 @@ class CuiAbsolute : public yarp::dev::DeviceDriver, public yarp::dev::IControlLi
     virtual bool getVelLimitsRaw(int axis, double *min, double *max);
 
     //  --------- IControlModeRaw Declarations. Implementation in IControlMode2RawImpl.cpp ---------
-    virtual bool setPositionModeRaw(int j);
-    virtual bool setVelocityModeRaw(int j);
-    virtual bool setTorqueModeRaw(int j);
-    virtual bool setImpedancePositionModeRaw(int j);
-    virtual bool setImpedanceVelocityModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 
diff --git a/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp
index c3aa1ce5d..713fb9ebf 100644
--- a/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/CuiAbsolute/IControlMode2RawImpl.cpp
@@ -4,66 +4,6 @@
 
 // ############################## IControlModeRaw Related ##############################
 
-bool roboticslab::CuiAbsolute::setPositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::setVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::setTorqueModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::setImpedancePositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::CuiAbsolute::setImpedanceVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::CuiAbsolute::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -105,18 +45,7 @@ bool roboticslab::CuiAbsolute::setControlModeRaw(const int j, const int mode)
     //-- Check index within range
     if ( j != 0 ) return false;
 
-    if( mode == VOCAB_CM_POSITION )
-        return setPositionModeRaw(j);
-    else if( mode == VOCAB_CM_VELOCITY )
-        return setVelocityModeRaw(j);
-    else if( mode == VOCAB_CM_TORQUE )
-        return setTorqueModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_POS )
-        return setImpedancePositionModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_VEL )
-        return setImpedanceVelocityModeRaw(j);
-
-    return false;
+    return true;
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/FakeControlboard/CMakeLists.txt b/libraries/YarpPlugins/FakeControlboard/CMakeLists.txt
index 2fdcf11f4..5e07be1ba 100644
--- a/libraries/YarpPlugins/FakeControlboard/CMakeLists.txt
+++ b/libraries/YarpPlugins/FakeControlboard/CMakeLists.txt
@@ -12,6 +12,7 @@ if(NOT SKIP_FakeControlboard)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # yarp plugin builder needs this
 
 yarp_add_plugin(FakeControlboard FakeControlboard.hpp
+                                 FakeControlboard.cpp
                                  DeviceDriverImpl.cpp
                                  IControlLimitsImpl.cpp
                                  IControlLimits2Impl.cpp
diff --git a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.cpp b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.cpp
new file mode 100644
index 000000000..10cb1730d
--- /dev/null
+++ b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.cpp
@@ -0,0 +1,46 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "FakeControlboard.hpp"
+
+#include <ColorDebug.hpp>
+
+// ------------------- Miscellanea ------------------------------------
+
+bool roboticslab::FakeControlboard::setPositionMode(int j)
+{
+    CD_DEBUG("(%d)\n", j);
+
+    if (modePosVel == POSITION_MODE)
+    {
+        return true;  // Simply return true if we were already in pos mode.
+    }
+
+    // Do anything additional before setting flag to pos...
+    if (!stop(j))
+    {
+        CD_ERROR("failed to stop joint %d\n", j);
+        return false;
+    }
+
+    modePosVel = POSITION_MODE;
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::FakeControlboard::setVelocityMode(int j)
+{
+    CD_DEBUG("(%d)\n", j);
+    modePosVel = VELOCITY_MODE;
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::FakeControlboard::setTorqueMode(int j)
+{
+    CD_DEBUG("(%d)\n", j);
+    return true;
+}
+
+// -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
index af6a8142f..8462641e3 100644
--- a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
+++ b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
@@ -492,41 +492,6 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
 
 //  --------- IControlMode Declarations. Implementation in IControlModeImpl.cpp ---------
 
-    /**
-     * Set position mode, single axis.
-     * @param j joint number
-     * @return true/false success failure.
-     */
-    virtual bool setPositionMode(int j);
-
-    /**
-     * Set velocity mode, single axis.
-     * @param j joint number
-     * @return true/false success failure.
-     */
-    virtual bool setVelocityMode(int j);
-
-    /**
-     * Set torque mode, single axis.
-     * @param j joint number
-     * @return true/false success failure.
-     */
-    virtual bool setTorqueMode(int j);
-
-    /**
-     * Set impedance position mode, single axis.
-     * @param j joint number
-     * @return true/false success failure.
-     */
-    virtual bool setImpedancePositionMode(int j);
-
-    /**
-     * Set impedance velocity mode, single axis.
-     * @param j joint number
-     * @return true/false success failure.
-     */
-    virtual bool setImpedanceVelocityMode(int j);
-
     /**
      * Get the current control mode.
      * @param j joint number
@@ -740,6 +705,10 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
     enum jmc_state { NOT_MOVING, POSITION_MOVE, RELATIVE_MOVE, VELOCITY_MOVE };
     enum jmc_mode { POSITION_MODE, VELOCITY_MODE };
 
+    bool setPositionMode(int j);
+    bool setVelocityMode(int j);
+    bool setTorqueMode(int j);
+
     // General Joint Motion Controller parameters //
     unsigned int axes;
     double jmcMs;
diff --git a/libraries/YarpPlugins/FakeControlboard/IControlMode2Impl.cpp b/libraries/YarpPlugins/FakeControlboard/IControlMode2Impl.cpp
index 2ad8c3e2a..a6ecda605 100644
--- a/libraries/YarpPlugins/FakeControlboard/IControlMode2Impl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/IControlMode2Impl.cpp
@@ -39,10 +39,6 @@ bool roboticslab::FakeControlboard::setControlMode(const int j, const int mode)
         return setVelocityMode(j);
     case VOCAB_CM_TORQUE:
         return setTorqueMode(j);
-    case VOCAB_CM_IMPEDANCE_POS:
-        return setImpedancePositionMode(j);
-    case VOCAB_CM_IMPEDANCE_VEL:
-        return setImpedanceVelocityMode(j);
     default:
         return false;
     }
diff --git a/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp b/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp
index 391c68067..9197f470a 100644
--- a/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/IControlModeImpl.cpp
@@ -4,62 +4,7 @@
 
 #include <ColorDebug.hpp>
 
-// ------------------- IControlLimits Related ------------------------------------
-
-bool roboticslab::FakeControlboard::setPositionMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-
-    if (modePosVel == POSITION_MODE)
-    {
-        return true;  // Simply return true if we were already in pos mode.
-    }
-
-    // Do anything additional before setting flag to pos...
-    if (!stop(j))
-    {
-        CD_ERROR("failed to stop joint %d\n", j);
-        return false;
-    }
-
-    modePosVel = POSITION_MODE;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setVelocityMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    modePosVel = VELOCITY_MODE;
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setTorqueMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setImpedancePositionMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeControlboard::setImpedanceVelocityMode(int j)
-{
-    CD_DEBUG("(%d)\n", j);
-    return true;
-}
-
-// -----------------------------------------------------------------------------
+// ------------------- IControlMode Related ------------------------------------
 
 bool roboticslab::FakeControlboard::getControlMode(int j, int *mode)
 {
diff --git a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
index 020f8741c..7304859cb 100644
--- a/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
+++ b/libraries/YarpPlugins/FakeJoint/FakeJoint.hpp
@@ -74,11 +74,6 @@ class FakeJoint : public yarp::dev::DeviceDriver, public yarp::dev::IControlLimi
     virtual bool getVelLimitsRaw(int axis, double *min, double *max);
 
     //  --------- IControlModeRaw Declarations. Implementation in IControlMode2RawImpl.cpp ---------
-    virtual bool setPositionModeRaw(int j);
-    virtual bool setVelocityModeRaw(int j);
-    virtual bool setTorqueModeRaw(int j);
-    virtual bool setImpedancePositionModeRaw(int j);
-    virtual bool setImpedanceVelocityModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 
diff --git a/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp
index b0447abe1..708662f99 100644
--- a/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/FakeJoint/IControlMode2RawImpl.cpp
@@ -4,66 +4,6 @@
 
 // ############################## IControlModeRaw Related ##############################
 
-bool roboticslab::FakeJoint::setPositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setTorqueModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setImpedancePositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::FakeJoint::setImpedanceVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::FakeJoint::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -105,18 +45,7 @@ bool roboticslab::FakeJoint::setControlModeRaw(const int j, const int mode)
     //-- Check index within range
     if ( j != 0 ) return false;
 
-    if( mode == VOCAB_CM_POSITION )
-        return setPositionModeRaw(j);
-    else if( mode == VOCAB_CM_VELOCITY )
-        return setVelocityModeRaw(j);
-    else if( mode == VOCAB_CM_TORQUE )
-        return setTorqueModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_POS )
-        return setImpedancePositionModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_VEL )
-        return setImpedanceVelocityModeRaw(j);
-
-    return false;
+    return true;
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp
index ce36644af..977caf689 100644
--- a/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/LacqueyFetch/IControlMode2RawImpl.cpp
@@ -4,66 +4,6 @@
 
 // ############################## IControlModeRaw Related ##############################
 
-bool roboticslab::LacqueyFetch::setPositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setTorqueModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setImpedancePositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::LacqueyFetch::setImpedanceVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::LacqueyFetch::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -105,18 +45,7 @@ bool roboticslab::LacqueyFetch::setControlModeRaw(const int j, const int mode)
     //-- Check index within range
     if ( j != 0 ) return false;
 
-    if( mode == VOCAB_CM_POSITION )
-        return setPositionModeRaw(j);
-    else if( mode == VOCAB_CM_VELOCITY )
-        return setVelocityModeRaw(j);
-    else if( mode == VOCAB_CM_TORQUE )
-        return setTorqueModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_POS )
-        return setImpedancePositionModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_VEL )
-        return setImpedanceVelocityModeRaw(j);
-
-    return false;
+    return true;
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
index 300a0a81d..8e6662845 100644
--- a/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
+++ b/libraries/YarpPlugins/LacqueyFetch/LacqueyFetch.hpp
@@ -75,11 +75,6 @@ class LacqueyFetch : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool getVelLimitsRaw(int axis, double *min, double *max);
 
     //  --------- IControlModeRaw Declarations. Implementation in IControlMode2RawImpl.cpp ---------
-    virtual bool setPositionModeRaw(int j);
-    virtual bool setVelocityModeRaw(int j);
-    virtual bool setTorqueModeRaw(int j);
-    virtual bool setImpedancePositionModeRaw(int j);
-    virtual bool setImpedanceVelocityModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp
index 5e386bcef..f6d3e1e62 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/IControlMode2RawImpl.cpp
@@ -114,35 +114,87 @@ bool roboticslab::TechnosoftIpos::setTorqueModeRaw3()
 
     return true;
 }
-/*************************************************************************/
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setImpedancePositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
-
-    return true;
-}
 
+/*************************************************************************/
 // -----------------------------------------------------------------------------
 
-bool roboticslab::TechnosoftIpos::setImpedanceVelocityModeRaw(int j)
+bool roboticslab::TechnosoftIpos::setPositionDirectModeRaw()
 {
-    CD_INFO("(%d)\n",j);
+    CD_INFO("\n");
 
-    //-- Check index within range
-    if ( j != 0 ) return false;
+    //-- ptprepare: pg. 165 (181/263)
+    //*************************************************************
+    //-- 1. - 4. From start to enable.
+    //*************************************************************
+    //-- 5. Disable the RPDO3. Write zero in object 1602 h sub-index 0, this will disable the PDO.
+    //-- Send the following message (SDO access to object 1602 h sub-index 0, 8-bit value 0):
+    uint8_t disableRPDO3[]= {0x2F,0x02,0x16,0x00,0x00,0x00,0x00,0x00};
+    if ( ! send(0x600,8,disableRPDO3) )
+        return false;
+    //*************************************************************
+    //-- 6. Map the new objects.
+    //-- a) Write in object 1602 h sub-index 1 the description of the interpolated data record
+    //-- sub-index 1:
+    //-- Send the following message (SDO access to object 1602 h sub-index 1, 32-bit value 60C10120 h ):
+    uint8_t mapSDOsub1[]= {0x23,0x02,0x16,0x01,0x20,0x01,0xC1,0x60};
+    if ( ! send(0x600,8,mapSDOsub1) )
+        return false;
+    //* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+    //-- b) Write in object 1601 h sub-index 2 the description of the interpolated data record
+    //-- sub-index 2:
+    //-- Send the following message (SDO access to object 1602 h sub-index 2, 32-bit value 60C10220 h ):
+    uint8_t mapSDOsub2[]= {0x23,0x02,0x16,0x02,0x20,0x02,0xC1,0x60};
+    if ( ! send(0x600,8,mapSDOsub2) )
+        return false;
+    //*************************************************************
+    //-- 7. Enable the RPDO3. Set the object 1601 h sub-index 0 with the value 2.
+    //-- Send the following message (SDO access to object 1601 h sub-index 0, 8-bit value 2):
+    uint8_t enableRPDO3[]= {0x2F,0x02,0x16,0x00,0x02,0x00,0x00,0x00};
+    if ( !  send(0x600,8,enableRPDO3) )
+        return false;
+    //*************************************************************
+    //-- 8. Mode of operation. Select interpolation position mode.
+    //-- Send the following message (SDO access to object 6060 h , 8-bit value 7 h ):
+    uint8_t opMode[]= {0x2F,0x60,0x60,0x00,0x07,0x00,0x00,0x00};
+    if ( ! send(0x600,8,opMode) )
+        return false;
+    //*************************************************************
+    //-- 9. Interpolation sub mode select. Select PT interpolation position mode.
+    //-- Send the following message (SDO access to object 60C0 h , 16-bit value 0000 h ):
+    uint8_t subMode[]= {0x2E,0xC0,0x60,0x00,0x00,0x00,0x00,0x00};
+    if ( ! send(0x600,8,subMode) )
+        return false;
+    //*************************************************************
+    //-- 10. Interpolated position buffer length. Set the buffer length to 12. The maximum length is 15.
+    //uint8_t buffLength[]={0x2B,0x74,0x20,0x00,0x00,0x0C,0x00,0x00};  //-- 12
+    uint8_t buffLength[]= {0x2B,0x74,0x20,0x00,0x00,0x0F,0x00,0x00}; //-- 15
+    if ( ! send(0x600,8,buffLength) )
+        return false;
+    //*************************************************************
+    //-- 11. Interpolated position buffer configuration. By setting the value A001 h , the buffer is
+    //-- cleared and the integrity counter will be set to 1. Send the following message (SDO
+    //-- access to object 2074 h , 16-bit value C h ):
+    uint8_t buffConf[]= {0x2B,0x74,0x20,0x00,0x01,0xA0,0x00,0x00};
+    if ( ! send(0x600,8,buffConf) )
+        return false;
+    //*************************************************************
+    //-- 12. Interpolated position initial position. Set the initial position to 0.5 rotations. By using a
+    //-- 500 lines incremental encoder the corresponding value of object 2079 h expressed in
+    //-- encoder counts is (1000 d ) 3E8 h . By using the settings done so far, if the final position
+    //-- command were to be 0, the drive would travel to (Actual position – 1000).
+    //-- Send the following message (SDO access to object 2079 h , 32-bit value 0 h ):
+    //uint8_t initPos[]={0x23,0x79,0x20,0x00,0xE8,0x03,0x00,0x00};  // Put 3E8 h.
+    uint8_t initPos[]= {0x23,0x79,0x20,0x00,0x00,0x00,0x00,0x00}; // Put 0 h instead.
+    if ( ! send(0x600,8,initPos) )
+        return false;
+    //*************************************************************
 
-    CD_WARNING("Not implemented yet (TechnosoftIpos).\n");
+    yarp::os::Time::delay(1);  //-- Seems like a "must".
 
     return true;
 }
 
+/*************************************************************************/
 // -----------------------------------------------------------------------------
 
 bool roboticslab::TechnosoftIpos::getControlModeRaw(int j, int *mode)
@@ -289,10 +341,6 @@ bool roboticslab::TechnosoftIpos::setControlModeRaw(const int j, const int mode)
         return setVelocityModeRaw(j);
     else if( mode == VOCAB_CM_TORQUE )
         return setTorqueModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_POS )
-        return setImpedancePositionModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_VEL )
-        return setImpedanceVelocityModeRaw(j);
     else if( mode == VOCAB_CM_POSITION_DIRECT )
         return setPositionDirectModeRaw();
 
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
index bc40379f1..3c9a89306 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
@@ -54,81 +54,3 @@ bool roboticslab::TechnosoftIpos::send(uint32_t cob, uint16_t len, uint8_t * msg
 }
 
 // -----------------------------------------------------------------------------
-
-bool roboticslab::TechnosoftIpos::setPositionDirectModeRaw()
-{
-    CD_INFO("\n");
-
-    //-- ptprepare: pg. 165 (181/263)
-    //*************************************************************
-    //-- 1. - 4. From start to enable.
-    //*************************************************************
-    //-- 5. Disable the RPDO3. Write zero in object 1602 h sub-index 0, this will disable the PDO.
-    //-- Send the following message (SDO access to object 1602 h sub-index 0, 8-bit value 0):
-    uint8_t disableRPDO3[]= {0x2F,0x02,0x16,0x00,0x00,0x00,0x00,0x00};
-    if ( ! send(0x600,8,disableRPDO3) )
-        return false;
-    //*************************************************************
-    //-- 6. Map the new objects.
-    //-- a) Write in object 1602 h sub-index 1 the description of the interpolated data record
-    //-- sub-index 1:
-    //-- Send the following message (SDO access to object 1602 h sub-index 1, 32-bit value 60C10120 h ):
-    uint8_t mapSDOsub1[]= {0x23,0x02,0x16,0x01,0x20,0x01,0xC1,0x60};
-    if ( ! send(0x600,8,mapSDOsub1) )
-        return false;
-    //* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-    //-- b) Write in object 1601 h sub-index 2 the description of the interpolated data record
-    //-- sub-index 2:
-    //-- Send the following message (SDO access to object 1602 h sub-index 2, 32-bit value 60C10220 h ):
-    uint8_t mapSDOsub2[]= {0x23,0x02,0x16,0x02,0x20,0x02,0xC1,0x60};
-    if ( ! send(0x600,8,mapSDOsub2) )
-        return false;
-    //*************************************************************
-    //-- 7. Enable the RPDO3. Set the object 1601 h sub-index 0 with the value 2.
-    //-- Send the following message (SDO access to object 1601 h sub-index 0, 8-bit value 2):
-    uint8_t enableRPDO3[]= {0x2F,0x02,0x16,0x00,0x02,0x00,0x00,0x00};
-    if ( !  send(0x600,8,enableRPDO3) )
-        return false;
-    //*************************************************************
-    //-- 8. Mode of operation. Select interpolation position mode.
-    //-- Send the following message (SDO access to object 6060 h , 8-bit value 7 h ):
-    uint8_t opMode[]= {0x2F,0x60,0x60,0x00,0x07,0x00,0x00,0x00};
-    if ( ! send(0x600,8,opMode) )
-        return false;
-    //*************************************************************
-    //-- 9. Interpolation sub mode select. Select PT interpolation position mode.
-    //-- Send the following message (SDO access to object 60C0 h , 16-bit value 0000 h ):
-    uint8_t subMode[]= {0x2E,0xC0,0x60,0x00,0x00,0x00,0x00,0x00};
-    if ( ! send(0x600,8,subMode) )
-        return false;
-    //*************************************************************
-    //-- 10. Interpolated position buffer length. Set the buffer length to 12. The maximum length is 15.
-    //uint8_t buffLength[]={0x2B,0x74,0x20,0x00,0x00,0x0C,0x00,0x00};  //-- 12
-    uint8_t buffLength[]= {0x2B,0x74,0x20,0x00,0x00,0x0F,0x00,0x00}; //-- 15
-    if ( ! send(0x600,8,buffLength) )
-        return false;
-    //*************************************************************
-    //-- 11. Interpolated position buffer configuration. By setting the value A001 h , the buffer is
-    //-- cleared and the integrity counter will be set to 1. Send the following message (SDO
-    //-- access to object 2074 h , 16-bit value C h ):
-    uint8_t buffConf[]= {0x2B,0x74,0x20,0x00,0x01,0xA0,0x00,0x00};
-    if ( ! send(0x600,8,buffConf) )
-        return false;
-    //*************************************************************
-    //-- 12. Interpolated position initial position. Set the initial position to 0.5 rotations. By using a
-    //-- 500 lines incremental encoder the corresponding value of object 2079 h expressed in
-    //-- encoder counts is (1000 d ) 3E8 h . By using the settings done so far, if the final position
-    //-- command were to be 0, the drive would travel to (Actual position – 1000).
-    //-- Send the following message (SDO access to object 2079 h , 32-bit value 0 h ):
-    //uint8_t initPos[]={0x23,0x79,0x20,0x00,0xE8,0x03,0x00,0x00};  // Put 3E8 h.
-    uint8_t initPos[]= {0x23,0x79,0x20,0x00,0x00,0x00,0x00,0x00}; // Put 0 h instead.
-    if ( ! send(0x600,8,initPos) )
-        return false;
-    //*************************************************************
-
-    yarp::os::Time::delay(1);  //-- Seems like a "must".
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
index 4720ebb36..5e312ace4 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
@@ -81,16 +81,16 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
     bool setMaxLimitRaw(double max);
 
     //  --------- IControlModeRaw Declarations. Implementation in IControlMode2RawImpl.cpp ---------
-    virtual bool setPositionModeRaw(int j);
-    virtual bool setVelocityModeRaw(int j);
-    virtual bool setTorqueModeRaw(int j);
+    bool setPositionModeRaw(int j);
+    bool setVelocityModeRaw(int j);
+    bool setTorqueModeRaw(int j);
     //-- Auxiliary functions (splitted) of setTorqueModeRaw
     bool setTorqueModeRaw1();
     bool setTorqueModeRaw2();
     bool setTorqueModeRaw3();
+    //-- Old yarp::dev::IPositionDirectRaw implementation
+    bool setPositionDirectModeRaw();
 
-    virtual bool setImpedancePositionModeRaw(int j);
-    virtual bool setImpedanceVelocityModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     //-- Auxiliary functions (splitted) of getControlModeRaw
     bool getControlModeRaw1();
@@ -218,9 +218,6 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver, public yarp::dev::IContro
     std::string msgToStr(can_msg* message);
     std::string msgToStr(uint32_t cob, uint16_t len, uint8_t * msgData);
 
-    /** Old yarp::dev::IPositionDirectRaw implementation. */
-    bool setPositionDirectModeRaw();
-
     int canId;
     ICanBusHico *canDevicePtr;
     double lastUsage;
diff --git a/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp b/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp
index ecad8f043..944005da4 100644
--- a/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp
+++ b/libraries/YarpPlugins/TextilesHand/IControlMode2RawImpl.cpp
@@ -4,66 +4,6 @@
 
 // ############################## IControlModeRaw Related ##############################
 
-bool roboticslab::TextilesHand::setPositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setTorqueModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setImpedancePositionModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool roboticslab::TextilesHand::setImpedanceVelocityModeRaw(int j)
-{
-    CD_INFO("(%d)\n",j);
-
-    //-- Check index within range
-    if ( j != 0 ) return false;
-
-    return true;
-}
-
-// -----------------------------------------------------------------------------
-
 bool roboticslab::TextilesHand::getControlModeRaw(int j, int *mode)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream
@@ -105,18 +45,7 @@ bool roboticslab::TextilesHand::setControlModeRaw(const int j, const int mode)
     //-- Check index within range
     if ( j != 0 ) return false;
 
-    if( mode == VOCAB_CM_POSITION )
-        return setPositionModeRaw(j);
-    else if( mode == VOCAB_CM_VELOCITY )
-        return setVelocityModeRaw(j);
-    else if( mode == VOCAB_CM_TORQUE )
-        return setTorqueModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_POS )
-        return setImpedancePositionModeRaw(j);
-    else if( mode == VOCAB_CM_IMPEDANCE_VEL )
-        return setImpedanceVelocityModeRaw(j);
-
-    return false;
+    return true;
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
index dcbf48741..310df38b5 100644
--- a/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
+++ b/libraries/YarpPlugins/TextilesHand/TextilesHand.hpp
@@ -85,11 +85,6 @@ class TextilesHand : public yarp::dev::DeviceDriver, public yarp::dev::IControlL
     virtual bool getVelLimitsRaw(int axis, double *min, double *max);
 
     //  --------- IControlModeRaw Declarations. Implementation in IControlMode2RawImpl.cpp ---------
-    virtual bool setPositionModeRaw(int j);
-    virtual bool setVelocityModeRaw(int j);
-    virtual bool setTorqueModeRaw(int j);
-    virtual bool setImpedancePositionModeRaw(int j);
-    virtual bool setImpedanceVelocityModeRaw(int j);
     virtual bool getControlModeRaw(int j, int *mode);
     virtual bool getControlModesRaw(int *modes);
 

From 2b32cb7845bc96fa2d17366c4652118cde5c3338 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 23:07:09 +0200
Subject: [PATCH 07/10] Update examples, use newer interfaces

---
 .../exampleRemoteControlboard.cpp             | 20 +++++++++----------
 .../exampleTechnosoftIpos.cpp                 |  4 ++--
 examples/matlab/testRemoteRaveBot.m           |  6 ++++--
 examples/python/exampleRemoteControlboard.py  | 10 +++++-----
 4 files changed, 21 insertions(+), 19 deletions(-)

diff --git a/examples/cpp/exampleRemoteControlboard/exampleRemoteControlboard.cpp b/examples/cpp/exampleRemoteControlboard/exampleRemoteControlboard.cpp
index 21e36f4fa..ad475b733 100644
--- a/examples/cpp/exampleRemoteControlboard/exampleRemoteControlboard.cpp
+++ b/examples/cpp/exampleRemoteControlboard/exampleRemoteControlboard.cpp
@@ -34,6 +34,8 @@ make -j3
 #include <stdio.h>
 #include <stdlib.h>
 
+#include <vector>
+
 #include <yarp/os/all.h>
 #include <yarp/dev/all.h>
 
@@ -61,10 +63,10 @@ int main(int argc, char *argv[]) {
       return 1;
     }
 
-    IPositionControl *pos;
-    IVelocityControl *vel;
-    IEncoders *enc;
-    IControlMode *mode;
+    IPositionControl2 *pos;
+    IVelocityControl2 *vel;
+    IEncodersTimed *enc;
+    IControlMode2 *mode;
 
     bool ok = true;
     ok &= dd.view(pos);
@@ -80,8 +82,8 @@ int main(int argc, char *argv[]) {
     int axes;
     pos->getAxes(&axes);
 
-    for (unsigned int i = 0; i < axes; i++)
-        mode->setPositionMode(i);
+    std::vector<int> posModes(axes, VOCAB_CM_POSITION);
+    mode->setControlModes(posModes.data());
 
     printf("test positionMove(1,35)\n");
     pos->positionMove(1, 35);
@@ -89,8 +91,8 @@ int main(int argc, char *argv[]) {
     printf("Delaying 5 seconds...\n");
     Time::delay(5);
 
-    for (unsigned int i = 0; i < axes; i++)
-        mode->setVelocityMode(i);
+    std::vector<int> velModes(axes, VOCAB_CM_VELOCITY);
+    mode->setControlModes(velModes.data());
 
     printf("test velocityMove(0,10)\n");
     vel->velocityMove(0,10);
@@ -102,5 +104,3 @@ int main(int argc, char *argv[]) {
 
     return 0;
 }
-
-
diff --git a/examples/cpp/exampleTechnosoftIpos/exampleTechnosoftIpos.cpp b/examples/cpp/exampleTechnosoftIpos/exampleTechnosoftIpos.cpp
index 586235d7a..2bff1b2ec 100644
--- a/examples/cpp/exampleTechnosoftIpos/exampleTechnosoftIpos.cpp
+++ b/examples/cpp/exampleTechnosoftIpos/exampleTechnosoftIpos.cpp
@@ -122,7 +122,7 @@ int main(int argc, char *argv[])
     }
     else std::printf("[success] Viewing IVelocityControlRaw.\n");
 
-    yarp::dev::IControlModeRaw *mode;
+    yarp::dev::IControlMode2Raw *mode;
     ok = dd.view(mode);
     if (!ok)
     {
@@ -146,7 +146,7 @@ int main(int argc, char *argv[])
     iCanBusSharer->enable();
 
     //-- Commands on TechnosoftIpos.
-    ok = mode->setPositionModeRaw(0);
+    ok = mode->setControlModeRaw(0, VOCAB_CM_POSITION);
     if (!ok)
     {
         std::printf("[error] Problems in setPositionModeRaw.\n");
diff --git a/examples/matlab/testRemoteRaveBot.m b/examples/matlab/testRemoteRaveBot.m
index 2710e8585..7e1960fee 100644
--- a/examples/matlab/testRemoteRaveBot.m
+++ b/examples/matlab/testRemoteRaveBot.m
@@ -5,6 +5,8 @@
 %>
 %> @brief This example connects to a running \ref testRaveBot or \ref cartesianServer module to move in Joint space.
 %>
+%> <b>Requires YARP 2.3.72.</b>
+%>
 %> <b>Legal</b> 
 %>
 %> Copyright: (C) 2012 Universidad Carlos III de Madrid
@@ -69,7 +71,7 @@
 
 axes = enc.getAxes();
 for i = 1:axes
-    mode.setPositionMode(i-1);  % use the object to set the device to position mode (as opposed to velocity mode) (note: stops the robot)
+    mode.setControlMode(i-1, yarp.Vocab_encode('pos'));  % use the object to set the device to position mode (as opposed to velocity mode) (note: stops the robot)
 end
 
 disp 'test positionMove(1,35) -> moves motor 1 (start count at motor 0) to 35 degrees';
@@ -83,7 +85,7 @@
 disp (strcat('v[1] is: ',num2str(v.get(1))));  % print element 1 of 'v', note that motors and encoders start at 0
 
 for i = 1:axes
-    mode.setVelocityMode(i-1);  % use the object to set the device to velocity mode (as opposed to position mode)
+    mode.setControlMode(i-1, yarp.Vocab_encode('vel'));  % use the object to set the device to velocity mode (as opposed to position mode)
 end
 
 disp 'test velocityMove(0,10) -> moves motor 0 (start count at motor 0) at 10 degrees per second';
diff --git a/examples/python/exampleRemoteControlboard.py b/examples/python/exampleRemoteControlboard.py
index 514d137bf..5cc6324ca 100644
--- a/examples/python/exampleRemoteControlboard.py
+++ b/examples/python/exampleRemoteControlboard.py
@@ -6,6 +6,8 @@
 #
 # This example connects to a running \ref controlboardWrapper to move in Joint space.
 #
+# <b>Requires YARP 2.3.72.</b>
+#
 # <b>Legal</b> 
 #
 # Copyright: (C) 2017 Universidad Carlos III de Madrid
@@ -47,14 +49,13 @@
 pos = dd.viewIPositionControl()  # make a position controller object we call 'pos'
 vel = dd.viewIVelocityControl()  # make a velocity controller object we call 'vel'
 enc = dd.viewIEncoders()  # make an encoder controller object we call 'enc'
-mode = dd.viewIControlMode()  # make a operation mode controller object we call 'mode'
+mode = dd.viewIControlMode2()  # make a operation mode controller object we call 'mode'
 ll = dd.viewIControlLimits()  # make a limits controller object we call 'll'
 
 axes = enc.getAxes()  # retrieve number of joints
 
 # use the object to set the device to position mode (as opposed to velocity mode)(note: stops the robot)
-for j in range(axes):
-    mode.setPositionMode(j)
+mode.setControlModes(yarp.IVector(axes, yarp.Vocab_encode('pos')))
 
 print 'test positionMove(1,-35) -> moves motor 1 (start count at motor 0) to -35 degrees'
 pos.positionMove(1,-35)
@@ -71,8 +72,7 @@
 ll.getLimits(0,min,max)
 
 # use the object to set the device to velocity mode (as opposed to position mode)
-for j in range(axes):
-    mode.setVelocityMode(j)
+mode.setControlModes(yarp.IVector(axes, yarp.Vocab_encode('vel')))
 
 print 'test velocityMove(0,10) -> moves motor 0 (start count at motor 0) at 10 degrees per second'
 vel.velocityMove(0,10)

From de69db2497f875c7ec24559573704ae4110dce9a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 23:13:00 +0200
Subject: [PATCH 08/10] Convert CRLF endings to LF

---
 examples/matlab/testRemoteRaveBot.m |  202 ++---
 examples/simulink/sfun_time.c       |  202 ++---
 examples/simulink/utility.mdl       | 1254 +++++++++++++--------------
 3 files changed, 829 insertions(+), 829 deletions(-)

diff --git a/examples/matlab/testRemoteRaveBot.m b/examples/matlab/testRemoteRaveBot.m
index 7e1960fee..b97e82ed4 100644
--- a/examples/matlab/testRemoteRaveBot.m
+++ b/examples/matlab/testRemoteRaveBot.m
@@ -1,101 +1,101 @@
-% MATLAB example
-
-%> @ingroup asibot_examples_m
-%> \defgroup testRemoteRaveBotM testRemoteRaveBot.m
-%>
-%> @brief This example connects to a running \ref testRaveBot or \ref cartesianServer module to move in Joint space.
-%>
-%> <b>Requires YARP 2.3.72.</b>
-%>
-%> <b>Legal</b> 
-%>
-%> Copyright: (C) 2012 Universidad Carlos III de Madrid
-%>
-%> Author: Juan G Victores
-%>
-%> CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT
-%>
-%> <b>Running</b> (assuming correct installation)
-%>
-%> From within MATLAB, navigate to the ASIBOT MATLAB examples path and run the program:
-%>
-%>\verbatim [MATLAB console] testRemoteRaveBot.m \endverbatim
-%> <b>Modify</b>
-%>
-%> This file can be edited at repos/asibot-main/example/matlab/testRemoteRaveBot.m
-%>
-
-disp 'WARNING: requires a running instance of RaveBot (i.e. testRaveBot or cartesianServer)'
-
-LoadYarp();  % imports YARP and connects to YARP network
-
-options = yarp.Property();  % create an instance of Property, a nice YARP class for storing name-value (key-value) pairs
-options.put('device','remote_controlboard');  % we add a name-value pair that indicates the YARP device
-options.put('remote','/ravebot');  % we add info on to whom we will connect
-options.put('local','/matlab');  % we add info on how we will call ourselves on the YARP network
-
-dd = yarp.PolyDriver(options);  % create a YARP multi-use driver with the given options
-if isequal(dd.isValid(),1)
-    disp '[success] robot available';
-else
-    disp '[warning] robot NOT available, does it exist?';
-end
-
-pos = dd.viewIPositionControl();  % make a position controller object we call 'pos'
-if isequal(pos,[])
-    disp '[warning] position interface NOT available, does it exist?';
-else
-    disp '[success] position interface available';
-end
-
-vel = dd.viewIVelocityControl();  % make a velocity controller object we call 'vel'
-if isequal(vel,[])
-    disp '[warning] velocity interface NOT available, does it exist?';
-else
-    disp '[success] velocity interface available';
-end
-
-enc = dd.viewIEncoders();  % make an encoder controller object we call 'enc'
-if isequal(enc,[])
-    disp '[warning] encoder interface NOT available, does it exist?';
-else
-    disp '[success] encoder interface available';
-end
-
-mode = dd.viewIControlMode();  % make a mode controller object we call 'mode'
-if isequal(mode,[])
-    disp '[warning] control mode interface NOT available, does it exist?';
-else
-    disp '[success] control mode interface available';
-end
-
-axes = enc.getAxes();
-for i = 1:axes
-    mode.setControlMode(i-1, yarp.Vocab_encode('pos'));  % use the object to set the device to position mode (as opposed to velocity mode) (note: stops the robot)
-end
-
-disp 'test positionMove(1,35) -> moves motor 1 (start count at motor 0) to 35 degrees';
-pos.positionMove(1,35);
-
-disp 'test delay(5)';
-yarp.Time.delay(5);
-
-v = yarp.DVector(axes);  % create a YARP vector of doubles the size of the number of elements read by enc, call it 'v'
-enc.getEncoders(v);  % read the encoder values and put them into 'v'
-disp (strcat('v[1] is: ',num2str(v.get(1))));  % print element 1 of 'v', note that motors and encoders start at 0
-
-for i = 1:axes
-    mode.setControlMode(i-1, yarp.Vocab_encode('vel'));  % use the object to set the device to velocity mode (as opposed to position mode)
-end
-
-disp 'test velocityMove(0,10) -> moves motor 0 (start count at motor 0) at 10 degrees per second';
-vel.velocityMove(0,10);
-
-disp 'test delay(5)';
-yarp.Time.delay(5);
-
-vel.velocityMove(0,0);
-dd.close();
-disp 'bye!';
-yarp.Network.fini();  % disconnect from the YARP network
-
+% MATLAB example
+
+%> @ingroup asibot_examples_m
+%> \defgroup testRemoteRaveBotM testRemoteRaveBot.m
+%>
+%> @brief This example connects to a running \ref testRaveBot or \ref cartesianServer module to move in Joint space.
+%>
+%> <b>Requires YARP 2.3.72.</b>
+%>
+%> <b>Legal</b> 
+%>
+%> Copyright: (C) 2012 Universidad Carlos III de Madrid
+%>
+%> Author: Juan G Victores
+%>
+%> CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT
+%>
+%> <b>Running</b> (assuming correct installation)
+%>
+%> From within MATLAB, navigate to the ASIBOT MATLAB examples path and run the program:
+%>
+%>\verbatim [MATLAB console] testRemoteRaveBot.m \endverbatim
+%> <b>Modify</b>
+%>
+%> This file can be edited at repos/asibot-main/example/matlab/testRemoteRaveBot.m
+%>
+
+disp 'WARNING: requires a running instance of RaveBot (i.e. testRaveBot or cartesianServer)'
+
+LoadYarp();  % imports YARP and connects to YARP network
+
+options = yarp.Property();  % create an instance of Property, a nice YARP class for storing name-value (key-value) pairs
+options.put('device','remote_controlboard');  % we add a name-value pair that indicates the YARP device
+options.put('remote','/ravebot');  % we add info on to whom we will connect
+options.put('local','/matlab');  % we add info on how we will call ourselves on the YARP network
+
+dd = yarp.PolyDriver(options);  % create a YARP multi-use driver with the given options
+if isequal(dd.isValid(),1)
+    disp '[success] robot available';
+else
+    disp '[warning] robot NOT available, does it exist?';
+end
+
+pos = dd.viewIPositionControl();  % make a position controller object we call 'pos'
+if isequal(pos,[])
+    disp '[warning] position interface NOT available, does it exist?';
+else
+    disp '[success] position interface available';
+end
+
+vel = dd.viewIVelocityControl();  % make a velocity controller object we call 'vel'
+if isequal(vel,[])
+    disp '[warning] velocity interface NOT available, does it exist?';
+else
+    disp '[success] velocity interface available';
+end
+
+enc = dd.viewIEncoders();  % make an encoder controller object we call 'enc'
+if isequal(enc,[])
+    disp '[warning] encoder interface NOT available, does it exist?';
+else
+    disp '[success] encoder interface available';
+end
+
+mode = dd.viewIControlMode();  % make a mode controller object we call 'mode'
+if isequal(mode,[])
+    disp '[warning] control mode interface NOT available, does it exist?';
+else
+    disp '[success] control mode interface available';
+end
+
+axes = enc.getAxes();
+for i = 1:axes
+    mode.setControlMode(i-1, yarp.Vocab_encode('pos'));  % use the object to set the device to position mode (as opposed to velocity mode) (note: stops the robot)
+end
+
+disp 'test positionMove(1,35) -> moves motor 1 (start count at motor 0) to 35 degrees';
+pos.positionMove(1,35);
+
+disp 'test delay(5)';
+yarp.Time.delay(5);
+
+v = yarp.DVector(axes);  % create a YARP vector of doubles the size of the number of elements read by enc, call it 'v'
+enc.getEncoders(v);  % read the encoder values and put them into 'v'
+disp (strcat('v[1] is: ',num2str(v.get(1))));  % print element 1 of 'v', note that motors and encoders start at 0
+
+for i = 1:axes
+    mode.setControlMode(i-1, yarp.Vocab_encode('vel'));  % use the object to set the device to velocity mode (as opposed to position mode)
+end
+
+disp 'test velocityMove(0,10) -> moves motor 0 (start count at motor 0) at 10 degrees per second';
+vel.velocityMove(0,10);
+
+disp 'test delay(5)';
+yarp.Time.delay(5);
+
+vel.velocityMove(0,0);
+dd.close();
+disp 'bye!';
+yarp.Network.fini();  % disconnect from the YARP network
+
diff --git a/examples/simulink/sfun_time.c b/examples/simulink/sfun_time.c
index cd8299d6e..09f87e566 100644
--- a/examples/simulink/sfun_time.c
+++ b/examples/simulink/sfun_time.c
@@ -1,101 +1,101 @@
-#define S_FUNCTION_LEVEL 2
-#define S_FUNCTION_NAME  sfun_time
-
-#define TIME_SCALE_FACTOR(S) ssGetSFcnParam(S,0)
-
-/*
- * Need to include simstruc.h for the definition of the SimStruct and
- * its associated macro definitions.
- */
-#include "simstruc.h"
-
-/*
- *  Include the standard ANSI C header for handling time functions:
- *  ---------------------------------------------------------------
- */
-#include <time.h>
-
-static void mdlInitializeSizes(SimStruct *S)
-{
-
-   ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
-
-   if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
-
-    ssSetNumContStates(S, 0);
-    ssSetNumDiscStates(S, 0);
-
-   if (!ssSetNumInputPorts(S, 0)) return;
-
-   if (!ssSetNumOutputPorts(S, 0)) return;
-   
-   ssSetNumSampleTimes(S, 1);
-   ssSetNumRWork(S, 1);
-   ssSetNumIWork(S, 0);
-   ssSetNumPWork(S, 0);
-   ssSetNumModes(S, 0);
-   ssSetNumNonsampledZCs(S, 0);
-   ssSetOptions(S, 0);
-}
-
-#define MDL_INITIALIZE_SAMPLE_TIMES
-static void mdlInitializeSampleTimes(SimStruct *S)
-{
-   ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
-   ssSetOffsetTime(S, 0, 0.0);
-}
-
-#define MDL_START
-static void mdlStart(SimStruct *S)
-{
-    ssSetRWorkValue(S,0,ssGetTStart(S));
-}
-
-static void mdlOutputs(SimStruct *S, int_T tid)
-{
-   real_T             t_previousSimTime = ssGetRWorkValue(S,0);
-   const real_T      *scaleFactor = mxGetPr(TIME_SCALE_FACTOR(S));
-   time_T             t_SimTime = ssGetT(S);
-   real_T             t_diff = 0.0;
-   real_T             dt;
-   real_T             t_current;
-   real_T             t_0;
-   real_T             t_previous;
-   real_T             t_elapsed;
-   
-   /* Desired Delta time */
-   dt = (t_SimTime - t_previousSimTime) * (scaleFactor[0]);
-   /* Get clock time at the beginning of this step*/   
-   t_previous = (real_T)clock()/CLOCKS_PER_SEC;
-   t_0 = (real_T)clock()/CLOCKS_PER_SEC;
-
-   /* Wait to reach the desired time */
-   while (t_diff<dt){
-        t_current = (real_T) clock()/CLOCKS_PER_SEC;
-        /* Look for wrapup */
-        if (t_current<t_previous){
-            t_elapsed = t_previous - t_0;
-            t_0 = (real_T) clock()/CLOCKS_PER_SEC - t_elapsed;
-        }
-        t_diff = t_current - t_0;
-        t_previous = t_current;
-   }
-   
-   /* Store current time to be used in next time step*/
-   ssSetRWorkValue(S, 0, t_SimTime);
-}
-
-static void mdlTerminate(SimStruct *S)
-{
-    UNUSED_ARG(S); /* unused input argument */
-}
-
-/*
- *  Required S-function trailer:
- *  ----------------------------
- */
-#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
-#include "simulink.c"      /* MEX-file interface mechanism */
-#else
-#include "cg_sfun.h"       /* Code generation registration function */
-#endif
+#define S_FUNCTION_LEVEL 2
+#define S_FUNCTION_NAME  sfun_time
+
+#define TIME_SCALE_FACTOR(S) ssGetSFcnParam(S,0)
+
+/*
+ * Need to include simstruc.h for the definition of the SimStruct and
+ * its associated macro definitions.
+ */
+#include "simstruc.h"
+
+/*
+ *  Include the standard ANSI C header for handling time functions:
+ *  ---------------------------------------------------------------
+ */
+#include <time.h>
+
+static void mdlInitializeSizes(SimStruct *S)
+{
+
+   ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
+
+   if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
+
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);
+
+   if (!ssSetNumInputPorts(S, 0)) return;
+
+   if (!ssSetNumOutputPorts(S, 0)) return;
+   
+   ssSetNumSampleTimes(S, 1);
+   ssSetNumRWork(S, 1);
+   ssSetNumIWork(S, 0);
+   ssSetNumPWork(S, 0);
+   ssSetNumModes(S, 0);
+   ssSetNumNonsampledZCs(S, 0);
+   ssSetOptions(S, 0);
+}
+
+#define MDL_INITIALIZE_SAMPLE_TIMES
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+   ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
+   ssSetOffsetTime(S, 0, 0.0);
+}
+
+#define MDL_START
+static void mdlStart(SimStruct *S)
+{
+    ssSetRWorkValue(S,0,ssGetTStart(S));
+}
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+   real_T             t_previousSimTime = ssGetRWorkValue(S,0);
+   const real_T      *scaleFactor = mxGetPr(TIME_SCALE_FACTOR(S));
+   time_T             t_SimTime = ssGetT(S);
+   real_T             t_diff = 0.0;
+   real_T             dt;
+   real_T             t_current;
+   real_T             t_0;
+   real_T             t_previous;
+   real_T             t_elapsed;
+   
+   /* Desired Delta time */
+   dt = (t_SimTime - t_previousSimTime) * (scaleFactor[0]);
+   /* Get clock time at the beginning of this step*/   
+   t_previous = (real_T)clock()/CLOCKS_PER_SEC;
+   t_0 = (real_T)clock()/CLOCKS_PER_SEC;
+
+   /* Wait to reach the desired time */
+   while (t_diff<dt){
+        t_current = (real_T) clock()/CLOCKS_PER_SEC;
+        /* Look for wrapup */
+        if (t_current<t_previous){
+            t_elapsed = t_previous - t_0;
+            t_0 = (real_T) clock()/CLOCKS_PER_SEC - t_elapsed;
+        }
+        t_diff = t_current - t_0;
+        t_previous = t_current;
+   }
+   
+   /* Store current time to be used in next time step*/
+   ssSetRWorkValue(S, 0, t_SimTime);
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+    UNUSED_ARG(S); /* unused input argument */
+}
+
+/*
+ *  Required S-function trailer:
+ *  ----------------------------
+ */
+#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
+#include "simulink.c"      /* MEX-file interface mechanism */
+#else
+#include "cg_sfun.h"       /* Code generation registration function */
+#endif
diff --git a/examples/simulink/utility.mdl b/examples/simulink/utility.mdl
index 54d4b4d32..431eedc74 100644
--- a/examples/simulink/utility.mdl
+++ b/examples/simulink/utility.mdl
@@ -1,627 +1,627 @@
-Library {
-  Name			  "utility"
-  Version		  7.5
-  MdlSubVersion		  0
-  SavedCharacterEncoding  "windows-1252"
-  LibraryType		  "BlockLibrary"
-  SaveDefaultBlockParams  on
-  ScopeRefreshTime	  0.035000
-  OverrideScopeRefreshTime on
-  DisableAllScopes	  off
-  MaxMDLFileLineLength	  120
-  Created		  "Wed Oct 08 17:37:30 2008"
-  Creator		  "grouleau"
-  UpdateHistory		  "UpdateHistoryNever"
-  ModifiedByFormat	  "%<Auto>"
-  LastModifiedBy	  "grouleau"
-  ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Wed Jul 21 11:42:45 2010"
-  RTWModifiedTimeStamp	  201613320
-  ModelVersionFormat	  "1.%<AutoIncrement:7>"
-  ConfigurationManager	  "None"
-  SampleTimeColors	  off
-  SampleTimeAnnotations	  off
-  LibraryLinkDisplay	  "none"
-  WideLines		  off
-  ShowLineDimensions	  off
-  ShowPortDataTypes	  off
-  ShowLoopsOnError	  on
-  IgnoreBidirectionalLines off
-  ShowStorageClass	  off
-  ShowTestPointIcons	  on
-  ShowSignalResolutionIcons on
-  ShowViewerIcons	  on
-  SortedOrder		  off
-  ExecutionContextIcon	  off
-  ShowLinearizationAnnotations on
-  BlockNameDataTip	  off
-  BlockParametersDataTip  off
-  BlockDescriptionStringDataTip	off
-  ToolBar		  on
-  StatusBar		  on
-  BrowserShowLibraryLinks off
-  BrowserLookUnderMasks	  off
-  SimulationMode	  "normal"
-  LinearizationMsg	  "none"
-  Profile		  off
-  ParamWorkspaceSource	  "MATLABWorkspace"
-  RecordCoverage	  off
-  CovSaveName		  "covdata"
-  CovMetricSettings	  "dw"
-  CovNameIncrementing	  off
-  CovHtmlReporting	  on
-  CovForceBlockReductionOff on
-  covSaveCumulativeToWorkspaceVar on
-  CovSaveSingleToWorkspaceVar on
-  CovCumulativeReport	  off
-  CovReportOnPause	  on
-  CovModelRefEnable	  "Off"
-  CovExternalEMLEnable	  off
-  ExtModeBatchMode	  off
-  ExtModeEnableFloating	  on
-  ExtModeTrigType	  "manual"
-  ExtModeTrigMode	  "normal"
-  ExtModeTrigPort	  "1"
-  ExtModeTrigElement	  "any"
-  ExtModeTrigDuration	  1000
-  ExtModeTrigDurationFloating "auto"
-  ExtModeTrigHoldOff	  0
-  ExtModeTrigDelay	  0
-  ExtModeTrigDirection	  "rising"
-  ExtModeTrigLevel	  0
-  ExtModeArchiveMode	  "off"
-  ExtModeAutoIncOneShot	  off
-  ExtModeIncDirWhenArm	  off
-  ExtModeAddSuffixToVar	  off
-  ExtModeWriteAllDataToWs off
-  ExtModeArmWhenConnect	  on
-  ExtModeSkipDownloadWhenConnect off
-  ExtModeLogAll		  on
-  ExtModeAutoUpdateStatusClock on
-  ShowModelReferenceBlockVersion off
-  ShowModelReferenceBlockIO off
-  Array {
-    Type		    "Handle"
-    Dimension		    1
-    Simulink.ConfigSet {
-      $ObjectID		      1
-      Version		      "1.10.0"
-      Array {
-	Type			"Handle"
-	Dimension		9
-	Simulink.SolverCC {
-	  $ObjectID		  2
-	  Version		  "1.10.0"
-	  StartTime		  "0.0"
-	  StopTime		  "10.0"
-	  AbsTol		  "auto"
-	  FixedStep		  "auto"
-	  InitialStep		  "auto"
-	  MaxNumMinSteps	  "-1"
-	  MaxOrder		  5
-	  ZcThreshold		  "auto"
-	  ConsecutiveZCsStepRelTol "10*128*eps"
-	  MaxConsecutiveZCs	  "1000"
-	  ExtrapolationOrder	  4
-	  NumberNewtonIterations  1
-	  MaxStep		  "auto"
-	  MinStep		  "auto"
-	  MaxConsecutiveMinStep	  "1"
-	  RelTol		  "1e-3"
-	  SolverMode		  "Auto"
-	  Solver		  "ode45"
-	  SolverName		  "ode45"
-	  SolverJacobianMethodControl "auto"
-	  ShapePreserveControl	  "DisableAll"
-	  ZeroCrossControl	  "UseLocalSettings"
-	  ZeroCrossAlgorithm	  "Nonadaptive"
-	  AlgebraicLoopSolver	  "TrustRegion"
-	  SolverResetMethod	  "Fast"
-	  PositivePriorityOrder	  off
-	  AutoInsertRateTranBlk	  off
-	  SampleTimeConstraint	  "Unconstrained"
-	  InsertRTBMode		  "Whenever possible"
-	}
-	Simulink.DataIOCC {
-	  $ObjectID		  3
-	  Version		  "1.10.0"
-	  Decimation		  "1"
-	  ExternalInput		  "[t, u]"
-	  FinalStateName	  "xFinal"
-	  InitialState		  "xInitial"
-	  LimitDataPoints	  on
-	  MaxDataPoints		  "1000"
-	  LoadExternalInput	  off
-	  LoadInitialState	  off
-	  SaveFinalState	  off
-	  SaveCompleteFinalSimState off
-	  SaveFormat		  "Array"
-	  SaveOutput		  on
-	  SaveState		  off
-	  SignalLogging		  on
-	  DSMLogging		  on
-	  InspectSignalLogs	  off
-	  SaveTime		  on
-	  ReturnWorkspaceOutputs  off
-	  StateSaveName		  "xout"
-	  TimeSaveName		  "tout"
-	  OutputSaveName	  "yout"
-	  SignalLoggingName	  "logsout"
-	  DSMLoggingName	  "dsmout"
-	  OutputOption		  "RefineOutputTimes"
-	  OutputTimes		  "[]"
-	  ReturnWorkspaceOutputsName "out"
-	  Refine		  "1"
-	}
-	Simulink.OptimizationCC {
-	  $ObjectID		  4
-	  Version		  "1.10.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    4
-	    Cell		    "ZeroExternalMemoryAtStartup"
-	    Cell		    "ZeroInternalMemoryAtStartup"
-	    Cell		    "OptimizeModelRefInitCode"
-	    Cell		    "NoFixptDivByZeroProtection"
-	    PropName		    "DisabledProps"
-	  }
-	  BlockReduction	  on
-	  BooleanDataType	  on
-	  ConditionallyExecuteInputs on
-	  InlineParams		  off
-	  UseIntDivNetSlope	  off
-	  InlineInvariantSignals  off
-	  OptimizeBlockIOStorage  on
-	  BufferReuse		  on
-	  EnhancedBackFolding	  off
-	  StrengthReduction	  off
-	  EnforceIntegerDowncast  on
-	  ExpressionFolding	  on
-	  BooleansAsBitfields	  off
-	  BitfieldContainerType	  "uint_T"
-	  EnableMemcpy		  on
-	  MemcpyThreshold	  64
-	  PassReuseOutputArgsAs	  "Structure reference"
-	  ExpressionDepthLimit	  2147483647
-	  FoldNonRolledExpr	  on
-	  LocalBlockOutputs	  on
-	  RollThreshold		  5
-	  SystemCodeInlineAuto	  off
-	  StateBitsets		  off
-	  DataBitsets		  off
-	  UseTempVars		  off
-	  ZeroExternalMemoryAtStartup on
-	  ZeroInternalMemoryAtStartup on
-	  InitFltsAndDblsToZero	  off
-	  NoFixptDivByZeroProtection off
-	  EfficientFloat2IntCast  off
-	  EfficientMapNaN2IntZero on
-	  OptimizeModelRefInitCode off
-	  LifeSpan		  "inf"
-	  MaxStackSize		  "Inherit from target"
-	  BufferReusableBoundary  on
-	  SimCompilerOptimization "Off"
-	  AccelVerboseBuild	  off
-	}
-	Simulink.DebuggingCC {
-	  $ObjectID		  5
-	  Version		  "1.10.0"
-	  RTPrefix		  "error"
-	  ConsistencyChecking	  "none"
-	  ArrayBoundsChecking	  "none"
-	  SignalInfNanChecking	  "none"
-	  SignalRangeChecking	  "none"
-	  ReadBeforeWriteMsg	  "UseLocalSettings"
-	  WriteAfterWriteMsg	  "UseLocalSettings"
-	  WriteAfterReadMsg	  "UseLocalSettings"
-	  AlgebraicLoopMsg	  "warning"
-	  ArtificialAlgebraicLoopMsg "warning"
-	  SaveWithDisabledLinksMsg "warning"
-	  SaveWithParameterizedLinksMsg	"warning"
-	  CheckSSInitialOutputMsg on
-	  UnderspecifiedInitializationDetection	"Classic"
-	  MergeDetectMultiDrivingBlocksExec "none"
-	  CheckExecutionContextPreStartOutputMsg off
-	  CheckExecutionContextRuntimeOutputMsg	off
-	  SignalResolutionControl "UseLocalSettings"
-	  BlockPriorityViolationMsg "warning"
-	  MinStepSizeMsg	  "warning"
-	  TimeAdjustmentMsg	  "none"
-	  MaxConsecutiveZCsMsg	  "error"
-	  SolverPrmCheckMsg	  "warning"
-	  InheritedTsInSrcMsg	  "warning"
-	  DiscreteInheritContinuousMsg "warning"
-	  MultiTaskDSMMsg	  "error"
-	  MultiTaskCondExecSysMsg "error"
-	  MultiTaskRateTransMsg	  "error"
-	  SingleTaskRateTransMsg  "none"
-	  TasksWithSamePriorityMsg "warning"
-	  SigSpecEnsureSampleTimeMsg "warning"
-	  CheckMatrixSingularityMsg "none"
-	  IntegerOverflowMsg	  "warning"
-	  Int32ToFloatConvMsg	  "warning"
-	  ParameterDowncastMsg	  "error"
-	  ParameterOverflowMsg	  "error"
-	  ParameterUnderflowMsg	  "none"
-	  ParameterPrecisionLossMsg "warning"
-	  ParameterTunabilityLossMsg "warning"
-	  FixptConstUnderflowMsg  "none"
-	  FixptConstOverflowMsg	  "none"
-	  FixptConstPrecisionLossMsg "none"
-	  UnderSpecifiedDataTypeMsg "none"
-	  UnnecessaryDatatypeConvMsg "none"
-	  VectorMatrixConversionMsg "none"
-	  InvalidFcnCallConnMsg	  "error"
-	  FcnCallInpInsideContextMsg "Use local settings"
-	  SignalLabelMismatchMsg  "none"
-	  UnconnectedInputMsg	  "warning"
-	  UnconnectedOutputMsg	  "warning"
-	  UnconnectedLineMsg	  "warning"
-	  SFcnCompatibilityMsg	  "none"
-	  UniqueDataStoreMsg	  "none"
-	  BusObjectLabelMismatch  "warning"
-	  RootOutportRequireBusObject "warning"
-	  AssertControl		  "UseLocalSettings"
-	  EnableOverflowDetection off
-	  ModelReferenceIOMsg	  "none"
-	  ModelReferenceVersionMismatchMessage "none"
-	  ModelReferenceIOMismatchMessage "none"
-	  ModelReferenceCSMismatchMessage "none"
-	  UnknownTsInhSupMsg	  "warning"
-	  ModelReferenceDataLoggingMessage "warning"
-	  ModelReferenceSymbolNameMessage "warning"
-	  ModelReferenceExtraNoncontSigs "error"
-	  StateNameClashWarn	  "warning"
-	  SimStateInterfaceChecksumMismatchMsg "warning"
-	  StrictBusMsg		  "Warning"
-	  BusNameAdapt		  "WarnAndRepair"
-	  NonBusSignalsTreatedAsBus "none"
-	  LoggingUnavailableSignals "error"
-	  BlockIODiagnostic	  "none"
-	}
-	Simulink.HardwareCC {
-	  $ObjectID		  6
-	  Version		  "1.10.0"
-	  ProdBitPerChar	  8
-	  ProdBitPerShort	  16
-	  ProdBitPerInt		  32
-	  ProdBitPerLong	  32
-	  ProdIntDivRoundTo	  "Undefined"
-	  ProdEndianess		  "Unspecified"
-	  ProdWordSize		  32
-	  ProdShiftRightIntArith  on
-	  ProdHWDeviceType	  "32-bit Generic"
-	  TargetBitPerChar	  8
-	  TargetBitPerShort	  16
-	  TargetBitPerInt	  32
-	  TargetBitPerLong	  32
-	  TargetShiftRightIntArith on
-	  TargetIntDivRoundTo	  "Undefined"
-	  TargetEndianess	  "Unspecified"
-	  TargetWordSize	  32
-	  TargetTypeEmulationWarnSuppressLevel 0
-	  TargetPreprocMaxBitsSint 32
-	  TargetPreprocMaxBitsUint 32
-	  TargetHWDeviceType	  "Specified"
-	  TargetUnknown		  off
-	  ProdEqTarget		  on
-	}
-	Simulink.ModelReferenceCC {
-	  $ObjectID		  7
-	  Version		  "1.10.0"
-	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
-	  CheckModelReferenceTargetMessage "error"
-	  EnableParallelModelReferenceBuilds off
-	  ParallelModelReferenceMATLABWorkerInit "None"
-	  ModelReferenceNumInstancesAllowed "Multi"
-	  PropagateVarSize	  "Infer from blocks in model"
-	  ModelReferencePassRootInputsByReference on
-	  ModelReferenceMinAlgLoopOccurrences off
-	  PropagateSignalLabelsOutOfModel off
-	  SupportModelReferenceSimTargetCustomCode off
-	}
-	Simulink.SFSimCC {
-	  $ObjectID		  8
-	  Version		  "1.10.0"
-	  SFSimEnableDebug	  on
-	  SFSimOverflowDetection  on
-	  SFSimEcho		  on
-	  SimBlas		  on
-	  SimCtrlC		  on
-	  SimExtrinsic		  on
-	  SimIntegrity		  on
-	  SimUseLocalCustomCode	  off
-	  SimBuildMode		  "sf_incremental_build"
-	}
-	Simulink.RTWCC {
-	  $BackupClass		  "Simulink.RTWCC"
-	  $ObjectID		  9
-	  Version		  "1.10.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    6
-	    Cell		    "IncludeHyperlinkInReport"
-	    Cell		    "GenerateTraceInfo"
-	    Cell		    "GenerateTraceReport"
-	    Cell		    "GenerateTraceReportSl"
-	    Cell		    "GenerateTraceReportSf"
-	    Cell		    "GenerateTraceReportEml"
-	    PropName		    "DisabledProps"
-	  }
-	  SystemTargetFile	  "grt.tlc"
-	  GenCodeOnly		  off
-	  MakeCommand		  "make_rtw"
-	  GenerateMakefile	  on
-	  TemplateMakefile	  "grt_default_tmf"
-	  GenerateReport	  off
-	  SaveLog		  off
-	  RTWVerbose		  on
-	  RetainRTWFile		  off
-	  ProfileTLC		  off
-	  TLCDebug		  off
-	  TLCCoverage		  off
-	  TLCAssert		  off
-	  ProcessScriptMode	  "Default"
-	  ConfigurationMode	  "Optimized"
-	  ConfigAtBuild		  off
-	  RTWUseLocalCustomCode	  off
-	  RTWUseSimCustomCode	  off
-	  IncludeHyperlinkInReport off
-	  LaunchReport		  off
-	  TargetLang		  "C"
-	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
-	  IncludeERTFirstTime	  off
-	  GenerateTraceInfo	  off
-	  GenerateTraceReport	  off
-	  GenerateTraceReportSl	  off
-	  GenerateTraceReportSf	  off
-	  GenerateTraceReportEml  off
-	  GenerateCodeInfo	  off
-	  RTWCompilerOptimization "Off"
-	  CheckMdlBeforeBuild	  "Off"
-	  CustomRebuildMode	  "OnUpdate"
-	  Array {
-	    Type		    "Handle"
-	    Dimension		    2
-	    Simulink.CodeAppCC {
-	      $ObjectID		      10
-	      Version		      "1.10.0"
-	      Array {
-		Type			"Cell"
-		Dimension		17
-		Cell			"IgnoreCustomStorageClasses"
-		Cell			"IgnoreTestpoints"
-		Cell			"InsertBlockDesc"
-		Cell			"SFDataObjDesc"
-		Cell			"SimulinkDataObjDesc"
-		Cell			"DefineNamingRule"
-		Cell			"SignalNamingRule"
-		Cell			"ParamNamingRule"
-		Cell			"InlinedPrmAccess"
-		Cell			"CustomSymbolStr"
-		Cell			"CustomSymbolStrGlobalVar"
-		Cell			"CustomSymbolStrType"
-		Cell			"CustomSymbolStrField"
-		Cell			"CustomSymbolStrFcn"
-		Cell			"CustomSymbolStrBlkIO"
-		Cell			"CustomSymbolStrTmpVar"
-		Cell			"CustomSymbolStrMacro"
-		PropName		"DisabledProps"
-	      }
-	      ForceParamTrailComments off
-	      GenerateComments	      on
-	      IgnoreCustomStorageClasses on
-	      IgnoreTestpoints	      off
-	      IncHierarchyInIds	      off
-	      MaxIdLength	      31
-	      PreserveName	      off
-	      PreserveNameWithParent  off
-	      ShowEliminatedStatement off
-	      IncAutoGenComments      off
-	      SimulinkDataObjDesc     off
-	      SFDataObjDesc	      off
-	      IncDataTypeInIds	      off
-	      MangleLength	      1
-	      CustomSymbolStrGlobalVar "$R$N$M"
-	      CustomSymbolStrType     "$N$R$M"
-	      CustomSymbolStrField    "$N$M"
-	      CustomSymbolStrFcn      "$R$N$M$F"
-	      CustomSymbolStrFcnArg   "rt$I$N$M"
-	      CustomSymbolStrBlkIO    "rtb_$N$M"
-	      CustomSymbolStrTmpVar   "$N$M"
-	      CustomSymbolStrMacro    "$R$N$M"
-	      DefineNamingRule	      "None"
-	      ParamNamingRule	      "None"
-	      SignalNamingRule	      "None"
-	      InsertBlockDesc	      off
-	      SimulinkBlockComments   on
-	      EnableCustomComments    off
-	      InlinedPrmAccess	      "Literals"
-	      ReqsInCode	      off
-	      UseSimReservedNames     off
-	    }
-	    Simulink.GRTTargetCC {
-	      $BackupClass	      "Simulink.TargetCC"
-	      $ObjectID		      11
-	      Version		      "1.10.0"
-	      Array {
-		Type			"Cell"
-		Dimension		16
-		Cell			"IncludeMdlTerminateFcn"
-		Cell			"CombineOutputUpdateFcns"
-		Cell			"SuppressErrorStatus"
-		Cell			"ERTCustomFileBanners"
-		Cell			"GenerateSampleERTMain"
-		Cell			"GenerateTestInterfaces"
-		Cell			"ModelStepFunctionPrototypeControlCompliant"
-		Cell			"CPPClassGenCompliant"
-		Cell			"MultiInstanceERTCode"
-		Cell			"PurelyIntegerCode"
-		Cell			"SupportNonFinite"
-		Cell			"SupportComplex"
-		Cell			"SupportAbsoluteTime"
-		Cell			"SupportContinuousTime"
-		Cell			"SupportNonInlinedSFcns"
-		Cell			"PortableWordSizes"
-		PropName		"DisabledProps"
-	      }
-	      TargetFcnLib	      "ansi_tfl_table_tmw.mat"
-	      TargetLibSuffix	      ""
-	      TargetPreCompLibLocation ""
-	      TargetFunctionLibrary   "ANSI_C"
-	      UtilityFuncGeneration   "Auto"
-	      ERTMultiwordTypeDef     "System defined"
-	      ERTCodeCoverageTool     "None"
-	      ERTMultiwordLength      256
-	      MultiwordLength	      2048
-	      GenerateFullHeader      on
-	      GenerateSampleERTMain   off
-	      GenerateTestInterfaces  off
-	      IsPILTarget	      off
-	      ModelReferenceCompliant on
-	      ParMdlRefBuildCompliant on
-	      CompOptLevelCompliant   on
-	      IncludeMdlTerminateFcn  on
-	      GeneratePreprocessorConditionals "Disable all"
-	      CombineOutputUpdateFcns off
-	      SuppressErrorStatus     off
-	      ERTFirstTimeCompliant   off
-	      IncludeFileDelimiter    "Auto"
-	      ERTCustomFileBanners    off
-	      SupportAbsoluteTime     on
-	      LogVarNameModifier      "rt_"
-	      MatFileLogging	      on
-	      MultiInstanceERTCode    off
-	      SupportNonFinite	      on
-	      SupportComplex	      on
-	      PurelyIntegerCode	      off
-	      SupportContinuousTime   on
-	      SupportNonInlinedSFcns  on
-	      SupportVariableSizeSignals off
-	      EnableShiftOperators    on
-	      ParenthesesLevel	      "Nominal"
-	      PortableWordSizes	      off
-	      ModelStepFunctionPrototypeControlCompliant off
-	      CPPClassGenCompliant    off
-	      AutosarCompliant	      off
-	      UseMalloc		      off
-	      ExtMode		      off
-	      ExtModeStaticAlloc      off
-	      ExtModeTesting	      off
-	      ExtModeStaticAllocSize  1000000
-	      ExtModeTransport	      0
-	      ExtModeMexFile	      "ext_comm"
-	      ExtModeIntrfLevel	      "Level1"
-	      RTWCAPISignals	      off
-	      RTWCAPIParams	      off
-	      RTWCAPIStates	      off
-	      GenerateASAP2	      off
-	    }
-	    PropName		    "Components"
-	  }
-	}
-	hdlcoderui.hdlcc {
-	  $ObjectID		  12
-	  Version		  "1.10.0"
-	  Description		  "HDL Coder custom configuration component"
-	  Name			  "HDL Coder"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    1
-	    Cell		    ""
-	    PropName		    "HDLConfigFile"
-	  }
-	  HDLCActiveTab		  "0"
-	}
-	PropName		"Components"
-      }
-      Name		      "Configuration"
-      CurrentDlgPage	      "Solver"
-      ConfigPrmDlgPosition    " [ 520, 285, 1400, 915 ] "
-    }
-    PropName		    "ConfigurationSets"
-  }
-  BlockDefaults {
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    NamePlacement	    "normal"
-    FontName		    "Helvetica"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    ShowName		    on
-    BlockRotation	    0
-    BlockMirror		    off
-  }
-  AnnotationDefaults {
-    HorizontalAlignment	    "center"
-    VerticalAlignment	    "middle"
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    FontName		    "Helvetica"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    UseDisplayTextAsClickCallback off
-  }
-  LineDefaults {
-    FontName		    "Helvetica"
-    FontSize		    9
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-  }
-  BlockParameterDefaults {
-    Block {
-      BlockType		      "S-Function"
-      FunctionName	      "system"
-      SFunctionModules	      "''"
-      PortCounts	      "[]"
-      SFunctionDeploymentMode off
-    }
-  }
-  System {
-    Name		    "utility"
-    Location		    [480, 85, 708, 201]
-    Open		    on
-    ModelBrowserVisibility  off
-    ModelBrowserWidth	    200
-    ScreenColor		    "white"
-    PaperOrientation	    "landscape"
-    PaperPositionMode	    "auto"
-    PaperType		    "usletter"
-    PaperUnits		    "inches"
-    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
-    TiledPageScale	    1
-    ShowPageBoundaries	    off
-    ZoomFactor		    "100"
-    ReportName		    "simulink-default.rpt"
-    SIDHighWatermark	    5
-    Block {
-      BlockType		      "S-Function"
-      Name		      "Soft Real Time"
-      SID		      5
-      Ports		      []
-      Position		      [73, 30, 160, 61]
-      ShowName		      off
-      FunctionName	      "sfun_time"
-      Parameters	      "x"
-      EnableBusSupport	      off
-      MaskPromptString	      "Time Scaling Factor"
-      MaskStyleString	      "edit"
-      MaskTunableValueString  "on"
-      MaskEnableString	      "on"
-      MaskVisibilityString    "on"
-      MaskToolTipString	      "on"
-      MaskVariables	      "x=@1;"
-      MaskDisplay	      "color('red')\ndisp('Soft Real Time')\n"
-      MaskIconFrame	      on
-      MaskIconOpaque	      on
-      MaskIconRotate	      "none"
-      MaskPortRotate	      "default"
-      MaskIconUnits	      "autoscale"
-      MaskValueString	      "1"
-    }
-  }
-}
+Library {
+  Name			  "utility"
+  Version		  7.5
+  MdlSubVersion		  0
+  SavedCharacterEncoding  "windows-1252"
+  LibraryType		  "BlockLibrary"
+  SaveDefaultBlockParams  on
+  ScopeRefreshTime	  0.035000
+  OverrideScopeRefreshTime on
+  DisableAllScopes	  off
+  MaxMDLFileLineLength	  120
+  Created		  "Wed Oct 08 17:37:30 2008"
+  Creator		  "grouleau"
+  UpdateHistory		  "UpdateHistoryNever"
+  ModifiedByFormat	  "%<Auto>"
+  LastModifiedBy	  "grouleau"
+  ModifiedDateFormat	  "%<Auto>"
+  LastModifiedDate	  "Wed Jul 21 11:42:45 2010"
+  RTWModifiedTimeStamp	  201613320
+  ModelVersionFormat	  "1.%<AutoIncrement:7>"
+  ConfigurationManager	  "None"
+  SampleTimeColors	  off
+  SampleTimeAnnotations	  off
+  LibraryLinkDisplay	  "none"
+  WideLines		  off
+  ShowLineDimensions	  off
+  ShowPortDataTypes	  off
+  ShowLoopsOnError	  on
+  IgnoreBidirectionalLines off
+  ShowStorageClass	  off
+  ShowTestPointIcons	  on
+  ShowSignalResolutionIcons on
+  ShowViewerIcons	  on
+  SortedOrder		  off
+  ExecutionContextIcon	  off
+  ShowLinearizationAnnotations on
+  BlockNameDataTip	  off
+  BlockParametersDataTip  off
+  BlockDescriptionStringDataTip	off
+  ToolBar		  on
+  StatusBar		  on
+  BrowserShowLibraryLinks off
+  BrowserLookUnderMasks	  off
+  SimulationMode	  "normal"
+  LinearizationMsg	  "none"
+  Profile		  off
+  ParamWorkspaceSource	  "MATLABWorkspace"
+  RecordCoverage	  off
+  CovSaveName		  "covdata"
+  CovMetricSettings	  "dw"
+  CovNameIncrementing	  off
+  CovHtmlReporting	  on
+  CovForceBlockReductionOff on
+  covSaveCumulativeToWorkspaceVar on
+  CovSaveSingleToWorkspaceVar on
+  CovCumulativeReport	  off
+  CovReportOnPause	  on
+  CovModelRefEnable	  "Off"
+  CovExternalEMLEnable	  off
+  ExtModeBatchMode	  off
+  ExtModeEnableFloating	  on
+  ExtModeTrigType	  "manual"
+  ExtModeTrigMode	  "normal"
+  ExtModeTrigPort	  "1"
+  ExtModeTrigElement	  "any"
+  ExtModeTrigDuration	  1000
+  ExtModeTrigDurationFloating "auto"
+  ExtModeTrigHoldOff	  0
+  ExtModeTrigDelay	  0
+  ExtModeTrigDirection	  "rising"
+  ExtModeTrigLevel	  0
+  ExtModeArchiveMode	  "off"
+  ExtModeAutoIncOneShot	  off
+  ExtModeIncDirWhenArm	  off
+  ExtModeAddSuffixToVar	  off
+  ExtModeWriteAllDataToWs off
+  ExtModeArmWhenConnect	  on
+  ExtModeSkipDownloadWhenConnect off
+  ExtModeLogAll		  on
+  ExtModeAutoUpdateStatusClock on
+  ShowModelReferenceBlockVersion off
+  ShowModelReferenceBlockIO off
+  Array {
+    Type		    "Handle"
+    Dimension		    1
+    Simulink.ConfigSet {
+      $ObjectID		      1
+      Version		      "1.10.0"
+      Array {
+	Type			"Handle"
+	Dimension		9
+	Simulink.SolverCC {
+	  $ObjectID		  2
+	  Version		  "1.10.0"
+	  StartTime		  "0.0"
+	  StopTime		  "10.0"
+	  AbsTol		  "auto"
+	  FixedStep		  "auto"
+	  InitialStep		  "auto"
+	  MaxNumMinSteps	  "-1"
+	  MaxOrder		  5
+	  ZcThreshold		  "auto"
+	  ConsecutiveZCsStepRelTol "10*128*eps"
+	  MaxConsecutiveZCs	  "1000"
+	  ExtrapolationOrder	  4
+	  NumberNewtonIterations  1
+	  MaxStep		  "auto"
+	  MinStep		  "auto"
+	  MaxConsecutiveMinStep	  "1"
+	  RelTol		  "1e-3"
+	  SolverMode		  "Auto"
+	  Solver		  "ode45"
+	  SolverName		  "ode45"
+	  SolverJacobianMethodControl "auto"
+	  ShapePreserveControl	  "DisableAll"
+	  ZeroCrossControl	  "UseLocalSettings"
+	  ZeroCrossAlgorithm	  "Nonadaptive"
+	  AlgebraicLoopSolver	  "TrustRegion"
+	  SolverResetMethod	  "Fast"
+	  PositivePriorityOrder	  off
+	  AutoInsertRateTranBlk	  off
+	  SampleTimeConstraint	  "Unconstrained"
+	  InsertRTBMode		  "Whenever possible"
+	}
+	Simulink.DataIOCC {
+	  $ObjectID		  3
+	  Version		  "1.10.0"
+	  Decimation		  "1"
+	  ExternalInput		  "[t, u]"
+	  FinalStateName	  "xFinal"
+	  InitialState		  "xInitial"
+	  LimitDataPoints	  on
+	  MaxDataPoints		  "1000"
+	  LoadExternalInput	  off
+	  LoadInitialState	  off
+	  SaveFinalState	  off
+	  SaveCompleteFinalSimState off
+	  SaveFormat		  "Array"
+	  SaveOutput		  on
+	  SaveState		  off
+	  SignalLogging		  on
+	  DSMLogging		  on
+	  InspectSignalLogs	  off
+	  SaveTime		  on
+	  ReturnWorkspaceOutputs  off
+	  StateSaveName		  "xout"
+	  TimeSaveName		  "tout"
+	  OutputSaveName	  "yout"
+	  SignalLoggingName	  "logsout"
+	  DSMLoggingName	  "dsmout"
+	  OutputOption		  "RefineOutputTimes"
+	  OutputTimes		  "[]"
+	  ReturnWorkspaceOutputsName "out"
+	  Refine		  "1"
+	}
+	Simulink.OptimizationCC {
+	  $ObjectID		  4
+	  Version		  "1.10.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    4
+	    Cell		    "ZeroExternalMemoryAtStartup"
+	    Cell		    "ZeroInternalMemoryAtStartup"
+	    Cell		    "OptimizeModelRefInitCode"
+	    Cell		    "NoFixptDivByZeroProtection"
+	    PropName		    "DisabledProps"
+	  }
+	  BlockReduction	  on
+	  BooleanDataType	  on
+	  ConditionallyExecuteInputs on
+	  InlineParams		  off
+	  UseIntDivNetSlope	  off
+	  InlineInvariantSignals  off
+	  OptimizeBlockIOStorage  on
+	  BufferReuse		  on
+	  EnhancedBackFolding	  off
+	  StrengthReduction	  off
+	  EnforceIntegerDowncast  on
+	  ExpressionFolding	  on
+	  BooleansAsBitfields	  off
+	  BitfieldContainerType	  "uint_T"
+	  EnableMemcpy		  on
+	  MemcpyThreshold	  64
+	  PassReuseOutputArgsAs	  "Structure reference"
+	  ExpressionDepthLimit	  2147483647
+	  FoldNonRolledExpr	  on
+	  LocalBlockOutputs	  on
+	  RollThreshold		  5
+	  SystemCodeInlineAuto	  off
+	  StateBitsets		  off
+	  DataBitsets		  off
+	  UseTempVars		  off
+	  ZeroExternalMemoryAtStartup on
+	  ZeroInternalMemoryAtStartup on
+	  InitFltsAndDblsToZero	  off
+	  NoFixptDivByZeroProtection off
+	  EfficientFloat2IntCast  off
+	  EfficientMapNaN2IntZero on
+	  OptimizeModelRefInitCode off
+	  LifeSpan		  "inf"
+	  MaxStackSize		  "Inherit from target"
+	  BufferReusableBoundary  on
+	  SimCompilerOptimization "Off"
+	  AccelVerboseBuild	  off
+	}
+	Simulink.DebuggingCC {
+	  $ObjectID		  5
+	  Version		  "1.10.0"
+	  RTPrefix		  "error"
+	  ConsistencyChecking	  "none"
+	  ArrayBoundsChecking	  "none"
+	  SignalInfNanChecking	  "none"
+	  SignalRangeChecking	  "none"
+	  ReadBeforeWriteMsg	  "UseLocalSettings"
+	  WriteAfterWriteMsg	  "UseLocalSettings"
+	  WriteAfterReadMsg	  "UseLocalSettings"
+	  AlgebraicLoopMsg	  "warning"
+	  ArtificialAlgebraicLoopMsg "warning"
+	  SaveWithDisabledLinksMsg "warning"
+	  SaveWithParameterizedLinksMsg	"warning"
+	  CheckSSInitialOutputMsg on
+	  UnderspecifiedInitializationDetection	"Classic"
+	  MergeDetectMultiDrivingBlocksExec "none"
+	  CheckExecutionContextPreStartOutputMsg off
+	  CheckExecutionContextRuntimeOutputMsg	off
+	  SignalResolutionControl "UseLocalSettings"
+	  BlockPriorityViolationMsg "warning"
+	  MinStepSizeMsg	  "warning"
+	  TimeAdjustmentMsg	  "none"
+	  MaxConsecutiveZCsMsg	  "error"
+	  SolverPrmCheckMsg	  "warning"
+	  InheritedTsInSrcMsg	  "warning"
+	  DiscreteInheritContinuousMsg "warning"
+	  MultiTaskDSMMsg	  "error"
+	  MultiTaskCondExecSysMsg "error"
+	  MultiTaskRateTransMsg	  "error"
+	  SingleTaskRateTransMsg  "none"
+	  TasksWithSamePriorityMsg "warning"
+	  SigSpecEnsureSampleTimeMsg "warning"
+	  CheckMatrixSingularityMsg "none"
+	  IntegerOverflowMsg	  "warning"
+	  Int32ToFloatConvMsg	  "warning"
+	  ParameterDowncastMsg	  "error"
+	  ParameterOverflowMsg	  "error"
+	  ParameterUnderflowMsg	  "none"
+	  ParameterPrecisionLossMsg "warning"
+	  ParameterTunabilityLossMsg "warning"
+	  FixptConstUnderflowMsg  "none"
+	  FixptConstOverflowMsg	  "none"
+	  FixptConstPrecisionLossMsg "none"
+	  UnderSpecifiedDataTypeMsg "none"
+	  UnnecessaryDatatypeConvMsg "none"
+	  VectorMatrixConversionMsg "none"
+	  InvalidFcnCallConnMsg	  "error"
+	  FcnCallInpInsideContextMsg "Use local settings"
+	  SignalLabelMismatchMsg  "none"
+	  UnconnectedInputMsg	  "warning"
+	  UnconnectedOutputMsg	  "warning"
+	  UnconnectedLineMsg	  "warning"
+	  SFcnCompatibilityMsg	  "none"
+	  UniqueDataStoreMsg	  "none"
+	  BusObjectLabelMismatch  "warning"
+	  RootOutportRequireBusObject "warning"
+	  AssertControl		  "UseLocalSettings"
+	  EnableOverflowDetection off
+	  ModelReferenceIOMsg	  "none"
+	  ModelReferenceVersionMismatchMessage "none"
+	  ModelReferenceIOMismatchMessage "none"
+	  ModelReferenceCSMismatchMessage "none"
+	  UnknownTsInhSupMsg	  "warning"
+	  ModelReferenceDataLoggingMessage "warning"
+	  ModelReferenceSymbolNameMessage "warning"
+	  ModelReferenceExtraNoncontSigs "error"
+	  StateNameClashWarn	  "warning"
+	  SimStateInterfaceChecksumMismatchMsg "warning"
+	  StrictBusMsg		  "Warning"
+	  BusNameAdapt		  "WarnAndRepair"
+	  NonBusSignalsTreatedAsBus "none"
+	  LoggingUnavailableSignals "error"
+	  BlockIODiagnostic	  "none"
+	}
+	Simulink.HardwareCC {
+	  $ObjectID		  6
+	  Version		  "1.10.0"
+	  ProdBitPerChar	  8
+	  ProdBitPerShort	  16
+	  ProdBitPerInt		  32
+	  ProdBitPerLong	  32
+	  ProdIntDivRoundTo	  "Undefined"
+	  ProdEndianess		  "Unspecified"
+	  ProdWordSize		  32
+	  ProdShiftRightIntArith  on
+	  ProdHWDeviceType	  "32-bit Generic"
+	  TargetBitPerChar	  8
+	  TargetBitPerShort	  16
+	  TargetBitPerInt	  32
+	  TargetBitPerLong	  32
+	  TargetShiftRightIntArith on
+	  TargetIntDivRoundTo	  "Undefined"
+	  TargetEndianess	  "Unspecified"
+	  TargetWordSize	  32
+	  TargetTypeEmulationWarnSuppressLevel 0
+	  TargetPreprocMaxBitsSint 32
+	  TargetPreprocMaxBitsUint 32
+	  TargetHWDeviceType	  "Specified"
+	  TargetUnknown		  off
+	  ProdEqTarget		  on
+	}
+	Simulink.ModelReferenceCC {
+	  $ObjectID		  7
+	  Version		  "1.10.0"
+	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
+	  CheckModelReferenceTargetMessage "error"
+	  EnableParallelModelReferenceBuilds off
+	  ParallelModelReferenceMATLABWorkerInit "None"
+	  ModelReferenceNumInstancesAllowed "Multi"
+	  PropagateVarSize	  "Infer from blocks in model"
+	  ModelReferencePassRootInputsByReference on
+	  ModelReferenceMinAlgLoopOccurrences off
+	  PropagateSignalLabelsOutOfModel off
+	  SupportModelReferenceSimTargetCustomCode off
+	}
+	Simulink.SFSimCC {
+	  $ObjectID		  8
+	  Version		  "1.10.0"
+	  SFSimEnableDebug	  on
+	  SFSimOverflowDetection  on
+	  SFSimEcho		  on
+	  SimBlas		  on
+	  SimCtrlC		  on
+	  SimExtrinsic		  on
+	  SimIntegrity		  on
+	  SimUseLocalCustomCode	  off
+	  SimBuildMode		  "sf_incremental_build"
+	}
+	Simulink.RTWCC {
+	  $BackupClass		  "Simulink.RTWCC"
+	  $ObjectID		  9
+	  Version		  "1.10.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    6
+	    Cell		    "IncludeHyperlinkInReport"
+	    Cell		    "GenerateTraceInfo"
+	    Cell		    "GenerateTraceReport"
+	    Cell		    "GenerateTraceReportSl"
+	    Cell		    "GenerateTraceReportSf"
+	    Cell		    "GenerateTraceReportEml"
+	    PropName		    "DisabledProps"
+	  }
+	  SystemTargetFile	  "grt.tlc"
+	  GenCodeOnly		  off
+	  MakeCommand		  "make_rtw"
+	  GenerateMakefile	  on
+	  TemplateMakefile	  "grt_default_tmf"
+	  GenerateReport	  off
+	  SaveLog		  off
+	  RTWVerbose		  on
+	  RetainRTWFile		  off
+	  ProfileTLC		  off
+	  TLCDebug		  off
+	  TLCCoverage		  off
+	  TLCAssert		  off
+	  ProcessScriptMode	  "Default"
+	  ConfigurationMode	  "Optimized"
+	  ConfigAtBuild		  off
+	  RTWUseLocalCustomCode	  off
+	  RTWUseSimCustomCode	  off
+	  IncludeHyperlinkInReport off
+	  LaunchReport		  off
+	  TargetLang		  "C"
+	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
+	  IncludeERTFirstTime	  off
+	  GenerateTraceInfo	  off
+	  GenerateTraceReport	  off
+	  GenerateTraceReportSl	  off
+	  GenerateTraceReportSf	  off
+	  GenerateTraceReportEml  off
+	  GenerateCodeInfo	  off
+	  RTWCompilerOptimization "Off"
+	  CheckMdlBeforeBuild	  "Off"
+	  CustomRebuildMode	  "OnUpdate"
+	  Array {
+	    Type		    "Handle"
+	    Dimension		    2
+	    Simulink.CodeAppCC {
+	      $ObjectID		      10
+	      Version		      "1.10.0"
+	      Array {
+		Type			"Cell"
+		Dimension		17
+		Cell			"IgnoreCustomStorageClasses"
+		Cell			"IgnoreTestpoints"
+		Cell			"InsertBlockDesc"
+		Cell			"SFDataObjDesc"
+		Cell			"SimulinkDataObjDesc"
+		Cell			"DefineNamingRule"
+		Cell			"SignalNamingRule"
+		Cell			"ParamNamingRule"
+		Cell			"InlinedPrmAccess"
+		Cell			"CustomSymbolStr"
+		Cell			"CustomSymbolStrGlobalVar"
+		Cell			"CustomSymbolStrType"
+		Cell			"CustomSymbolStrField"
+		Cell			"CustomSymbolStrFcn"
+		Cell			"CustomSymbolStrBlkIO"
+		Cell			"CustomSymbolStrTmpVar"
+		Cell			"CustomSymbolStrMacro"
+		PropName		"DisabledProps"
+	      }
+	      ForceParamTrailComments off
+	      GenerateComments	      on
+	      IgnoreCustomStorageClasses on
+	      IgnoreTestpoints	      off
+	      IncHierarchyInIds	      off
+	      MaxIdLength	      31
+	      PreserveName	      off
+	      PreserveNameWithParent  off
+	      ShowEliminatedStatement off
+	      IncAutoGenComments      off
+	      SimulinkDataObjDesc     off
+	      SFDataObjDesc	      off
+	      IncDataTypeInIds	      off
+	      MangleLength	      1
+	      CustomSymbolStrGlobalVar "$R$N$M"
+	      CustomSymbolStrType     "$N$R$M"
+	      CustomSymbolStrField    "$N$M"
+	      CustomSymbolStrFcn      "$R$N$M$F"
+	      CustomSymbolStrFcnArg   "rt$I$N$M"
+	      CustomSymbolStrBlkIO    "rtb_$N$M"
+	      CustomSymbolStrTmpVar   "$N$M"
+	      CustomSymbolStrMacro    "$R$N$M"
+	      DefineNamingRule	      "None"
+	      ParamNamingRule	      "None"
+	      SignalNamingRule	      "None"
+	      InsertBlockDesc	      off
+	      SimulinkBlockComments   on
+	      EnableCustomComments    off
+	      InlinedPrmAccess	      "Literals"
+	      ReqsInCode	      off
+	      UseSimReservedNames     off
+	    }
+	    Simulink.GRTTargetCC {
+	      $BackupClass	      "Simulink.TargetCC"
+	      $ObjectID		      11
+	      Version		      "1.10.0"
+	      Array {
+		Type			"Cell"
+		Dimension		16
+		Cell			"IncludeMdlTerminateFcn"
+		Cell			"CombineOutputUpdateFcns"
+		Cell			"SuppressErrorStatus"
+		Cell			"ERTCustomFileBanners"
+		Cell			"GenerateSampleERTMain"
+		Cell			"GenerateTestInterfaces"
+		Cell			"ModelStepFunctionPrototypeControlCompliant"
+		Cell			"CPPClassGenCompliant"
+		Cell			"MultiInstanceERTCode"
+		Cell			"PurelyIntegerCode"
+		Cell			"SupportNonFinite"
+		Cell			"SupportComplex"
+		Cell			"SupportAbsoluteTime"
+		Cell			"SupportContinuousTime"
+		Cell			"SupportNonInlinedSFcns"
+		Cell			"PortableWordSizes"
+		PropName		"DisabledProps"
+	      }
+	      TargetFcnLib	      "ansi_tfl_table_tmw.mat"
+	      TargetLibSuffix	      ""
+	      TargetPreCompLibLocation ""
+	      TargetFunctionLibrary   "ANSI_C"
+	      UtilityFuncGeneration   "Auto"
+	      ERTMultiwordTypeDef     "System defined"
+	      ERTCodeCoverageTool     "None"
+	      ERTMultiwordLength      256
+	      MultiwordLength	      2048
+	      GenerateFullHeader      on
+	      GenerateSampleERTMain   off
+	      GenerateTestInterfaces  off
+	      IsPILTarget	      off
+	      ModelReferenceCompliant on
+	      ParMdlRefBuildCompliant on
+	      CompOptLevelCompliant   on
+	      IncludeMdlTerminateFcn  on
+	      GeneratePreprocessorConditionals "Disable all"
+	      CombineOutputUpdateFcns off
+	      SuppressErrorStatus     off
+	      ERTFirstTimeCompliant   off
+	      IncludeFileDelimiter    "Auto"
+	      ERTCustomFileBanners    off
+	      SupportAbsoluteTime     on
+	      LogVarNameModifier      "rt_"
+	      MatFileLogging	      on
+	      MultiInstanceERTCode    off
+	      SupportNonFinite	      on
+	      SupportComplex	      on
+	      PurelyIntegerCode	      off
+	      SupportContinuousTime   on
+	      SupportNonInlinedSFcns  on
+	      SupportVariableSizeSignals off
+	      EnableShiftOperators    on
+	      ParenthesesLevel	      "Nominal"
+	      PortableWordSizes	      off
+	      ModelStepFunctionPrototypeControlCompliant off
+	      CPPClassGenCompliant    off
+	      AutosarCompliant	      off
+	      UseMalloc		      off
+	      ExtMode		      off
+	      ExtModeStaticAlloc      off
+	      ExtModeTesting	      off
+	      ExtModeStaticAllocSize  1000000
+	      ExtModeTransport	      0
+	      ExtModeMexFile	      "ext_comm"
+	      ExtModeIntrfLevel	      "Level1"
+	      RTWCAPISignals	      off
+	      RTWCAPIParams	      off
+	      RTWCAPIStates	      off
+	      GenerateASAP2	      off
+	    }
+	    PropName		    "Components"
+	  }
+	}
+	hdlcoderui.hdlcc {
+	  $ObjectID		  12
+	  Version		  "1.10.0"
+	  Description		  "HDL Coder custom configuration component"
+	  Name			  "HDL Coder"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    1
+	    Cell		    ""
+	    PropName		    "HDLConfigFile"
+	  }
+	  HDLCActiveTab		  "0"
+	}
+	PropName		"Components"
+      }
+      Name		      "Configuration"
+      CurrentDlgPage	      "Solver"
+      ConfigPrmDlgPosition    " [ 520, 285, 1400, 915 ] "
+    }
+    PropName		    "ConfigurationSets"
+  }
+  BlockDefaults {
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    NamePlacement	    "normal"
+    FontName		    "Helvetica"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    ShowName		    on
+    BlockRotation	    0
+    BlockMirror		    off
+  }
+  AnnotationDefaults {
+    HorizontalAlignment	    "center"
+    VerticalAlignment	    "middle"
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    FontName		    "Helvetica"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    UseDisplayTextAsClickCallback off
+  }
+  LineDefaults {
+    FontName		    "Helvetica"
+    FontSize		    9
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+  }
+  BlockParameterDefaults {
+    Block {
+      BlockType		      "S-Function"
+      FunctionName	      "system"
+      SFunctionModules	      "''"
+      PortCounts	      "[]"
+      SFunctionDeploymentMode off
+    }
+  }
+  System {
+    Name		    "utility"
+    Location		    [480, 85, 708, 201]
+    Open		    on
+    ModelBrowserVisibility  off
+    ModelBrowserWidth	    200
+    ScreenColor		    "white"
+    PaperOrientation	    "landscape"
+    PaperPositionMode	    "auto"
+    PaperType		    "usletter"
+    PaperUnits		    "inches"
+    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
+    TiledPageScale	    1
+    ShowPageBoundaries	    off
+    ZoomFactor		    "100"
+    ReportName		    "simulink-default.rpt"
+    SIDHighWatermark	    5
+    Block {
+      BlockType		      "S-Function"
+      Name		      "Soft Real Time"
+      SID		      5
+      Ports		      []
+      Position		      [73, 30, 160, 61]
+      ShowName		      off
+      FunctionName	      "sfun_time"
+      Parameters	      "x"
+      EnableBusSupport	      off
+      MaskPromptString	      "Time Scaling Factor"
+      MaskStyleString	      "edit"
+      MaskTunableValueString  "on"
+      MaskEnableString	      "on"
+      MaskVisibilityString    "on"
+      MaskToolTipString	      "on"
+      MaskVariables	      "x=@1;"
+      MaskDisplay	      "color('red')\ndisp('Soft Real Time')\n"
+      MaskIconFrame	      on
+      MaskIconOpaque	      on
+      MaskIconRotate	      "none"
+      MaskPortRotate	      "default"
+      MaskIconUnits	      "autoscale"
+      MaskValueString	      "1"
+    }
+  }
+}

From e56e5edc131f094c46428d27272100a67c33afd7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 May 2018 23:48:50 +0200
Subject: [PATCH 09/10] Implement remaining ITorqueControl methods

---
 .../CanBusControlboard/CanBusControlboard.hpp | 21 ++++++++++
 .../CanBusControlboard/ITorqueControlImpl.cpp | 38 +++++++++++++++++++
 .../FakeControlboard/FakeControlboard.hpp     | 21 ++++++++++
 .../FakeControlboard/ITorqueControlImpl.cpp   | 21 ++++++++++
 4 files changed, 101 insertions(+)

diff --git a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
index 43a18482a..a844ec825 100644
--- a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
+++ b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
@@ -535,6 +535,27 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
      */
     virtual bool setRefTorque(int j, double t);
 
+    /** Set new torque reference for a subset of joints.
+     * @param joints pointer to the array of joint numbers
+     * @param refs   pointer to the array specifing the new torque reference
+     * @return true/false on success/failure
+     */
+    virtual bool setRefTorques(const int n_joint, const int *joints, const double *t);
+
+    /** Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
+     * @param j joint number
+     * @param params a struct containing the motor parameters to be retrieved
+     * @return true/false on success/failure
+     */
+    virtual bool getMotorTorqueParams(int j,  yarp::dev::MotorTorqueParameters *params);
+
+    /** Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
+     * @param j joint number
+     * @param params a struct containing the motor parameters to be set
+     * @return true/false on success/failure
+     */
+    virtual bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params);
+
     /** Get the value of the torque on a given joint (this is the
      * feedback if you have a torque sensor).
      * @param j joint number
diff --git a/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
index 303e73a43..afdb816c0 100644
--- a/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/CanBusControlboard/ITorqueControlImpl.cpp
@@ -57,6 +57,44 @@ bool roboticslab::CanBusControlboard::setRefTorque(int j, double t)
 
 // -----------------------------------------------------------------------------
 
+bool roboticslab::CanBusControlboard::setRefTorques(const int n_joint, const int *joints, const double *t)
+{
+    CD_DEBUG("(%d)\n",n_joint);
+
+    bool ok = true;
+    for(int j=0; j<n_joint; j++)
+    {
+        ok &= this->setRefTorque(joints[j],t[j]);
+    }
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::CanBusControlboard::getMotorTorqueParams(int j,  yarp::dev::MotorTorqueParameters *params)
+{
+    CD_DEBUG("(%d)\n",j);
+
+    //-- Check index within range
+    if ( ! this->indexWithinRange(j) ) return false;
+
+    return iTorqueControlRaw[j]->getMotorTorqueParamsRaw( 0, params );
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::CanBusControlboard::setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params)
+{
+    CD_DEBUG("(%d)\n",j);
+
+    //-- Check index within range
+    if ( ! this->indexWithinRange(j) ) return false;
+
+    return iTorqueControlRaw[j]->setMotorTorqueParamsRaw( 0, params );
+}
+
+// -----------------------------------------------------------------------------
+
 bool roboticslab::CanBusControlboard::getTorque(int j, double *t)
 {
     //CD_INFO("(%d)\n",j);  //-- Too verbose in controlboardwrapper2 stream.
diff --git a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
index 8462641e3..63a766178 100644
--- a/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
+++ b/libraries/YarpPlugins/FakeControlboard/FakeControlboard.hpp
@@ -592,6 +592,27 @@ class FakeControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool setRefTorque(int j, double t);
 
+    /** Set new torque reference for a subset of joints.
+     * @param joints pointer to the array of joint numbers
+     * @param refs   pointer to the array specifing the new torque reference
+     * @return true/false on success/failure
+     */
+    virtual bool setRefTorques(const int n_joint, const int *joints, const double *t);
+
+    /** Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
+     * @param j joint number
+     * @param params a struct containing the motor parameters to be retrieved
+     * @return true/false on success/failure
+     */
+    virtual bool getMotorTorqueParams(int j,  yarp::dev::MotorTorqueParameters *params);
+
+    /** Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
+     * @param j joint number
+     * @param params a struct containing the motor parameters to be set
+     * @return true/false on success/failure
+     */
+    virtual bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params);
+
     /**
      * Get the value of the torque on a given joint (this is the
      * feedback if you have a torque sensor).
diff --git a/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp b/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
index 91b1f2de2..9477b4fba 100644
--- a/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
+++ b/libraries/YarpPlugins/FakeControlboard/ITorqueControlImpl.cpp
@@ -35,6 +35,27 @@ bool roboticslab::FakeControlboard::setRefTorque(int j, double t)
 
 // -----------------------------------------------------------------------------
 
+bool roboticslab::FakeControlboard::setRefTorques(const int n_joint, const int *joints, const double *t)
+{
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::FakeControlboard::getMotorTorqueParams(int j,  yarp::dev::MotorTorqueParameters *params)
+{
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::FakeControlboard::setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params)
+{
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
 bool roboticslab::FakeControlboard::getTorque(int j, double *t)
 {
     //CD_DEBUG("joint: %d.\n",j);  //-- Way too verbose

From 4d50a6ff1c42835424e2e2e1688bded426feea12 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Fri, 18 May 2018 00:15:17 +0200
Subject: [PATCH 10/10] Fool CMake to think that devel YARP is at 3.0

---
 libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt b/libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt
index e981ae1dd..b12f992fe 100644
--- a/libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt
+++ b/libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt
@@ -32,6 +32,12 @@ target_link_libraries(CanBusControlboard YARP::YARP_OS
                                          YARP::YARP_dev
                                          YarpDevicesInterfaces)
 
+# Hack, fool CMake to think that YARP already switched to 3.0 while still at devel.
+# https://github.com/roboticslab-uc3m/yarp-devices/issues/180
+if(NOT YARP_VERSION_SHORT VERSION_LESS 2.3.73)
+    target_compile_definitions(CanBusControlboard PUBLIC YARP_VERSION_MAJOR=3)
+endif()
+
 yarp_install(TARGETS CanBusControlboard
              COMPONENT runtime
              LIBRARY DESTINATION ${ROBOTICSLAB-YARP-DEVICES_DYNAMIC_PLUGINS_INSTALL_DIR}