Fix Haptic glove transmission - HapticGloveModule (Recovered PR)

.gitignore

@@ -43,4 +43,6 @@ build/
 
 TAGS
 compile_commands.json
-CMakeLists.txt.user
+CMakeLists.txt.user
+.vscode/
+

app/scripts/ergoCub-Teleoperation-2.xml

@@ -15,7 +15,7 @@
     <parameters>--from openXRHeadsetParameters.ini</parameters>
     <node>icub-virtualizer</node>
   </module>
-  
+
   <module>
     <name>yarp</name>
     <parameters>repeat /depthCamera/repeated/rgbImage:o </parameters>

modules/HapticGlove_module/include/GloveControlHelper.hpp

@@ -93,6 +93,12 @@ class HapticGlove::GloveControlHelper
      */
     bool getHandJointAngles(std::vector<double>& jointAngles);
 
+    /**
+     * Set the desired haptic feedback reference to the user fingertip
+     * @return true/false in case of success/failure
+     */
+    bool sendFingertipHapticFeedbackReferences();
+
     /**
      * Set the desired force feedback reference to the user fingertip
      * @param desiredValue desired force feedback values

modules/HapticGlove_module/include/GloveWearable.hpp

@@ -9,7 +9,7 @@
 #include <vector>
 // wearable
 #include <Wearable/IWear/IWear.h>
-#include <thrift/WearableActuatorCommand.h>
+#include <thrift/GloveActuatorCommand.h>
 // YARP
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/os/BufferedPort.h>
@@ -72,6 +72,8 @@ class HapticGlove::GloveWearableImpl
 private:
     std::string m_logPrefix;
 
+    const size_t m_numHapticFeedback; /**< Number of the motors to produce haptic feedback to the
+                         human*/
     const size_t m_numForceFeedback; /**< Number of the motors to produce force feedback to the
                          human*/
     const size_t m_numVibrotactileFeedback; /**< Number of the vibrotactile to produce vibrotactile
@@ -83,7 +85,7 @@ class HapticGlove::GloveWearableImpl
 
     wearable::IWear* m_iWear{nullptr}; /**< Sense glove wearable interface. */
 
-    BufferedPort<wearable::msg::WearableActuatorCommand> m_iWearActuatorPort;
+    BufferedPort<wearable::msg::GloveActuatorCommand> m_iWearGloveActuatorPort;
 
     std::string m_handLinkName;
 
@@ -169,6 +171,14 @@ class HapticGlove::GloveWearableImpl
      */
     bool getFingertipPoseValues(Eigen::MatrixXd& values);
 
