From b5be2e734e72516b234ad96dee2f670c03f33301 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 3 Apr 2021 20:26:51 +0200 Subject: [PATCH 1/5] Use spdlog in YarpRobotControl class --- src/RobotInterface/CMakeLists.txt | 2 +- .../src/YarpRobotControl.cpp | 111 +++++++++--------- 2 files changed, 55 insertions(+), 58 deletions(-) diff --git a/src/RobotInterface/CMakeLists.txt b/src/RobotInterface/CMakeLists.txt index a4f1996456..bbe9e7c10c 100644 --- a/src/RobotInterface/CMakeLists.txt +++ b/src/RobotInterface/CMakeLists.txt @@ -11,6 +11,7 @@ add_bipedal_locomotion_library( SOURCES src/IRobotControl.cpp PUBLIC_HEADERS ${H_PREFIX}/ISensorBridge.h ${H_PREFIX}/IRobotControl.h PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler Eigen3::Eigen + PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging SUBDIRECTORIES YarpImplementation tests) endif() @@ -23,4 +24,3 @@ add_bipedal_locomotion_library( PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler ${OpenCV_LIBS} ${PCL_LIBRARIES} INSTALLATION_FOLDER RobotInterface) endif() - diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp index 0cbb12be37..d279771157 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp @@ -20,6 +20,7 @@ #include #include +#include using namespace BipedalLocomotion::RobotInterface; @@ -132,11 +133,11 @@ struct YarpRobotControl::Impl bool getControlModes() { - constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::getControlModes] "; + constexpr auto errorPrefix = "[YarpRobotControl::Impl::getControlModes]"; if (this->controlModeInterface == nullptr) { - std::cerr << errorPrefix << "The control mode I/F is not ready." << std::endl; + log()->error("{} The control mode I/F is not ready.", errorPrefix); return false; } @@ -146,7 +147,7 @@ struct YarpRobotControl::Impl { if (++counter == this->maxReadingAttempts) { - std::cerr << errorPrefix << "Error while reading the control mode." << std::endl; + log()->error("{} Error while reading the control mode.", errorPrefix); return false; } @@ -164,11 +165,11 @@ struct YarpRobotControl::Impl bool setControlModes(const std::vector& controlModes) { - constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::setControlModes] "; + constexpr auto errorPrefix = "[YarpRobotControl::Impl::setControlModes]"; if (this->controlModeInterface == nullptr) { - std::cerr << errorPrefix << "The control mode I/F is not ready." << std::endl; + log()->error("{} The control mode I/F is not ready.", errorPrefix); return false; } @@ -191,7 +192,7 @@ struct YarpRobotControl::Impl // set the control mode if (!this->controlModeInterface->setControlModes(this->controlModesYarp.data())) { - std::cerr << errorPrefix << "Error settings the control mode." << std::endl; + log()->error("{} Error settings the control mode.", errorPrefix); return false; } @@ -206,11 +207,11 @@ struct YarpRobotControl::Impl bool getJointPos() { - constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::getJointPos] "; + constexpr auto errorPrefix = "[YarpRobotControl::Impl::getJointPos]"; if (this->encodersInterface == nullptr) { - std::cerr << errorPrefix << "The encoder I/F is not ready." << std::endl; + log()->error("{} The control mode I/F is not ready.", errorPrefix); return false; } @@ -220,7 +221,7 @@ struct YarpRobotControl::Impl { if (++counter == this->maxReadingAttempts) { - std::cerr << errorPrefix << "Error while reading the encoders." << std::endl; + log()->error("{} Error while reading the encoders.", errorPrefix); return false; } @@ -236,57 +237,55 @@ struct YarpRobotControl::Impl bool setDriver(std::shared_ptr robotDevice) { - constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::setDriver] "; + constexpr auto errorPrefix = "[YarpRobotControl::Impl::setDriver]"; if (robotDevice == nullptr) { - std::cerr << errorPrefix - << "The robotDevice is pointing to an non initialized memory." - << std::endl; + log()->error("{} The robotDevice is pointing to an non initialized memory.", + errorPrefix); return false; } // obtain the interfaces if (!robotDevice->view(encodersInterface) || encodersInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the IEncodersTimed interface." << std::endl; + log()->error("{} Cannot load the IEncodersTimed interface.", errorPrefix); return false; } if (!robotDevice->view(positionInterface) || positionInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the IPositionControl interface." << std::endl; + log()->error("{} Cannot load the IPositionControl interface.", errorPrefix); return false; } if (!robotDevice->view(positionDirectInterface) || positionDirectInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the IPositionDirect interface." << std::endl; + log()->error("{} Cannot load the IPositionDirect interface.", errorPrefix); return false; } if (!robotDevice->view(velocityInterface) || velocityInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the IVelocityInterface interface." - << std::endl; + log()->error("{} Cannot load the IVelocityControl interface.", errorPrefix); return false; } if (!robotDevice->view(torqueInterface) || torqueInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the ITorqueInterface interface." << std::endl; + log()->error("{} Cannot load the ITorqueControl interface.", errorPrefix); return false; } if (!robotDevice->view(controlModeInterface) || controlModeInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the IControlMode interface." << std::endl; + log()->error("{} Cannot load the IControlMode interface.", errorPrefix); return false; } if (!robotDevice->view(axisInfoInterface) || axisInfoInterface == nullptr) { - std::cerr << errorPrefix << "Cannot load the IAxisInfo interface." << std::endl; + log()->error("{} Cannot load the IAxisInfo interface.", errorPrefix); return false; } @@ -294,7 +293,7 @@ struct YarpRobotControl::Impl int dofs = 0; if (!encodersInterface->getAxes(&dofs)) { - std::cerr << errorPrefix << "Cannot get the actuated DoFs." << std::endl; + log()->error("{} Cannot get the actuated DoFs.", errorPrefix); return false; } this->actuatedDOFs = dofs; @@ -317,7 +316,7 @@ struct YarpRobotControl::Impl if (!this->getControlModes()) { - std::cerr << errorPrefix << "Unable to get the control modes." << std::endl; + log()->error("{} Unable to get the control modes.", errorPrefix); return false; } @@ -407,11 +406,11 @@ struct YarpRobotControl::Impl bool setReferences(Eigen::Ref jointValues) { - constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::setReferences] "; + constexpr auto errorPrefix = "[YarpRobotControl::Impl::setReferences]"; if(!this->getJointPos()) { - std::cerr << errorPrefix << "Unable to get the joint position." << std::endl; + log()->error("{} Unable to get the joint position.", errorPrefix); return false; } @@ -420,10 +419,12 @@ struct YarpRobotControl::Impl if (worstError.second > this->positionDirectMaxAdmissibleError) { - std::cerr << errorPrefix << "The worst error between the current and the " - << "desired position of the joint named '" << this->axesName[worstError.first] - << "' is greater than " << this->positionDirectMaxAdmissibleError - << " rad. Error = " << worstError.second << " rad." << std::endl; + log()->error("{} The worst error between the current and the desired position of the " + "joint named '{}' is greater than {} deg. Error = {} deg.", + errorPrefix, + this->axesName[worstError.first], + 180 / M_PI * this->positionDirectMaxAdmissibleError, + 180 / M_PI * worstError.second); return false; } @@ -438,15 +439,14 @@ struct YarpRobotControl::Impl else if (mode == IRobotControl::ControlMode::Unknown) { - std::string error = " The following joints does not have a specified control " - "mode: "; - + std::string joints = ""; for (const auto& index : indeces) - error += "'" + this->axesName[index] + "' "; - - std::cerr << errorPrefix << error << ". Please set a feasible control mode." - << std::endl; + joints += " '" + this->axesName[index] + "'"; + log()->error("{} The following joints does not have a specified control " + "mode:{}. Please set a feasible control mode.", + errorPrefix, + joints); return false; } else if (mode == IRobotControl::ControlMode::Position) @@ -483,7 +483,7 @@ struct YarpRobotControl::Impl this->desiredJointValuesAndMode.value[mode].data())) { - std::cerr << errorPrefix << "Unable to set the desired joint values." << std::endl; + log()->error("{} Unable to set the desired joint values.", errorPrefix); return false; } } @@ -505,13 +505,13 @@ bool YarpRobotControl::setDriver(std::shared_ptr robotDev bool YarpRobotControl::initialize(std::weak_ptr handler) { - constexpr std::string_view errorPrefix = "[YarpRobotControl::initialize] "; + constexpr auto errorPrefix = "[YarpRobotControl::initialize]"; auto ptr = handler.lock(); if (ptr == nullptr) { - std::cerr << errorPrefix << "The handler is not pointing to an already initialized memory." - << std ::endl; + log()->error("{} The handler is not pointing to an already initialized memory.", + errorPrefix); return false; } @@ -522,8 +522,8 @@ bool YarpRobotControl::initialize(std::weak_ptrerror("{} 'reading_timeout' parameter has to be a positive number.", + errorPrefix); return false; } m_pimpl->readingTimeout = temp; @@ -534,9 +534,10 @@ bool YarpRobotControl::initialize(std::weak_ptrerror("{} 'max_reading_attempts' parameter has to be a strictly positive " + "number.", + errorPrefix); + return false; } m_pimpl->maxReadingAttempts = temp; @@ -562,9 +563,7 @@ bool YarpRobotControl::setReferences(Eigen::Ref jointValu m_pimpl->controlModes = controlModes; if (!m_pimpl->setControlModes(m_pimpl->controlModes)) { - std::cerr << "[YarpRobotControl::setReferences] Unable to switch in " - "position-direct control mode." - << std::endl; + log()->error("[YarpRobotControl::setReferences] Unable to set the control modes."); return false; } } @@ -584,8 +583,7 @@ bool YarpRobotControl::setReferences(Eigen::Ref desiredJo std::fill(m_pimpl->controlModes.begin(), m_pimpl->controlModes.end(), mode); if (!m_pimpl->setControlModes(m_pimpl->controlModes)) { - std::cerr << "[YarpRobotControl::setReferences] Unable to the desired control mode." - << std::endl; + log()->error("[YarpRobotControl::setReferences] Unable to set the control modes."); return false; } } @@ -594,22 +592,21 @@ bool YarpRobotControl::setReferences(Eigen::Ref desiredJo } bool YarpRobotControl::checkMotionDone(bool& motionDone, - bool& isTimeExpired, - std::vector>& info) + bool& isTimeExpired, + std::vector>& info) { - constexpr std::string_view errorPrefix = "[YarpRobotControl::checkMotionDone] "; + constexpr auto errorPrefix = "[YarpRobotControl::checkMotionDone]"; if (!m_pimpl->positionInterface->checkMotionDone(&motionDone)) { - std::cerr << errorPrefix - << "Unable to check if the motion is terminated from the Yarp interface." - << std::endl; + log()->error("{} Unable to check if the motion is terminated from the Yarp interface.", + errorPrefix); return false; } if (!m_pimpl->getJointPos()) { - std::cerr << errorPrefix << "Unable to get the joint position." << std::endl; + log()->error("{} Unable to get the joint position.", errorPrefix); return false; } From 317b86fffcbcd2f263bf76785c11269df5aa3ef7 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 3 Apr 2021 21:47:30 +0200 Subject: [PATCH 2/5] Call setRefSpeeds() only once in YarpRobotControl --- .../src/YarpRobotControl.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp index d279771157..0147a5df89 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp @@ -67,6 +67,8 @@ struct YarpRobotControl::Impl JointsControlValuesAndMode desiredJointValuesAndMode; /**< Struct containing the information regarding the desired joint value and the control mode */ + std::vector positionControlRefSpeeds; /**< Vector containing the ref speed in + deg/seconds for the position control joints. */ double positioningDuration{0.0}; /**< Duration of the trajectory generated when the joint is controlled in position mode */ @@ -202,6 +204,10 @@ struct YarpRobotControl::Impl this->desiredJointValuesAndMode.value[mode].resize(indeces.size()); } + // resize the position control reference speed vector + this->positionControlRefSpeeds.resize( + this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Position].size()); + return true; } @@ -303,7 +309,7 @@ struct YarpRobotControl::Impl this->controlModes.resize(this->actuatedDOFs); this->controlModesYarp.resize(this->actuatedDOFs); this->axesName.resize(this->actuatedDOFs); - + this->positionControlRefSpeeds.resize(this->actuatedDOFs); // populate the axesName vector for (int i = 0; i < this->actuatedDOFs; i++) @@ -451,7 +457,6 @@ struct YarpRobotControl::Impl } else if (mode == IRobotControl::ControlMode::Position) { - std::vector refSpeeds(indeces.size()); for (int i = 0; i < indeces.size(); i++) { const auto jointError = std::abs(jointValues[indeces[i]] @@ -459,12 +464,19 @@ struct YarpRobotControl::Impl constexpr double scaling = 180 / M_PI; constexpr double maxVelocityInDegPerSeconds = 3.0; - refSpeeds[i] = std::max(maxVelocityInDegPerSeconds, - scaling * (jointError / this->positioningDuration)); + this->positionControlRefSpeeds[i] + = std::max(maxVelocityInDegPerSeconds, + scaling * (jointError / this->positioningDuration)); + } - this->positionInterface->setRefSpeeds(indeces.size(), - indeces.data(), - refSpeeds.data()); + if (!this->positionInterface->setRefSpeeds(indeces.size(), + indeces.data(), + this->positionControlRefSpeeds.data())) + { + log()->error("{} Unable to set the reference speed for the position control " + "joints.", + errorPrefix); + return false; } this->startPositionControlInstant = yarp::os::Time::now(); From 86596134571fd7924961b7e790d6abbc099e28c9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Apr 2021 12:31:48 +0200 Subject: [PATCH 3/5] YarpRobotControl positionControlRefSpeeds is now resized using only the joints in position control. --- .../YarpImplementation/src/YarpRobotControl.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp index 0147a5df89..a15f4eada2 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp @@ -309,7 +309,6 @@ struct YarpRobotControl::Impl this->controlModes.resize(this->actuatedDOFs); this->controlModesYarp.resize(this->actuatedDOFs); this->axesName.resize(this->actuatedDOFs); - this->positionControlRefSpeeds.resize(this->actuatedDOFs); // populate the axesName vector for (int i = 0; i < this->actuatedDOFs; i++) @@ -344,6 +343,11 @@ struct YarpRobotControl::Impl this->desiredJointValuesAndMode.value[mode].resize(indeces.size()); } + // resize the reference speed for the position control mode + // The size of the vector is equal to the size of the joints in position control + this->positionControlRefSpeeds.resize( + this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Position].size()); + return true; } From 15ce6e739a6e3e880744168f2e05f55e5b31c039 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Apr 2021 12:32:28 +0200 Subject: [PATCH 4/5] Rename maxVelocityInDegPerSecond into minVelocityInDegPerSecond in YarpRobotControl --- .../YarpImplementation/src/YarpRobotControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp index a15f4eada2..11e06366ac 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp @@ -467,9 +467,9 @@ struct YarpRobotControl::Impl - this->positionFeedback[indeces[i]]); constexpr double scaling = 180 / M_PI; - constexpr double maxVelocityInDegPerSeconds = 3.0; + constexpr double minVelocityInDegPerSeconds = 3.0; this->positionControlRefSpeeds[i] - = std::max(maxVelocityInDegPerSeconds, + = std::max(minVelocityInDegPerSeconds, scaling * (jointError / this->positioningDuration)); } From 786fc80a2dcc97a6b56f3b38fa1c00c1e3bb72bc Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Apr 2021 12:34:25 +0200 Subject: [PATCH 5/5] Update the CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c1a7bde244..5613858c45 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -41,6 +41,7 @@ All notable changes to this project are documented in this file. - Avoid the "Generating the Urdf Model from" message to appear when doing ccmake. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/243) - Fixed the installation path of public headers related to perception libraries. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/245) - Fixed InstallBasicPackageFiles to avoid the same problem of https://github.com/dic-iit/matio-cpp/pull/41 (https://github.com/dic-iit/bipedal-locomotion-framework/pull/253) +- Call `positionInterface->setRefSpeeds()` only once when a position reference is set in `YarpRobotControl` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/271) ## [0.1.0] - 2021-02-22 ### Added