From 4d61702bb2e83e8cff45c76285cdadfa4c534ac2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 30 Mar 2024 21:28:11 +0100 Subject: [PATCH] Remove all the usage of getRandomModel in QPInverseKinematicsTest and QPFixedBaseInverseKinematicsTest --- .../QPFixedBaseInverseKinematicsTest.cpp | 44 ++++++++++++++++++- src/IK/tests/QPInverseKinematicsTest.cpp | 39 +++++++++++----- 2 files changed, 70 insertions(+), 13 deletions(-) diff --git a/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp b/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp index 4f3d725ae7..c0ce914f4b 100644 --- a/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp +++ b/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp @@ -110,7 +110,6 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, out.regularizationTask = std::make_shared(); - REQUIRE(out.regularizationTask->setKinDyn(kinDyn)); REQUIRE(out.regularizationTask->initialize(handler->getGroup("REGULARIZATION_TASK"))); REQUIRE(out.ik->addTask(out.regularizationTask, @@ -210,6 +209,42 @@ System getSystem(std::shared_ptr kinDyn) return out; } +inline std::string customInt2string(int i) +{ + std::stringstream ss; + + ss << i; + + return ss.str(); +} + +// Workaround for https://github.com/ami-iit/bipedal-locomotion-framework/issues/799 and +// https://github.com/robotology/idyntree/pull/1171 +inline iDynTree::Model customGetRandomModelWithNoPrismaticJoints(unsigned int nrOfJoints, + size_t nrOfAdditionalFrames = 10, + bool onlyRevoluteJoints = false) +{ + iDynTree::Model model; + + model.addLink("baseLink", iDynTree::getRandomLink()); + + for (unsigned int i = 0; i < nrOfJoints; i++) + { + std::string parentLink = iDynTree::getRandomLinkOfModel(model); + std::string linkName = "link" + customInt2string(i); + iDynTree::addRandomLinkToModel(model, parentLink, linkName, onlyRevoluteJoints); + } + + for (unsigned int i = 0; i < nrOfAdditionalFrames; i++) + { + std::string parentLink = iDynTree::getRandomLinkOfModel(model); + std::string frameName = "additionalFrame" + customInt2string(i); + iDynTree::addRandomAdditionalFrameToModel(model, parentLink, frameName); + } + + return model; +} + TEST_CASE("QP-IK") { auto kinDyn = std::make_shared(); @@ -226,7 +261,12 @@ TEST_CASE("QP-IK") DYNAMIC_SECTION("Model with " << numberOfJoints << " joints") { // create the model - const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + constexpr std::size_t nrOfAdditionalFrames = 10; + constexpr bool onlyRevoluteJoints = true; + const iDynTree::Model model + = customGetRandomModelWithNoPrismaticJoints(numberOfJoints, + nrOfAdditionalFrames, + onlyRevoluteJoints); REQUIRE(kinDyn->loadRobotModel(model)); const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints); diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index 65e4e35b12..87f757859c 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -320,31 +320,33 @@ inline std::string customInt2string(int i) return ss.str(); } -// Workaround for https://github.com/ami-iit/bipedal-locomotion-framework/issues/799 -inline iDynTree::Model customGetRandomModelWithNoPrismaticJoints(unsigned int nrOfJoints, size_t nrOfAdditionalFrames = 10, bool onlyRevoluteJoints=false) +// Workaround for https://github.com/ami-iit/bipedal-locomotion-framework/issues/799 and +// https://github.com/robotology/idyntree/pull/1171 +inline iDynTree::Model customGetRandomModelWithNoPrismaticJoints(unsigned int nrOfJoints, + size_t nrOfAdditionalFrames = 10, + bool onlyRevoluteJoints = false) { iDynTree::Model model; model.addLink("baseLink", iDynTree::getRandomLink()); - for(unsigned int i=0; i < nrOfJoints; i++) + for (unsigned int i = 0; i < nrOfJoints; i++) { std::string parentLink = iDynTree::getRandomLinkOfModel(model); std::string linkName = "link" + customInt2string(i); - iDynTree::addRandomLinkToModel(model,parentLink,linkName,onlyRevoluteJoints); + iDynTree::addRandomLinkToModel(model, parentLink, linkName, onlyRevoluteJoints); } - for(unsigned int i=0; i < nrOfAdditionalFrames; i++) + for (unsigned int i = 0; i < nrOfAdditionalFrames; i++) { std::string parentLink = iDynTree::getRandomLinkOfModel(model); std::string frameName = "additionalFrame" + customInt2string(i); - iDynTree::addRandomAdditionalFrameToModel(model,parentLink,frameName); + iDynTree::addRandomAdditionalFrameToModel(model, parentLink, frameName); } return model; } - TEST_CASE("QP-IK") { auto kinDyn = std::make_shared(); @@ -365,7 +367,10 @@ TEST_CASE("QP-IK") // create the model size_t nrOfAdditionalFrames = 10; bool onlyRevoluteJoints = true; - const iDynTree::Model model = customGetRandomModelWithNoPrismaticJoints(numberOfJoints,nrOfAdditionalFrames,onlyRevoluteJoints); + const iDynTree::Model model + = customGetRandomModelWithNoPrismaticJoints(numberOfJoints, + nrOfAdditionalFrames, + onlyRevoluteJoints); REQUIRE(kinDyn->loadRobotModel(model)); const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints); @@ -499,7 +504,11 @@ TEST_CASE("QP-IK [With strict limits]") constexpr std::size_t numberOfJoints = 30; // create the model - const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + size_t nrOfAdditionalFrames = 10; + bool onlyRevoluteJoints = true; + const iDynTree::Model model = customGetRandomModelWithNoPrismaticJoints(numberOfJoints, + nrOfAdditionalFrames, + onlyRevoluteJoints); REQUIRE(kinDyn->loadRobotModel(model)); const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints); @@ -629,7 +638,11 @@ TEST_CASE("QP-IK [With builder]") constexpr std::size_t numberOfJoints = 30; // create the model - const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + size_t nrOfAdditionalFrames = 10; + bool onlyRevoluteJoints = true; + const iDynTree::Model model = customGetRandomModelWithNoPrismaticJoints(numberOfJoints, + nrOfAdditionalFrames, + onlyRevoluteJoints); REQUIRE(kinDyn->loadRobotModel(model)); // VariableHandler and IK params @@ -782,7 +795,11 @@ TEST_CASE("QP-IK [Distance and Gravity tasks]") constexpr std::size_t numberOfJoints = 30; // create the model - const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + size_t nrOfAdditionalFrames = 10; + bool onlyRevoluteJoints = true; + const iDynTree::Model model = customGetRandomModelWithNoPrismaticJoints(numberOfJoints, + nrOfAdditionalFrames, + onlyRevoluteJoints); REQUIRE(kinDyn->loadRobotModel(model)); const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints);