Skip to content

Commit

Permalink
Remove all the usage of getRandomModel in QPInverseKinematicsTest and…
Browse files Browse the repository at this point in the history 
… QPFixedBaseInverseKinematicsTest
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi committed Mar 30, 2024
1 parent cf4ed6d commit 4d61702
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 13 deletions.
44 changes: 42 additions & 2 deletions src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,6 @@ InverseKinematicsAndTasks createIK(std::shared_ptr<IParametersHandler> handler,

out.regularizationTask = std::make_shared<JointTrackingTask>();


REQUIRE(out.regularizationTask->setKinDyn(kinDyn));
REQUIRE(out.regularizationTask->initialize(handler->getGroup("REGULARIZATION_TASK")));
REQUIRE(out.ik->addTask(out.regularizationTask,
Expand Down Expand Up @@ -210,6 +209,42 @@ System getSystem(std::shared_ptr<iDynTree::KinDynComputations> 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<iDynTree::KinDynComputations>();
Expand All @@ -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);
Expand Down
39 changes: 28 additions & 11 deletions src/IK/tests/QPInverseKinematicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<iDynTree::KinDynComputations>();
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 4d61702

Please sign in to comment.