+    /**
+     * set the Haptic feedback values associated with all the human hand fingertips
+     * @param values the vector of force and vibrotactile feedback values to the human fingertips, from thumb to
+     * pinky, range: [0, 100]
+     * @return true/false in case of success/failure
+     */
+    bool setFingertipHapticFeedbackValues(const std::vector<int>& forceValues, const std::vector<int>& vibroValues);
+
     /**
      * set the force feedback actuator values associated with all the human hand fingertips
      * @param values the vector of force feedback values to the human fingertips, from thumb to

modules/HapticGlove_module/include/Teleoperation.hpp

@@ -91,6 +91,9 @@ struct HapticGlove::Data
     std::vector<double> humanJointValues; /// <summary> juman joint values
     Eigen::MatrixXd humanFingertipPoses; /// <summary> human fingertip poses (size: number of hand
                                          /// fingertips (i.e., 5) x 7)
+    std::vector<double>
+        humanHapticFeedbacks; /// <summary> haptic feedback vector to the human fingertips
+
     std::vector<double>
         humanForceFeedbacks; /// <summary> force feedback vector to the human fingertips
 

modules/HapticGlove_module/src/GloveControlHelper.cpp

@@ -178,7 +178,7 @@ bool GloveControlHelper::setFingertipForceFeedbackReferences(
                               / m_maxForceFeedback);
     }
 
-    return m_pImp->setFingertipForceFeedbackValues(m_desiredForceValues);
+    return true;
 }
 
 bool GloveControlHelper::setFingertipVibrotactileFeedbackReferences(
@@ -199,8 +199,14 @@ bool GloveControlHelper::setFingertipVibrotactileFeedbackReferences(
             = (int)std::round(std::max(0.0, std::min(desiredValue[i], 100.0)));
     }
 
-    return m_pImp->setFingertipVibrotactileValues(m_desiredVibrotactileValues);
+    return true;
+}
+
+bool GloveControlHelper::sendFingertipHapticFeedbackReferences()
+{
+    return m_pImp->setFingertipHapticFeedbackValues(m_desiredForceValues, m_desiredVibrotactileValues);
 }
+
 bool GloveControlHelper::stopPalmVibrotactileFeedback()
 {
     return m_pImp->setPalmVibrotactileValue(to_underlying(
@@ -211,13 +217,13 @@ bool GloveControlHelper::stopPalmVibrotactileFeedback()
 bool GloveControlHelper::stopVibrotactileFeedback()
 {
     std::fill(m_desiredVibrotactileValues.begin(), m_desiredVibrotactileValues.end(), 0.0);
-    return m_pImp->setFingertipVibrotactileValues(m_desiredVibrotactileValues);
+    return true;
 }
 
 bool GloveControlHelper::stopForceFeedback()
 {
     std::fill(m_desiredForceValues.begin(), m_desiredForceValues.end(), 0.0);
-    return m_pImp->setFingertipForceFeedbackValues(m_desiredForceValues);
+    return true;
 }
 
 bool GloveControlHelper::stopHapticFeedback()
@@ -361,6 +367,7 @@ bool GloveControlHelper::findHumanMotionRange()
 
     std::vector<double> desiredValue(m_numVibrotactileFeedback, 35);
     this->setFingertipVibrotactileFeedbackReferences(desiredValue);
+    this->sendFingertipHapticFeedbackReferences();
     return true;
 }
 

modules/HapticGlove_module/src/GloveWearable.cpp

@@ -17,6 +17,7 @@ GloveWearableImpl::GloveWearableImpl(const size_t& numFingers,
     , m_numForceFeedback(numForceFeedback)
     , m_numVibrotactileFeedback(numVibrotactileFeedback)
     , m_numHandJoints(numHandJoints)
+    , m_numHapticFeedback(numForceFeedback + numVibrotactileFeedback)
 {
     m_logPrefix = "GloveWearableImpl::";
 }
@@ -141,13 +142,13 @@ bool GloveWearableImpl::configure(const yarp::os::Searchable& config,
         return false;
     }
 
-    if (!m_iWearActuatorPort.open(portNameOut))
+    if (!m_iWearGloveActuatorPort.open(portNameOut))
     {
         yError() << m_logPrefix << portNameOut << " port already open.";
         return false;
     }
 
-    if (!Network::connect(m_iWearActuatorPort.getName(), portNameIn, "fast_tcp"))
+    if (!Network::connect(m_iWearGloveActuatorPort.getName(), portNameIn, "fast_tcp"))
     {
         yError() << m_logPrefix << "output port: " << portNameOut << "input port: " << portNameIn
                  << " unable to connect the ports.";
@@ -327,63 +328,34 @@ bool GloveWearableImpl::getFingertipPoseValues(Eigen::MatrixXd& values)
     return true;
 }
 
-bool GloveWearableImpl::setFingertipForceFeedbackValues(const std::vector<int>& values)
+bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector<int>& forceValues, const std::vector<int>& vibroValues)
 {
-    if (values.size() != m_numForceFeedback)
+    if (forceValues.size() + vibroValues.size() != m_numHapticFeedback)
     {
         yError() << m_logPrefix
-                 << "size of the force feedback vector is not equal to the size of the default "
-                    "force feedback size.";
+                 << "size of the haptic feedback vector is not equal to the size of the default "
+                    "haptic feedback size.";
     }
-    for (size_t i = 0; i < values.size(); i++)
-    {
-        std::string fingerName = m_humanFingerNameList[i];
-
-        wearable::msg::WearableActuatorCommand& wearableActuatorCommand
-            = m_iWearActuatorPort.prepare();
-
-        wearableActuatorCommand.value = values[i];
-        wearableActuatorCommand.info.name = m_wearablePrefix
-                                            + wearable::actuator::IHaptic::getPrefix() + fingerName
-                                            + "::ForceFeedback";
-        wearableActuatorCommand.info.type = wearable::msg::ActuatorType::HAPTIC;
-        wearableActuatorCommand.duration = 0;
-
-        m_iWearActuatorPort.write(true); // writeStrict option for wearable haptic device should be
-                                         // set to true to avoid the data loss for all actuators
-    }
-    return true;
-}
+    wearable::msg::GloveActuatorCommand& gloveActuatorCommand
+        = m_iWearGloveActuatorPort.prepare();
+    gloveActuatorCommand.forceValue.resize(m_numForceFeedback);
+    gloveActuatorCommand.vibroTactileValue.resize(m_numVibrotactileFeedback);
 
-bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector<int>& values)
-{
-    if (values.size() != m_numVibrotactileFeedback)
+    for (size_t i = 0; i < forceValues.size(); i++)
     {
-        yError()
-            << m_logPrefix
-            << "size of the vibrotactile feedback vector is not equal to the size of the default "
-               "vibrotactile feedback size.";
+        gloveActuatorCommand.forceValue[i] = forceValues[i];
     }
-
-    for (size_t i = 0; i < values.size(); i++)
+    for (size_t i = 0; i < vibroValues.size(); i++)
     {
-        std::string fingerName = m_humanFingerNameList[i];
-
-        wearable::msg::WearableActuatorCommand& wearableActuatorCommand
-            = m_iWearActuatorPort.prepare();
-
-        wearableActuatorCommand.value = values[i];
-        wearableActuatorCommand.info.name = m_wearablePrefix
-                                            + wearable::actuator::IHaptic::getPrefix() + fingerName
-                                            + "::VibroTactileFeedback";
-
-        wearableActuatorCommand.info.type = wearable::msg::ActuatorType::HAPTIC;
-        wearableActuatorCommand.duration = 0;
-
-        m_iWearActuatorPort.write(true); // writeStrict option for wearable haptic device should be
-                                         // set to true to avoid the data loss for all actuators
+        gloveActuatorCommand.vibroTactileValue[i] = vibroValues[i];
     }
 
+        gloveActuatorCommand.info.name = m_wearablePrefix
+                                            + wearable::actuator::IHaptic::getPrefix() +
+                                            + "HapticFeedback";
+        gloveActuatorCommand.info.type = wearable::msg::ActuatorType::HAPTIC;
+
+        m_iWearGloveActuatorPort.write();
     return true;
 }
 
@@ -403,15 +375,6 @@ bool GloveWearableImpl::setPalmVibrotactileValue(const int& value)
      * Object_Grasp_30 = 9
      */
 
