Add HumanWrenchWrapper and HumanWrenchRemapper

conf/xml/Human.xml

@@ -182,15 +182,15 @@
         <action phase="shutdown" level="5" type="detach" />
     </device>
 
-    <device type="analogServer" name="HumanWrenchWrapper">
-        <param name="name">/HDE/HumanWrenchWrapper/wrench:o</param>
-        <param name="period">10</param>
+    <device type="human_wrench_wrapper" name="HumanWrenchWrapper">
+        <param name="period">0.1</param>
+        <param name="outputPort">/HDE/HumanWrenchWrapper/wrench:o</param>
         <action phase="startup" level="5" type="attach">
             <paramlist name="networks">
-                <elem name="HumanWrenchWrapperLabel">HumanWrenchProvider</elem>
+                <elem name="HumanDynamicsWrapperLabel">HumanWrenchProvider</elem>
             </paramlist>
         </action>
-        <action phase="shutdown" level="5" type="detach" />
+        <action phase="shutdown" level="5" type="detach"/>
     </device>
 
     <!--Wrapper to publish WrenchStamped message to Rviz-->

devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp

@@ -26,7 +26,6 @@
 
 #include <yarp/os/LogStream.h>
 #include <yarp/os/ResourceFinder.h>
-#include <yarp/dev/IAnalogSensor.h>
 #include <iDynTree/yarp/YARPConversions.h>
 
 #include <mutex>
@@ -761,7 +760,6 @@ class HumanDynamicsEstimator::Impl
     // Attached interfaces
     hde::interfaces::IHumanState* iHumanState = nullptr;
     hde::interfaces::IHumanWrench* iHumanWrench = nullptr;
-    yarp::dev::IAnalogSensor* iAnalogSensor = nullptr;
 
     mutable std::mutex mutex;
     iDynTree::Vector3 gravity;
@@ -1258,18 +1256,6 @@ bool HumanDynamicsEstimator::Impl::attach(yarp::dev::PolyDriver* poly)
         yInfo() << LogPrefix << "Device" << deviceName << "attached successfully as IHumanWrench";
         iHumanWrench = tmpIHumanWrench;
 
-        // Try to attach IAnalogSensor
-        if(!poly->view(iAnalogSensor)){
-            yError() << LogPrefix << "Device" << deviceName << "must implement also the IAnalog interface!"; 
-            return false;
-        }
-
-        // Check the interface
-        if (iAnalogSensor->getChannels() != 6 * numberOfWrenchSources) {
-            yError() << LogPrefix << "The IAnalogSensor interface might not be ready";
-            return false;
-        }
-
     }
 
     if(!tmpIHumanState && !tmpIHumanWrench){
@@ -1325,7 +1311,6 @@ bool HumanDynamicsEstimator::detachAll()
 
     pImpl->iHumanState = nullptr;
     pImpl->iHumanWrench = nullptr;
-    pImpl->iAnalogSensor = nullptr;
 
     return true;
 }

modules/HumanStateVisualizer/CMakeLists.txt

@@ -39,6 +39,7 @@ target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC
   iDynTree::idyntree-modelio
   iDynTree::idyntree-visualization
   IHumanState
-  IWearableTargets)
+  IWearableTargets
+  IHumanWrench)
 
 install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin)

modules/HumanStateVisualizer/src/main.cpp

@@ -2,6 +2,7 @@
 // SPDX-License-Identifier: BSD-3-Clause
 
 #include <hde/interfaces/IHumanState.h>
+#include <hde/interfaces/IHumanWrench.h>
 #include <hde/interfaces/IWearableTargets.h>
 
 #include <yarp/os/Network.h>
@@ -379,19 +380,19 @@ int main(int argc, char* argv[])
 
 
     // initialize iHumanState interface from remapper
-    yarp::dev::PolyDriver remapperDevice;
+    yarp::dev::PolyDriver humanStateRemapperDevice;
     hde::interfaces::IHumanState* iHumanState{nullptr};
 
     yarp::os::Property remapperOptions;
     remapperOptions.put("device", "human_state_remapper");
     remapperOptions.put("humanStateDataPort", humanStateDataPortName);
 
-    if(!remapperDevice.open(remapperOptions))
+    if(!humanStateRemapperDevice.open(remapperOptions))
     {
         yError() << LogPrefix << "Failed to connect remapper device";
         return EXIT_FAILURE;
     }
-    if(!remapperDevice.view(iHumanState) || !iHumanState )
+    if(!humanStateRemapperDevice.view(iHumanState) || !iHumanState )
     {
         yError() << LogPrefix << "Failed to view iHumanState interface";
         return EXIT_FAILURE;
@@ -485,32 +486,37 @@ int main(int argc, char* argv[])
         }
     }
 
