From b6aa6c5f27687952352ad7096589edaac517f6b7 Mon Sep 17 00:00:00 2001 From: Mattia Fussi Date: Fri, 11 Feb 2022 14:10:07 +0100 Subject: [PATCH 1/5] Add FT sensor to right upper leg of iCubGenova09 --- .../data/icub3/ICUB_3_all_options.yaml | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/simmechanics/data/icub3/ICUB_3_all_options.yaml b/simmechanics/data/icub3/ICUB_3_all_options.yaml index cc7e6ad..7a8f4e3 100644 --- a/simmechanics/data/icub3/ICUB_3_all_options.yaml +++ b/simmechanics/data/icub3/ICUB_3_all_options.yaml @@ -267,15 +267,15 @@ forceTorqueSensors: model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini # right leg - #- jointName: r_leg_ft_sensor - # directionChildToParent: Yes - # frame: sensor - # frameName: SCSYS_R_HIP_2_FT - # sensorBlobs: - # - | - # - # model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini - # + - jointName: r_leg_ft_sensor + directionChildToParent: Yes + frame: sensor + frameName: SCSYS_R_HIP_2_FT + sensorBlobs: + - | + + model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini + - jointName: r_foot_front_ft_sensor directionChildToParent: No frame: sensor @@ -831,5 +831,3 @@ XMLBlobs: 1 - - From e25ddf9296343f7f06c095c8a07791e7737e8ffe Mon Sep 17 00:00:00 2001 From: Mattia Fussi Date: Fri, 11 Feb 2022 14:16:31 +0100 Subject: [PATCH 2/5] add right leg ft on yaml.in file --- .../icub3/ICUB_3_all_options_gazebo.yaml.in | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in b/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in index 39a763c..c02afbe 100644 --- a/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in +++ b/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in @@ -264,15 +264,15 @@ forceTorqueSensors: model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini # right leg - #- jointName: r_leg_ft_sensor - # directionChildToParent: Yes - # frame: sensor - # frameName: SCSYS_R_HIP_2_FT - # sensorBlobs: - # - | - # - # model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini - # + - jointName: r_leg_ft_sensor + directionChildToParent: Yes + frame: sensor + frameName: SCSYS_R_HIP_2_FT + sensorBlobs: + - | + + model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini + - jointName: r_foot_front_ft_sensor directionChildToParent: No frame: sensor @@ -919,6 +919,3 @@ XMLBlobs: 1 - - - From 23c57c08574e641630378b401c60b3428824224c Mon Sep 17 00:00:00 2001 From: Mattia Fussi Date: Fri, 11 Feb 2022 17:14:06 +0100 Subject: [PATCH 3/5] Changed test accounting for an odd number of FTs --- tests/icub-model-test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tests/icub-model-test.cpp b/tests/icub-model-test.cpp index 5bb0a14..09b7aff 100644 --- a/tests/icub-model-test.cpp +++ b/tests/icub-model-test.cpp @@ -360,9 +360,9 @@ bool checkAxisDirectionsV3(iDynTree::KinDynComputations & comp) } /** - * All the iCub have a even and not null number of F/T sensors. + * All the iCub have a odd and not null number of F/T sensors. */ -bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader) +bool checkFTSensorsAreOddAndNotNull(iDynTree::ModelLoader & mdlLoader) { int nrOfFTSensors = mdlLoader.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); @@ -372,9 +372,9 @@ bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader) return false; } - if( nrOfFTSensors % 2 == 1 ) + if( nrOfFTSensors % 2 == 0 ) { - std::cerr << "icub-model-test : odd number of F/T sensor found in the model" << std::endl; + std::cerr << "icub-model-test : even number of F/T sensor found in the model" << std::endl; return false; } @@ -535,7 +535,7 @@ int main(int argc, char ** argv) } // Now some test that test the sensors - if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) ) + if( !checkFTSensorsAreOddAndNotNull(mdlLoader) ) { return EXIT_FAILURE; } From 2bdc9cec8a630ff99967f8dc333db54bf49c690d Mon Sep 17 00:00:00 2001 From: Mattia Fussi Date: Fri, 11 Feb 2022 17:36:05 +0100 Subject: [PATCH 4/5] Compare to even number of n Ft sensors if icub2 as before --- tests/icub-model-test.cpp | 40 ++++++++++++++++++++++++++++++++++----- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/tests/icub-model-test.cpp b/tests/icub-model-test.cpp index 09b7aff..7cfa21d 100644 --- a/tests/icub-model-test.cpp +++ b/tests/icub-model-test.cpp @@ -382,6 +382,30 @@ bool checkFTSensorsAreOddAndNotNull(iDynTree::ModelLoader & mdlLoader) return true; } +/** + * All the iCub have a even and not null number of F/T sensors. + */ +bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader) +{ + int nrOfFTSensors = mdlLoader.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); + + if( nrOfFTSensors == 0 ) + { + std::cerr << "icub-model-test error: no F/T sensor found in the model" << std::endl; + return false; + } + + if( nrOfFTSensors % 2 == 1 ) + { + std::cerr << "icub-model-test : odd number of F/T sensor found in the model" << std::endl; + return false; + } + + + return true; +} + + bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp, const iDynTree::Rotation& expected, const std::string& sensorName) @@ -535,14 +559,15 @@ int main(int argc, char ** argv) } // Now some test that test the sensors - if( !checkFTSensorsAreOddAndNotNull(mdlLoader) ) - { - return EXIT_FAILURE; - } - // The ft sensors orientation respect to the root_link are different to iCubV2 and they are under investigation. if (modelPath.find("Genova09") != std::string::npos || modelPath.find("GazeboV3") != std::string::npos) { + + if( !checkFTSensorsAreOddAndNotNull(mdlLoader) ) + { + return EXIT_FAILURE; + } + if (!checkFTSensorsAreCorrectlyOrientedV3(comp)) { return EXIT_FAILURE; @@ -550,6 +575,11 @@ int main(int argc, char ** argv) } else { + if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) ) + { + return EXIT_FAILURE; + } + if (!checkFTSensorsAreCorrectlyOrientedV2(comp)) { return EXIT_FAILURE; From 0e98268786954549de679f262c702dffb7620b8c Mon Sep 17 00:00:00 2001 From: Mattia Fussi Date: Mon, 14 Feb 2022 15:32:48 +0100 Subject: [PATCH 5/5] Update test with rotation matrix of FT sensor on right upper leg --- tests/icub-model-test.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/icub-model-test.cpp b/tests/icub-model-test.cpp index 7cfa21d..91c7930 100644 --- a/tests/icub-model-test.cpp +++ b/tests/icub-model-test.cpp @@ -478,9 +478,14 @@ bool checkFTSensorsAreCorrectlyOrientedV3(iDynTree::KinDynComputations & comp) -0.866025, -0.5, 0, 0, 0, 1); + iDynTree::Rotation rootLink_R_sensorFrameExpectedLeg = + iDynTree::Rotation(-0.866025, -0.5, 0, + -0.5, 0.866025, 0, + 0, 0, -1); + bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft_sensor"); ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft_sensor") && ok; - // l_leg_ft_sensor and r_leg_ft_sensor frames seems to be wrong(see https://github.com/robotology/icub-models/issues/71) + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft_sensor") && ok; ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft_sensor") && ok; ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft_sensor") && ok; ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft_sensor") && ok;