From 878054a9ce8329c48fe42f539dac7bddfb9313b1 Mon Sep 17 00:00:00 2001 From: Ehsan Ranjbari Date: Fri, 26 Jan 2024 11:27:12 +0100 Subject: [PATCH 01/10] dummychange --- modules/HapticGlove_module/dummyChange.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 modules/HapticGlove_module/dummyChange.txt diff --git a/modules/HapticGlove_module/dummyChange.txt b/modules/HapticGlove_module/dummyChange.txt new file mode 100644 index 00000000..7a800fc9 --- /dev/null +++ b/modules/HapticGlove_module/dummyChange.txt @@ -0,0 +1 @@ +dummy change From 2db05f22ac6d33a8e9c88b15d27a8c766104ba1f Mon Sep 17 00:00:00 2001 From: Ehsan Ranjbari Date: Tue, 30 Jan 2024 13:39:42 +0100 Subject: [PATCH 02/10] modified haptic msgs, now only one msg is written on the port --- .gitignore | 4 +- modules/HapticGlove_module/dummyChange.txt | 1 - .../include/GloveControlHelper.hpp | 13 ++ .../include/GloveWearable.hpp | 10 ++ .../include/Teleoperation.hpp | 3 + .../src/GloveControlHelper.cpp | 36 ++++++ .../HapticGlove_module/src/GloveWearable.cpp | 113 +++++++++++------- .../HapticGlove_module/src/Teleoperation.cpp | 1 + 8 files changed, 138 insertions(+), 43 deletions(-) delete mode 100644 modules/HapticGlove_module/dummyChange.txt diff --git a/.gitignore b/.gitignore index b6bd7bd1..35b8c128 100644 --- a/.gitignore +++ b/.gitignore @@ -43,4 +43,6 @@ build/ TAGS compile_commands.json -CMakeLists.txt.user \ No newline at end of file +CMakeLists.txt.user +.vscode/c_cpp_properties.json +.vscode/settings.json diff --git a/modules/HapticGlove_module/dummyChange.txt b/modules/HapticGlove_module/dummyChange.txt deleted file mode 100644 index 7a800fc9..00000000 --- a/modules/HapticGlove_module/dummyChange.txt +++ /dev/null @@ -1 +0,0 @@ -dummy change diff --git a/modules/HapticGlove_module/include/GloveControlHelper.hpp b/modules/HapticGlove_module/include/GloveControlHelper.hpp index 52cfddcb..fe8df27e 100644 --- a/modules/HapticGlove_module/include/GloveControlHelper.hpp +++ b/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 m_desiredHapticValues; /**< Desired haptic feedback values; range: [0, 100]. */ std::vector m_desiredForceValues; /**< Desired force feedback values; range: [0, 100]. */ std::vector m_desiredVibrotactileValues; /**< Desired vibrotactile feedback values; range: [0, 100] */ @@ -93,6 +94,12 @@ class HapticGlove::GloveControlHelper */ bool getHandJointAngles(std::vector& 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 stopHapticsFeedback(); + /** * stop the haptic feedback (force feedback, fingertip vibrotactile feedback, palm vibrotactile * feedback) to the user diff --git a/modules/HapticGlove_module/include/GloveWearable.hpp b/modules/HapticGlove_module/include/GloveWearable.hpp index d3890cab..5a238ce0 100644 --- a/modules/HapticGlove_module/include/GloveWearable.hpp +++ b/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& values); + /** * 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 diff --git a/modules/HapticGlove_module/include/Teleoperation.hpp b/modules/HapticGlove_module/include/Teleoperation.hpp index a0abce31..83b3eded 100644 --- a/modules/HapticGlove_module/include/Teleoperation.hpp +++ b/modules/HapticGlove_module/include/Teleoperation.hpp @@ -91,6 +91,9 @@ struct HapticGlove::Data std::vector humanJointValues; /// juman joint values Eigen::MatrixXd humanFingertipPoses; /// human fingertip poses (size: number of hand /// fingertips (i.e., 5) x 7) + std::vector + humanHapticFeedbacks; /// haptic feedback vector to the human fingertips + std::vector humanForceFeedbacks; /// force feedback vector to the human fingertips diff --git a/modules/HapticGlove_module/src/GloveControlHelper.cpp b/modules/HapticGlove_module/src/GloveControlHelper.cpp index 7037c260..a006f790 100644 --- a/modules/HapticGlove_module/src/GloveControlHelper.cpp +++ b/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); @@ -201,6 +202,30 @@ bool GloveControlHelper::setFingertipVibrotactileFeedbackReferences( return m_pImp->setFingertipVibrotactileValues(m_desiredVibrotactileValues); } + +bool GloveControlHelper::setFingertipHapticFeedbackReferences() +{ + // if (desiredValue.size() != m_numHapticFeedback) + // { + // yError() << m_logPrefix + // << "the size of the input " + // "desired vecotr [" + // << desiredValue.size() << "] and the number of haptic feedbacks [ " + // << m_numHapticFeedback << " ]are not equal."; + // return false; + // } + + for (size_t i = 0; i < m_numForceFeedback; i++) + { + m_desiredHapticValues[i] = m_desiredForceValues[i]; + } + for (size_t i = 0; i < m_numVibrotactileFeedback; i++) + { + m_desiredHapticValues[i + m_numForceFeedback] = m_desiredVibrotactileValues[i]; + } + + return m_pImp->setFingertipHapticFeedbackValues(m_desiredHapticValues); +} bool GloveControlHelper::stopPalmVibrotactileFeedback() { return m_pImp->setPalmVibrotactileValue(to_underlying( @@ -220,6 +245,12 @@ bool GloveControlHelper::stopForceFeedback() return m_pImp->setFingertipForceFeedbackValues(m_desiredForceValues); } +bool GloveControlHelper::stopHapticsFeedback() +{ + std::fill(m_desiredHapticValues.begin(), m_desiredHapticValues.end(), 0.0); + return m_pImp->setFingertipHapticFeedbackValues(m_desiredHapticValues); +} + bool GloveControlHelper::stopHapticFeedback() { @@ -239,6 +270,11 @@ bool GloveControlHelper::stopHapticFeedback() "vibrotactile feedback"; return false; } + if (!stopHapticsFeedback()) + { + yError() << m_logPrefix << "Cannot turn off the fingertip haptics feedback"; + return false; + } return true; } diff --git a/modules/HapticGlove_module/src/GloveWearable.cpp b/modules/HapticGlove_module/src/GloveWearable.cpp index 48dc04e4..3da4f524 100644 --- a/modules/HapticGlove_module/src/GloveWearable.cpp +++ b/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::"; } @@ -329,59 +330,89 @@ bool GloveWearableImpl::getFingertipPoseValues(Eigen::MatrixXd& values) bool GloveWearableImpl::setFingertipForceFeedbackValues(const std::vector& values) { - if (values.size() != m_numForceFeedback) - { - yError() << m_logPrefix - << "size of the force feedback vector is not equal to the size of the default " - "force 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; + // if (values.size() != m_numForceFeedback) + // { + // yError() << m_logPrefix + // << "size of the force feedback vector is not equal to the size of the default " + // "force 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; +} - m_iWearActuatorPort.write(true); // writeStrict option for wearable haptic device should be +bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector& values) +{ + // if (values.size() != m_numVibrotactileFeedback) + // { + // yError() + // << m_logPrefix + // << "size of the vibrotactile feedback vector is not equal to the size of the default " + // "vibrotactile 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 + // + "::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 - } + // } + return true; } -bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector& values) +bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector& values) { - if (values.size() != m_numVibrotactileFeedback) + yDebug() << "values to be sent: " << values; + if (values.size() != m_numHapticFeedback) { - yError() - << m_logPrefix - << "size of the vibrotactile feedback vector is not equal to the size of the default " - "vibrotactile feedback size."; + yError() << m_logPrefix + << "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++) + for (size_t i = 0; i < values.size() / 2; i++) { std::string fingerName = m_humanFingerNameList[i]; wearable::msg::WearableActuatorCommand& wearableActuatorCommand = m_iWearActuatorPort.prepare(); - wearableActuatorCommand.value = values[i]; + wearableActuatorCommand.forceValue = values[i]; + wearableActuatorCommand.vibroTactileValue = values[i + m_numForceFeedback]; wearableActuatorCommand.info.name = m_wearablePrefix + wearable::actuator::IHaptic::getPrefix() + fingerName - + "::VibroTactileFeedback"; - + + "::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(true); } return true; @@ -403,15 +434,15 @@ bool GloveWearableImpl::setPalmVibrotactileValue(const int& value) * Object_Grasp_30 = 9 */ - wearable::msg::WearableActuatorCommand& wearableActuatorCommand = m_iWearActuatorPort.prepare(); + // 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; + // 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); + // m_iWearActuatorPort.write(false); return true; } diff --git a/modules/HapticGlove_module/src/Teleoperation.cpp b/modules/HapticGlove_module/src/Teleoperation.cpp index ab86ddef..06a90c8d 100644 --- a/modules/HapticGlove_module/src/Teleoperation.cpp +++ b/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) From 957f84545757f10b6a08da2fd1100c26e6dcc2cf Mon Sep 17 00:00:00 2001 From: Ehsan Ranjbari Date: Wed, 31 Jan 2024 16:14:33 +0100 Subject: [PATCH 03/10] merge all fingers msgs - HapticGloveModule --- .../HapticGlove_module/src/GloveWearable.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/modules/HapticGlove_module/src/GloveWearable.cpp b/modules/HapticGlove_module/src/GloveWearable.cpp index 3da4f524..cc002d4f 100644 --- a/modules/HapticGlove_module/src/GloveWearable.cpp +++ b/modules/HapticGlove_module/src/GloveWearable.cpp @@ -390,31 +390,31 @@ bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector& v bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector& values) { - yDebug() << "values to be sent: " << values; if (values.size() != m_numHapticFeedback) { yError() << m_logPrefix << "size of the haptic feedback vector is not equal to the size of the default " "haptic feedback size."; } + wearable::msg::WearableActuatorCommand& wearableActuatorCommand + = m_iWearActuatorPort.prepare(); + wearableActuatorCommand.forceValue.resize(m_numForceFeedback, 0); + wearableActuatorCommand.vibroTactileValue.resize(m_numVibrotactileFeedback, 0); + wearableActuatorCommand.duration.resize(m_numForceFeedback + m_numVibrotactileFeedback, 0); + for (size_t i = 0; i < values.size() / 2; i++) { - std::string fingerName = m_humanFingerNameList[i]; - - wearable::msg::WearableActuatorCommand& wearableActuatorCommand - = m_iWearActuatorPort.prepare(); + // std::string fingerName = m_humanFingerNameList[i]; + wearableActuatorCommand.forceValue[i] = values[i]; + wearableActuatorCommand.vibroTactileValue[i] = values[i + m_numForceFeedback]; + } - wearableActuatorCommand.forceValue = values[i]; - wearableActuatorCommand.vibroTactileValue = values[i + m_numForceFeedback]; wearableActuatorCommand.info.name = m_wearablePrefix - + wearable::actuator::IHaptic::getPrefix() + fingerName - + "::HapticFeedback"; + + wearable::actuator::IHaptic::getPrefix() + + + "HapticFeedback"; wearableActuatorCommand.info.type = wearable::msg::ActuatorType::HAPTIC; - wearableActuatorCommand.duration = 0; - - m_iWearActuatorPort.write(true); - } + m_iWearActuatorPort.write(); return true; } From ae645afa80eb0df681ee61b4280e62da762e47ad Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Mon, 5 Feb 2024 16:15:05 +0100 Subject: [PATCH 04/10] Added configuration files for ergocub sn 001 --- app/robots/ergoCubSN001/hapticGloveConfig.ini | 31 ++++ .../leftFingersHapticRetargetingParams.ini | 171 ++++++++++++++++++ .../ergoCubSN001/openXRHeadsetParameters.ini | 145 +++++++++++++++ app/robots/ergoCubSN001/openXRJoypad.ini | 92 ++++++++++ .../rightFingersHapticRetargetingParams.ini | 170 +++++++++++++++++ 5 files changed, 609 insertions(+) create mode 100644 app/robots/ergoCubSN001/hapticGloveConfig.ini create mode 100644 app/robots/ergoCubSN001/leftFingersHapticRetargetingParams.ini create mode 100644 app/robots/ergoCubSN001/openXRHeadsetParameters.ini create mode 100644 app/robots/ergoCubSN001/openXRJoypad.ini create mode 100644 app/robots/ergoCubSN001/rightFingersHapticRetargetingParams.ini diff --git a/app/robots/ergoCubSN001/hapticGloveConfig.ini b/app/robots/ergoCubSN001/hapticGloveConfig.ini new file mode 100644 index 00000000..e13f056a --- /dev/null +++ b/app/robots/ergoCubSN001/hapticGloveConfig.ini @@ -0,0 +1,31 @@ +name HapticGloveRetargeting + + +[GENERAL] +robot ergocub +samplingTime 0.1 +enableMoveRobot 1 +enableLogger 0 +useLeftHand 1 +useRightHand 1 +isMandatory 0 +useSkin 0 +#calibrationTimePeriod [sec] +calibrationTimePeriod 1.0 +#robotInitializationTime [sec] +robotInitializationTime 3.0 +getHumanMotionRange 1 +# the counter to check if steady state is reached [steps] +steadyStateCounterThreshold 5 +# the threshold to check the error between the desired and feedback values for the steady state check [rad] +steadyStateThreshold 0.05 +# the time teleoperation should wait before running [sec] +waitingDurationTime 5.0 +# threshold in which an skin is considered as working fine [no units] +tactileWorkingThreshold 0.0001 +# threshold in which an skin data is considered updated [no units] +tactileUpdateThreshold -1.0 + +# include fingers parameters +[include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] +[include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/ergoCubSN001/leftFingersHapticRetargetingParams.ini b/app/robots/ergoCubSN001/leftFingersHapticRetargetingParams.ini new file mode 100644 index 00000000..a27d0911 --- /dev/null +++ b/app/robots/ergoCubSN001/leftFingersHapticRetargetingParams.ini @@ -0,0 +1,171 @@ +############### +### ROBOT +############### +remote_control_boards ("left_arm") + +remote_sensor_boards "left_hand" + +axis_list ( "l_thumb_add", "l_thumb_oc", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc") + +all_axis_list ( "l_thumb_add", "l_thumb_oc", "l_index_add", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc") + +joint_list ( "l_thumb_add", "l_thumb_oc", "l_index_add", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc" ) + +# the custom range of axes motion [degrees] +# axis-name , min value, max value +# axes_custom_motion_range ( ( ${AXES_NAME} , ${MIN}, ${MAX}) ) + +# 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) ) + +# 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 +analog_list ( ) + +#Adding the groups of each axis and associated joints +l_thumb_add ( "l_thumb_add" ) +l_thumb_oc ( "l_thumb_oc" ) +l_index_add ( "l_index_add" ) +l_index_oc ( "l_index_oc" ) +l_middle_oc ( "l_middle_oc" ) +l_ring_pinky_oc ( "l_ring_pinky_oc" ) + +robot_finger_list ( "l_thumb_finger", "l_index_finger", "l_middle_finger", "l_ring_finger", "l_little_finger") + +############### +### HUMAN +############### +human_joint_list ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_middle", "l_thumb_distal", + "l_index_abduction", "l_index_proximal", "l_index_middle", "l_index_distal", + "l_middle_abduction", "l_middle_proximal", "l_middle_middle", "l_middle_distal", + "l_ring_abduction", "l_ring_proximal", "l_ring_middle", "l_ring_distal", + "l_pinky_abduction", "l_pinky_proximal", "l_pinky_middle", "l_pinky_distal" ) + +human_finger_list ( "l_thumb_finger", "l_index_finger", "l_middle_finger", "l_ring_finger", "l_little_finger") + +hand_link l_hand + +wearable_data_ports ("/WearableData/HapticGlove/LeftHand/data:o") + +wearable_data_actuator_ports_out "/WearableData/HapticGlove/LeftHand/Actuators/input:o" + +wearable_data_actuator_ports_in "/WearableData/HapticGlove/LeftHand/Actuators/input:i" + +############### +### RETARGETING +############### + +motorsJointsCoupled 0 + +robot_to_human_map ( ("l_thumb_add", "l_thumb_oppose") + ("l_thumb_oc", "l_thumb_distal") + ("l_index_add", "l_index_abduction" ) + ("l_index_oc", "l_index_distal") + ("l_middle_oc", "l_middle_distal") + ("l_ring_pinky_oc", "l_ring_distal") ) + +# haptic feedback retargeting from the robot axis groups to the human finger +l_thumb_finger ( "l_thumb_add", "l_thumb_oc" ) +l_index_finger ( "l_index_add", "l_index_oc" ) +l_middle_finger ( "l_middle_oc" ) +l_ring_finger ( "l_ring_pinky_oc" ) +l_little_finger ( "l_ring_pinky_oc" ) + +# This gain is multiplied to the total error for each motor/axis to compute the force feedback to the user associated with each axis, +# defined according to user experience +# Number of gains = number of motors/axis +gainTotalError ( 0.0 15.0 15.0 15.0 15.0 15.0) +#gainTotalError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# check this issue for the velocity Gain: https://github.com/dic-iit/element_retargeting-from-human/issues/141 +# number of gains = number of motors/axis +gainVelocityError ( 0.0 0.1 0.1 0.1 0.1 0.1) +#gainVelocityError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# this value is multiplied to forcefeedback and provides haptic feedback to the user +gainVibrotactile (0.8 0.8 0.8 0.8 0.8 ) +#gainVibrotactile ( 0.0 0.0 0.0 0.0 0.0 ) + +# scaling and biased values for maping the human to robot motion +# the sign of the following vector is important, while the absolute values will be found at configuration file +human_to_robot_joint_angles_scaling ( -1.0 1.0 -1.0 1.0 1.0 1.0 ) + +human_to_robot_joint_anlges_bias ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + +axisContactThreshold 0.1 + +############################## +### ROBOT CONTROL & ESTIMATION +############################## +useVelocity 0 + +referenceVelocityForPositionControl 100.0 + +# minimum and maximum values of the joints +# related to analog sensors +analog_joints_min_boundary ( ) +analog_joints_max_boundary ( ) + +# hall sensors minimum and maximum values +analog_sensors_raw_min_boundary ( ) +analog_sensors_raw_max_boundary ( ) + + +# in case each joint does not have independant motor to actuate, they are coupled +axesJointsCoupled 0 + +# in case a calibration phase necessary before starting teleoperating the robot, +# otherwise read the calibration matrix from the config file +doCalibration 0 + +#robot controller exponential filter gain +exponentialFilterGain 0.9 + +# in the Quadratic optimizartion problem to compute the motor values from the joint values : xT Q X + uT R u +# q_matrix_joint_motor is a list that identifies the main diagonal values of matrix Q: (q x q) matrix; +# size of q: is the number of desired joints to control +# r_matrix_joint_motor is a list that identifies the main diagonal of matrix R: (m x m) matrix; +# size of m: is the number of desired motors to control + +q_qp_control ( 1.0 1.0 1.0 1.0 1.0 1.0 ) + +r_qp_control ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + + +# in the Kalman filter problem to estimate the axis values, velocity and acceleration: +# q_matrix_kf is a list that identifies the main diagonal of matrix: E[ (w(t) -w_bar(t)) (w(t) -w_bar(t))^T ], +# size: m*m positive matrix, Dx(t)= Fx(t)+ Gw(t), the process noise */ +# r_matrix_kf is a list that identifies the main diagonal of matrix: E[ v(t) v(t)^T ], +# size: p*p positive matrix, Z(t)= Hx(t)+ v(t), the measurement noise +no_states_kf 3 +no_measurement_kf 1 +q_matrix_kf ( 10.0 150.0 100000.0 ) +r_matrix_kf ( 0.0000001 ) + +##################### +## TACTILE SENSORS ## +##################### +## length of the tactile sensor port +# noTactileSensors 192 + +## indices to read from the port for each finger +## starting index ending index contact threshold threshold multiplier contact feedback gain derivative threshold threshold multiplier gain derivative vibrotactile feedback +# l_thumb_finger_tactile_info ( 48 59 0.14 1.0 200.0 0.5 3.0 40.0) +# l_index_finger_tactile_info ( 0 11 0.14 1.0 200.0 0.5 3.0 40.0) +# l_middle_finger_tactile_info ( 12 23 0.14 1.0 200.0 0.5 3.0 40.0) +# l_ring_finger_tactile_info ( 24 35 0.14 1.0 200.0 0.5 3.0 40.0) +# l_little_finger_tactile_info ( 36 47 0.14 1.0 200.0 0.5 3.0 40.0) + +# absolute vibrotactile feedback nonlinear function parameters +# 15.0 * std::log(2 * std::pow(x, 0.7) + 1) + 0.5 * std::pow(x, 1.1); +# reference to https://github.com/ami-iit/element_retargeting-from-human/issues/182#issuecomment-1000472012 +# absoluteVibrotactileFeedbackNonlinearFunctionParameters ( 15.0 2.0 0.7 1.0 0.5 1.1 ) + +# percentage dedicated to absolute skin data for providing the vibrotactile feedback +# the value is between [0,1] +# 0 : 0% absolute skin value for the vibrotactile feedback; 100% derivative skin value for the vibrotactile feedback +# 1 : 100% absolute skin value for the vibrotactile feedback; 0% derivative skin value for the vibrotactile feedback +# absoluteSkinValuePercentage 1.0 +# skinDerivativeSmoothingGain 1.0 diff --git a/app/robots/ergoCubSN001/openXRHeadsetParameters.ini b/app/robots/ergoCubSN001/openXRHeadsetParameters.ini new file mode 100644 index 00000000..25429324 --- /dev/null +++ b/app/robots/ergoCubSN001/openXRHeadsetParameters.ini @@ -0,0 +1,145 @@ +###Use as subdevice +### Device params +device JoypadControlServer +use_separate_ports 1 +subdevice openxrheadset +stick_as_axis false +#### + +###Use as direct device +#device openxrheadset +### + +period 10 +name /headset + +mono_eye true + +vr_poses_prediction_in_ms 0 + +### Camera correction +eye_z_position -0.8 + +### Custom frames +custom_poses 3 + +### Gui params +gui_elements 1 +labels 4 +slides 2 + +[CUSTOM_POSE_0] +name stable_waist +parent_frame vive_tracker_waist_pose +euler_angles xzy +relative_position (0.0, 0.0, 0.0) +relative_rotation (*, *, 0.0) + +[CUSTOM_POSE_1] +name world +parent_frame stable_waist +euler_angles zyx +relative_position (0.0, -0.1, 0.1) +relative_rotation (0.0, -1.5708, -1.5708) + +[CUSTOM_POSE_2] +name root_link_desired +parent_frame world +euler_angles zyx +relative_position (0.0, 0.0, 0.0) +relative_rotation (0.0, -0.2, 0.0) + +[GUI_0] +width 1.50 +height 1.50 +x 0.0 +y 0.0 +z -0.7999 +visibility both +follow_eyes true +port_id augmentedReality + +[LABEL_0] +width 0.5 +height 0.05 +x 0.0 +y 0.045 +z -0.5 +prefix "" +suffix "" +pixel_size 45 +color (0.0, 0.0, 1.0, 1.0) +background_color (1.0, 1.0, 1.0, 0.01) +font "Roboto/Roboto-Black.ttf" +horizontal_alignement center +vertical_alignement top +automatically_enabled false +disable_timeout_in_S 1.8 + +[LABEL_1] +width 0.15 +height 0.05 +x -0.12 +y 0.08 +z -0.5 +prefix "Weight L: " +suffix " Kg" +pixel_size 256 +color (1.0, 1.0, 1.0, 1.0) +background_color (0.0, 0.0, 0.0, 1.0) +font "Roboto/Roboto-Medium.ttf" +horizontal_alignement left +vertical_alignement center +automatically_enabled false +disable_timeout_in_S 5.0 + +[LABEL_2] +width 0.15 +height 0.05 +x 0.12 +y 0.08 +z -0.5 +prefix "Weight R: " +suffix " Kg" +pixel_size 256 +color (1.0, 1.0, 1.0, 1.0) +background_color (0.0, 0.0, 0.0, 1.0) +font "Roboto/Roboto-Medium.ttf" +horizontal_alignement left +vertical_alignement center +automatically_enabled false +disable_timeout_in_S 5.0 + +[LABEL_3] +width 0.15 +height 0.05 +x 0.0 +y 0.08 +z -0.5 +prefix "WARNING! Risk: " +suffix "" +pixel_size 256 +color (1.0, 1.0, 1.0, 1.0) +background_color (0.0, 0.0, 0.0, 1.0) +font "Roboto/Roboto-Medium.ttf" +horizontal_alignement center +vertical_alignement center +automatically_enabled false +disable_timeout_in_S 2.0 +port_id risk_label + +[SLIDE_0] +width 0.1 +height 0.1 +x 0.0 +y -0.2 +z -0.5 +slides_path C:/element_ifeel-walking-retargeting/arrow/linear_arrow + +[SLIDE_1] +width 0.1 +height 0.1 +x 0.0 +y -0.2 +z -0.5 +slides_path C:/element_ifeel-walking-retargeting/arrow/rotation_arrow diff --git a/app/robots/ergoCubSN001/openXRJoypad.ini b/app/robots/ergoCubSN001/openXRJoypad.ini new file mode 100644 index 00000000..17152366 --- /dev/null +++ b/app/robots/ergoCubSN001/openXRJoypad.ini @@ -0,0 +1,92 @@ +name openXRJoypad + +# ports +rpcWalkingPort_name /walkingRpc +robotGoalOutputPort_name /walkingGoal +print_buttons 0 +print_axes 0 +print_sticks 0 + +[GENERAL] +samplingTime 0.01 +robot ergocub + +# include fingers parameters +[LEFT_FINGERS_RETARGETING] +enabled 1 +remote_control_boards ("left_arm") +joints_list ( "l_thumb_add", "l_thumb_oc", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc" ) + +useVelocity 1 + +fingersScaling (1, 2.0, 3.5, 3.5, 3.5) + +[RIGHT_FINGERS_RETARGETING] +enabled 1 +remote_control_boards ("right_arm") +joints_list ( "r_thumb_add", "r_thumb_oc", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc") + +useVelocity 1 + +fingersScaling (1, 2.0, 3.5, 3.5, 3.5) + +[OPENXR] +check_hands_inverted 0 +joypad_device_remote /headset +left_hand_frame_name openxr_left_hand +right_hand_frame_name openxr_right_hand +head_frame_name openxr_head + +deadzone 0.3 +fullscale 1.0 + +# The following code is as follows: +# - the first character is either A or S. A indicates "Axis", S indicates "Stick" +# - the second character is either a "+" or a "-" indicating if the sign has to be inverted or not +# - the following characters are either the axis or the stick index +# - in case of a stick, it is also necessary to specify the dof index after a "." +# As an example S-1.2 indicates the dof number 2 (0-based) of the stick number 1 (0-based), changing sign. +# Another example: A+3 indicates the axis number 3 (0-based) and the sign does not need to change. + +left_x_code S+0.1 +left_y_code S-0.0 +right_x_code S+1.1 +right_y_code S-1.0 + +left_fingers_velocity_code A+0 +right_fingers_velocity_code A+1 + +# Vive buttons order +# "vive_left_menu", +# "vive_left_trigger_click", +# "vive_left_squeeze_click", +# "vive_left_trackpad_click", +# "vive_right_menu", +# "vive_right_trigger_click", +# "vive_right_squeeze_click", +# "vive_right_trackpad_click" + + +# In the buttons maps we have the following: +# 1 means that the button needs to be pressed +# 0 means that the button should not be pressed +# -1 means that the button is ignored + +prepare_walking_buttons_map ( 0, -1, -1, -1, 1, -1, -1, -1) # right menu ONLY +start_walking_buttons_map ( 1, -1, -1, -1, 0, -1, -1, -1) # left menu ONLY +stop_walking_buttons_map ( 1, -1, -1, -1, 1, -1, -1, -1) # left and right menu + +# Buttons to use when the left and right hand are swapped +prepare_walking_buttons_swapped_map ( 1, -1, -1, -1, 0, -1, -1, -1) # left menu ONLY +start_walking_buttons_map_swapped ( 0, -1, -1, -1, 1, -1, -1, -1) # right menu ONLY +stop_walking_buttons_swapped_map ( 1, -1, -1, -1, 1, -1, -1, -1) # identical to the non-swapped case + +walking_command_release_buttons_map ( 0, -1, -1, -1, 0, -1, -1, -1) # Send the command only when both menu buttons have been released + +left_fingers_squeeze_buttons_map (-1, -1, 0, -1, -1, -1, -1, -1) # left squeeze deactivated +left_fingers_release_buttons_map (-1, -1, 1, -1, -1, -1, -1, -1) # left squeeze deactivated +left_walking_buttons_map (-1, -1, -1, 1, -1, -1, -1, -1) # left_trackpad_click activated + +right_fingers_squeeze_buttons_map (-1, -1, -1, -1, -1, -1, 0, -1) # right squeeze deactivated +right_fingers_release_buttons_map (-1, -1, -1, -1, -1, -1, 1, -1) # right squeeze deactivated +right_walking_buttons_map (-1, -1, -1, -1, -1, -1, -1, 1) # right_trackpad_click activated diff --git a/app/robots/ergoCubSN001/rightFingersHapticRetargetingParams.ini b/app/robots/ergoCubSN001/rightFingersHapticRetargetingParams.ini new file mode 100644 index 00000000..9e464efa --- /dev/null +++ b/app/robots/ergoCubSN001/rightFingersHapticRetargetingParams.ini @@ -0,0 +1,170 @@ +############### +### ROBOT +############### +remote_control_boards ("right_arm") + +remote_sensor_boards "right_hand" + +axis_list ( "r_thumb_add", "r_thumb_oc", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc") + +all_axis_list ( "r_thumb_add", "r_thumb_oc", "r_index_add", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc") + +joint_list ( "r_thumb_add", "r_thumb_oc", "r_index_add", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc" ) + +# the custom range of axes motion [degrees] +# axis-name , min value, max value +# axes_custom_motion_range ( ( ${AXES_NAME} , ${MIN}, ${MAX}) ) + +# 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) ) + +# if a joint is not listed here, the sensory information is of encoder type. +# notice that "r_thumb_oppose" and "r_hand_fingers" is measured by joint encoders not the analog sensors +analog_list ( ) + +#Adding the groups of each axis and associated joints +r_thumb_add ( "r_thumb_add" ) +r_thumb_oc ( "r_thumb_oc" ) +r_index_add ( "r_index_add" ) +r_index_oc ( "r_index_oc" ) +r_middle_oc ( "r_middle_oc" ) +r_ring_pinky_oc ( "r_ring_pinky_oc" ) + +robot_finger_list ( "r_thumb_finger", "r_index_finger", "r_middle_finger", "r_ring_finger", "r_little_finger") + +############### +### HUMAN +############### +human_joint_list ( "r_thumb_oppose", "r_thumb_proximal", "r_thumb_middle", "r_thumb_distal", + "r_index_abduction", "r_index_proximal", "r_index_middle", "r_index_distal", + "r_middle_abduction", "r_middle_proximal", "r_middle_middle", "r_middle_distal", + "r_ring_abduction", "r_ring_proximal", "r_ring_middle", "r_ring_distal", + "r_pinky_abduction", "r_pinky_proximal", "r_pinky_middle", "r_pinky_distal" ) + +human_finger_list ( "r_thumb_finger", "r_index_finger", "r_middle_finger", "r_ring_finger", "r_little_finger") + +hand_link r_hand + +wearable_data_ports ("/WearableData/HapticGlove/RightHand/data:o") + +wearable_data_actuator_ports_out "/WearableData/HapticGlove/RightHand/Actuators/input:o" + +wearable_data_actuator_ports_in "/WearableData/HapticGlove/RightHand/Actuators/input:i" + +############### +### RETARGETING +############### + +motorsJointsCoupled 0 + +robot_to_human_map ( ("r_thumb_add", "r_thumb_oppose") + ("r_thumb_oc", "r_thumb_distal") + ("r_index_add", "r_index_abduction" ) + ("r_index_oc", "r_index_distal") + ("r_middle_oc", "r_middle_distal") + ("r_ring_pinky_oc", "r_ring_distal") ) + +# haptic feedback retargeting from the robot axis groups to the human finger +r_thumb_finger ( "r_thumb_add", "r_thumb_oc" ) +r_index_finger ( "r_index_add", "r_index_oc" ) +r_middle_finger ( "r_middle_oc" ) +r_ring_finger ( "r_ring_pinky_oc" ) +r_little_finger ( "r_ring_pinky_oc" ) + +# This gain is multiplied to the total error for each motor/axis to compute the force feedback to the user associated with each axis, +# defined according to user experience +# Number of gains = number of motors/axis +gainTotalError ( 0.0 15.0 15.0 15.0 15.0 15.0) +#gainTotalError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# check this issue for the velocity Gain: https://github.com/dic-iit/element_retargeting-from-human/issues/141 +# number of gains = number of motors/axis +gainVelocityError ( 0.0 0.1 0.1 0.1 0.1 0.1) +#gainVelocityError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# this value is multiplied to forcefeedback and provides haptic feedback to the user +gainVibrotactile ( 0.8 0.8 0.8 0.8 0.8 ) +#gainVibrotactile ( 0.0 0.0 0.0 0.0 0.0 ) + +# scaling and biased values for maping the human to robot motion +# the sign of the following vector is important, while the absolute values will be found at configuration file +human_to_robot_joint_angles_scaling ( 1.0 1.0 1.0 1.0 1.0 1.0 ) + +human_to_robot_joint_anlges_bias ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + +axisContactThreshold 0.1 + +############################## +### ROBOT CONTROL & ESTIMATION +############################## +useVelocity 0 + +referenceVelocityForPositionControl 100.0 + +# minimum and maximum values of the joints +# related to analog sensors +analog_joints_min_boundary ( ) +analog_joints_max_boundary ( ) + +# hall sensors minimum and maximum values +analog_sensors_raw_min_boundary ( ) +analog_sensors_raw_max_boundary ( ) + + +# in case each joint does not have independant motor to actuate, they are coupled +axesJointsCoupled 0 + +# in case a calibration phase necessary before starting teleoperating the robot, +# otherwise read the calibration matrix from the config file +doCalibration 0 + +#robot controller exponential filter gain +exponentialFilterGain 0.9 + +# in the Quadratic optimizartion problem to compute the motor values from the joint values : xT Q X + uT R u +# q_matrix_joint_motor is a list that identifies the main diagonal values of matrix Q: (q x q) matrix; +# size of q: is the number of desired joints to control +# r_matrix_joint_motor is a list that identifies the main diagonal of matrix R: (m x m) matrix; +# size of m: is the number of desired motors to control + +q_qp_control ( 1.0 1.0 1.0 1.0 1.0 1.0 ) + +r_qp_control ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + + +# in the Kalman filter problem to estimate the axis values, velocity and acceleration: +# q_matrix_kf is a list that identifies the main diagonal of matrix: E[ (w(t) -w_bar(t)) (w(t) -w_bar(t))^T ], +# size: m*m positive matrix, Dx(t)= Fx(t)+ Gw(t), the process noise */ +# r_matrix_kf is a list that identifies the main diagonal of matrix: E[ v(t) v(t)^T ], +# size: p*p positive matrix, Z(t)= Hx(t)+ v(t), the measurement noise +no_states_kf 3 +no_measurement_kf 1 +q_matrix_kf ( 10.0 150.0 100000.0 ) +r_matrix_kf ( 0.0000001 ) + +##################### +## TACTILE SENSORS ## +##################### +## length of the tactile sensor port +# noTactileSensors 192 + +## indices to read from the port for each finger +## starting index ending index contact threshold threshold multiplier contact feedback gain derivative threshold threshold multiplier gain derivative vibrotactile feedback +# r_thumb_finger_tactile_info ( 48 59 0.14 1.0 200.0 0.5 3.0 40.0) +# r_index_finger_tactile_info ( 0 11 0.14 1.0 200.0 0.5 3.0 40.0) +# r_middle_finger_tactile_info ( 12 23 0.14 1.0 200.0 0.5 3.0 40.0) +# r_ring_finger_tactile_info ( 24 35 0.14 1.0 200.0 0.5 3.0 40.0) +# r_little_finger_tactile_info ( 36 47 0.14 1.0 200.0 0.5 3.0 40.0) + +# absolute vibrotactile feedback nonlinear function parameters +# 15.0 * std::log(2 * std::pow(x, 0.7) + 1) + 0.5 * std::pow(x, 1.1); +# reference to https://github.com/ami-iit/element_retargeting-from-human/issues/182#issuecomment-1000472012 +# absoluteVibrotactileFeedbackNonlinearFunctionParameters ( 15.0 2.0 0.7 1.0 0.5 1.1 ) + +# percentage dedicated to absolute skin data for providing the vibrotactile feedback +# the value is between [0,1] +# 0 : 0% absolute skin value for the vibrotactile feedback; 100% derivative skin value for the vibrotactile feedback +# 1 : 100% absolute skin value for the vibrotactile feedback; 0% derivative skin value for the vibrotactile feedback +# absoluteSkinValuePercentage 1.0 +# skinDerivativeSmoothingGain 1.0 From 2dea8dfd39ce8d01d4412df9925683b608e5a09c Mon Sep 17 00:00:00 2001 From: Ehsan Ranjbari Date: Tue, 12 Mar 2024 16:32:25 +0100 Subject: [PATCH 05/10] squash commits to resolve conflict --- app/robots/ergoCubSN001/hapticGloveConfig.ini | 2 +- .../src/GloveControlHelper.cpp | 11 +--- .../HapticGlove_module/src/GloveWearable.cpp | 60 ------------------- 3 files changed, 3 insertions(+), 70 deletions(-) diff --git a/app/robots/ergoCubSN001/hapticGloveConfig.ini b/app/robots/ergoCubSN001/hapticGloveConfig.ini index e13f056a..572b9739 100644 --- a/app/robots/ergoCubSN001/hapticGloveConfig.ini +++ b/app/robots/ergoCubSN001/hapticGloveConfig.ini @@ -14,7 +14,7 @@ useSkin 0 calibrationTimePeriod 1.0 #robotInitializationTime [sec] robotInitializationTime 3.0 -getHumanMotionRange 1 +getHumanMotionRange 0 # the counter to check if steady state is reached [steps] steadyStateCounterThreshold 5 # the threshold to check the error between the desired and feedback values for the steady state check [rad] diff --git a/modules/HapticGlove_module/src/GloveControlHelper.cpp b/modules/HapticGlove_module/src/GloveControlHelper.cpp index a006f790..53cf7590 100644 --- a/modules/HapticGlove_module/src/GloveControlHelper.cpp +++ b/modules/HapticGlove_module/src/GloveControlHelper.cpp @@ -205,15 +205,6 @@ bool GloveControlHelper::setFingertipVibrotactileFeedbackReferences( bool GloveControlHelper::setFingertipHapticFeedbackReferences() { - // if (desiredValue.size() != m_numHapticFeedback) - // { - // yError() << m_logPrefix - // << "the size of the input " - // "desired vecotr [" - // << desiredValue.size() << "] and the number of haptic feedbacks [ " - // << m_numHapticFeedback << " ]are not equal."; - // return false; - // } for (size_t i = 0; i < m_numForceFeedback; i++) { @@ -226,6 +217,7 @@ bool GloveControlHelper::setFingertipHapticFeedbackReferences() return m_pImp->setFingertipHapticFeedbackValues(m_desiredHapticValues); } + bool GloveControlHelper::stopPalmVibrotactileFeedback() { return m_pImp->setPalmVibrotactileValue(to_underlying( @@ -397,6 +389,7 @@ bool GloveControlHelper::findHumanMotionRange() std::vector desiredValue(m_numVibrotactileFeedback, 35); this->setFingertipVibrotactileFeedbackReferences(desiredValue); + this->setFingertipHapticFeedbackReferences(); return true; } diff --git a/modules/HapticGlove_module/src/GloveWearable.cpp b/modules/HapticGlove_module/src/GloveWearable.cpp index cc002d4f..46c92134 100644 --- a/modules/HapticGlove_module/src/GloveWearable.cpp +++ b/modules/HapticGlove_module/src/GloveWearable.cpp @@ -330,61 +330,11 @@ bool GloveWearableImpl::getFingertipPoseValues(Eigen::MatrixXd& values) bool GloveWearableImpl::setFingertipForceFeedbackValues(const std::vector& values) { - // if (values.size() != m_numForceFeedback) - // { - // yError() << m_logPrefix - // << "size of the force feedback vector is not equal to the size of the default " - // "force 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; } bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector& values) { - // if (values.size() != m_numVibrotactileFeedback) - // { - // yError() - // << m_logPrefix - // << "size of the vibrotactile feedback vector is not equal to the size of the default " - // "vibrotactile 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 - // + "::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 - // } - return true; } @@ -404,7 +354,6 @@ bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector& for (size_t i = 0; i < values.size() / 2; i++) { - // std::string fingerName = m_humanFingerNameList[i]; wearableActuatorCommand.forceValue[i] = values[i]; wearableActuatorCommand.vibroTactileValue[i] = values[i + m_numForceFeedback]; } @@ -434,15 +383,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; } From 5c692d3f3db0904230bef887230277d846a7ebaa Mon Sep 17 00:00:00 2001 From: Ehsan Ranjbari Date: Fri, 15 Mar 2024 15:18:07 +0100 Subject: [PATCH 06/10] seperate force and vibro desired vectors + minor changes related to the PR#139 --- .gitignore | 3 +- .../include/GloveControlHelper.hpp | 2 +- .../include/GloveWearable.hpp | 2 +- .../src/GloveControlHelper.cpp | 26 ++++++----------- .../HapticGlove_module/src/GloveWearable.cpp | 28 +++++++------------ 5 files changed, 21 insertions(+), 40 deletions(-) diff --git a/.gitignore b/.gitignore index 35b8c128..d4af118f 100644 --- a/.gitignore +++ b/.gitignore @@ -44,5 +44,4 @@ build/ TAGS compile_commands.json CMakeLists.txt.user -.vscode/c_cpp_properties.json -.vscode/settings.json +.vscode/ diff --git a/modules/HapticGlove_module/include/GloveControlHelper.hpp b/modules/HapticGlove_module/include/GloveControlHelper.hpp index fe8df27e..e816d44b 100644 --- a/modules/HapticGlove_module/include/GloveControlHelper.hpp +++ b/modules/HapticGlove_module/include/GloveControlHelper.hpp @@ -126,7 +126,7 @@ class HapticGlove::GloveControlHelper * stop the haptics feedback (force feedback, fingertip vibrotactile feedback) to the user * @return true/false in case of success/failure */ - bool stopHapticsFeedback(); + bool stopAllHapticFeedback(); /** * stop the haptic feedback (force feedback, fingertip vibrotactile feedback, palm vibrotactile diff --git a/modules/HapticGlove_module/include/GloveWearable.hpp b/modules/HapticGlove_module/include/GloveWearable.hpp index 5a238ce0..e5dc5b6b 100644 --- a/modules/HapticGlove_module/include/GloveWearable.hpp +++ b/modules/HapticGlove_module/include/GloveWearable.hpp @@ -177,7 +177,7 @@ class HapticGlove::GloveWearableImpl * pinky, range: [0, 100] * @return true/false in case of success/failure */ - bool setFingertipHapticFeedbackValues(const std::vector& values); + bool setFingertipHapticFeedbackValues(const std::vector& forceValues, const std::vector& vibroValues); /** * set the force feedback actuator values associated with all the human hand fingertips diff --git a/modules/HapticGlove_module/src/GloveControlHelper.cpp b/modules/HapticGlove_module/src/GloveControlHelper.cpp index 53cf7590..e45bf8d9 100644 --- a/modules/HapticGlove_module/src/GloveControlHelper.cpp +++ b/modules/HapticGlove_module/src/GloveControlHelper.cpp @@ -179,7 +179,7 @@ bool GloveControlHelper::setFingertipForceFeedbackReferences( / m_maxForceFeedback); } - return m_pImp->setFingertipForceFeedbackValues(m_desiredForceValues); + return true; } bool GloveControlHelper::setFingertipVibrotactileFeedbackReferences( @@ -200,22 +200,12 @@ 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() { - - for (size_t i = 0; i < m_numForceFeedback; i++) - { - m_desiredHapticValues[i] = m_desiredForceValues[i]; - } - for (size_t i = 0; i < m_numVibrotactileFeedback; i++) - { - m_desiredHapticValues[i + m_numForceFeedback] = m_desiredVibrotactileValues[i]; - } - - return m_pImp->setFingertipHapticFeedbackValues(m_desiredHapticValues); + return m_pImp->setFingertipHapticFeedbackValues(m_desiredForceValues, m_desiredVibrotactileValues); } bool GloveControlHelper::stopPalmVibrotactileFeedback() @@ -228,19 +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::stopHapticsFeedback() +bool GloveControlHelper::stopAllHapticFeedback() { std::fill(m_desiredHapticValues.begin(), m_desiredHapticValues.end(), 0.0); - return m_pImp->setFingertipHapticFeedbackValues(m_desiredHapticValues); + return m_pImp->setFingertipHapticFeedbackValues(m_desiredForceValues, m_desiredVibrotactileValues); } bool GloveControlHelper::stopHapticFeedback() @@ -262,7 +252,7 @@ bool GloveControlHelper::stopHapticFeedback() "vibrotactile feedback"; return false; } - if (!stopHapticsFeedback()) + if (!stopAllHapticFeedback()) { yError() << m_logPrefix << "Cannot turn off the fingertip haptics feedback"; return false; diff --git a/modules/HapticGlove_module/src/GloveWearable.cpp b/modules/HapticGlove_module/src/GloveWearable.cpp index 46c92134..8abfdd23 100644 --- a/modules/HapticGlove_module/src/GloveWearable.cpp +++ b/modules/HapticGlove_module/src/GloveWearable.cpp @@ -328,19 +328,9 @@ bool GloveWearableImpl::getFingertipPoseValues(Eigen::MatrixXd& values) return true; } -bool GloveWearableImpl::setFingertipForceFeedbackValues(const std::vector& values) +bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector& forceValues, const std::vector& vibroValues) { - return true; -} - -bool GloveWearableImpl::setFingertipVibrotactileValues(const std::vector& values) -{ - return true; -} - -bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector& values) -{ - if (values.size() != m_numHapticFeedback) + if (forceValues.size() + vibroValues.size() != m_numHapticFeedback) { yError() << m_logPrefix << "size of the haptic feedback vector is not equal to the size of the default " @@ -348,14 +338,16 @@ bool GloveWearableImpl::setFingertipHapticFeedbackValues(const std::vector& } wearable::msg::WearableActuatorCommand& wearableActuatorCommand = m_iWearActuatorPort.prepare(); - wearableActuatorCommand.forceValue.resize(m_numForceFeedback, 0); - wearableActuatorCommand.vibroTactileValue.resize(m_numVibrotactileFeedback, 0); - wearableActuatorCommand.duration.resize(m_numForceFeedback + m_numVibrotactileFeedback, 0); + wearableActuatorCommand.forceValue.resize(m_numForceFeedback); + wearableActuatorCommand.vibroTactileValue.resize(m_numVibrotactileFeedback); - for (size_t i = 0; i < values.size() / 2; i++) + for (size_t i = 0; i < forceValues.size(); i++) + { + wearableActuatorCommand.forceValue[i] = forceValues[i]; + } + for (size_t i = 0; i < vibroValues.size(); i++) { - wearableActuatorCommand.forceValue[i] = values[i]; - wearableActuatorCommand.vibroTactileValue[i] = values[i + m_numForceFeedback]; + wearableActuatorCommand.vibroTactileValue[i] = vibroValues[i]; } wearableActuatorCommand.info.name = m_wearablePrefix From ce3f03fd55617e566aea8b6a7f604d562e91b1f5 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 6 Feb 2024 13:26:03 +0100 Subject: [PATCH 07/10] Using ffmpeg carrier to connect to the camera --- app/scripts/ergoCub-Teleoperation-2.xml | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/app/scripts/ergoCub-Teleoperation-2.xml b/app/scripts/ergoCub-Teleoperation-2.xml index d81cfa7a..ade1c9c8 100644 --- a/app/scripts/ergoCub-Teleoperation-2.xml +++ b/app/scripts/ergoCub-Teleoperation-2.xml @@ -15,6 +15,12 @@ --from openXRHeadsetParameters.ini icub-virtualizer + + + yarp + repeat /depthCamera/repeated/rgbImage:o + ergocub001-lap + yarprobotinterface @@ -25,14 +31,20 @@ /depthCamera/rgbImage:o - /headset/display:i + /depthCamera/repeated/rgbImage:o mjpeg + + /depthCamera/repeated/rgbImage:o + /headset/display:i + udp+send.portmonitor+file.image_compression_ffmpeg+recv.portmonitor+file.image_compression_ffmpeg+type.dll+custom_enc.hevc_nvenc+custom_dec.hevc_cuvid+pixel_format.23+cq.31+delay.0+tune.ull + + /object-distance/augmented_reality:o /headset/augmentedReality - mjpeg + udp+send.portmonitor+file.image_compression_ffmpeg+recv.portmonitor+file.image_compression_ffmpeg+type.dll+custom_enc.hevc_nvenc+custom_dec.hevc_cuvid+pixel_format.23+cq.31+delay.0+tune.ull From d1d3e9d68b600beb41a7af0bf2550b12a613ad29 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 16 Feb 2024 12:50:14 +0100 Subject: [PATCH 08/10] Using slow control board in sranipal module --- app/scripts/ergoCub-Teleoperation-4.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/app/scripts/ergoCub-Teleoperation-4.xml b/app/scripts/ergoCub-Teleoperation-4.xml index bd607d48..c7f1f68b 100644 --- a/app/scripts/ergoCub-Teleoperation-4.xml +++ b/app/scripts/ergoCub-Teleoperation-4.xml @@ -19,7 +19,7 @@ SRanipalModule - --skipEyeCalibration --robot ergocub --noEyebrows --noEyelids --gazeVelocityGain 2.0 --gazeDeadzone 0.1 --gazeDeadzoneActivationOffset 0.1 --gazeDeadzoneMinActivationTime 0.2 --eyesVersionName none --eyesVergenceName none --eyesTiltName camera_tilt --blinkToDisableGUIs "(0)" + --skipEyeCalibration --robot ergocub --noEyebrows --noEyelids --gazeVelocityGain 2.0 --gazeDeadzone 0.1 --gazeDeadzoneActivationOffset 0.1 --gazeDeadzoneMinActivationTime 0.2 --eyesVersionName none --eyesVergenceName none --eyesTiltName camera_tilt --blinkToDisableGUIs "(0)" --headControlBoardName head_slow icub-virtualizer From f6f5dc15a1f7c6f4dff88e2e35cbb8e221026d9f Mon Sep 17 00:00:00 2001 From: icub Date: Mon, 19 Feb 2024 18:47:24 +0100 Subject: [PATCH 09/10] use fast tcp and delay.2 for camera --- app/scripts/ergoCub-Teleoperation-2.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/app/scripts/ergoCub-Teleoperation-2.xml b/app/scripts/ergoCub-Teleoperation-2.xml index ade1c9c8..e3c94646 100644 --- a/app/scripts/ergoCub-Teleoperation-2.xml +++ b/app/scripts/ergoCub-Teleoperation-2.xml @@ -38,13 +38,13 @@ /depthCamera/repeated/rgbImage:o /headset/display:i - udp+send.portmonitor+file.image_compression_ffmpeg+recv.portmonitor+file.image_compression_ffmpeg+type.dll+custom_enc.hevc_nvenc+custom_dec.hevc_cuvid+pixel_format.23+cq.31+delay.0+tune.ull + fast_tcp+send.portmonitor+file.image_compression_ffmpeg+recv.portmonitor+file.image_compression_ffmpeg+type.dll+custom_enc.hevc_nvenc+custom_dec.hevc_cuvid+pixel_format.23+cq.31+delay.2+tune.ull /object-distance/augmented_reality:o /headset/augmentedReality - udp+send.portmonitor+file.image_compression_ffmpeg+recv.portmonitor+file.image_compression_ffmpeg+type.dll+custom_enc.hevc_nvenc+custom_dec.hevc_cuvid+pixel_format.23+cq.31+delay.0+tune.ull + fast_tcp+send.portmonitor+file.image_compression_ffmpeg+recv.portmonitor+file.image_compression_ffmpeg+type.dll+custom_enc.hevc_nvenc+custom_dec.hevc_cuvid+pixel_format.23+cq.31+delay.2+tune.ull From 4a0f950cbd914704895fc111e5ff388f0ae464ad Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 18 Apr 2024 16:56:50 +0200 Subject: [PATCH 10/10] add config files for ergoCubSN002 --- app/robots/ergoCubSN002/hapticGloveConfig.ini | 31 ++++ .../leftFingersHapticRetargetingParams.ini | 171 ++++++++++++++++++ .../ergoCubSN002/openXRHeadsetParameters.ini | 145 +++++++++++++++ app/robots/ergoCubSN002/openXRJoypad.ini | 92 ++++++++++ .../rightFingersHapticRetargetingParams.ini | 170 +++++++++++++++++ 5 files changed, 609 insertions(+) create mode 100644 app/robots/ergoCubSN002/hapticGloveConfig.ini create mode 100644 app/robots/ergoCubSN002/leftFingersHapticRetargetingParams.ini create mode 100644 app/robots/ergoCubSN002/openXRHeadsetParameters.ini create mode 100644 app/robots/ergoCubSN002/openXRJoypad.ini create mode 100644 app/robots/ergoCubSN002/rightFingersHapticRetargetingParams.ini diff --git a/app/robots/ergoCubSN002/hapticGloveConfig.ini b/app/robots/ergoCubSN002/hapticGloveConfig.ini new file mode 100644 index 00000000..e13f056a --- /dev/null +++ b/app/robots/ergoCubSN002/hapticGloveConfig.ini @@ -0,0 +1,31 @@ +name HapticGloveRetargeting + + +[GENERAL] +robot ergocub +samplingTime 0.1 +enableMoveRobot 1 +enableLogger 0 +useLeftHand 1 +useRightHand 1 +isMandatory 0 +useSkin 0 +#calibrationTimePeriod [sec] +calibrationTimePeriod 1.0 +#robotInitializationTime [sec] +robotInitializationTime 3.0 +getHumanMotionRange 1 +# the counter to check if steady state is reached [steps] +steadyStateCounterThreshold 5 +# the threshold to check the error between the desired and feedback values for the steady state check [rad] +steadyStateThreshold 0.05 +# the time teleoperation should wait before running [sec] +waitingDurationTime 5.0 +# threshold in which an skin is considered as working fine [no units] +tactileWorkingThreshold 0.0001 +# threshold in which an skin data is considered updated [no units] +tactileUpdateThreshold -1.0 + +# include fingers parameters +[include LEFT_FINGERS_RETARGETING "leftFingersHapticRetargetingParams.ini"] +[include RIGHT_FINGERS_RETARGETING "rightFingersHapticRetargetingParams.ini"] diff --git a/app/robots/ergoCubSN002/leftFingersHapticRetargetingParams.ini b/app/robots/ergoCubSN002/leftFingersHapticRetargetingParams.ini new file mode 100644 index 00000000..a27d0911 --- /dev/null +++ b/app/robots/ergoCubSN002/leftFingersHapticRetargetingParams.ini @@ -0,0 +1,171 @@ +############### +### ROBOT +############### +remote_control_boards ("left_arm") + +remote_sensor_boards "left_hand" + +axis_list ( "l_thumb_add", "l_thumb_oc", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc") + +all_axis_list ( "l_thumb_add", "l_thumb_oc", "l_index_add", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc") + +joint_list ( "l_thumb_add", "l_thumb_oc", "l_index_add", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc" ) + +# the custom range of axes motion [degrees] +# axis-name , min value, max value +# axes_custom_motion_range ( ( ${AXES_NAME} , ${MIN}, ${MAX}) ) + +# 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) ) + +# 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 +analog_list ( ) + +#Adding the groups of each axis and associated joints +l_thumb_add ( "l_thumb_add" ) +l_thumb_oc ( "l_thumb_oc" ) +l_index_add ( "l_index_add" ) +l_index_oc ( "l_index_oc" ) +l_middle_oc ( "l_middle_oc" ) +l_ring_pinky_oc ( "l_ring_pinky_oc" ) + +robot_finger_list ( "l_thumb_finger", "l_index_finger", "l_middle_finger", "l_ring_finger", "l_little_finger") + +############### +### HUMAN +############### +human_joint_list ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_middle", "l_thumb_distal", + "l_index_abduction", "l_index_proximal", "l_index_middle", "l_index_distal", + "l_middle_abduction", "l_middle_proximal", "l_middle_middle", "l_middle_distal", + "l_ring_abduction", "l_ring_proximal", "l_ring_middle", "l_ring_distal", + "l_pinky_abduction", "l_pinky_proximal", "l_pinky_middle", "l_pinky_distal" ) + +human_finger_list ( "l_thumb_finger", "l_index_finger", "l_middle_finger", "l_ring_finger", "l_little_finger") + +hand_link l_hand + +wearable_data_ports ("/WearableData/HapticGlove/LeftHand/data:o") + +wearable_data_actuator_ports_out "/WearableData/HapticGlove/LeftHand/Actuators/input:o" + +wearable_data_actuator_ports_in "/WearableData/HapticGlove/LeftHand/Actuators/input:i" + +############### +### RETARGETING +############### + +motorsJointsCoupled 0 + +robot_to_human_map ( ("l_thumb_add", "l_thumb_oppose") + ("l_thumb_oc", "l_thumb_distal") + ("l_index_add", "l_index_abduction" ) + ("l_index_oc", "l_index_distal") + ("l_middle_oc", "l_middle_distal") + ("l_ring_pinky_oc", "l_ring_distal") ) + +# haptic feedback retargeting from the robot axis groups to the human finger +l_thumb_finger ( "l_thumb_add", "l_thumb_oc" ) +l_index_finger ( "l_index_add", "l_index_oc" ) +l_middle_finger ( "l_middle_oc" ) +l_ring_finger ( "l_ring_pinky_oc" ) +l_little_finger ( "l_ring_pinky_oc" ) + +# This gain is multiplied to the total error for each motor/axis to compute the force feedback to the user associated with each axis, +# defined according to user experience +# Number of gains = number of motors/axis +gainTotalError ( 0.0 15.0 15.0 15.0 15.0 15.0) +#gainTotalError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# check this issue for the velocity Gain: https://github.com/dic-iit/element_retargeting-from-human/issues/141 +# number of gains = number of motors/axis +gainVelocityError ( 0.0 0.1 0.1 0.1 0.1 0.1) +#gainVelocityError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# this value is multiplied to forcefeedback and provides haptic feedback to the user +gainVibrotactile (0.8 0.8 0.8 0.8 0.8 ) +#gainVibrotactile ( 0.0 0.0 0.0 0.0 0.0 ) + +# scaling and biased values for maping the human to robot motion +# the sign of the following vector is important, while the absolute values will be found at configuration file +human_to_robot_joint_angles_scaling ( -1.0 1.0 -1.0 1.0 1.0 1.0 ) + +human_to_robot_joint_anlges_bias ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + +axisContactThreshold 0.1 + +############################## +### ROBOT CONTROL & ESTIMATION +############################## +useVelocity 0 + +referenceVelocityForPositionControl 100.0 + +# minimum and maximum values of the joints +# related to analog sensors +analog_joints_min_boundary ( ) +analog_joints_max_boundary ( ) + +# hall sensors minimum and maximum values +analog_sensors_raw_min_boundary ( ) +analog_sensors_raw_max_boundary ( ) + + +# in case each joint does not have independant motor to actuate, they are coupled +axesJointsCoupled 0 + +# in case a calibration phase necessary before starting teleoperating the robot, +# otherwise read the calibration matrix from the config file +doCalibration 0 + +#robot controller exponential filter gain +exponentialFilterGain 0.9 + +# in the Quadratic optimizartion problem to compute the motor values from the joint values : xT Q X + uT R u +# q_matrix_joint_motor is a list that identifies the main diagonal values of matrix Q: (q x q) matrix; +# size of q: is the number of desired joints to control +# r_matrix_joint_motor is a list that identifies the main diagonal of matrix R: (m x m) matrix; +# size of m: is the number of desired motors to control + +q_qp_control ( 1.0 1.0 1.0 1.0 1.0 1.0 ) + +r_qp_control ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + + +# in the Kalman filter problem to estimate the axis values, velocity and acceleration: +# q_matrix_kf is a list that identifies the main diagonal of matrix: E[ (w(t) -w_bar(t)) (w(t) -w_bar(t))^T ], +# size: m*m positive matrix, Dx(t)= Fx(t)+ Gw(t), the process noise */ +# r_matrix_kf is a list that identifies the main diagonal of matrix: E[ v(t) v(t)^T ], +# size: p*p positive matrix, Z(t)= Hx(t)+ v(t), the measurement noise +no_states_kf 3 +no_measurement_kf 1 +q_matrix_kf ( 10.0 150.0 100000.0 ) +r_matrix_kf ( 0.0000001 ) + +##################### +## TACTILE SENSORS ## +##################### +## length of the tactile sensor port +# noTactileSensors 192 + +## indices to read from the port for each finger +## starting index ending index contact threshold threshold multiplier contact feedback gain derivative threshold threshold multiplier gain derivative vibrotactile feedback +# l_thumb_finger_tactile_info ( 48 59 0.14 1.0 200.0 0.5 3.0 40.0) +# l_index_finger_tactile_info ( 0 11 0.14 1.0 200.0 0.5 3.0 40.0) +# l_middle_finger_tactile_info ( 12 23 0.14 1.0 200.0 0.5 3.0 40.0) +# l_ring_finger_tactile_info ( 24 35 0.14 1.0 200.0 0.5 3.0 40.0) +# l_little_finger_tactile_info ( 36 47 0.14 1.0 200.0 0.5 3.0 40.0) + +# absolute vibrotactile feedback nonlinear function parameters +# 15.0 * std::log(2 * std::pow(x, 0.7) + 1) + 0.5 * std::pow(x, 1.1); +# reference to https://github.com/ami-iit/element_retargeting-from-human/issues/182#issuecomment-1000472012 +# absoluteVibrotactileFeedbackNonlinearFunctionParameters ( 15.0 2.0 0.7 1.0 0.5 1.1 ) + +# percentage dedicated to absolute skin data for providing the vibrotactile feedback +# the value is between [0,1] +# 0 : 0% absolute skin value for the vibrotactile feedback; 100% derivative skin value for the vibrotactile feedback +# 1 : 100% absolute skin value for the vibrotactile feedback; 0% derivative skin value for the vibrotactile feedback +# absoluteSkinValuePercentage 1.0 +# skinDerivativeSmoothingGain 1.0 diff --git a/app/robots/ergoCubSN002/openXRHeadsetParameters.ini b/app/robots/ergoCubSN002/openXRHeadsetParameters.ini new file mode 100644 index 00000000..25429324 --- /dev/null +++ b/app/robots/ergoCubSN002/openXRHeadsetParameters.ini @@ -0,0 +1,145 @@ +###Use as subdevice +### Device params +device JoypadControlServer +use_separate_ports 1 +subdevice openxrheadset +stick_as_axis false +#### + +###Use as direct device +#device openxrheadset +### + +period 10 +name /headset + +mono_eye true + +vr_poses_prediction_in_ms 0 + +### Camera correction +eye_z_position -0.8 + +### Custom frames +custom_poses 3 + +### Gui params +gui_elements 1 +labels 4 +slides 2 + +[CUSTOM_POSE_0] +name stable_waist +parent_frame vive_tracker_waist_pose +euler_angles xzy +relative_position (0.0, 0.0, 0.0) +relative_rotation (*, *, 0.0) + +[CUSTOM_POSE_1] +name world +parent_frame stable_waist +euler_angles zyx +relative_position (0.0, -0.1, 0.1) +relative_rotation (0.0, -1.5708, -1.5708) + +[CUSTOM_POSE_2] +name root_link_desired +parent_frame world +euler_angles zyx +relative_position (0.0, 0.0, 0.0) +relative_rotation (0.0, -0.2, 0.0) + +[GUI_0] +width 1.50 +height 1.50 +x 0.0 +y 0.0 +z -0.7999 +visibility both +follow_eyes true +port_id augmentedReality + +[LABEL_0] +width 0.5 +height 0.05 +x 0.0 +y 0.045 +z -0.5 +prefix "" +suffix "" +pixel_size 45 +color (0.0, 0.0, 1.0, 1.0) +background_color (1.0, 1.0, 1.0, 0.01) +font "Roboto/Roboto-Black.ttf" +horizontal_alignement center +vertical_alignement top +automatically_enabled false +disable_timeout_in_S 1.8 + +[LABEL_1] +width 0.15 +height 0.05 +x -0.12 +y 0.08 +z -0.5 +prefix "Weight L: " +suffix " Kg" +pixel_size 256 +color (1.0, 1.0, 1.0, 1.0) +background_color (0.0, 0.0, 0.0, 1.0) +font "Roboto/Roboto-Medium.ttf" +horizontal_alignement left +vertical_alignement center +automatically_enabled false +disable_timeout_in_S 5.0 + +[LABEL_2] +width 0.15 +height 0.05 +x 0.12 +y 0.08 +z -0.5 +prefix "Weight R: " +suffix " Kg" +pixel_size 256 +color (1.0, 1.0, 1.0, 1.0) +background_color (0.0, 0.0, 0.0, 1.0) +font "Roboto/Roboto-Medium.ttf" +horizontal_alignement left +vertical_alignement center +automatically_enabled false +disable_timeout_in_S 5.0 + +[LABEL_3] +width 0.15 +height 0.05 +x 0.0 +y 0.08 +z -0.5 +prefix "WARNING! Risk: " +suffix "" +pixel_size 256 +color (1.0, 1.0, 1.0, 1.0) +background_color (0.0, 0.0, 0.0, 1.0) +font "Roboto/Roboto-Medium.ttf" +horizontal_alignement center +vertical_alignement center +automatically_enabled false +disable_timeout_in_S 2.0 +port_id risk_label + +[SLIDE_0] +width 0.1 +height 0.1 +x 0.0 +y -0.2 +z -0.5 +slides_path C:/element_ifeel-walking-retargeting/arrow/linear_arrow + +[SLIDE_1] +width 0.1 +height 0.1 +x 0.0 +y -0.2 +z -0.5 +slides_path C:/element_ifeel-walking-retargeting/arrow/rotation_arrow diff --git a/app/robots/ergoCubSN002/openXRJoypad.ini b/app/robots/ergoCubSN002/openXRJoypad.ini new file mode 100644 index 00000000..17152366 --- /dev/null +++ b/app/robots/ergoCubSN002/openXRJoypad.ini @@ -0,0 +1,92 @@ +name openXRJoypad + +# ports +rpcWalkingPort_name /walkingRpc +robotGoalOutputPort_name /walkingGoal +print_buttons 0 +print_axes 0 +print_sticks 0 + +[GENERAL] +samplingTime 0.01 +robot ergocub + +# include fingers parameters +[LEFT_FINGERS_RETARGETING] +enabled 1 +remote_control_boards ("left_arm") +joints_list ( "l_thumb_add", "l_thumb_oc", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc" ) + +useVelocity 1 + +fingersScaling (1, 2.0, 3.5, 3.5, 3.5) + +[RIGHT_FINGERS_RETARGETING] +enabled 1 +remote_control_boards ("right_arm") +joints_list ( "r_thumb_add", "r_thumb_oc", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc") + +useVelocity 1 + +fingersScaling (1, 2.0, 3.5, 3.5, 3.5) + +[OPENXR] +check_hands_inverted 0 +joypad_device_remote /headset +left_hand_frame_name openxr_left_hand +right_hand_frame_name openxr_right_hand +head_frame_name openxr_head + +deadzone 0.3 +fullscale 1.0 + +# The following code is as follows: +# - the first character is either A or S. A indicates "Axis", S indicates "Stick" +# - the second character is either a "+" or a "-" indicating if the sign has to be inverted or not +# - the following characters are either the axis or the stick index +# - in case of a stick, it is also necessary to specify the dof index after a "." +# As an example S-1.2 indicates the dof number 2 (0-based) of the stick number 1 (0-based), changing sign. +# Another example: A+3 indicates the axis number 3 (0-based) and the sign does not need to change. + +left_x_code S+0.1 +left_y_code S-0.0 +right_x_code S+1.1 +right_y_code S-1.0 + +left_fingers_velocity_code A+0 +right_fingers_velocity_code A+1 + +# Vive buttons order +# "vive_left_menu", +# "vive_left_trigger_click", +# "vive_left_squeeze_click", +# "vive_left_trackpad_click", +# "vive_right_menu", +# "vive_right_trigger_click", +# "vive_right_squeeze_click", +# "vive_right_trackpad_click" + + +# In the buttons maps we have the following: +# 1 means that the button needs to be pressed +# 0 means that the button should not be pressed +# -1 means that the button is ignored + +prepare_walking_buttons_map ( 0, -1, -1, -1, 1, -1, -1, -1) # right menu ONLY +start_walking_buttons_map ( 1, -1, -1, -1, 0, -1, -1, -1) # left menu ONLY +stop_walking_buttons_map ( 1, -1, -1, -1, 1, -1, -1, -1) # left and right menu + +# Buttons to use when the left and right hand are swapped +prepare_walking_buttons_swapped_map ( 1, -1, -1, -1, 0, -1, -1, -1) # left menu ONLY +start_walking_buttons_map_swapped ( 0, -1, -1, -1, 1, -1, -1, -1) # right menu ONLY +stop_walking_buttons_swapped_map ( 1, -1, -1, -1, 1, -1, -1, -1) # identical to the non-swapped case + +walking_command_release_buttons_map ( 0, -1, -1, -1, 0, -1, -1, -1) # Send the command only when both menu buttons have been released + +left_fingers_squeeze_buttons_map (-1, -1, 0, -1, -1, -1, -1, -1) # left squeeze deactivated +left_fingers_release_buttons_map (-1, -1, 1, -1, -1, -1, -1, -1) # left squeeze deactivated +left_walking_buttons_map (-1, -1, -1, 1, -1, -1, -1, -1) # left_trackpad_click activated + +right_fingers_squeeze_buttons_map (-1, -1, -1, -1, -1, -1, 0, -1) # right squeeze deactivated +right_fingers_release_buttons_map (-1, -1, -1, -1, -1, -1, 1, -1) # right squeeze deactivated +right_walking_buttons_map (-1, -1, -1, -1, -1, -1, -1, 1) # right_trackpad_click activated diff --git a/app/robots/ergoCubSN002/rightFingersHapticRetargetingParams.ini b/app/robots/ergoCubSN002/rightFingersHapticRetargetingParams.ini new file mode 100644 index 00000000..9e464efa --- /dev/null +++ b/app/robots/ergoCubSN002/rightFingersHapticRetargetingParams.ini @@ -0,0 +1,170 @@ +############### +### ROBOT +############### +remote_control_boards ("right_arm") + +remote_sensor_boards "right_hand" + +axis_list ( "r_thumb_add", "r_thumb_oc", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc") + +all_axis_list ( "r_thumb_add", "r_thumb_oc", "r_index_add", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc") + +joint_list ( "r_thumb_add", "r_thumb_oc", "r_index_add", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc" ) + +# the custom range of axes motion [degrees] +# axis-name , min value, max value +# axes_custom_motion_range ( ( ${AXES_NAME} , ${MIN}, ${MAX}) ) + +# 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) ) + +# if a joint is not listed here, the sensory information is of encoder type. +# notice that "r_thumb_oppose" and "r_hand_fingers" is measured by joint encoders not the analog sensors +analog_list ( ) + +#Adding the groups of each axis and associated joints +r_thumb_add ( "r_thumb_add" ) +r_thumb_oc ( "r_thumb_oc" ) +r_index_add ( "r_index_add" ) +r_index_oc ( "r_index_oc" ) +r_middle_oc ( "r_middle_oc" ) +r_ring_pinky_oc ( "r_ring_pinky_oc" ) + +robot_finger_list ( "r_thumb_finger", "r_index_finger", "r_middle_finger", "r_ring_finger", "r_little_finger") + +############### +### HUMAN +############### +human_joint_list ( "r_thumb_oppose", "r_thumb_proximal", "r_thumb_middle", "r_thumb_distal", + "r_index_abduction", "r_index_proximal", "r_index_middle", "r_index_distal", + "r_middle_abduction", "r_middle_proximal", "r_middle_middle", "r_middle_distal", + "r_ring_abduction", "r_ring_proximal", "r_ring_middle", "r_ring_distal", + "r_pinky_abduction", "r_pinky_proximal", "r_pinky_middle", "r_pinky_distal" ) + +human_finger_list ( "r_thumb_finger", "r_index_finger", "r_middle_finger", "r_ring_finger", "r_little_finger") + +hand_link r_hand + +wearable_data_ports ("/WearableData/HapticGlove/RightHand/data:o") + +wearable_data_actuator_ports_out "/WearableData/HapticGlove/RightHand/Actuators/input:o" + +wearable_data_actuator_ports_in "/WearableData/HapticGlove/RightHand/Actuators/input:i" + +############### +### RETARGETING +############### + +motorsJointsCoupled 0 + +robot_to_human_map ( ("r_thumb_add", "r_thumb_oppose") + ("r_thumb_oc", "r_thumb_distal") + ("r_index_add", "r_index_abduction" ) + ("r_index_oc", "r_index_distal") + ("r_middle_oc", "r_middle_distal") + ("r_ring_pinky_oc", "r_ring_distal") ) + +# haptic feedback retargeting from the robot axis groups to the human finger +r_thumb_finger ( "r_thumb_add", "r_thumb_oc" ) +r_index_finger ( "r_index_add", "r_index_oc" ) +r_middle_finger ( "r_middle_oc" ) +r_ring_finger ( "r_ring_pinky_oc" ) +r_little_finger ( "r_ring_pinky_oc" ) + +# This gain is multiplied to the total error for each motor/axis to compute the force feedback to the user associated with each axis, +# defined according to user experience +# Number of gains = number of motors/axis +gainTotalError ( 0.0 15.0 15.0 15.0 15.0 15.0) +#gainTotalError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# check this issue for the velocity Gain: https://github.com/dic-iit/element_retargeting-from-human/issues/141 +# number of gains = number of motors/axis +gainVelocityError ( 0.0 0.1 0.1 0.1 0.1 0.1) +#gainVelocityError ( 0.0 0.0 0.0 0.0 0.0 0.0) + +# this value is multiplied to forcefeedback and provides haptic feedback to the user +gainVibrotactile ( 0.8 0.8 0.8 0.8 0.8 ) +#gainVibrotactile ( 0.0 0.0 0.0 0.0 0.0 ) + +# scaling and biased values for maping the human to robot motion +# the sign of the following vector is important, while the absolute values will be found at configuration file +human_to_robot_joint_angles_scaling ( 1.0 1.0 1.0 1.0 1.0 1.0 ) + +human_to_robot_joint_anlges_bias ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + +axisContactThreshold 0.1 + +############################## +### ROBOT CONTROL & ESTIMATION +############################## +useVelocity 0 + +referenceVelocityForPositionControl 100.0 + +# minimum and maximum values of the joints +# related to analog sensors +analog_joints_min_boundary ( ) +analog_joints_max_boundary ( ) + +# hall sensors minimum and maximum values +analog_sensors_raw_min_boundary ( ) +analog_sensors_raw_max_boundary ( ) + + +# in case each joint does not have independant motor to actuate, they are coupled +axesJointsCoupled 0 + +# in case a calibration phase necessary before starting teleoperating the robot, +# otherwise read the calibration matrix from the config file +doCalibration 0 + +#robot controller exponential filter gain +exponentialFilterGain 0.9 + +# in the Quadratic optimizartion problem to compute the motor values from the joint values : xT Q X + uT R u +# q_matrix_joint_motor is a list that identifies the main diagonal values of matrix Q: (q x q) matrix; +# size of q: is the number of desired joints to control +# r_matrix_joint_motor is a list that identifies the main diagonal of matrix R: (m x m) matrix; +# size of m: is the number of desired motors to control + +q_qp_control ( 1.0 1.0 1.0 1.0 1.0 1.0 ) + +r_qp_control ( 0.0 0.0 0.0 0.0 0.0 0.0 ) + + +# in the Kalman filter problem to estimate the axis values, velocity and acceleration: +# q_matrix_kf is a list that identifies the main diagonal of matrix: E[ (w(t) -w_bar(t)) (w(t) -w_bar(t))^T ], +# size: m*m positive matrix, Dx(t)= Fx(t)+ Gw(t), the process noise */ +# r_matrix_kf is a list that identifies the main diagonal of matrix: E[ v(t) v(t)^T ], +# size: p*p positive matrix, Z(t)= Hx(t)+ v(t), the measurement noise +no_states_kf 3 +no_measurement_kf 1 +q_matrix_kf ( 10.0 150.0 100000.0 ) +r_matrix_kf ( 0.0000001 ) + +##################### +## TACTILE SENSORS ## +##################### +## length of the tactile sensor port +# noTactileSensors 192 + +## indices to read from the port for each finger +## starting index ending index contact threshold threshold multiplier contact feedback gain derivative threshold threshold multiplier gain derivative vibrotactile feedback +# r_thumb_finger_tactile_info ( 48 59 0.14 1.0 200.0 0.5 3.0 40.0) +# r_index_finger_tactile_info ( 0 11 0.14 1.0 200.0 0.5 3.0 40.0) +# r_middle_finger_tactile_info ( 12 23 0.14 1.0 200.0 0.5 3.0 40.0) +# r_ring_finger_tactile_info ( 24 35 0.14 1.0 200.0 0.5 3.0 40.0) +# r_little_finger_tactile_info ( 36 47 0.14 1.0 200.0 0.5 3.0 40.0) + +# absolute vibrotactile feedback nonlinear function parameters +# 15.0 * std::log(2 * std::pow(x, 0.7) + 1) + 0.5 * std::pow(x, 1.1); +# reference to https://github.com/ami-iit/element_retargeting-from-human/issues/182#issuecomment-1000472012 +# absoluteVibrotactileFeedbackNonlinearFunctionParameters ( 15.0 2.0 0.7 1.0 0.5 1.1 ) + +# percentage dedicated to absolute skin data for providing the vibrotactile feedback +# the value is between [0,1] +# 0 : 0% absolute skin value for the vibrotactile feedback; 100% derivative skin value for the vibrotactile feedback +# 1 : 100% absolute skin value for the vibrotactile feedback; 0% derivative skin value for the vibrotactile feedback +# absoluteSkinValuePercentage 1.0 +# skinDerivativeSmoothingGain 1.0