Fix Haptic glove transmission - HapticGloveModule

.gitignore

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

modules/HapticGlove_module/include/GloveControlHelper.hpp

@@ -40,6 +40,7 @@ class HapticGlove::GloveControlHelper
 
     bool m_isRightHand; /**< true if the glove is the right hand*/
 
+    std::vector<int> m_desiredHapticValues; /**< Desired haptic feedback values; range: [0, 100]. */
     std::vector<int> m_desiredForceValues; /**< Desired force feedback values; range: [0, 100]. */
     std::vector<int>
         m_desiredVibrotactileValues; /**< Desired vibrotactile feedback values; range: [0, 100] */
@@ -93,6 +94,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 setFingertipHapticFeedbackReferences();
+
     /**
      * Set the desired force feedback reference to the user fingertip
      * @param desiredValue desired force feedback values
@@ -115,6 +122,12 @@ class HapticGlove::GloveControlHelper
      */
     bool setPalmVibrotactileFeedbackReference(const int& desiredValue);
 
+    /**
+     * stop the haptics feedback (force feedback, fingertip vibrotactile feedback) to the user
+     * @return  true/false in case of success/failure
+     */
+    bool stopAllHapticFeedback();
+
     /**
      * stop the haptic feedback (force feedback, fingertip vibrotactile feedback, palm vibrotactile
      * feedback) to the user

modules/HapticGlove_module/include/GloveWearable.hpp

@@ -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
@@ -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

@@ -32,6 +32,7 @@ bool GloveControlHelper::configure(const yarp::os::Searchable& config,
 
     m_desiredVibrotactileValues.resize(m_numVibrotactileFeedback, 0);
     m_desiredForceValues.resize(m_numForceFeedback, 0);
+    m_desiredHapticValues.resize(m_numForceFeedback + m_numVibrotactileFeedback, 0);
 
     m_JointsValues.resize(m_numHandJoints, 0.0);
 
@@ -178,7 +179,7 @@ bool GloveControlHelper::setFingertipForceFeedbackReferences(
                               / m_maxForceFeedback);
     }
 
-    return m_pImp->setFingertipForceFeedbackValues(m_desiredForceValues);
+    return true;
 }
 
 bool GloveControlHelper::setFingertipVibrotactileFeedbackReferences(
@@ -199,8 +200,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::setFingertipHapticFeedbackReferences()
+{
+    return m_pImp->setFingertipHapticFeedbackValues(m_desiredForceValues, m_desiredVibrotactileValues);
+}
+
 bool GloveControlHelper::stopPalmVibrotactileFeedback()
 {
     return m_pImp->setPalmVibrotactileValue(to_underlying(
@@ -211,13 +218,19 @@ 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::stopAllHapticFeedback()
+{
+    std::fill(m_desiredHapticValues.begin(), m_desiredHapticValues.end(), 0.0);
+    return m_pImp->setFingertipHapticFeedbackValues(m_desiredForceValues, m_desiredVibrotactileValues);
 }
 
 bool GloveControlHelper::stopHapticFeedback()
@@ -239,6 +252,11 @@ bool GloveControlHelper::stopHapticFeedback()
                     "vibrotactile feedback";
         return false;
     }
+    if (!stopAllHapticFeedback())
+    {
+        yError() << m_logPrefix << "Cannot turn off the fingertip haptics feedback";
+        return false;
+    }
     return true;
 }
 
@@ -361,6 +379,7 @@ bool GloveControlHelper::findHumanMotionRange()
 
     std::vector<double> desiredValue(m_numVibrotactileFeedback, 35);
     this->setFingertipVibrotactileFeedbackReferences(desiredValue);
+    this->setFingertipHapticFeedbackReferences();
     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::";
 }
@@ -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::WearableActuatorCommand& wearableActuatorCommand
+        = m_iWearActuatorPort.prepare();
+    wearableActuatorCommand.forceValue.resize(m_numForceFeedback);
+    wearableActuatorCommand.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.";
+        wearableActuatorCommand.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.vibroTactileValue[i] = vibroValues[i];
+    }
 
-        wearableActuatorCommand.value = values[i];
         wearableActuatorCommand.info.name = m_wearablePrefix
-                                            + wearable::actuator::IHaptic::getPrefix() + fingerName
-                                            + "::VibroTactileFeedback";
-
+                                            + wearable::actuator::IHaptic::getPrefix() +
+                                            + "HapticFeedback";
         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
-    }
 
+        m_iWearActuatorPort.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;
 }
 

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->setFingertipHapticFeedbackReferences();
     }
 
     if (m_enableLogger)