From f93b70208feadd4db44c684e01572c75f924e4ef Mon Sep 17 00:00:00 2001 From: icub Date: Mon, 11 Nov 2024 12:32:37 +0100 Subject: [PATCH] Add KF in JoinTorqueControlDevice to estimate joint and motor velocities. --- .../JointTorqueControlDevice.h | 22 ++ .../src/JointTorqueControlDevice.cpp | 292 ++++++++++++++++-- 2 files changed, 296 insertions(+), 18 deletions(-) diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h index a2b2c39c1e..662a1a37d3 100644 --- a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h +++ b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h @@ -27,6 +27,8 @@ #include +#include + #include @@ -183,6 +185,13 @@ class BipedalLocomotion::JointTorqueControlDevice std::vector m_gearRatios; std::vector m_axisNames; LowPassFilterParameters m_lowPassFilterParameters; + + iDynTree::VectorDynSize m_measurementKF; + iDynTree::VectorDynSize m_estimateKF; + std::vector m_KFJointList; + std::vector m_KFMotorList; + bool m_estimateJointVelocity = false; + bool m_estimateMotorVelocity = false; yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ @@ -200,6 +209,15 @@ class BipedalLocomotion::JointTorqueControlDevice bool openCalledCorrectly{false}; + /** + * Construct the Kalman filters + * @param paramHandler pointer to the parameter handler + * @param kfList reference to the Kalman filter helper + * @return true if the Kalman filters have been constructed successfully, false otherwise + */ + bool constructKalmanFilters(std::weak_ptr paramHandler, + std::vector & kfList); + // HIJACKING CONTROL /** * vector of getAxes() size. @@ -275,6 +293,10 @@ class BipedalLocomotion::JointTorqueControlDevice virtual bool setRefTorque(int j, double t); // HACK MOTOR ACCELERATION TO PUBLISH FRICTION TORQUES + virtual bool getEncoderSpeed(int j, double* sp); + virtual bool getEncoderSpeeds(double* spds); + virtual bool getMotorEncoderSpeed(int m, double* sp); + virtual bool getMotorEncoderSpeeds(double* spds); virtual bool getMotorEncoderAcceleration(int j, double* acc); virtual bool getMotorEncoderAccelerations(double* accs); diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp index ae81c9486e..81ac8bc28b 100644 --- a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp +++ b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp @@ -606,24 +606,6 @@ void JointTorqueControlDevice::readStatus() { auto logPrefix = "[JointTorqueControlDevice::readStatus]"; - if (!this->PassThroughControlBoard::getEncoderSpeeds(measuredJointVelocities.data())) - { - 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())) - { - log()->error("{} Failed to get Motor encoders speed", logPrefix); - } else - { - yarp::eigen::toEigen(measuredMotorVelocities) - = couplingMatrices.fromJointVelocitiesToMotorVelocities - * yarp::eigen::toEigen(measuredJointVelocities); - } - } if (!this->PassThroughControlBoard::getTorques(measuredJointTorques.data())) { log()->error("{} Failed to get joint torque", logPrefix); @@ -636,6 +618,74 @@ void JointTorqueControlDevice::readStatus() { log()->error("{} Failed to get motor position", logPrefix); } + + if (m_estimateJointVelocity) + { + for (size_t i = 0; i < measuredJointPositions.size(); ++i) + { + if (!m_KFJointList[i].kfPredict()) + { + log()->error("{} Failed to predict the Kalman filter", logPrefix); + } + m_measurementKF(0) = measuredJointPositions[i]; + if (!m_KFJointList[i].kfSetMeasurementVector(m_measurementKF)) + { + log()->error("{} Failed to set measurement vector", logPrefix); + } + if (!m_KFJointList[i].kfUpdate()) + { + log()->error("{} Failed to update the Kalman filter", logPrefix); + } + m_KFJointList[i].kfGetStates(m_estimateKF); + measuredJointVelocities[i] = m_estimateKF(1); + } + } + else + { + if (!this->PassThroughControlBoard::getEncoderSpeeds(measuredJointVelocities.data())) + { + log()->error("{} Failed to get Motor encoders speed", logPrefix); + } + } + + if (m_estimateMotorVelocity) + { + for (size_t i = 0; i < measuredMotorPositions.size(); ++i) + { + if (!m_KFMotorList[i].kfPredict()) + { + log()->error("{} Failed to predict the Kalman filter", logPrefix); + } + m_measurementKF(0) = measuredMotorPositions[i]; + if (!m_KFMotorList[i].kfSetMeasurementVector(m_measurementKF)) + { + log()->error("{} Failed to set measurement vector", logPrefix); + } + if (!m_KFMotorList[i].kfUpdate()) + { + log()->error("{} Failed to update the Kalman filter", logPrefix); + } + m_KFMotorList[i].kfGetStates(m_estimateKF); + measuredMotorVelocities[i] = m_estimateKF(1); + } + } + else + { + 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())) + { + log()->error("{} Failed to get Motor encoders speed", logPrefix); + } else + { + yarp::eigen::toEigen(measuredMotorVelocities) + = couplingMatrices.fromJointVelocitiesToMotorVelocities + * yarp::eigen::toEigen(measuredJointVelocities); + } + } + } } bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config, @@ -899,6 +949,46 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config) } this->setPeriod(rate * 0.001); + // Read group KF_JOINT + auto kfJointGroup = params->getGroup("KF_JOINT").lock(); + if (kfJointGroup == nullptr) + { + log()->warn("{} Group `KF_JOINT` not found in configuration.", logPrefix); + } + else + { + // Create KF for joint velocity estimation + if (!constructKalmanFilters(kfJointGroup, m_KFJointList)) + { + log()->warn("{} Failed to construct Kalman filter", logPrefix); + } + else + { + log()->info("{} Kalman filter for joint velocity estimation created", logPrefix); + m_estimateJointVelocity = true; + } + } + + // Read group KF_MOTOR + auto kfMotorGroup = params->getGroup("KF_MOTOR").lock(); + if (kfMotorGroup == nullptr) + { + log()->warn("{} Group `KF_MOTOR` not found in configuration.", logPrefix); + } + else + { + // Create KF for motor velocity estimation + if (!constructKalmanFilters(kfMotorGroup, m_KFMotorList)) + { + log()->warn("{} Failed to construct Kalman filter", logPrefix); + } + else + { + log()->info("{} Kalman filter for motor velocity estimation created", logPrefix); + m_estimateMotorVelocity = true; + } + } + auto torqueGroup = params->getGroup("TORQUE_CURRENT_PARAMS").lock(); if (torqueGroup == nullptr) { @@ -1104,6 +1194,135 @@ void JointTorqueControlDevice::publishStatus() } } +bool JointTorqueControlDevice::constructKalmanFilters(std::weak_ptr paramHandler, + std::vector & kfList) +{ + auto kfGroup = paramHandler.lock(); + if (kfGroup == nullptr) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Invalid parameter handler"); + return false; + } + + Eigen::VectorXd q0; + if (!kfGroup->getParameter("Q0", q0)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Parameter `Q0` not found"); + return false; + } + + Eigen::VectorXd q1; + if (!kfGroup->getParameter("Q1", q1)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Parameter `Q1` not found"); + return false; + } + + Eigen::VectorXd q2; + if (!kfGroup->getParameter("Q2", q2)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Parameter `Q2` not found"); + return false; + } + + Eigen::VectorXd r; + if (!kfGroup->getParameter("R", r)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Parameter `R` not found"); + return false; + } + + Eigen::VectorXd p0; + if (!kfGroup->getParameter("P0", p0)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Parameter `P0` not found"); + return false; + } + + int numJoints = r.size(); + double dt = this->getPeriod(); + + kfList.resize(numJoints); + + iDynTree::MatrixDynSize A_idyn; + A_idyn.resize(3, 3); + A_idyn.zero(); + // Fill the diagonal blocks for A_total_idyn + A_idyn(0, 0) = 1; + A_idyn(0, 1) = dt; + A_idyn(1, 1) = 1; + A_idyn(1, 2) = dt; + A_idyn(2, 2) = 1; + + // Construct the H_total_idyn matrix + iDynTree::MatrixDynSize H_idyn; + H_idyn.resize(1, 3); + H_idyn.zero(); + H_idyn(0, 0) = 1; + + for (int joint = 0; joint < numJoints; ++joint) + { + if (!kfList[joint].constructKalmanFilter(A_idyn, H_idyn)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Failed to construct Kalman filter"); + return false; + } + + // Construct the Q matrix (system noise covariance) + iDynTree::MatrixDynSize Q_idyn; + Q_idyn.resize(3, 3); + Q_idyn.zero(); + Q_idyn(0, 0) = q0(joint); + Q_idyn(1, 1) = q1(joint); + Q_idyn(2, 2) = q2(joint); + if (!kfList[joint].kfSetSystemNoiseCovariance(Q_idyn)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Failed to set system noise covariance"); + return false; + } + + // Construct the R matrix (measurement noise covariance) + iDynTree::MatrixDynSize R_idyn; + R_idyn.resize(1, 1); + R_idyn(0, 0) = r(joint); + if (!kfList[joint].kfSetMeasurementNoiseCovariance(R_idyn)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Failed to set measurement noise covariance"); + return false; + } + + // Set initial state x0 and covariance P0 + iDynTree::VectorDynSize x0; + x0.resize(3); + x0.zero(); + if (!kfList[joint].kfSetInitialState(x0)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Failed to set initial state"); + return false; + } + + iDynTree::MatrixDynSize P0_idyn; + P0_idyn.resize(3, 3); + P0_idyn.zero(); + P0_idyn(0, 0) = p0(joint); + P0_idyn(1, 1) = p0(joint); + P0_idyn(2, 2) = p0(joint); + if (!kfList[joint].kfSetStateCovariance(P0_idyn)) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Failed to set state covariance"); + return false; + } + + if (!kfList[joint].kfInit()) + { + log()->error("[JointTorqueControlDevice::constructKalmanFilters] Failed to initialize Kalman filter"); + return false; + } + } + + return true; +} + bool JointTorqueControlDevice::attachAll(const PolyDriverList& p) { if (!openCalledCorrectly) @@ -1140,6 +1359,15 @@ bool JointTorqueControlDevice::attachAll(const PolyDriverList& p) m_status.m_currentLogging.resize(axes, 1); } + // Initialize variables for KF idyntree + if (ret) + { + m_KFJointList.resize(axes); + m_KFMotorList.resize(axes); + m_measurementKF.resize(1); + m_estimateKF.resize(3); + } + // Load coupling matrices if (ret) { @@ -1399,6 +1627,34 @@ bool JointTorqueControlDevice::getRefTorque(int j, double* trq) return true; } +bool JointTorqueControlDevice::getEncoderSpeeds(double* spds) +{ + std::lock_guard(this->globalMutex); + memcpy(spds, measuredJointVelocities.data(), this->axes * sizeof(double)); + return true; +} + +bool JointTorqueControlDevice::getEncoderSpeed(int j, double* sp) +{ + std::lock_guard(this->globalMutex); + *sp = measuredJointVelocities[j]; + return true; +} + +bool JointTorqueControlDevice::getMotorEncoderSpeeds(double* spds) +{ + std::lock_guard(this->globalMutex); + memcpy(spds, measuredMotorVelocities.data(), this->axes * sizeof(double)); + return true; +} + +bool JointTorqueControlDevice::getMotorEncoderSpeed(int j, double* sp) +{ + std::lock_guard(this->globalMutex); + *sp = measuredMotorVelocities[j]; + return true; +} + // TO BE UPDATE // We publish the friction torque by using the motor acceleration as the friction // torque does not have a dedicated interface in the remote control board.