Changes after review

devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h

@@ -1,7 +1,7 @@
 /**
  * @file JointTorqueControlDevice.h
  * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
@@ -41,9 +41,13 @@ class JointTorqueControlDevice;
  */
 struct CouplingMatrices
 {
-    Eigen::MatrixXd fromJointTorquesToMotorTorques;
-    Eigen::MatrixXd fromMotorTorquesToJointTorques;
-    Eigen::MatrixXd fromJointVelocitiesToMotorVelocities;
+    Eigen::MatrixXd fromJointTorquesToMotorTorques; /**< matrix to convert joint torques to motor torques */
+    Eigen::MatrixXd fromMotorTorquesToJointTorques; /**< matrix to convert motor torques to joint torques */
+    Eigen::MatrixXd fromJointVelocitiesToMotorVelocities; /**< matrix to convert joint velocities to motor velocities */
+
+    /**
+     * Reset the coupling matrices to identity
+     */
     void reset(int NDOF)
     {
         fromJointTorquesToMotorTorques = Eigen::MatrixXd::Identity(NDOF, NDOF);
@@ -58,10 +62,10 @@ struct CouplingMatrices
  */
 struct MotorTorqueCurrentParameters
 {
-    double kt; ///< motor torque to current gain
-    double kfc; ///< friction compensation weight parameter
-    double kp; ///< proportional gain
-    double maxCurr; ///< maximum current
+    double kt; /**< motor torque to current gain */
+    double kfc; /**< friction compensation weight parameter */
+    double kp; /**< proportional gain */
+    double maxCurr; /**< maximum current */
     std::string frictionModel; ///< friction model
 
     void reset()
@@ -77,11 +81,14 @@ struct MotorTorqueCurrentParameters
  */
 struct PINNParameters
 {
-    std::string modelPath; ///< PINN model path
-    int threadNumber; ///< number of threads
-    int historyLength; ///< history length
-    int inputNumber; ///< number of inputs
+    std::string modelPath; /**< PINN model path */
+    int threadNumber; /**< number of threads */
+    int historyLength; /**< history length */
+    int inputNumber; /**< number of inputs */
 
+    /**
+     * Reset the parameters
+     */
     void reset()
     {
         modelPath = "";
@@ -97,9 +104,9 @@ struct PINNParameters
  */
 struct CoulombViscousParameters
 {
-    double kc; ///< coulomb friction
-    double kv; ///< viscous friction
-    double ka; ///< viscous friction
+    double kc; /**< coulomb friction */
+    double kv; /**< viscous friction */
+    double ka; /**< viscous friction */
 
     void reset()
     {
@@ -113,13 +120,16 @@ struct CoulombViscousParameters
  */
 struct CoulombViscousStribeckParameters
 {
-    double kc; ///< coulomb friction
-    double kv; ///< viscous friction
-    double vs; ///< stiction velocity
-    double ka; ///< tanh gain
-    double ks; ///< stribeck friction
-    double alpha; // power factor
+    double kc; /**< coulomb friction */
+    double kv; /**< viscous friction */
+    double vs; /**< stiction velocity */
+    double ka; /**< tanh gain */
+    double ks; /**< stribeck friction */
+    double alpha; /**< power factor */
 
+    /**
+     * Reset the parameters
+     */
     void reset()
     {
         kc = kv = ka = vs = ks = alpha = 0.0;
@@ -139,7 +149,7 @@ class BipedalLocomotion::JointTorqueControlDevice
     std::vector<CoulombViscousParameters> coulombViscousParameters;
     std::vector<CoulombViscousStribeckParameters> coulombViscousStribeckParameters;
     std::vector<std::unique_ptr<PINNFrictionEstimator>> frictionEstimators;
-    std::mutex mutexTorqueControlParam_; // The mutex for protecting the parameters
+    std::mutex mutexTorqueControlParam_; /**< The mutex for protecting the parameters of the torque control. */
     yarp::sig::Vector desiredJointTorques;
     yarp::sig::Vector desiredMotorCurrents;
     yarp::sig::Vector measuredJointVelocities;
@@ -215,7 +225,7 @@ class BipedalLocomotion::JointTorqueControlDevice
     std::mutex globalMutex; ///< mutex protecting control variables & proxy interface methods
 
     // Method that actually executes one control loop
-    double timeOfLastControlLoop{-1.0};
+    std::chrono::nanoseconds timeOfLastControlLoop{std::chrono::nanoseconds::zero()};
     void controlLoop();
 
 

devices/JointTorqueControlDevice/include/BipedalLocomotion/PINNFrictionEstimator.h

@@ -1,7 +1,7 @@
 /**
  * @file PINNFrictionEstimator.h
  * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 

devices/JointTorqueControlDevice/include/BipedalLocomotion/PassThroughControlBoard.h

@@ -1,7 +1,7 @@
 /**
  * @file PassThroughControlBoard.h
  * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 

devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp

@@ -1,7 +1,7 @@
 /**
  * @file JointTorqueControlDevice.cpp
  * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
@@ -324,16 +324,6 @@ double JointTorqueControlDevice::computeFrictionTorque(int joint)
 
 void JointTorqueControlDevice::computeDesiredCurrents()
 {
-    // compute timestep
-    double dt;
-    if (timeOfLastControlLoop < 0.0)
-    {
-        dt = this->getPeriod();
-    } else
-    {
-        dt = yarp::os::Time::now() - timeOfLastControlLoop;
-    }
-
     toEigenVector(desiredJointTorques)
         = couplingMatrices.fromJointTorquesToMotorTorques * toEigenVector(desiredJointTorques);
 
@@ -392,17 +382,19 @@ void JointTorqueControlDevice::computeDesiredCurrents()
 
 void JointTorqueControlDevice::readStatus()
 {
+    auto logPrefix = "[JointTorqueControlDevice::readStatus]";
+
     if (!this->PassThroughControlBoard::getEncoderSpeeds(measuredJointVelocities.data()))
     {
-        std::cerr << "Failed to get Motor encoders speed" << std::endl;
+        log()->error("{} Failed to get Motor encoders speed", logPrefix);
     }
     if (!this->PassThroughControlBoard::getMotorEncoderSpeeds(measuredMotorVelocities.data()))
     {
         // In case the motor speed can not be directly measured, it tries to estimate it from joint
         // velocity if available
         if (!this->PassThroughControlBoard::getEncoderSpeeds(measuredJointVelocities.data()))
         {
-            std::cerr << "Failed to get Motor encoders speed" << std::endl;
+            log()->error("{} Failed to get Motor encoders speed", logPrefix);
         } else
         {
             toEigenVector(measuredMotorVelocities)
@@ -412,26 +404,28 @@ void JointTorqueControlDevice::readStatus()
     }
     if (!this->PassThroughControlBoard::getTorques(measuredJointTorques.data()))
     {
-        std::cerr << "Failed to get joint torque" << std::endl;
+        log()->error("{} Failed to get joint torque", logPrefix);
     }
     if (!this->PassThroughControlBoard::getEncoders(measuredJointPositions.data()))
     {
-        std::cerr << "Failed to get joint position" << std::endl;
+        log()->error("{} Failed to get joint position", logPrefix);
     }
     if (!this->PassThroughControlBoard::getMotorEncoders(measuredMotorPositions.data()))
     {
-        std::cerr << "Failed to get motor position" << std::endl;
+        log()->error("{} Failed to get motor position", logPrefix);
     }
 }
 
 bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config,
                                                   CouplingMatrices& coupling_matrix,
                                                   std::string group_name)
 {
+    auto logPrefix = "[JointTorqueControlDevice::loadCouplingMatrix]";
+
     if (!config.check(group_name))
     {
         coupling_matrix.reset(this->axes);
-        yWarning("COUPLING MATRIX option not found in configuration file");
+        log()->warn("{} COUPLING MATRIX option not found in configuration file", logPrefix);
         return true;
     } else
     {
@@ -440,14 +434,12 @@ bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config,
 
         if (!checkVectorExistInConfiguration(couplings_bot, "axesNames", this->axes))
         {
-            std::cerr << "JointTorqueControlDevice: error in loading axesNames parameter"
-                      << std::endl;
+            log()->error("{} Error in loading axesNames parameter", logPrefix);
             return false;
         }
         if (!checkVectorExistInConfiguration(couplings_bot, "motorNames", this->axes))
         {
-            std::cerr << "JointTorqueControlDevice: error in loading motorNames parameter"
-                      << std::endl;
+            log()->error("{} Error in loading motorNames parameter", logPrefix);
             return false;
         }
         std::vector<std::string> motorNames(this->axes);
@@ -463,9 +455,10 @@ bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config,
             std::string axis_name = axesNames[axis_id];
             if (!couplings_bot.check(axis_name) || !(couplings_bot.find(axis_name).isList()))
             {
-                std::cerr << "[ERR] " << group_name
-                          << " group found, but no coupling found for joint " << axis_name
-                          << ", exiting" << std::endl;
+                log()->error("{} {} group found, but no coupling found for joint {}, exiting",
+                             logPrefix,
+                             group_name,
+                             axis_name);
                 return false;
             }
 
@@ -478,17 +471,21 @@ bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config,
                     || !(axis_coupling_bot->get(coupled_motor).asList()->get(0).isFloat64())
                     || !(axis_coupling_bot->get(coupled_motor).asList()->get(1).isString()))
                 {
-                    std::cerr << "[ERR] " << group_name << " group found, but coupling for axis "
-                              << axis_name << " is malformed" << std::endl;
+                    log()->error("{} {} group found, but coupling for axis {} is malformed",
+                             logPrefix,
+                             group_name,
+                             axis_name);
                     return false;
                 }
 
                 std::string motorName
                     = axis_coupling_bot->get(coupled_motor).asList()->get(1).asString().c_str();
                 if (!contains(motorNames, motorName))
                 {
-                    std::cerr << "[ERR] " << group_name << "group found, but motor name  "
-                              << motorName << " is not part of the motor list" << std::endl;
+                    log()->error("{} {} group found, but motor name {} is not part of the motor list",
+                             logPrefix,
+                             group_name,
+                             motorName);
                     return false;
                 }
             }
@@ -512,11 +509,8 @@ bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config,
             }
         }
 
-        std::cerr << "loadCouplingMatrix DEBUG: " << std::endl;
-        std::cerr << "loaded kinematic coupling matrix from group " << group_name << std::endl;
-        std::cerr << coupling_matrix.fromJointVelocitiesToMotorVelocities << std::endl;
-        // std::cerr << "loaded torque coupling matrix from group " << group_name << std::endl;
-        // std::cerr << coupling_matrix.fromJointTorquesToMotorTorques << std::endl;
+        log()->error("{} LoadCouplingMatrix DEBUG: loaded kinematic coupling matrix from group {}", logPrefix, group_name);
+        log()->error("{}", coupling_matrix.fromJointVelocitiesToMotorVelocities);
 
         coupling_matrix.fromJointTorquesToMotorTorques
             = coupling_matrix.fromJointVelocitiesToMotorVelocities.transpose();
@@ -1174,10 +1168,10 @@ void JointTorqueControlDevice::threadRelease()
 
 void JointTorqueControlDevice::run()
 {
-    double now = yarp::os::Time::now();
+    std::chrono::nanoseconds now = BipedalLocomotion::clock().now();
 
     std::lock_guard<std::mutex> lock(globalMutex);
-    if (now - timeOfLastControlLoop >= this->getPeriod())
+    if (now.count() - timeOfLastControlLoop.count() >= this->getPeriod())
     {
         this->controlLoop();
     }
@@ -1207,5 +1201,5 @@ void JointTorqueControlDevice::controlLoop()
                          hijackedMotors.data(),
                          desiredMotorCurrentsHijackedMotors.data());
 
-    timeOfLastControlLoop = yarp::os::Time::now();
+    timeOfLastControlLoop = BipedalLocomotion::clock().now();;
 }

devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp

@@ -1,7 +1,7 @@
 /**
  * @file PINNFrictionEstimator.cpp
  * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 

devices/JointTorqueControlDevice/src/PassThroughControlBoard.cpp

@@ -1,7 +1,7 @@
 /**
  * @file PassThroughControlBoard.cpp
  * @authors Ines Sorrentino
- * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
@@ -61,19 +61,6 @@ void PassThroughControlBoard::resetPointers()
 // DEVICE DRIVER
 bool PassThroughControlBoard::open(yarp::os::Searchable& config)
 {
-    bool legacyOption = false;
-    legacyOption = legacyOption || config.check("proxy_remote");
-    legacyOption = legacyOption || config.check("proxy_local");
-
-    if (legacyOption)
-    {
-        yError("PassThroughControlBoard error: legacy option proxy_remote and proxy_local are not "
-               "supported anymore. Please use the yarprobotinterface to compose devices.\n");
-        return false;
-    }
-
-    // Do nothing, the actual proxies are initialized in the attachAll method,
-    // that is called by usercode or by the yarprobotinterface
     return true;
 }