-    // initialize wrench port
-    yarp::os::BufferedPort<yarp::sig::Vector> wrenchPort;
-    yarp::sig::Vector* wrenchMeasuresVector;
+    // initialize iHumanWrench 
+    yarp::dev::PolyDriver humanWrenchRemapperDevice;
+    hde::interfaces::IHumanWrench* iHumanWrench{nullptr};
     if (visualizeWrenches)
     {
-        wrenchPort.open("/HumanStateVisualizer" + humanWrenchWrapperPortName);
-        if (wrenchPort.isClosed())
+        yarp::os::Property humanWrenchRemapperOptions;
+        humanWrenchRemapperOptions.put("device", "human_wrench_remapper");
+        humanWrenchRemapperOptions.put("humanWrenchDataPort", humanWrenchWrapperPortName);
+
+        if(!humanWrenchRemapperDevice.open(humanWrenchRemapperOptions))
         {
-            yError() << LogPrefix << "failed to open the port /HumanStateVisualizer" << humanWrenchWrapperPortName;
+            yError() << LogPrefix << "Failed to connect remapper device";
             return EXIT_FAILURE;
         }
-        if (!yarp.connect(humanWrenchWrapperPortName, wrenchPort.getName()))
+        if(!humanWrenchRemapperDevice.view(iHumanWrench) || !iHumanWrench )
         {
-            yError() << LogPrefix << "failed to connect to the port"  << humanWrenchWrapperPortName;
+            yError() << LogPrefix << "Failed to view iHumanWrench interface";
             return EXIT_FAILURE;
         }
-        wrenchMeasuresVector = wrenchPort.read(true);
-        if (wrenchMeasuresVector == nullptr)
+
+        // wait for the iHumanWrench to be initialized
+        while (iHumanWrench->getWrenchSourceNames().empty())
         {
-            yError() << LogPrefix << "no data coming from the port " << humanWrenchWrapperPortName;
-            return EXIT_FAILURE;
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+            yInfo() << LogPrefix << "Waiting for data from HumanWrenchRemapper";
         }
-        if(wrenchMeasuresVector->size() != (numberOfWrenchElements) )
+
+        if(iHumanWrench->getNumberOfWrenchSources() != wrenchSourceLinks.size() )
         {
-            yError() << LogPrefix << "expected " << numberOfWrenchElements << " elements in port " << humanWrenchWrapperPortName
-                    << ", received " << wrenchMeasuresVector->size();
+            yError() << LogPrefix << "expected " << wrenchSourceLinks.size() << " wrench sources in port " << humanWrenchWrapperPortName
+                    << ", received " << iHumanWrench->getNumberOfWrenchSources();
             return EXIT_FAILURE;
         }
     }
@@ -556,7 +562,7 @@ int main(int argc, char* argv[])
             linkTransform = viz.modelViz("human").getWorldLinkTransform(wrenchSourceLinkIndices.at(vectorIndex));
             for (size_t i = 0; i < 3; i++)
             {
-                force.setVal(i, forceScalingFactor * wrenchMeasuresVector->data()[6 * vectorIndex + i]);
+                force.setVal(i, forceScalingFactor * iHumanWrench->getWrenches()[6 * vectorIndex + i]);
             }
             force = linkTransform.getRotation() * force;
             viz.vectors().addVector(linkTransform.getPosition(), force);
@@ -649,13 +655,12 @@ int main(int argc, char* argv[])
         size_t vectorsIterator = 0;
         if (visualizeWrenches)
         {
-            wrenchMeasuresVector = wrenchPort.read(true);
             for (size_t vectorIndex = 0; vectorIndex < wrenchSourceLinks.size(); vectorIndex++)
             {
                 linkTransform = viz.modelViz("human").getWorldLinkTransform(wrenchSourceLinkIndices.at(vectorIndex));
                 for (size_t i = 0; i < 3; i++)
                 {
-                    force.setVal(i, forceScalingFactor * wrenchMeasuresVector->data()[6 * vectorIndex + i]);
+                    force.setVal(i, forceScalingFactor * iHumanWrench->getWrenches()[6 * vectorIndex + i]);
                 }
                 force = linkTransform.getRotation() * force;
                 viz.vectors().updateVector(vectorIndex, linkTransform.getPosition(), force);
@@ -735,9 +740,9 @@ int main(int argc, char* argv[])
     }
 
     viz.close();
-    remapperDevice.close();
+    humanStateRemapperDevice.close();
     wearableTargetsRemapperDevice.close();
-    wrenchPort.close();
+    humanWrenchRemapperDevice.close();
 
     return 0;
 }

