Implement low-pass filter for estimated friction torque

CHANGELOG.md

@@ -4,6 +4,7 @@ All notable changes to this project are documented in this file.
 ## [unreleased]
 ### Added
 - Add `USE_SYSTEM_tiny-process-library` CMake option to use `tiny-process-library` found in system (https://github.com/ami-iit/bipedal-locomotion-framework/pull/891)
+- Implement low-pass filter for estimated friction torques in `JointTorqueControlDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/892)
 
 ### Changed
 

devices/JointTorqueControlDevice/CMakeLists.txt

@@ -25,6 +25,7 @@ if(FRAMEWORK_COMPILE_JointTorqueControlDevice)
       BipedalLocomotion::VectorsCollection
       BipedalLocomotion::JointTorqueControlCommands
       BipedalLocomotion::Math
+      BipedalLocomotion::ContinuousDynamicalSystem
       PUBLIC_LINK_LIBRARIES
       onnxruntime::onnxruntime
       Eigen3::Eigen

devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h

@@ -14,6 +14,7 @@
 #include <BipedalLocomotion/PINNFrictionEstimator.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollection.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/ButterworthLowPassFilter.h>
 
 #include <iostream>
 #include <memory>
@@ -66,13 +67,15 @@ struct MotorTorqueCurrentParameters
     double kp; /**< proportional gain */
     double maxCurr; /**< maximum current */
     std::string frictionModel; ///< friction model
+    double maxOutputFriction; /**< maximum output of the friction model */
 
     /**
      * Reset the parameters
      */
     void reset()
     {
         kt = kfc = kp = maxCurr = 0.0;
+        maxOutputFriction = 0.0;
         frictionModel = "";
     }
 };
@@ -137,6 +140,14 @@ struct CoulombViscousStribeckParameters
     }
 };
 
+struct LowPassFilterParameters
+{
+    double cutoffFrequency; /**< cutoff frequency */
+    int order; /**< order of the filter */
+    double samplingTime; /**< sampling time */
+    bool enabled; /**< true if the filter is enabled */
+};
+
 /**
  * @brief This class implements a device that allows to control the joints of a robot in torque mode.
  * The device is able to estimate the friction torque acting on the joints and to compensate it.
@@ -155,6 +166,7 @@ class BipedalLocomotion::JointTorqueControlDevice
     std::vector<CoulombViscousParameters> coulombViscousParameters;
     std::vector<CoulombViscousStribeckParameters> coulombViscousStribeckParameters;
     std::vector<std::unique_ptr<PINNFrictionEstimator>> frictionEstimators;
+    BipedalLocomotion::ContinuousDynamicalSystem::ButterworthLowPassFilter lowPassFilter;
     std::mutex mutexTorqueControlParam_; /**< The mutex for protecting the parameters of the torque control. */
     yarp::sig::Vector desiredJointTorques;
     yarp::sig::Vector desiredMotorCurrents;
@@ -172,6 +184,7 @@ class BipedalLocomotion::JointTorqueControlDevice
     std::vector<double> m_motorPositionCorrected;
     std::vector<double> m_motorPositionsRadians;
     std::vector<std::string> m_axisNames;
+    LowPassFilterParameters m_lowPassFilterParameters;
 
     yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */
 
@@ -187,7 +200,7 @@ class BipedalLocomotion::JointTorqueControlDevice
     struct Status
     {
         std::mutex mutex;
-        std::vector<double> m_torqueLogging;
+        std::vector<double> m_motorPositionError;
         std::vector<double> m_frictionLogging;
         std::vector<double> m_currentLogging;
     } m_status;
@@ -273,10 +286,12 @@ class BipedalLocomotion::JointTorqueControlDevice
     virtual void run();
     virtual void threadRelease();
 
-    virtual bool setKpJtcvc(const std::string& jointName, const double kp);
+    virtual bool setKpJtcvc(const std::string& jointName, const double kp) override;
     virtual double getKpJtcvc(const std::string& jointName) override;
     virtual bool setKfcJtcvc(const std::string& jointName, const double kfc) override;
     virtual double getKfcJtcvc(const std::string& jointName) override;
+    virtual bool setMaxFrictionTorque(const std::string& jointName, const double maxFriction) override;
+    virtual double getMaxFrictionTorque(const std::string& jointName) override;
     virtual bool setFrictionModel(const std::string& jointName, const std::string& model) override;
     virtual std::string getFrictionModel(const std::string& jointName) override;
 };

devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp

@@ -189,6 +189,52 @@ double JointTorqueControlDevice::getKfcJtcvc(const std::string& jointName)
     return -1;
 }
 
+bool JointTorqueControlDevice::setMaxFrictionTorque(const std::string& jointName, const double maxFriction)
+{
+    // Use std::find to locate the jointName in m_axisNames
+    auto it = std::find(m_axisNames.begin(), m_axisNames.end(), jointName);
+
+    // If jointName is found
+    if (it != m_axisNames.end())
+    {
+        // Calculate the index of the found element
+        size_t index = std::distance(m_axisNames.begin(), it);
+
+        // Lock the mutex to safely modify motorTorqueCurrentParameters
+        std::lock_guard<std::mutex> lock(mutexTorqueControlParam_);
+
+        // Update the maxOutputFriction value
+        motorTorqueCurrentParameters[index].maxOutputFriction = maxFriction;
+
+        return true;
+    }
+
+    // jointName was not found
+    return false;
+}
+
+double JointTorqueControlDevice::getMaxFrictionTorque(const std::string& jointName)
+{
+    // Use std::find to locate the jointName in m_axisNames
+    auto it = std::find(m_axisNames.begin(), m_axisNames.end(), jointName);
+
+    // If jointName is found
+    if (it != m_axisNames.end())
+    {
+        // Calculate the index of the found element
+        size_t index = std::distance(m_axisNames.begin(), it);
+
+        // Lock the mutex to safely access motorTorqueCurrentParameters
+        std::lock_guard<std::mutex> lock(mutexTorqueControlParam_);
+
+        // Return the maxOutputFriction value
+        return motorTorqueCurrentParameters[index].maxOutputFriction;
+    }
+
+    // jointName was not found, return default value
+    return -1;
+}
+
 bool JointTorqueControlDevice::setFrictionModel(const std::string& jointName,
                                                 const std::string& model)
 {
@@ -330,6 +376,10 @@ double JointTorqueControlDevice::computeFrictionTorque(int joint)
         }
     }
 
+    frictionTorque = saturation(frictionTorque,
+                                motorTorqueCurrentParameters[joint].maxOutputFriction,
+                                -motorTorqueCurrentParameters[joint].maxOutputFriction);
+
     return frictionTorque;
 }
 
@@ -340,16 +390,41 @@ void JointTorqueControlDevice::computeDesiredCurrents()
 
     estimatedFrictionTorques.zero();
 
+    std::lock_guard<std::mutex> lock(mutexTorqueControlParam_);
+
     for (int j = 0; j < this->axes; j++)
     {
-        std::lock_guard<std::mutex> lock(mutexTorqueControlParam_);
-
         if (this->hijackingTorqueControl[j])
         {
             if (motorTorqueCurrentParameters[j].kfc > 0.0)
             {
                 estimatedFrictionTorques[j] = computeFrictionTorque(j);
             }
+        }
+    }
+
+    if (m_lowPassFilterParameters.enabled)
+    {
+        if (!lowPassFilter.setInput(yarp::eigen::toEigen(estimatedFrictionTorques)))
+        {
+            log()->error("Error in setting the input of the low pass filter");
+        }
+
+        if (!lowPassFilter.advance())
+        {
+            log()->error("Error in advancing the low pass filter");
+        }
+
+        for (int idx = 0; idx < estimatedFrictionTorques.size(); idx++)
+        {
+            estimatedFrictionTorques[idx] = lowPassFilter.getOutput()[idx];
+        }
+    }
+
+    for (int j = 0; j < this->axes; j++)
+    {
+        if (this->hijackingTorqueControl[j])
+        {
 
             desiredMotorCurrents[j]
                 = (desiredJointTorques[j]
@@ -367,7 +442,7 @@ void JointTorqueControlDevice::computeDesiredCurrents()
             {
                 std::lock_guard<std::mutex> lockOutput(m_status.mutex);
                 m_status.m_frictionLogging[j] = estimatedFrictionTorques[j];
-                m_status.m_torqueLogging[j] = desiredJointTorques[j];
+                m_status.m_motorPositionError[j] = m_motorPositionError[j];
                 m_status.m_currentLogging[j] = desiredMotorCurrents[j];
             }
         }
@@ -727,6 +802,33 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config)
         return false;
     }
 
+    if (!torqueGroup->getParameter("bwfilter_cutoff_freq", m_lowPassFilterParameters.cutoffFrequency))
+    {
+        log()->error("{} Parameter `bwfilter_cutoff_freq` not found", logPrefix);
+        return false;
+    }
+
+    if (!torqueGroup->getParameter("bwfilter_order", m_lowPassFilterParameters.order))
+    {
+        log()->error("{} Parameter `bwfilter_order` not found", logPrefix);
+        return false;
+    }
+
+    if (!torqueGroup->getParameter("bwfilter_enabled", m_lowPassFilterParameters.enabled))
+    {
+        log()->error("{} Parameter `bwfilter_enabled` not found", logPrefix);
+        return false;
+    }
+
+    m_lowPassFilterParameters.samplingTime = rate * 0.001;
+
+    std::vector<double> maxOutputFriction;
+    if (!torqueGroup->getParameter("max_output_friction", maxOutputFriction))
+    {
+        log()->error("{} Parameter `max_output_friction` not found", logPrefix);
+        return false;
+    }
+
     motorTorqueCurrentParameters.resize(kt.size());
     pinnParameters.resize(kt.size());
     coulombViscousParameters.resize(kt.size());
@@ -739,6 +841,19 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config)
         motorTorqueCurrentParameters[i].kp = kp[i];
         motorTorqueCurrentParameters[i].maxCurr = maxCurr[i];
         motorTorqueCurrentParameters[i].frictionModel = frictionModels[i];
+        motorTorqueCurrentParameters[i].maxOutputFriction = maxOutputFriction[i];
+    }
+
+    auto filterParams = std::make_shared<ParametersHandler::YarpImplementation>();
+    filterParams->setParameter("cutoff_frequency", m_lowPassFilterParameters.cutoffFrequency);
+    filterParams->setParameter("order", m_lowPassFilterParameters.order);
+    filterParams->setParameter("sampling_time", m_lowPassFilterParameters.samplingTime);
+    if (m_lowPassFilterParameters.enabled)
+    {
+        lowPassFilter.initialize(filterParams);
+        Eigen::VectorXd initialFrictionTorque(kt.size());
+        initialFrictionTorque.setZero();
+        lowPassFilter.reset(initialFrictionTorque);
     }
 
     if (!this->loadFrictionParams(params))
@@ -793,7 +908,7 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config)
     }
 
     m_vectorsCollectionServer.populateMetadata("motor_currents::desired", joint_list);
-    m_vectorsCollectionServer.populateMetadata("joint_torques::desired", joint_list);
+    m_vectorsCollectionServer.populateMetadata("position_error::input_network", joint_list);
     m_vectorsCollectionServer.populateMetadata("friction_torques::estimated", joint_list);
     m_vectorsCollectionServer.finalizeMetadata();
 
@@ -835,8 +950,8 @@ void JointTorqueControlDevice::publishStatus()
             std::lock_guard<std::mutex> lockOutput(m_status.mutex);
             m_vectorsCollectionServer.populateData("motor_currents::desired",
                                                    m_status.m_currentLogging);
-            m_vectorsCollectionServer.populateData("joint_torques::desired",
-                                                   m_status.m_torqueLogging);
+            m_vectorsCollectionServer.populateData("position_error::input_network",
+                                                   m_status.m_motorPositionError);
             m_vectorsCollectionServer.populateData("friction_torques::estimated",
                                                    m_status.m_frictionLogging);
             m_vectorsCollectionServer.sendData();
@@ -885,7 +1000,7 @@ bool JointTorqueControlDevice::attachAll(const PolyDriverList& p)
         m_motorPositionError.resize(axes, 1);
         m_motorPositionCorrected.resize(axes, 1);
         m_motorPositionsRadians.resize(axes, 1);
-        m_status.m_torqueLogging.resize(axes, 1);
+        m_status.m_motorPositionError.resize(axes, 1);
         m_status.m_frictionLogging.resize(axes, 1);
         m_status.m_currentLogging.resize(axes, 1);
     }

devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift

@@ -1,5 +1,5 @@
 /**
- * @file walkingCommands.thrift
+ * @file jointTorqueControlCommands.thrift
  * @authors Ines Sorrentino <ines.sorrentino@iit.it>
  * @copyright 2024 iCub Facility - Istituto Italiano di Tecnologia
  *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
@@ -16,6 +16,10 @@ service JointTorqueControlCommands
 
     double getKfcJtcvc(1:string jointName);
 
+    bool setMaxFrictionTorque(1:string jointName, 2:double maxFriction);
+
+    double getMaxFrictionTorque(1:string jointName);
+
     bool setFrictionModel(1:string jointName, 2:string model);
 
     string getFrictionModel(1:string jointName);