From eb6c96a9b5069c5a462341ae61a6ac4fb581a8fc Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Sat, 8 Oct 2022 23:01:10 +0200 Subject: [PATCH 1/4] Add parameter to add offsets on the human hand motion range --- .../leftFingersHapticRetargetingParams.ini | 3 + .../rightFingersHapticRetargetingParams.ini | 3 + .../include/GloveControlHelper.hpp | 3 + .../src/GloveControlHelper.cpp | 58 +++++++++++++++++++ 4 files changed, 67 insertions(+) diff --git a/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini b/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini index 80cb66fad..5e4f51f68 100644 --- a/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini +++ b/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini @@ -64,6 +64,9 @@ 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 ############### diff --git a/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini b/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini index 2572d68c8..45b8609e9 100644 --- a/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini +++ b/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini @@ -64,6 +64,9 @@ 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 ############### diff --git a/modules/HapticGlove_module/include/GloveControlHelper.hpp b/modules/HapticGlove_module/include/GloveControlHelper.hpp index dea0d742d..ba8f49499 100644 --- a/modules/HapticGlove_module/include/GloveControlHelper.hpp +++ b/modules/HapticGlove_module/include/GloveControlHelper.hpp @@ -62,6 +62,9 @@ class HapticGlove::GloveControlHelper std::vector m_jointRangeMax; /**< Max value for every human hand joint, computed at the end of the preparation phase*/ + std::vector m_jointRangeMaxDelta; + std::vector m_jointRangeMinDelta; + std::unique_ptr m_pImp; /**< Sense glove wearable interface impelemntation. */ diff --git a/modules/HapticGlove_module/src/GloveControlHelper.cpp b/modules/HapticGlove_module/src/GloveControlHelper.cpp index 2ea9e6ed8..bd6468f1f 100644 --- a/modules/HapticGlove_module/src/GloveControlHelper.cpp +++ b/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( @@ -324,6 +374,14 @@ bool GloveControlHelper::getHumanFingerJointsMotionRange(std::vector& 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; From 159d5b8d5a3eacd98810036db5f157c16f44ae06 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Sun, 9 Oct 2022 11:51:19 +0200 Subject: [PATCH 2/4] Update configuration file --- .../iCubGenova09/leftFingersHapticRetargetingParams.ini | 6 +++--- .../iCubGenova09/rightFingersHapticRetargetingParams.ini | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini b/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini index 5e4f51f68..8cfd7ca19 100644 --- a/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini +++ b/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini @@ -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 @@ -65,7 +65,7 @@ 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) ) +motion_range_min_delta ( ( "l_thumb_oppose" , -30) ) ############### ### RETARGETING diff --git a/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini b/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini index 45b8609e9..640480aaa 100644 --- a/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini +++ b/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 From c8ac0d12c76cf398b7bd73ac4162bc22031eea90 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Sun, 9 Oct 2022 14:54:32 +0200 Subject: [PATCH 3/4] Add robot_to_human map parameter --- .../leftFingersHapticRetargetingParams.ini | 21 ++++- .../rightFingersHapticRetargetingParams.ini | 19 +++++ .../include/Retargeting.hpp | 7 +- .../HapticGlove_module/src/Retargeting.cpp | 77 +++++++++++++++++-- 4 files changed, 113 insertions(+), 11 deletions(-) diff --git a/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini b/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini index 8cfd7ca19..1e6e4ad56 100644 --- a/app/robots/iCubGenova09/leftFingersHapticRetargetingParams.ini +++ b/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" ) @@ -73,6 +73,25 @@ motion_range_min_delta ( ( "l_thumb_oppose" , -30) ) 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" ) diff --git a/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini b/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini index 640480aaa..d71505b5e 100644 --- a/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini +++ b/app/robots/iCubGenova09/rightFingersHapticRetargetingParams.ini @@ -73,6 +73,25 @@ motion_range_min_delta ( ( "r_thumb_oppose" , 30) ) 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" ) diff --git a/modules/HapticGlove_module/include/Retargeting.hpp b/modules/HapticGlove_module/include/Retargeting.hpp index 55b2b88f2..23134ab47 100644 --- a/modules/HapticGlove_module/include/Retargeting.hpp +++ b/modules/HapticGlove_module/include/Retargeting.hpp @@ -101,6 +101,7 @@ class HapticGlove::Retargeting std::map> 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& humanJointNames, - const std::vector& robotJointNames, - std::map& robotToHumanMap); + bool getSemanticMapFromRobotToHuman(const std::vector& humanJointNames, + const std::vector& robotJointNames, + std::map& robotToHumanMap); public: /** diff --git a/modules/HapticGlove_module/src/Retargeting.cpp b/modules/HapticGlove_module/src/Retargeting.cpp index 43d060912..2376dfe73 100644 --- a/modules/HapticGlove_module/src/Retargeting.cpp +++ b/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(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& axisValueErrors, return true; } -bool Retargeting::semanticMapFromRobotTHuman(const std::vector& humanJointNames, +bool Retargeting::getSemanticMapFromRobotToHuman(const std::vector& humanJointNames, const std::vector& robotJointNames, std::map& robotToHumanMap) { From 319647e5b188f582051de73e50604236a98f1ea7 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Sat, 8 Oct 2022 18:38:07 +0200 Subject: [PATCH 4/4] Add timer to disable a finger if a taxel is activated for long time --- .../HapticGlove_module/include/RobotSkin.hpp | 4 ++- modules/HapticGlove_module/src/RobotSkin.cpp | 25 +++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/modules/HapticGlove_module/include/RobotSkin.hpp b/modules/HapticGlove_module/include/RobotSkin.hpp index c5477b820..41588c4bf 100644 --- a/modules/HapticGlove_module/include/RobotSkin.hpp +++ b/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 m_areTactileSensorsWorking; std::vector m_areFingersInContact; + std::vector m_fingersInContactTimer; + std::vector m_fingersLastElementInContact; std::vector m_areFingersContactChanges; std::vector m_fingersVibrotactileAbsoluteFeedback; diff --git a/modules/HapticGlove_module/src/RobotSkin.cpp b/modules/HapticGlove_module/src/RobotSkin.cpp index e18829d57..25a3ab183 100644 --- a/modules/HapticGlove_module/src/RobotSkin.cpp +++ b/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());