Merge pull request #124 from lrapetti/HapticGlove_module/new-features

Haptic glove module new features

app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini

@@ -5,7 +5,7 @@ remote_control_boards   ("left_arm")
 
 remote_sensor_boards    "left_hand"
 
-axis_list             ( "l_hand_finger", "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index_distal", "l_middle_proximal", "l_middle_distal", "l_pinky" )
+axis_list             ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index_distal", "l_middle_proximal", "l_middle_distal", "l_pinky" )
 
 all_axis_list         ( "l_hand_finger", "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal", "l_index_proximal", "l_index_distal", "l_middle_proximal", "l_middle_distal", "l_pinky" )
 
@@ -21,8 +21,8 @@ joint_list                   (  "l_thumb_oppose", "l_thumb_proximal", "l_thumb_m
 
 # the custom axes home angle value [degrees]
 # By default it home values are set to min axis values
-#                        axis-name ,         axis value
-axes_custom_home_angle  (   ( "l_thumb_oppose" ,  30) )
+#                                  axis-name ,  axis value
+# axes_custom_home_angle  (   ( ${AXES_NAME} ,      ${VAL}) )
 
 # if a joint is not listed here, the sensory information is of encoder type.
 # notice that "l_thumb_oppose" and "l_hand_fingers" is measured by joint encoders not the analog sensors
@@ -64,12 +64,34 @@ wearable_data_actuator_ports_out "/WearableData/HapticGlove/LeftHand/Actuators/i
 
 wearable_data_actuator_ports_in "/WearableData/HapticGlove/LeftHand/Actuators/input:i"
 
+# delta on human finger joint motion range
+motion_range_min_delta  (   ( "l_thumb_oppose" ,  -30) )
+
 ###############
 ### RETARGETING
 ###############
 
 motorsJointsCoupled      1
 
+# motion retargetin
+# motion retargetin
+robot_to_human_map              ( ("l_thumb_oppose",    "l_thumb_oppose")
+                                  ("l_thumb_proximal",  "l_thumb_proximal")
+                                  ("l_thumb_distal",    "l_thumb_distal" )
+                                  ("l_thumb_middle",    "l_thumb_middle" )
+                                  ("l_index_proximal",  "l_index_proximal")
+                                  ("l_index_distal",    "l_index_distal")
+                                  ("l_index_middle",    "l_index_middle")
+                                  ("l_middle_proximal", "l_middle_proximal")
+                                  ("l_middle_distal",   "l_middle_distal")
+                                  ("l_middle_middle",   "l_middle_middle")
+                                  ("l_ring_proximal", "l_ring_proximal")
+                                  ("l_ring_distal",   "l_ring_distal")
+                                  ("l_ring_middle",   "l_ring_middle")
+                                  ("l_pinky_proximal", "l_pinky_proximal")
+                                  ("l_pinky_distal",   "l_pinky_distal")
+                                  ("l_pinky_middle",   "l_pinky_middle") )
+
 # haptic feedback retargeting from the robot axis groups to the human finger
 l_thumb_finger                  ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_distal" )
 l_index_finger                  ( "l_hand_finger", "l_index_proximal", "l_index_distal" )

app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini

@@ -21,8 +21,8 @@ joint_list            (  "r_thumb_oppose",    "r_thumb_proximal",  "r_thumb_midd
 
 # the custom axes home angle value [degrees]
 # By default it home values are set to min axis values
-#                        axis-name ,         axis value
-axes_custom_home_angle  (   ( "r_thumb_oppose" ,  30) )
+#                                  axis-name ,  axis value
+# axes_custom_home_angle  (   ( ${AXES_NAME} ,      ${VAL}) )
 
 # if a joint is not listed here, the sensory information is of encoder type.
 # notice that "r_thumb_oppose" and "r_hand_finger" is measured by joint encoders not the analog sensors
@@ -64,12 +64,34 @@ wearable_data_actuator_ports_out "/WearableData/HapticGlove/RightHand/Actuators/
 
 wearable_data_actuator_ports_in "/WearableData/HapticGlove/RightHand/Actuators/input:i"
 
+# delta on human finger joint motion range
+motion_range_min_delta  (   ( "r_thumb_oppose" ,  30) )
+
 ###############
 ### RETARGETING
 ###############
 
 motorsJointsCoupled      1
 
+# motion retargetin
+robot_to_human_map              ( ("r_thumb_oppose",    "r_thumb_oppose")
+                                  ("r_thumb_proximal",  "r_thumb_proximal")
+                                  ("r_thumb_distal",    "r_thumb_distal" )
+                                  ("r_thumb_middle",    "r_thumb_middle" )
+                                  ("r_index_proximal",  "r_index_proximal")
+                                  ("r_index_distal",    "r_index_distal")
+                                  ("r_index_middle",    "r_index_middle")
+                                  ("r_middle_proximal", "r_index_proximal")
+                                  ("r_middle_distal",   "r_index_distal")
+                                  ("r_middle_middle",   "r_index_middle")
+                                  ("r_ring_proximal", "r_ring_proximal")
+                                  ("r_ring_distal",   "r_ring_distal")
+                                  ("r_ring_middle",   "r_ring_middle")
+                                  ("r_pinky_proximal", "r_pinky_proximal")
+                                  ("r_pinky_distal",   "r_pinky_distal")
+                                  ("r_pinky_middle",   "r_pinky_middle") )
+
+
 # haptic feedback retargeting from the robot axis groups to the human finger
 r_thumb_finger                  ( "r_thumb_oppose",   "r_thumb_proximal", "r_thumb_distal"  )
 r_index_finger                  ( "r_hand_finger", "r_index_proximal", "r_index-distal"  )

modules/HapticGlove_module/include/GloveControlHelper.hpp

@@ -62,6 +62,9 @@ class HapticGlove::GloveControlHelper
     std::vector<double> m_jointRangeMax; /**< Max value for every human hand joint, computed at the
                                             end of the preparation phase*/
 
+    std::vector<double> m_jointRangeMaxDelta;
+    std::vector<double> m_jointRangeMinDelta;
+
     std::unique_ptr<GloveWearableImpl>
         m_pImp; /**< Sense glove wearable interface impelemntation. */
 

modules/HapticGlove_module/include/Retargeting.hpp

@@ -101,6 +101,7 @@ class HapticGlove::Retargeting
     std::map<size_t, std::vector<size_t>>
         m_fingerAxesMap; /**< a map showing for each finger which axis of robot is relevant*/
 
+    bool m_useSemanticMap = false; /**< a flag that defines if the robot to human joint map is done using semantic map*/
     // methods
 
     /**
@@ -153,9 +154,9 @@ class HapticGlove::Retargeting
      * the human joints
      * @return true/false in case of success/failure
      */
-    bool semanticMapFromRobotTHuman(const std::vector<std::string>& humanJointNames,
-                                    const std::vector<std::string>& robotJointNames,
-                                    std::map<size_t, size_t>& robotToHumanMap);
+    bool getSemanticMapFromRobotToHuman(const std::vector<std::string>& humanJointNames,
+                                        const std::vector<std::string>& robotJointNames,
+                                        std::map<size_t, size_t>& robotToHumanMap);
 
 public:
     /**

modules/HapticGlove_module/include/RobotSkin.hpp

@@ -86,7 +86,7 @@ struct HapticGlove::FingertipTactileData
                             - o: number of observations (logged data),
                             - m: number of tactile sensors*/
 
-    bool isFingerInContact = false;
+    bool isFingerContactEnabled = true;
 
     double maxTactileFeedbackAbsoluteValue()
     {
@@ -165,6 +165,8 @@ class HapticGlove::RobotSkin : RobotSkinService
 
     std::vector<bool> m_areTactileSensorsWorking;
     std::vector<bool> m_areFingersInContact;
+    std::vector<double> m_fingersInContactTimer;
+    std::vector<int> m_fingersLastElementInContact;
     std::vector<bool> m_areFingersContactChanges;
 
     std::vector<double> m_fingersVibrotactileAbsoluteFeedback;

modules/HapticGlove_module/src/GloveControlHelper.cpp

@@ -83,6 +83,56 @@ bool GloveControlHelper::configure(const yarp::os::Searchable& config,
     }
     m_jointRangeMin.resize(m_humanJointNameList.size(), 0.0);
     m_jointRangeMax.resize(m_humanJointNameList.size(), 0.0);
+    m_jointRangeMinDelta.resize(m_humanJointNameList.size(), 0.0);
+    m_jointRangeMaxDelta.resize(m_humanJointNameList.size(), 0.0);
+
+    // human motion range minimum delta
+    if (config.check("motion_range_min_delta") && config.find("motion_range_min_delta").isList())
+    {
+        yarp::os::Bottle* jointRangeMinDeltaMap = config.find("motion_range_min_delta").asList();
+        for (size_t i = 0; i < jointRangeMinDeltaMap->size(); i++)
+        {
+            yarp::os::Bottle* jointRangeMinDeltaValue = jointRangeMinDeltaMap->get(i).asList();
+            std::string jointName = jointRangeMinDeltaValue->get(0).asString();
+
+            double deltaMinVal =jointRangeMinDeltaValue->get(1).asFloat64() * M_PI / 180; // [rad]
+
+            auto jointElement
+                = std::find(std::begin(m_humanJointNameList), std::end(m_humanJointNameList), jointName);
+            if (jointElement == std::end(m_humanJointNameList))
+            {
+                yError() << m_logPrefix << "cannot find the joint " << jointName
+                         << "written in `motion_range_min_delta` among the humanFingerNameList.";
+                return false;
+            }
+
+            m_jointRangeMinDelta[std::distance(m_humanJointNameList.begin(), jointElement)] = deltaMinVal;
+        }
+    }
+
+    // human motion range maximum delta
+    if (config.check("motion_range_max_delta") && config.find("motion_range_max_delta").isList())
+    {
+        yarp::os::Bottle* jointRangeMaxDeltaMap = config.find("motion_range_max_delta").asList();
+        for (size_t i = 0; i < jointRangeMaxDeltaMap->size(); i++)
+        {
+            yarp::os::Bottle* jointRangeMaxDeltaValue = jointRangeMaxDeltaMap->get(i).asList();
+            std::string jointName = jointRangeMaxDeltaValue->get(0).asString();
+
+            double deltaMaxVal =jointRangeMaxDeltaValue->get(1).asFloat64() * M_PI / 180; // [rad]
+
+            auto jointElement
+                = std::find(std::begin(m_humanJointNameList), std::end(m_humanJointNameList), jointName);
+            if (jointElement == std::end(m_humanJointNameList))
+            {
+                yError() << m_logPrefix << "cannot find the joint " << jointName
+                         << "written in `motion_range_max_delta` among the humanFingerNameList.";
+                return false;
+            }
+
+            m_jointRangeMaxDelta[std::distance(m_humanJointNameList.begin(), jointElement)] = deltaMaxVal;
+        }
+    }
 
     // wearable device
     m_pImp = std::make_unique<GloveWearableImpl>(
@@ -324,6 +374,14 @@ bool GloveControlHelper::getHumanFingerJointsMotionRange(std::vector<double>& jo
 {
     jointRangeMax = m_jointRangeMax;
     jointRangeMin = m_jointRangeMin;
+
+    // Add deltas on human motion range
+    for (size_t i = 0; i < m_humanJointNameList.size(); i++)
+    {
+        jointRangeMax[i] = jointRangeMax[i] + m_jointRangeMaxDelta[i];
+        jointRangeMin[i] = jointRangeMin[i] + m_jointRangeMinDelta[i];
+    }
+
     yInfo() << m_logPrefix << "human joint names:" << m_humanJointNameList;
     yInfo() << m_logPrefix << "human min joint range:" << m_jointRangeMin;
     yInfo() << m_logPrefix << "human max joint range:" << m_jointRangeMax;

modules/HapticGlove_module/src/Retargeting.cpp

@@ -167,13 +167,76 @@ bool Retargeting::configure(const yarp::os::Searchable& config,
         return false;
     }
 
-    // get human and robot joint list and find the mapping between them
-    if (!this->semanticMapFromRobotTHuman(
-            m_humanJointNames, m_robotActuatedJointNames, m_robotToHumanJointIndicesMap))
+    // check if a semantic map or robot_to_human_map configuration paramter should be used instead
+    if (config.check("useSemanticMap") && config.find("useSemanticMap").isBool())
     {
-        yError() << m_logPrefix
-                 << "unable to find the map from robot actuated joints to the human joints";
-        return false;
+        m_useSemanticMap = config.find("useSemanticMap").asBool();
+    }
+
+    if (!m_useSemanticMap)
+    {
+        if (!m_robotToHumanJointIndicesMap.empty())
+        {
+            m_robotToHumanJointIndicesMap.clear();
+        }
+
+        // get human and robot joint list and find the mapping between them
+        if (config.check("robot_to_human_map") && config.find("robot_to_human_map").isList())
+        {
+            yarp::os::Bottle* robotToHumanMap = config.find("robot_to_human_map").asList();
+
+            if (robotToHumanMap->size() != m_robotActuatedJointNames.size())
+            {
+                yError() << m_logPrefix << "number of robot joint in robot_to_human_map" << robotToHumanMap->size()
+                                        << "does not correspond to actuated robot joints" << m_robotActuatedJointNames.size(); 
+                return false;
+            }
+
+            for (size_t i = 0; i < robotToHumanMap->size(); i++)
+            {
+                yarp::os::Bottle* robotToHumanMapValue = robotToHumanMap->get(i).asList();
+                std::string robotJoint = robotToHumanMapValue->get(0).asString();
+                std::string humanJoint = robotToHumanMapValue->get(1).asString();
+
+                auto indexHuman = std::find(std::begin(m_humanJointNames), std::end(m_humanJointNames), humanJoint);
+                if (indexHuman == std::end(m_humanJointNames))
+                {
+                    yError() << m_logPrefix << "in robot_to_human_map found non-exising human joint " 
+                             << humanJoint;
+                    return false;
+                }
+
+                auto indexRobot = std::find(std::begin(m_robotActuatedJointNames), std::end(m_robotActuatedJointNames), robotJoint);
+                if (indexRobot == std::end(m_robotActuatedJointNames))
+                {
+                    yError() << m_logPrefix << "in robot_to_human_map found non-exising robot joint " 
+                             << robotJoint;
+                    return false;
+                }
+
+                size_t indexHumanJoint = indexHuman - m_humanJointNames.begin();
+                size_t indexRobotJoint = indexRobot - m_robotActuatedJointNames.begin();
+                m_robotToHumanJointIndicesMap.insert(std::pair<size_t, size_t>(indexRobotJoint, indexHumanJoint));
+            }
+
+            // TODO check that all the robot joints are preent
+        }
+        else
+        {
+            yError() << m_logPrefix
+                    << "robot_to_human_map not found or not valid";
+            return false;
+        }
+    }
+    else {
+        if (!this->getSemanticMapFromRobotToHuman(
+                m_humanJointNames, m_robotActuatedJointNames, m_robotToHumanJointIndicesMap))
+        {
+            yError() << m_logPrefix
+                    << "unable to find the semantic map from robot actuated joints to the human joints";
+            return false;
+        }
+        yInfo() << m_logPrefix << "a semantic map is used to define the map from robot actuated joints to the human joints";
     }
 
     // find the human finger names
@@ -475,7 +538,7 @@ bool Retargeting::getAxisError(std::vector<double>& axisValueErrors,
     return true;
 }
 
-bool Retargeting::semanticMapFromRobotTHuman(const std::vector<std::string>& humanJointNames,
+bool Retargeting::getSemanticMapFromRobotToHuman(const std::vector<std::string>& humanJointNames,
                                              const std::vector<std::string>& robotJointNames,
                                              std::map<size_t, size_t>& robotToHumanMap)
 {

modules/HapticGlove_module/src/RobotSkin.cpp

@@ -51,6 +51,8 @@ bool RobotSkin::configure(const yarp::os::Searchable& config,
     m_totalNoTactile = 0;
 
     m_areFingersInContact.resize(m_noFingers, false);
+    m_fingersInContactTimer.resize(m_noFingers, 0.0);
+    m_fingersLastElementInContact.resize(m_noFingers, 0.0);
     m_areFingersContactChanges.resize(m_noFingers, false);
     m_areTactileSensorsWorking.resize(m_noFingers, false);
 
@@ -384,6 +386,29 @@ void RobotSkin::computeAreFingersInContact()
     {
         m_areFingersInContact[i] = m_fingersContactStrength[i]
                                    > m_fingersTactileData[i].contactThreshold();
+
+        // Update the timer only if the same taxel is causing the contact
+        if (m_areFingersInContact[i] &&
+            m_fingersLastElementInContact[i] == m_fingersTactileData[i].maxTactileFeedbackAbsoluteElement()) {
+            m_fingersInContactTimer[i] += m_samplingTime;
+        }
+        else {
+            m_fingersInContactTimer[i] = 0.0;
+            m_fingersTactileData[i].isFingerContactEnabled = true;
+        }
+
+        // If a taxel is causing the activation for longer then the given time, the sensor bias is increased.
+        if (m_fingersInContactTimer[i] > 2.0) { // TODO: move to a parameter
+            m_fingersTactileData[i].isFingerContactEnabled = false;
+        }
+
+        // Update last finger in contact element fi contact is detected
+        m_fingersLastElementInContact[i] = m_areFingersInContact[i] ? m_fingersTactileData[i].maxTactileFeedbackAbsoluteElement() : -1;
+
+        // Check if the finger has been diabled by the timer
+        m_areFingersInContact[i] = m_areFingersInContact[i] && m_fingersTactileData[i].isFingerContactEnabled;
+
+
         m_areFingersContactChanges[i] = m_areFingersInContact[i]
                                         && (m_fingersContactStrengthDerivate[i]
                                         > m_fingersTactileData[i].contactDerivativeThreshold());