Add the support of QP problems with no constraint in QPInverseKinematics and QPTSID

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)

src/IK/src/QPInverseKinematics.cpp

@@ -76,17 +76,22 @@ struct QPInverseKinematics::Impl
  return false;
  }
 
- Eigen::SparseMatrix<double> 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<double> 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<double> constraintsMatrixSparse = this->constraintMatrix.sparseView();
  if (!this->solver.updateLinearConstraintsMatrix(constraintsMatrixSparse))
  {
@@ -349,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;
  }
 
@@ -413,7 +425,6 @@ bool QPInverseKinematics::advance()
  return false;
  }
 
-
  // update of all the tasks
  for (auto& [name, task] : m_pimpl->tasks)
  {
@@ -560,7 +571,8 @@ std::weak_ptr<QPInverseKinematics::Task> QPInverseKinematics::getTask(const std:
  return task->second.task;
 }
 
-bool QPInverseKinematics::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
+bool QPInverseKinematics::initialize(
+ std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
 {
  constexpr auto logPrefix = "[QPInverseKinematics::initialize]";
 
@@ -642,7 +654,7 @@ QPInverseKinematics::build(std::weak_ptr<const ParametersHandler::IParametersHan
 
  BipedalLocomotion::System::VariablesHandler variablesHandler;
  if (!variablesHandler.addVariable(solver->m_pimpl->robotVelocityVariable.name,
- kinDyn->getNrOfDegreesOfFreedom() + 6))
+  kinDyn->getNrOfDegreesOfFreedom() + 6))
  {
  log()->error("{} Unable to add the variable named '{}'.",
  logPrefix,

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<iDynTree::KinDynComputations>();
+ 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<QPInverseKinematics>();
+ 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<double, 6, 1> baseVelocity = Eigen::Matrix<double, 6, 1>::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));
+}

src/TSID/src/QPTSID.cpp

@@ -84,17 +84,22 @@ struct QPTSID::Impl
  return false;
  }
 
- Eigen::SparseMatrix<double> 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<double> 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<double> constraintsMatrixSparse = this->constraintMatrix.sparseView();
  if (!this->solver.updateLinearConstraintsMatrix(constraintsMatrixSparse))
  {
@@ -427,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;
  }