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 7a5383e
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 18 deletions.
20 changes: 13 additions & 7 deletions src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ using namespace BipedalLocomotion::Conversions;
using namespace std::chrono_literals;

constexpr auto robotVelocity = "robotVelocity";
constexpr std::chrono::nanoseconds dT = 5ms;
constexpr std::chrono::nanoseconds dT = 10ms;

struct InverseKinematicsAndTasks
{
Expand All @@ -62,7 +62,7 @@ struct System

std::shared_ptr<IParametersHandler> createParameterHandler()
{
constexpr double gain = 0.5;
constexpr double gain = 20.0;

auto parameterHandler = std::make_shared<StdImplementation>();
parameterHandler->setParameter("robot_velocity_variable_name", robotVelocity);
Expand Down 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 @@ -189,15 +188,18 @@ System getSystem(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
Eigen::VectorXd jointPositions(kinDyn->getNrOfDegreesOfFreedom());
Eigen::Vector3d gravity;

REQUIRE(kinDyn->getRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity));
REQUIRE(kinDyn->getRobotState(basePose, //
jointPositions,
baseVelocity,
jointVelocities,
gravity));

// perturb the joint position
for (int i = 0; i < kinDyn->getNrOfDegreesOfFreedom(); i++)
{
jointPositions[i] += distribution(generator);
}


out.dynamics = std::make_shared<FloatingBaseSystemKinematics>();
out.dynamics->setState({basePose.topRightCorner<3, 1>(),
toManifRot(basePose.topLeftCorner<3, 3>()),
Expand Down Expand Up @@ -226,7 +228,11 @@ 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 = iDynTree::getRandomChain(numberOfJoints,
nrOfAdditionalFrames,
onlyRevoluteJoints);
REQUIRE(kinDyn->loadRobotModel(model));

const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints);
Expand All @@ -251,7 +257,7 @@ TEST_CASE("QP-IK")
REQUIRE(ikAndTasks.regularizationTask->setSetPoint(desiredSetPoints.joints));

// propagate the inverse kinematics for
constexpr std::size_t iterations = 10;
constexpr std::size_t iterations = 20;
Eigen::Vector3d gravity;
gravity << 0, 0, -9.81;
Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity();
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 7a5383e

Please sign in to comment.