diff --git a/CHANGELOG.md b/CHANGELOG.md index 2cdbb676bc..f210df17c3 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/devices/JointTorqueControlDevice/CMakeLists.txt b/devices/JointTorqueControlDevice/CMakeLists.txt index a67078e625..808fc99672 100644 --- a/devices/JointTorqueControlDevice/CMakeLists.txt +++ b/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 diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h index ba50327c78..97bef3b720 100644 --- a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h +++ b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -66,6 +67,7 @@ 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 @@ -73,6 +75,7 @@ struct MotorTorqueCurrentParameters 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; std::vector coulombViscousStribeckParameters; std::vector> 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 m_motorPositionCorrected; std::vector m_motorPositionsRadians; std::vector 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 m_torqueLogging; + std::vector m_motorPositionError; std::vector m_frictionLogging; std::vector 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; }; diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp index 315320081c..991d8491ec 100644 --- a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp +++ b/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 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 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 lock(mutexTorqueControlParam_); + for (int j = 0; j < this->axes; j++) { - std::lock_guard 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 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 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(); + 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 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); } diff --git a/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift b/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift index 0e6bca5fc9..869bbbc9c7 100644 --- a/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift +++ b/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift @@ -1,5 +1,5 @@ /** - * @file walkingCommands.thrift + * @file jointTorqueControlCommands.thrift * @authors Ines Sorrentino * @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);