msgs/yarp/thrift/CMakeLists.txt

@@ -100,3 +100,35 @@ install(TARGETS ${LIBRARY_TARGET_NAME}
         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}"
         LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}"
         PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/hde/msgs")
+
+set(LIBRARY_TARGET_NAME HumanWrenchMsg)
+
+yarp_idl_to_dir(hde/msgs/HumanWrench.thrift
+  OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated
+  SOURCES_VAR ${LIBRARY_TARGET_NAME}_GEN_SRC
+  HEADERS_VAR ${LIBRARY_TARGET_NAME}_GEN_HDR
+  INCLUDE_DIRS_VAR ${LIBRARY_TARGET_NAME}_INCLUDE_DIRS
+  CMAKE_SCRIPTS_VAR ${LIBRARY_TARGET_NAME}_CMAKE_SCRIPTS)
+foreach(_script ${${LIBRARY_TARGET_NAME}_CMAKE_SCRIPTS})
+  include("${_script}")
+endforeach()
+
+add_library(${LIBRARY_TARGET_NAME} ${${LIBRARY_TARGET_NAME}_GEN_SRC} ${${LIBRARY_TARGET_NAME}_GEN_HDR})
+target_link_libraries(${LIBRARY_TARGET_NAME} YARP::YARP_OS YARP::YARP_init)
+
+set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES
+                      PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_GEN_HDR}")
+
+target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC
+                            "$<BUILD_INTERFACE:${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}>"
+                            "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/hde/msgs>")
+
+set_property(TARGET ${LIBRARY_TARGET_NAME} PROPERTY PUBLIC_HEADER ${${LIBRARY_TARGET_NAME}_GEN_HDR})
+
+add_library(${PROJECT_NAME}::${LIBRARY_TARGET_NAME} ALIAS ${LIBRARY_TARGET_NAME})
+
+install(TARGETS ${LIBRARY_TARGET_NAME}
+        EXPORT ${PROJECT_NAME}
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}"
+        LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}"
+        PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/hde/msgs")

msgs/yarp/thrift/hde/msgs/HumanWrench.thrift

@@ -0,0 +1,9 @@
+namespace yarp hde.msgs
+
+/**
+ * Representation of the IHumanWrench interface
+ */
+struct HumanWrench {
+    1: list<string> wrenchSourceNames;
+    2: list<double> wrenches;
+}

remappers/CMakeLists.txt

@@ -3,4 +3,5 @@
 
 add_subdirectory(HumanStateRemapper)
 add_subdirectory(HumanDynamicsRemapper)
+add_subdirectory(HumanWrenchRemapper)
 add_subdirectory(WearableTargetsRemapper)

remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp

@@ -33,9 +33,9 @@ class HumanDynamicsRemapper::impl
     std::vector<double>  jointTorques;
 };
 
-// ==============
-// IWEAR REMAPPER
-// ==============
+// =======================
+// IHUMANDYNAMICS REMAPPER
+// =======================
 
 HumanDynamicsRemapper::HumanDynamicsRemapper()
     : PeriodicThread(1)

remappers/HumanStateRemapper/HumanStateRemapper.cpp

@@ -43,9 +43,9 @@ class HumanStateRemapper::impl
     std::array<double, 3> CoMVelocity;
 };
 
-// ==============
-// IWEAR REMAPPER
-// ==============
+// ====================
+// IHUMANSTATE REMAPPER
+// ====================
 
 HumanStateRemapper::HumanStateRemapper()
     : PeriodicThread(1)

remappers/HumanWrenchRemapper/CMakeLists.txt

@@ -0,0 +1,31 @@
+# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+yarp_prepare_plugin(human_wrench_remapper
+    TYPE hde::devices::HumanWrenchRemapper
+    INCLUDE HumanWrenchRemapper.h
+    CATEGORY device
+    ADVANCED
+    DEFAULT ON)
+
+yarp_add_plugin(HumanWrenchRemapper
+    HumanWrenchRemapper.cpp
+    HumanWrenchRemapper.h)
+
+target_include_directories(HumanWrenchRemapper PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>)
+
+target_link_libraries(HumanWrenchRemapper PUBLIC
+    IHumanWrench
+    HumanWrenchMsg
+    YARP::YARP_OS
+    YARP::YARP_dev
+    YARP::YARP_init)
+
+yarp_install(
+    TARGETS HumanWrenchRemapper
+    COMPONENT runtime
+    LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+    ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
+    YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR})
+