diff --git a/CHANGELOG.md b/CHANGELOG.md index 55cfc69a3..f6f8a49e4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Fixed - [`HumanControlBoard`] Fix attach method (https://github.com/robotology/human-dynamics-estimation/pull/365). +### Added +- Added `HumanWrenchWrapper` and `HumanWrenchRemapper` (https://github.com/robotology/human-dynamics-estimation/pull/362/files). +- Added `IHumanWrench` to `HumanLogger` (https://github.com/robotology/human-dynamics-estimation/pull/363). + ## [2.8.0] - 2023-09-06 ### Added diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp index 57b62835a..1a81879ff 100644 --- a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp +++ b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp @@ -1251,6 +1251,7 @@ bool HumanDynamicsEstimator::Impl::attach(yarp::dev::PolyDriver* poly) numberOfWrenchSources != tmpIHumanWrench->getWrenchSourceNames().size()) { yInfo() << LogPrefix << "IHumanWrench interface waiting for first data. Waiting..."; yarp::os::Time::delay(5); + numberOfWrenchSources = tmpIHumanWrench->getNumberOfWrenchSources(); } yInfo() << LogPrefix << "Device" << deviceName << "attached successfully as IHumanWrench"; diff --git a/devices/HumanLogger/CMakeLists.txt b/devices/HumanLogger/CMakeLists.txt index 62ad0a3bb..81a5f4393 100644 --- a/devices/HumanLogger/CMakeLists.txt +++ b/devices/HumanLogger/CMakeLists.txt @@ -27,6 +27,7 @@ target_link_libraries(HumanLogger PUBLIC robometry::robometry IHumanState IHumanDynamics + IHumanWrench ) if(MSVC) diff --git a/devices/HumanLogger/HumanLogger.cpp b/devices/HumanLogger/HumanLogger.cpp index 57eb835f5..354a3c6d9 100644 --- a/devices/HumanLogger/HumanLogger.cpp +++ b/devices/HumanLogger/HumanLogger.cpp @@ -4,6 +4,7 @@ #include "HumanLogger.h" #include #include +#include #include #include @@ -38,6 +39,7 @@ struct hde::devices::HumanLoggerSettings bool saveBufferManagerConfiguration{false}; bool logHumanState{false}; bool logHumanDynamics{false}; + bool logHumanWrench{false}; }; using namespace hde::devices; @@ -58,6 +60,7 @@ class HumanLogger::impl interfaces::IHumanState* iHumanState = nullptr; interfaces::IHumanDynamics* iHumanDynamics = nullptr; + interfaces::IHumanWrench* iHumanWrench = nullptr; HumanLoggerSettings settings; robometry::BufferConfig bufferConfig; robometry::BufferManager bufferManager; @@ -71,8 +74,12 @@ class HumanLogger::impl std::vector jointVelocities; std::vector jointNamesState; // iHumanDynamics - std::vector jointTorquesInterface; + std::vector jointTorques; std::vector jointNamesDynamics; + // iHumanWrench + std::vector wrenches; + std::vector wrenchSourceNames; + }; @@ -136,14 +143,31 @@ void HumanLogger::run() return; } - pImpl->jointTorquesInterface = pImpl->iHumanDynamics->getJointTorques(); + pImpl->jointTorques = pImpl->iHumanDynamics->getJointTorques(); if (pImpl->loggerType == LoggerType::MATLAB) { - pImpl->bufferManager.push_back(pImpl->jointTorquesInterface, + pImpl->bufferManager.push_back(pImpl->jointTorques, timeNow, "human_dynamics::joint_torques"); } } + + if (pImpl->settings.logHumanWrench) + { + if(!pImpl->iHumanWrench) { + yError() << LogPrefix << "The IHumanWrench pointer is null in the driver loop."; + askToStop(); + return; + } + + pImpl->wrenches = pImpl->iHumanWrench->getWrenches(); + + if (pImpl->loggerType == LoggerType::MATLAB) { + pImpl->bufferManager.push_back(pImpl->wrenches, + timeNow, + "human_wrench::wrenches"); + } + } } // ====================== @@ -207,6 +231,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) // load logger flag settings checkAndLoadBooleanOption(config, "logHumanState", settings.logHumanState); checkAndLoadBooleanOption(config, "logHumanDynamics", settings.logHumanDynamics); + checkAndLoadBooleanOption(config, "logHumanWrench", settings.logHumanWrench); // load buffer manager configuration settings checkAndLoadBooleanOption( @@ -319,6 +344,14 @@ bool HumanLogger::impl::configureBufferManager() ok = ok && bufferManager.addChannel({"human_dynamics::joint_torques", {jointNamesDynamics.size(), 1}, jointNamesDynamics}); } } + if (settings.logHumanWrench) + { + if (loggerType == LoggerType::MATLAB) + { + wrenchSourceNames = iHumanWrench->getWrenchSourceNames(); + ok = ok && bufferManager.addChannel({"human_wrench::wrenches", {6 * wrenchSourceNames.size(), 1}, wrenchSourceNames}); + } + } ok = ok && bufferManager.configure(bufferConfig); @@ -337,8 +370,8 @@ void HumanLogger::threadRelease() {} bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList) { - if (driverList.size() > 2) { - yError() << LogPrefix << "This wrapper accepts maximum two attached PolyDriver."; + if (driverList.size() > 3) { + yError() << LogPrefix << "This wrapper accepts maximum three attached PolyDriver."; return false; } @@ -354,6 +387,7 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList) const std::string deviceName = poly->getValue("device").asString(); interfaces::IHumanState* tmpIHumanState = nullptr; interfaces::IHumanDynamics* tmpIHumanDynamics = nullptr; + interfaces::IHumanWrench* tmpIHumanWrench = nullptr; // View IHumanState if(pImpl->settings.logHumanState && !pImpl->iHumanState && poly->view(tmpIHumanState)) @@ -381,7 +415,22 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList) pImpl->iHumanDynamics = tmpIHumanDynamics; } - if(!tmpIHumanState && !tmpIHumanDynamics) + // View IHumanWrench + if(pImpl->settings.logHumanWrench && !pImpl->iHumanWrench && poly->view(tmpIHumanWrench)) + { + // Check the interface + auto numberOfWrenchSources = tmpIHumanWrench->getNumberOfWrenchSources(); + while ( numberOfWrenchSources == 0 || + numberOfWrenchSources != tmpIHumanWrench->getWrenchSourceNames().size()) { + yInfo() << LogPrefix << "IHumanWrench interface waiting for first data. Waiting..."; + yarp::os::Time::delay(5); + numberOfWrenchSources = tmpIHumanWrench->getNumberOfWrenchSources(); + } + + pImpl->iHumanWrench = tmpIHumanWrench; + } + + if(!tmpIHumanState && !tmpIHumanDynamics && !tmpIHumanWrench) { yError()<settings.logHumanWrench && !pImpl->iHumanWrench) + { + yError()<configureBufferManager()) { yError() << LogPrefix << "Failed to configure buffer manager for the logger."; return false; @@ -430,6 +486,7 @@ bool HumanLogger::detachAll() pImpl->iHumanState = nullptr; pImpl->iHumanDynamics = nullptr; + pImpl->iHumanWrench = nullptr; return true; } diff --git a/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp b/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp index 03e01a285..3af463deb 100644 --- a/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp +++ b/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp @@ -125,6 +125,7 @@ bool HumanWrenchWrapper::attach(yarp::dev::PolyDriver* poly) numberOfWrenchSources != pImpl->humanWrench->getWrenchSourceNames().size()) { yInfo() << LogPrefix << "IHumanWrench interface waiting for first data. Waiting..."; yarp::os::Time::delay(5); + numberOfWrenchSources = pImpl->humanWrench->getNumberOfWrenchSources(); } // ====