[HumanLogger] Add IHumanWrench to HumanLogger

robotology · Oct 10, 2023 · 71c62de · 71c62de
1 parent f6e5f94
commit 71c62de
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 6 deletions.
diff --git 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
@@ -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
@@ -4,6 +4,7 @@
 #include "HumanLogger.h"
 #include <hde/interfaces/IHumanState.h>
 #include <hde/interfaces/IHumanDynamics.h>
+#include <hde/interfaces/IHumanWrench.h>
 
 #include <algorithm>
 #include <functional>
@@ -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<double> jointVelocities;
     std::vector<std::string> jointNamesState;
     // iHumanDynamics
-    std::vector<double> jointTorquesInterface;
+    std::vector<double> jointTorques;
     std::vector<std::string> jointNamesDynamics;
+    // iHumanWrench
+    std::vector<double> wrenches;
+    std::vector<std::string> 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()<<LogPrefix<<"The device"<<deviceName<<"does not implement any of the attachable interfaces!";
             return false;
@@ -406,6 +455,13 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList)
         return false;
     }
 
+    // Check IHumanDynamics interface
+    if (pImpl->settings.logHumanWrench && !pImpl->iHumanWrench)
+    {
+        yError()<<LogPrefix<<"No IHumanWrench interface attached, stopping";
+        return false;
+    }
+
     if (!pImpl->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
@@ -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();
     }
 
     // ====