-    wearable::msg::WearableActuatorCommand& wearableActuatorCommand = m_iWearActuatorPort.prepare();
-
-    wearableActuatorCommand.value = value;
-    wearableActuatorCommand.info.name = m_wearablePrefix + wearable::actuator::IHaptic::getPrefix()
-                                        + m_handLinkName + "::VibroTactileFeedback";
-    wearableActuatorCommand.info.type = wearable::msg::ActuatorType::HAPTIC;
-    wearableActuatorCommand.duration = 0;
-
-    m_iWearActuatorPort.write(false);
     return true;
 }
 
@@ -421,7 +384,7 @@ bool GloveWearableImpl::close()
     m_iWear = nullptr;
     m_wearableDevice.close();
 
-    m_iWearActuatorPort.close();
+    m_iWearGloveActuatorPort.close();
 
     return true;
 }

modules/HapticGlove_module/src/HapticGloveModule.cpp

@@ -22,8 +22,6 @@ bool HapticGloveModule::configure(yarp::os::ResourceFinder& rf)
 {
     m_state = HapticGloveFSM::Configuring;
 
-    yarp::os::Value* value;
-
     // check if the configuration file is empty
     if (rf.isNull())
     {

modules/HapticGlove_module/src/Teleoperation.cpp

@@ -354,6 +354,7 @@ bool Teleoperation::run()
         m_robotController->move();
         m_humanGlove->setFingertipForceFeedbackReferences(m_data.humanForceFeedbacks);
         m_humanGlove->setFingertipVibrotactileFeedbackReferences(m_data.humanVibrotactileFeedbacks);
+        m_humanGlove->sendFingertipHapticFeedbackReferences();
     }
 
     if (m_enableLogger)