From ad435d5926bc84279b20944a6ec3e32b4cfd51bf Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 6 Nov 2023 14:29:54 +0100 Subject: [PATCH 1/5] ergoCubV1_1: move down the l/r_sole frames It fixes #186 --- .../robots/ergoCubGazeboV1_1/model.urdf | 20 ++++++++-------- .../ergoCubGazeboV1_1_minContacts/model.urdf | 20 ++++++++-------- urdf/ergoCub/robots/ergoCubSN001/model.urdf | 24 +++++++++---------- 3 files changed, 32 insertions(+), 32 deletions(-) diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf index 3430d585..78d33724 100644 --- a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf +++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf @@ -1241,8 +1241,8 @@ - - + + @@ -1251,7 +1251,7 @@ - + @@ -1571,8 +1571,8 @@ - - + + @@ -1604,8 +1604,8 @@ - - + + @@ -1614,7 +1614,7 @@ - + @@ -2508,7 +2508,7 @@ - + @@ -2874,7 +2874,7 @@ - + diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf index 9c22d5ae..6a2cae1b 100644 --- a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf +++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf @@ -1133,8 +1133,8 @@ - - + + @@ -1143,7 +1143,7 @@ - + @@ -1433,8 +1433,8 @@ - - + + @@ -1463,8 +1463,8 @@ - - + + @@ -1473,7 +1473,7 @@ - + @@ -2328,7 +2328,7 @@ - + @@ -2694,7 +2694,7 @@ - + diff --git a/urdf/ergoCub/robots/ergoCubSN001/model.urdf b/urdf/ergoCub/robots/ergoCubSN001/model.urdf index c165fec4..e8e07897 100644 --- a/urdf/ergoCub/robots/ergoCubSN001/model.urdf +++ b/urdf/ergoCub/robots/ergoCubSN001/model.urdf @@ -1241,8 +1241,8 @@ - - + + @@ -1251,8 +1251,8 @@ - - + + @@ -1571,8 +1571,8 @@ - - + + @@ -1604,8 +1604,8 @@ - - + + @@ -1614,8 +1614,8 @@ - - + + @@ -2508,7 +2508,7 @@ - + @@ -2874,7 +2874,7 @@ - + From c6e9cb2ff9702b25ba4b5119b02cceb29b2a8cb3 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 6 Nov 2023 14:34:06 +0100 Subject: [PATCH 2/5] ergoCubV1: move down l/r_sole frames --- urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf | 4 ++-- urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf | 4 ++-- urdf/ergoCub/robots/ergoCubSN000/model.urdf | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf index 8c9325a4..97d64ff8 100644 --- a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf +++ b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf @@ -280,7 +280,7 @@ - + @@ -600,7 +600,7 @@ - + diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf index b9c9e20d..207ebe08 100644 --- a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf +++ b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf @@ -280,7 +280,7 @@ - + @@ -600,7 +600,7 @@ - + diff --git a/urdf/ergoCub/robots/ergoCubSN000/model.urdf b/urdf/ergoCub/robots/ergoCubSN000/model.urdf index 87413a34..995ec051 100644 --- a/urdf/ergoCub/robots/ergoCubSN000/model.urdf +++ b/urdf/ergoCub/robots/ergoCubSN000/model.urdf @@ -280,7 +280,7 @@ - + @@ -600,7 +600,7 @@ - + From 518dd7913018a3131b6d4675d5893685a0dbb471 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 6 Nov 2023 14:47:46 +0100 Subject: [PATCH 3/5] ergocub-model-test: add unit test checking the soles --- tests/ergocub-model-test.cpp | 57 ++++++++++++++++++++++++++---------- 1 file changed, 42 insertions(+), 15 deletions(-) diff --git a/tests/ergocub-model-test.cpp b/tests/ergocub-model-test.cpp index baa5e97a..15ad30fe 100644 --- a/tests/ergocub-model-test.cpp +++ b/tests/ergocub-model-test.cpp @@ -163,7 +163,7 @@ bool checkBaseLink(iDynTree::KinDynComputations & comp) return true; } -bool checkSolesAreParallel(iDynTree::KinDynComputations & comp) +bool checkSolesAreParallelAndCorrectlyPlaced(iDynTree::KinDynComputations & comp) { iDynTree::LinkIndex rootLinkIdx = comp.getFrameIndex("root_link"); @@ -192,6 +192,32 @@ bool checkSolesAreParallel(iDynTree::KinDynComputations & comp) iDynTree::Transform root_H_l_sole = comp.getRelativeTransform(rootLinkIdx,l_sole); iDynTree::Transform root_H_r_sole = comp.getRelativeTransform(rootLinkIdx,r_sole); + iDynTree::Transform root_H_l_sole_expected(iDynTree::Rotation(1, 0, 0, + 0, 1, 0, + 0, 0, 1), + iDynTree::Position(0.04403,0.0744,-0.7793)); + iDynTree::Transform root_H_r_sole_expected(iDynTree::Rotation(1, 0, 0, + 0, 1, 0, + 0, 0, 1), + iDynTree::Position(0.04403,-0.0744,-0.7793)); + + + if (!checkTransformAreEqual(root_H_l_sole, root_H_l_sole_expected, 1e-5)) + { + std::cerr << "ergocub-model-test : transform between root_H_l_sole is not the expected one, test failed." << std::endl; + std::cerr << "ergocub-model-test : root_H_l_sole :" << root_H_l_sole.toString() << std::endl; + std::cerr << "ergocub-model-test : root_H_l_sole_expected :" << root_H_l_sole_expected.toString() << std::endl; + return false; + } + + if (!checkTransformAreEqual(root_H_r_sole, root_H_r_sole_expected, 1e-5)) + { + std::cerr << "ergocub-model-test : transform between root_H_r_sole is not the expected one, test failed." << std::endl; + std::cerr << "ergocub-model-test : root_H_r_sole :" << root_H_r_sole.toString() << std::endl; + std::cerr << "ergocub-model-test : root_H_r_sole_expected :" << root_H_r_sole_expected.toString() << std::endl; + return false; + } + // height of the sole should be equal double l_sole_height = root_H_l_sole.getPosition().getVal(2); double r_sole_height = root_H_r_sole.getPosition().getVal(2); @@ -446,21 +472,21 @@ bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader) } -bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp, +bool checkFrameIsCorrectlyOriented(iDynTree::KinDynComputations & comp, const iDynTree::Rotation& expected, - const std::string& sensorName) + const std::string& frameName) { // Depending on the ergocub model, the sensor could be absent - if (!comp.model().isFrameNameUsed(sensorName)) + if (!comp.model().isFrameNameUsed(frameName)) { return true; } - iDynTree::Rotation actual = comp.getRelativeTransform("root_link", sensorName).getRotation(); + iDynTree::Rotation actual = comp.getRelativeTransform("root_link", frameName).getRotation(); if (!checkMatrixAreEqual(expected, actual, 1e-3)) { - std::cerr << "ergocub-model-test : transform between root_link and " << sensorName << " is not the expected one, test failed." << std::endl; + std::cerr << "ergocub-model-test : transform between root_link and " << frameName << " is not the expected one, test failed." << std::endl; std::cerr << "ergocub-model-test : Expected transform : " << expected.toString() << std::endl; std::cerr << "ergocub-model-test : Actual transform : " << actual.toString() << std::endl; return false; @@ -470,6 +496,7 @@ bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp, } + bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp) { @@ -497,14 +524,14 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp) 0.5, 0.866025, 0, 0, 0, -1); - bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft"); - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok; - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft") && ok; - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft") && ok; - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft") && ok; - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft") && ok; - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft") && ok; - ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_front_ft") && ok; + bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft"); + ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok; + ok = checkFrameIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft") && ok; + ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft") && ok; + ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft") && ok; + ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft") && ok; + ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft") && ok; + ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_front_ft") && ok; return ok; } @@ -726,7 +753,7 @@ int main(int argc, char ** argv) // Check if l_sole/r_sole have the same distance from the root_link - if( !checkSolesAreParallel(comp) ) + if( !checkSolesAreParallelAndCorrectlyPlaced(comp) ) { return EXIT_FAILURE; } From 8317c406edd76fb2180d49d1dc7e6ebb494eb25c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 6 Nov 2023 14:49:26 +0100 Subject: [PATCH 4/5] Update CHANGELOG --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 161888f2..9ee60d47 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,8 +7,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### + ## [0.5.0] - 2023-10-26 +### urdf +- Fixed wobbling of the models after moving feet link frames + ## [0.4.0] - 2023-09-04 ### urdf From 866b8be903c734a053c887910af6dc667035436a Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 6 Nov 2023 14:51:34 +0100 Subject: [PATCH 5/5] Update CHANGELOG --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9ee60d47..f650e2c0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,7 +7,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] -### +### urdf +- Fix location of `l/r_sole` frames(https://github.com/icub-tech-iit/ergocub-software/issues/186) ## [0.5.0] - 2023-10-26