diff --git a/CHANGELOG.md b/CHANGELOG.md index 8bd25653cb..f26bbe71ff 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,7 @@ All notable changes to this project are documented in this file. - Implement `velMANNAutoregressive`, `velMANNAutoregressiveInputBuilder`, and `velMANNTrajectoryGenerator` to generate trajectories using MANN model with velocity-based features in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/889) ### Changed +- Change device jtcvc to use motor velocity and joint velocity in input to PINN models (https://github.com/ami-iit/bipedal-locomotion-framework/pull/903) ### Fixed - Bug fix of `JointTorqueControlDevice` device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/890) diff --git a/devices/JointTorqueControlDevice/CMakeLists.txt b/devices/JointTorqueControlDevice/CMakeLists.txt index 808fc99672..aeb6ee7e29 100644 --- a/devices/JointTorqueControlDevice/CMakeLists.txt +++ b/devices/JointTorqueControlDevice/CMakeLists.txt @@ -33,4 +33,3 @@ if(FRAMEWORK_COMPILE_JointTorqueControlDevice) ${iDynTree_LIBRARIES} CONFIGURE_PACKAGE_NAME joint_torque_control_device) endif() - diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h index 97bef3b720..6e3bf6aa92 100644 --- a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h +++ b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h @@ -65,6 +65,7 @@ struct MotorTorqueCurrentParameters double kt; /**< motor torque to current gain */ double kfc; /**< friction compensation weight parameter */ double kp; /**< proportional gain */ + double ki; /**< integral gain */ double maxCurr; /**< maximum current */ std::string frictionModel; ///< friction model double maxOutputFriction; /**< maximum output of the friction model */ @@ -176,22 +177,15 @@ class BipedalLocomotion::JointTorqueControlDevice yarp::sig::Vector measuredJointPositions; yarp::sig::Vector measuredMotorPositions; yarp::sig::Vector estimatedFrictionTorques; + yarp::sig::Vector torqueIntegralErrors; std::string m_portPrefix{"/hijackingTrqCrl"}; /**< Default port prefix. */ BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorsCollectionServer; /**< Logger server. */ std::vector m_gearRatios; - std::vector m_initialDeltaMotorJointRadians; - std::vector m_motorPositionError; - std::vector m_motorPositionCorrected; - std::vector m_motorPositionsRadians; std::vector m_axisNames; LowPassFilterParameters m_lowPassFilterParameters; yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ - double m_tempJointPosRad = 0.0; - double m_tempJointPosMotorSideRad = 0.0; - double m_jointVelRadSec = 0.0; - CouplingMatrices couplingMatrices; bool m_torqueControlIsRunning{false}; /**< True if the estimator is running. */ @@ -200,7 +194,6 @@ class BipedalLocomotion::JointTorqueControlDevice struct Status { std::mutex mutex; - std::vector m_motorPositionError; std::vector m_frictionLogging; std::vector m_currentLogging; } m_status; @@ -288,13 +281,18 @@ class BipedalLocomotion::JointTorqueControlDevice virtual bool setKpJtcvc(const std::string& jointName, const double kp) override; virtual double getKpJtcvc(const std::string& jointName) override; + virtual bool setKiJtcvc(const std::string& jointName, const double ki) override; + virtual double getKiJtcvc(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; + virtual bool setPINNModel(const std::string& jointName, const std::string& pinnModelName) override; + virtual std::string getPINNModel(const std::string& jointName) override; + virtual bool setKtJtcvc(const std::string& jointName, const double kt) override; + virtual double getKtJtcvc(const std::string& jointName) override; }; #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_JOINT_TORQUE_CONTROL_DEVICE_H - diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/PINNFrictionEstimator.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/PINNFrictionEstimator.h index 2833dab61d..3b12c22eba 100644 --- a/devices/JointTorqueControlDevice/include/BipedalLocomotion/PINNFrictionEstimator.h +++ b/devices/JointTorqueControlDevice/include/BipedalLocomotion/PINNFrictionEstimator.h @@ -47,12 +47,14 @@ class PINNFrictionEstimator /** * Estimate the joint friction starting from raw data - * @param[in] inputDeltaPosition a double representing difference between the joint position and the motor position motor side (rad) + * @param[in] inputMotorVelocity a double representing the motor velocity (rad/sec) * @param[in] inputJointVelocity a double representing the joint velocity (rad/sec) * @param[out] output a double representing the joint friction torque * @return true if the estimation is successful, false otherwise */ - bool estimate(double inputDeltaPosition, double inputJointVelocity, double& output); + bool estimate(double inputMotorVelocity, + double inputJointVelocity, + double& output); private: @@ -61,4 +63,3 @@ class PINNFrictionEstimator }; #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_PINN_FRICTION_ESTIMATOR_H - diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp index 991d8491ec..fb16168348 100644 --- a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp +++ b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp @@ -143,6 +143,54 @@ double JointTorqueControlDevice::getKpJtcvc(const std::string& jointName) return -1; } +bool JointTorqueControlDevice::setKiJtcvc(const std::string& jointName, const double ki) +{ + auto it = std::find(m_axisNames.begin(), m_axisNames.end(), jointName); + + if (it != m_axisNames.end()) + { + // Calculate the index of the found element + std::size_t index = std::distance(m_axisNames.begin(), it); + + { + std::lock_guard lock(mutexTorqueControlParam_); + motorTorqueCurrentParameters[index].ki = ki; + + log()->info("Request for ki des = {}", ki); + log()->info("Setting value ki = {}", motorTorqueCurrentParameters[index].ki); + } + + return true; + } + + return false; +} + +double JointTorqueControlDevice::getKiJtcvc(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_); + + // Log the ki value + log()->info("ki value is {}", motorTorqueCurrentParameters[index].ki); + + // Return the ki value + return motorTorqueCurrentParameters[index].ki; + } + + // jointName was not found, return default value + return -1; +} + bool JointTorqueControlDevice::setKfcJtcvc(const std::string& jointName, const double kfc) { // Use std::find to locate the jointName in m_axisNames @@ -282,6 +330,106 @@ std::string JointTorqueControlDevice::getFrictionModel(const std::string& jointN return model; } +bool JointTorqueControlDevice::setPINNModel(const std::string& jointName, + const std::string& pinnModelName) +{ + 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); + + pinnParameters[index].modelPath = pinnModelName; + + // Lock the mutex to safely modify motorTorqueCurrentParameters + std::lock_guard lock(mutexTorqueControlParam_); + + if (!frictionEstimators[index]->initialize(pinnParameters[index].modelPath, + pinnParameters[index].threadNumber, + pinnParameters[index].threadNumber)) + { + log()->error("[JointTorqueControlDevice::setPINNModel] Failed to re-initialize friction estimator with model {}", pinnModelName); + return false; + } + + return true; + } + + log()->error("[JointTorqueControlDevice::setPINNModel] Failed to set the PINN model {}. The joint {} is not found.", pinnModelName, jointName); + return true; +} + +std::string JointTorqueControlDevice::getPINNModel(const std::string& jointName) +{ + std::string pinnModelName = "none"; + + size_t index = 0; + + do + { + if (m_axisNames[index] == jointName) + { + std::lock_guard lock(mutexTorqueControlParam_); + + return pinnParameters[index].modelPath; + } + + index++; + } while (index < m_axisNames.size()); + + return pinnModelName; +} + +bool JointTorqueControlDevice::setKtJtcvc(const std::string& jointName, const double kt) +{ + auto it = std::find(m_axisNames.begin(), m_axisNames.end(), jointName); + + if (it != m_axisNames.end()) + { + // Calculate the index of the found element + std::size_t index = std::distance(m_axisNames.begin(), it); + + { + std::lock_guard lock(mutexTorqueControlParam_); + motorTorqueCurrentParameters[index].kt = kt; + + log()->info("Request for kt des = {}", kt); + log()->info("Setting value kt = {}", motorTorqueCurrentParameters[index].kt); + } + + return true; + } + + return false; +} + +double JointTorqueControlDevice::getKtJtcvc(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_); + + // Log the kp value + log()->info("kt value is {}", motorTorqueCurrentParameters[index].kt); + + // Return the kp value + return motorTorqueCurrentParameters[index].kt; + } + + // jointName was not found, return default value + return -1; +} + // HIJACKING CONTROL void JointTorqueControlDevice::startHijackingTorqueControlIfNecessary(int j) { @@ -296,12 +444,6 @@ void JointTorqueControlDevice::startHijackingTorqueControlIfNecessary(int j) frictionEstimators[j]->resetEstimator(); this->readStatus(); - for (int i = 0; i < axes; i++) - { - m_initialDeltaMotorJointRadians[i] - = (measuredMotorPositions[i] - m_gearRatios[i] * measuredJointPositions[i]) - * M_PI / 180.0; - } } this->hijackingTorqueControl[j] = true; @@ -359,17 +501,9 @@ double JointTorqueControlDevice::computeFrictionTorque(int joint) frictionTorque = tauCoulomb + tauViscous + tauStribeck; } else if (motorTorqueCurrentParameters[joint].frictionModel == "FRICTION_PINN") { - m_tempJointPosRad = measuredJointPositions[joint] * M_PI / 180.0; - m_tempJointPosMotorSideRad = m_gearRatios[joint] * m_tempJointPosRad; - m_motorPositionsRadians[joint] = measuredMotorPositions[joint] * M_PI / 180.0; - m_motorPositionCorrected[joint] - = m_motorPositionsRadians[joint] - m_initialDeltaMotorJointRadians[joint]; - m_jointVelRadSec = measuredJointVelocities[joint] * M_PI / 180.0; - m_motorPositionError[joint] = m_tempJointPosMotorSideRad - m_motorPositionCorrected[joint]; - // Test network with inputs position error motor side, joint velocity - if (!frictionEstimators[joint]->estimate(m_motorPositionError[joint], - m_jointVelRadSec, + if (!frictionEstimators[joint]->estimate(measuredMotorVelocities[joint] * M_PI / 180.0, + measuredJointVelocities[joint] * M_PI / 180.0, frictionTorque)) { frictionTorque = 0.0; @@ -425,11 +559,14 @@ void JointTorqueControlDevice::computeDesiredCurrents() { if (this->hijackingTorqueControl[j]) { + torqueIntegralErrors[j] + += (desiredJointTorques[j] - measuredJointTorques[j]) * this->getPeriod(); desiredMotorCurrents[j] = (desiredJointTorques[j] + motorTorqueCurrentParameters[j].kp * (desiredJointTorques[j] - measuredJointTorques[j]) + + motorTorqueCurrentParameters[j].ki * torqueIntegralErrors[j] + motorTorqueCurrentParameters[j].kfc * estimatedFrictionTorques[j]) / motorTorqueCurrentParameters[j].kt; @@ -442,7 +579,6 @@ void JointTorqueControlDevice::computeDesiredCurrents() { std::lock_guard lockOutput(m_status.mutex); m_status.m_frictionLogging[j] = estimatedFrictionTorques[j]; - m_status.m_motorPositionError[j] = m_motorPositionError[j]; m_status.m_currentLogging[j] = desiredMotorCurrents[j]; } } @@ -788,6 +924,12 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config) log()->error("{} Parameter `kp` not found", logPrefix); return false; } + Eigen::VectorXd ki; + if (!torqueGroup->getParameter("ki", ki)) + { + log()->error("{} Parameter `ki` not found", logPrefix); + return false; + } Eigen::VectorXd maxCurr; if (!torqueGroup->getParameter("max_curr", maxCurr)) { @@ -839,6 +981,7 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config) motorTorqueCurrentParameters[i].kt = kt[i]; motorTorqueCurrentParameters[i].kfc = kfc[i]; motorTorqueCurrentParameters[i].kp = kp[i]; + motorTorqueCurrentParameters[i].ki = ki[i]; motorTorqueCurrentParameters[i].maxCurr = maxCurr[i]; motorTorqueCurrentParameters[i].frictionModel = frictionModels[i]; motorTorqueCurrentParameters[i].maxOutputFriction = maxOutputFriction[i]; @@ -862,8 +1005,6 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config) return false; } - int threadNumber = 1; - for (int i = 0; i < kt.size(); i++) { if (motorTorqueCurrentParameters[i].frictionModel == "FRICTION_PINN") @@ -871,8 +1012,8 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config) frictionEstimators[i] = std::make_unique(); if (!frictionEstimators[i]->initialize(pinnParameters[i].modelPath, - threadNumber, - threadNumber)) + pinnParameters[i].threadNumber, + pinnParameters[i].threadNumber)) { log()->error("{} Failed to initialize friction estimator", logPrefix); return false; @@ -950,8 +1091,6 @@ void JointTorqueControlDevice::publishStatus() std::lock_guard lockOutput(m_status.mutex); m_vectorsCollectionServer.populateData("motor_currents::desired", m_status.m_currentLogging); - m_vectorsCollectionServer.populateData("position_error::input_network", - m_status.m_motorPositionError); m_vectorsCollectionServer.populateData("friction_torques::estimated", m_status.m_frictionLogging); m_vectorsCollectionServer.sendData(); @@ -991,16 +1130,12 @@ bool JointTorqueControlDevice::attachAll(const PolyDriverList& p) measuredJointVelocities.resize(axes, 0.0); measuredMotorVelocities.resize(axes, 0.0); measuredJointTorques.resize(axes, 0.0); + torqueIntegralErrors.resize(axes, 0.0); measuredJointPositions.resize(axes, 0.0); measuredMotorPositions.resize(axes, 0.0); estimatedFrictionTorques.resize(axes, 0.0); m_gearRatios.resize(axes, 1); m_axisNames.resize(axes); - m_initialDeltaMotorJointRadians.resize(axes, 1); - m_motorPositionError.resize(axes, 1); - m_motorPositionCorrected.resize(axes, 1); - m_motorPositionsRadians.resize(axes, 1); - m_status.m_motorPositionError.resize(axes, 1); m_status.m_frictionLogging.resize(axes, 1); m_status.m_currentLogging.resize(axes, 1); } @@ -1309,4 +1444,3 @@ void JointTorqueControlDevice::controlLoop() timeOfLastControlLoop = BipedalLocomotion::clock().now();; } - diff --git a/devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp b/devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp index 4fc15c1a34..55077a09dd 100644 --- a/devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp +++ b/devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp @@ -29,7 +29,7 @@ struct PINNFrictionEstimator::Impl Ort::MemoryInfo memoryInfo; std::deque jointVelocityBuffer; - std::deque errorPositionBuffer; + std::deque motorVelocityBuffer; size_t historyLength; @@ -58,8 +58,8 @@ PINNFrictionEstimator::PINNFrictionEstimator() PINNFrictionEstimator::~PINNFrictionEstimator() = default; bool PINNFrictionEstimator::initialize(const std::string& networkModelPath, - const std::size_t intraOpNumThreads, - const std::size_t interOpNumThreads) + const std::size_t intraOpNumThreads, + const std::size_t interOpNumThreads) { std::basic_string networkModelPathAsOrtString(networkModelPath.begin(), networkModelPath.end()); @@ -80,7 +80,6 @@ bool PINNFrictionEstimator::initialize(const std::string& networkModelPath, networkModelPathAsOrtString.c_str(), sessionOptions); - if (m_pimpl->session == nullptr) { BipedalLocomotion::log()->error("Unable to load the model from the file: {}", networkModelPath); @@ -91,15 +90,18 @@ bool PINNFrictionEstimator::initialize(const std::string& networkModelPath, std::vector inputShape = m_pimpl->session->GetInputTypeInfo(0).GetTensorTypeAndShapeInfo().GetShape(); const std::size_t inputCount = inputShape[1]; - m_pimpl->historyLength = inputCount / 2; + + int numberOfInputs = 2; + + m_pimpl->historyLength = inputCount / numberOfInputs; // format the input - m_pimpl->structuredInput.rawData.resize(m_pimpl->historyLength * 2); + m_pimpl->structuredInput.rawData.resize(inputCount); m_pimpl->structuredInput.shape[0] = 1; // batch - m_pimpl->structuredInput.shape[1] = m_pimpl->historyLength * 2; + m_pimpl->structuredInput.shape[1] = inputCount; m_pimpl->jointVelocityBuffer.resize(m_pimpl->historyLength); - m_pimpl->errorPositionBuffer.resize(m_pimpl->historyLength); + m_pimpl->motorVelocityBuffer.resize(m_pimpl->historyLength); // create tensor required by onnx m_pimpl->structuredInput.tensor @@ -130,27 +132,27 @@ bool PINNFrictionEstimator::initialize(const std::string& networkModelPath, void PINNFrictionEstimator::resetEstimator() { + m_pimpl->motorVelocityBuffer.clear(); m_pimpl->jointVelocityBuffer.clear(); - m_pimpl->errorPositionBuffer.clear(); } -bool PINNFrictionEstimator::estimate(double inputDeltaPosition, - double inputJointVelocity, - double& output) +bool PINNFrictionEstimator::estimate(double inputMotorVelocity, + double inputJointVelocity, + double& output) { - if (m_pimpl->errorPositionBuffer.size() == m_pimpl->historyLength) + if (m_pimpl->motorVelocityBuffer.size() == m_pimpl->historyLength) { // The buffer is full, remove the oldest element - m_pimpl->errorPositionBuffer.pop_front(); + m_pimpl->motorVelocityBuffer.pop_front(); m_pimpl->jointVelocityBuffer.pop_front(); } // Push element into the queue - m_pimpl->errorPositionBuffer.push_back(inputDeltaPosition); + m_pimpl->motorVelocityBuffer.push_back(inputMotorVelocity); m_pimpl->jointVelocityBuffer.push_back(inputJointVelocity); // Check if the buffer is full - if (m_pimpl->errorPositionBuffer.size() < m_pimpl->historyLength) + if (m_pimpl->motorVelocityBuffer.size() < m_pimpl->historyLength) { // The buffer is not full yet return false; @@ -160,10 +162,10 @@ bool PINNFrictionEstimator::estimate(double inputDeltaPosition, // Copy the joint positions and then the motor positions in the // structured input without emptying the buffer // Use iterators to copy the data to the vector - std::copy(m_pimpl->errorPositionBuffer.cbegin(), - m_pimpl->errorPositionBuffer.cend(), + std::copy(m_pimpl->motorVelocityBuffer.cbegin(), + m_pimpl->motorVelocityBuffer.cend(), m_pimpl->structuredInput.rawData.begin()); - std::copy(m_pimpl->jointVelocityBuffer.cbegin(), + std::copy(m_pimpl->jointVelocityBuffer.cbegin(), m_pimpl->jointVelocityBuffer.cend(), m_pimpl->structuredInput.rawData.begin() + m_pimpl->historyLength); diff --git a/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift b/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift index 869bbbc9c7..a7f5ff4026 100644 --- a/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift +++ b/devices/JointTorqueControlDevice/thrifts/JointTorqueControlCommands.thrift @@ -23,5 +23,16 @@ service JointTorqueControlCommands bool setFrictionModel(1:string jointName, 2:string model); string getFrictionModel(1:string jointName); -} + bool setPINNModel(1:string jointName, 2:string pinnModelName); + + string getPINNModel(1:string jointName); + + bool setKtJtcvc(1:string jointName, 2:double kt); + + double getKtJtcvc(1:string jointName); + + bool setKiJtcvc(1:string jointName, 2:double ki); + + double getKiJtcvc(1:string jointName); +}