From afecca8ae2cdf07dcf71f50dc1a3ff8231820dc4 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Dec 2023 15:20:18 +0100 Subject: [PATCH 1/5] Support QPIK and TSID QP problems with no constraint --- src/IK/src/QPInverseKinematics.cpp | 31 ++++++++++++++++++++-------- src/TSID/src/QPTSID.cpp | 33 +++++++++++++++++++++--------- 2 files changed, 45 insertions(+), 19 deletions(-) diff --git a/src/IK/src/QPInverseKinematics.cpp b/src/IK/src/QPInverseKinematics.cpp index 8be8bd480f..c953275a56 100644 --- a/src/IK/src/QPInverseKinematics.cpp +++ b/src/IK/src/QPInverseKinematics.cpp @@ -76,17 +76,22 @@ struct QPInverseKinematics::Impl return false; } - Eigen::SparseMatrix constraintsMatrixSparse = this->constraintMatrix.sparseView(); - if (!this->solver.data()->setLinearConstraintsMatrix(constraintsMatrixSparse)) + // if there are constraints + if (this->numberOfConstraints > 0) { - log()->error("{} Unable to set the constraint matrix.", logPrefix); - return false; - } + Eigen::SparseMatrix constraintsMatrixSparse + = this->constraintMatrix.sparseView(); + if (!this->solver.data()->setLinearConstraintsMatrix(constraintsMatrixSparse)) + { + log()->error("{} Unable to set the constraint matrix.", logPrefix); + return false; + } - if (!this->solver.data()->setBounds(this->lowerBound, this->upperBound)) - { - log()->error("{} Unable to set the bounds.", logPrefix); - return false; + if (!this->solver.data()->setBounds(this->lowerBound, this->upperBound)) + { + log()->error("{} Unable to set the bounds.", logPrefix); + return false; + } } if (!this->solver.initSolver()) @@ -116,6 +121,14 @@ struct QPInverseKinematics::Impl return false; } + // if the number of constraints is equal to zero we do not need to update the constraint + if (this->numberOfConstraints == 0) + { + return true; + } + + // in this case the number of constraint is different from zero, so we have to update the + // constraint matrix and the bounds Eigen::SparseMatrix constraintsMatrixSparse = this->constraintMatrix.sparseView(); if (!this->solver.updateLinearConstraintsMatrix(constraintsMatrixSparse)) { diff --git a/src/TSID/src/QPTSID.cpp b/src/TSID/src/QPTSID.cpp index 9050049a98..04554fd9f7 100644 --- a/src/TSID/src/QPTSID.cpp +++ b/src/TSID/src/QPTSID.cpp @@ -84,17 +84,22 @@ struct QPTSID::Impl return false; } - Eigen::SparseMatrix constraintsMatrixSparse = this->constraintMatrix.sparseView(); - if (!this->solver.data()->setLinearConstraintsMatrix(constraintsMatrixSparse)) + // if the number of constraints is equal to zero we do not need to set the constraint + if (this->numberOfConstraints > 0) { - log()->error("{} Unable to set the constraint matrix.", logPrefix); - return false; - } - - if (!this->solver.data()->setBounds(this->lowerBound, this->upperBound)) - { - log()->error("{} Unable to set the bounds.", logPrefix); - return false; + Eigen::SparseMatrix constraintsMatrixSparse + = this->constraintMatrix.sparseView(); + if (!this->solver.data()->setLinearConstraintsMatrix(constraintsMatrixSparse)) + { + log()->error("{} Unable to set the constraint matrix.", logPrefix); + return false; + } + + if (!this->solver.data()->setBounds(this->lowerBound, this->upperBound)) + { + log()->error("{} Unable to set the bounds.", logPrefix); + return false; + } } if (!this->solver.initSolver()) @@ -124,6 +129,14 @@ struct QPTSID::Impl return false; } + // if the number of constraints is equal to zero we do not need to update the constraint + if (this->numberOfConstraints == 0) + { + return true; + } + + // In this case the number of constraints is not equal to zero. We need to update the + // constraint matrix and the bounds. Eigen::SparseMatrix constraintsMatrixSparse = this->constraintMatrix.sparseView(); if (!this->solver.updateLinearConstraintsMatrix(constraintsMatrixSparse)) { From cfe4f05b9387c3e8ecaebdaebc395c8c37ed9140 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Dec 2023 15:20:49 +0100 Subject: [PATCH 2/5] Format QPInverseKinematics.cpp --- src/IK/src/QPInverseKinematics.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/IK/src/QPInverseKinematics.cpp b/src/IK/src/QPInverseKinematics.cpp index c953275a56..c8ca1ebd98 100644 --- a/src/IK/src/QPInverseKinematics.cpp +++ b/src/IK/src/QPInverseKinematics.cpp @@ -362,10 +362,9 @@ bool QPInverseKinematics::finalize(const System::VariablesHandler& handler) // resize the temporary matrix usefull to reduce dynamics allocation when advance() is called for (auto& cost : m_pimpl->costs) { - if(cost.get().weightProvider == nullptr) + if (cost.get().weightProvider == nullptr) { - log()->error("{} One of the weight provider has been not correctly set.", - logPrefix); + log()->error("{} One of the weight provider has been not correctly set.", logPrefix); return false; } @@ -426,7 +425,6 @@ bool QPInverseKinematics::advance() return false; } - // update of all the tasks for (auto& [name, task] : m_pimpl->tasks) { @@ -573,7 +571,8 @@ std::weak_ptr QPInverseKinematics::getTask(const std: return task->second.task; } -bool QPInverseKinematics::initialize(std::weak_ptr handler) +bool QPInverseKinematics::initialize( + std::weak_ptr handler) { constexpr auto logPrefix = "[QPInverseKinematics::initialize]"; @@ -655,7 +654,7 @@ QPInverseKinematics::build(std::weak_ptrm_pimpl->robotVelocityVariable.name, - kinDyn->getNrOfDegreesOfFreedom() + 6)) + kinDyn->getNrOfDegreesOfFreedom() + 6)) { log()->error("{} Unable to add the variable named '{}'.", logPrefix, From dd998908324cbb1d40537cd540990032a28ae5fa Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Dec 2023 15:21:02 +0100 Subject: [PATCH 3/5] Format QPTSID.cpp --- src/TSID/src/QPTSID.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/TSID/src/QPTSID.cpp b/src/TSID/src/QPTSID.cpp index 04554fd9f7..5f53c752e7 100644 --- a/src/TSID/src/QPTSID.cpp +++ b/src/TSID/src/QPTSID.cpp @@ -440,10 +440,9 @@ bool QPTSID::finalize(const System::VariablesHandler& handler) // resize the temporary matrix useful to reduce dynamics allocation when advance() is called for (auto& cost : m_pimpl->costs) { - if(cost.get().weightProvider == nullptr) + if (cost.get().weightProvider == nullptr) { - log()->error("{} One of the weight provider has been not correctly set.", - logPrefix); + log()->error("{} One of the weight provider has been not correctly set.", logPrefix); return false; } From 5b7564962a1f63c2799d66d6d8a0b6737303d437 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Dec 2023 15:52:20 +0100 Subject: [PATCH 4/5] Test the support of unconstrained problems in QPInverseKinematics --- src/IK/tests/QPInverseKinematicsTest.cpp | 134 +++++++++++++++++++++++ 1 file changed, 134 insertions(+) diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index f90e55915d..4871b85b7c 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -854,3 +854,137 @@ TEST_CASE("QP-IK [Distance and Gravity tasks]") - desiredSetPoints.desiredBodyGravity; REQUIRE(gravityError.isZero(tolerance)); } + +TEST_CASE("QP-IK [Distance and Gravity tasks Unconstrained]") +{ + auto kinDyn = std::make_shared(); + auto parameterHandler = createParameterHandler(); + + constexpr double tolerance = 2e-2; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation( + iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + constexpr std::size_t numberOfJoints = 30; + + // create the model + const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + REQUIRE(kinDyn->loadRobotModel(model)); + + const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints); + + // Instantiate the handler + VariablesHandler variablesHandler; + variablesHandler.addVariable(robotVelocity, model.getNrOfDOFs() + 6); + + auto system = getSystem(kinDyn); + + // Set the frame name + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + + // create the IK + constexpr double jointLimitDelta = 0.5; + finalizeParameterHandler(parameterHandler, + kinDyn, + system, + Eigen::VectorXd::Constant(kinDyn->model().getNrOfDOFs(), + jointLimitDelta)); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); + + Eigen::VectorXd weightRegularization, weightDistance, weightGravity; + + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + + REQUIRE(parameterHandler->getGroup("DISTANCE_TASK") + .lock() + ->getParameter("weight", // + weightDistance)); + REQUIRE(ik->addTask(ikTasks.distanceTask, "distance_task", lowPriority, weightDistance)); + + REQUIRE(parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->getParameter("weight", // + weightGravity)); + REQUIRE(ik->addTask(ikTasks.gravityTask, "gravity_task", lowPriority, weightGravity)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + + REQUIRE(ikTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + + REQUIRE(ikTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + + // propagate the inverse kinematics for + constexpr std::size_t iterations = 30; + Eigen::Vector3d gravity; + gravity << 0, 0, -9.81; + Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); + Eigen::Matrix baseVelocity = Eigen::Matrix::Zero(); + Eigen::VectorXd jointVelocity = Eigen::VectorXd::Zero(model.getNrOfDOFs()); + + for (std::size_t iteration = 0; iteration < iterations; iteration++) + { + // get the solution of the integrator + const auto& [basePosition, baseRotation, jointPosition] = system.integrator->getSolution(); + + // update the KinDynComputations object + baseTransform.topLeftCorner<3, 3>() = baseRotation.rotation(); + baseTransform.topRightCorner<3, 1>() = basePosition; + REQUIRE(kinDyn->setRobotState(baseTransform, + jointPosition, + baseVelocity, + jointVelocity, + gravity)); + + // solve the IK + REQUIRE(ik->advance()); + + // get the output of the IK + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; + + // propagate the dynamical system + system.dynamics->setControlInput({baseVelocity, jointVelocity}); + system.integrator->integrate(0s, dT); + } + + // Check the distance error + REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) + .translation() + .norm() + == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); +} From 2947637fa11f5619a78b12e28b1b2ccd46764dc5 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Dec 2023 15:54:31 +0100 Subject: [PATCH 5/5] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4fd81ab127..b798543c9a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ All notable changes to this project are documented in this file. - Implement `VectorsCollectionServer` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776) - Implement `toString()` methods in `PlannedContact`, `ContactList`, `ContactPhase` and `ContactPhaseList` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/777) - Implement `LinearSpline` in `Math` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/782) +- Add the support of QP problems with no constraint in `QPInverseKinematics` and `QPTSID` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/784) ### Changed - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766)