Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/solvers/solver-HQP-eiquadprog.cpp	Lines:	0	114	0.0 %
Date:	2024-02-02 08:47:34	Branches:	0	354	0.0 %


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include "tsid/solvers/solver-HQP-eiquadprog.hpp"
#include "tsid/math/utils.hpp"
#include "eiquadprog/eiquadprog.hpp"
#include "tsid/utils/stop-watch.hpp"

using namespace tsid::math;
using namespace tsid::solvers;
using namespace Eigen;

SolverHQuadProg::SolverHQuadProg(const std::string& name)
    : SolverHQPBase(name),
      m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION) {
  m_n = 0;
  m_neq = 0;
  m_nin = 0;
}

void SolverHQuadProg::sendMsg(const std::string& s) {
  std::cout << "[SolverHQuadProg." << m_name << "] " << s << std::endl;
}

void SolverHQuadProg::resize(unsigned int n, unsigned int neq,
                             unsigned int nin) {
  const bool resizeVar = n != m_n;
  const bool resizeEq = (resizeVar || neq != m_neq);
  const bool resizeIn = (resizeVar || nin != m_nin);

  if (resizeEq) {
#ifndef NDEBUG
    sendMsg("Resizing equality constraints from " + toString(m_neq) + " to " +
            toString(neq));
#endif
    m_qpData.CE.resize(neq, n);
    m_qpData.ce0.resize(neq);
  }
  if (resizeIn) {
#ifndef NDEBUG
    sendMsg("Resizing inequality constraints from " + toString(m_nin) + " to " +
            toString(nin));
#endif
    m_qpData.CI.resize(2 * nin, n);
    m_qpData.ci0.resize(2 * nin);
  }
  if (resizeVar) {
#ifndef NDEBUG
    sendMsg("Resizing Hessian from " + toString(m_n) + " to " + toString(n));
#endif
    m_qpData.H.resize(n, n);
    m_qpData.g.resize(n);
    m_output.x.resize(n);
  }

  m_n = n;
  m_neq = neq;
  m_nin = nin;
}

void SolverHQuadProg::retrieveQPData(const HQPData& problemData,
                                     const bool /*hessianRegularization*/) {
  if (problemData.size() > 2) {
    PINOCCHIO_CHECK_INPUT_ARGUMENT(
        false, "Solver not implemented for more than 2 hierarchical levels.");
  }

  // Compute the constraint matrix sizes
  unsigned int neq = 0, nin = 0;
  const ConstraintLevel& cl0 = problemData[0];
  if (cl0.size() > 0) {
    const unsigned int n = cl0[0].second->cols();
    for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
         it++) {
      auto constr = it->second;
      assert(n == constr->cols());
      if (constr->isEquality())
        neq += constr->rows();
      else
        nin += constr->rows();
    }
    // If necessary, resize the constraint matrices
    resize(n, neq, nin);

    int i_eq = 0, i_in = 0;
    for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
         it++) {
      auto constr = it->second;
      if (constr->isEquality()) {
        m_qpData.CE.middleRows(i_eq, constr->rows()) = constr->matrix();
        m_qpData.ce0.segment(i_eq, constr->rows()) = -constr->vector();
        i_eq += constr->rows();
      } else if (constr->isInequality()) {
        m_qpData.CI.middleRows(i_in, constr->rows()) = constr->matrix();
        m_qpData.ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
        i_in += constr->rows();
        m_qpData.CI.middleRows(i_in, constr->rows()) = -constr->matrix();
        m_qpData.ci0.segment(i_in, constr->rows()) = constr->upperBound();
        i_in += constr->rows();
      } else if (constr->isBound()) {
        m_qpData.CI.middleRows(i_in, constr->rows()).setIdentity();
        m_qpData.ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
        i_in += constr->rows();
        m_qpData.CI.middleRows(i_in, constr->rows()) =
            -Matrix::Identity(m_n, m_n);
        m_qpData.ci0.segment(i_in, constr->rows()) = constr->upperBound();
        i_in += constr->rows();
      }
    }
  } else
    resize(m_n, neq, nin);

  if (problemData.size() > 1) {
    const ConstraintLevel& cl1 = problemData[1];
    m_qpData.H.setZero();
    m_qpData.g.setZero();
    for (ConstraintLevel::const_iterator it = cl1.begin(); it != cl1.end();
         it++) {
      const double& w = it->first;
      auto constr = it->second;
      if (!constr->isEquality())
        PINOCCHIO_CHECK_INPUT_ARGUMENT(
            false, "Inequalities in the cost function are not implemented yet");

      m_qpData.H += w * constr->matrix().transpose() * constr->matrix();
      m_qpData.g -= w * (constr->matrix().transpose() * constr->vector());
    }
    m_qpData.H.diagonal() += m_hessian_regularization * Vector::Ones(m_n);
  }

#ifdef ELIMINATE_EQUALITY_CONSTRAINTS

  // eliminate equality constraints
  const int r = m_neq;
  Matrix Z(m_n, m_n - m_neq);
  Matrix ZT(m_n, m_n);
  m_ZT_H_Z.resize(m_n - r, m_n - r);

  START_PROFILER("Eiquadprog eliminate equalities");
  if (m_neq > 0) {
    START_PROFILER("Eiquadprog CE decomposition");
    //    m_qpData.CE_dec.compute(m_qpData.CE, ComputeThinU | ComputeThinV);
    m_qpData.CE_dec.compute(m_qpData.CE);
    STOP_PROFILER("Eiquadprog CE decomposition");

    START_PROFILER("Eiquadprog get CE null-space basis");
    // get nullspace basis from SVD
    //    const int r = m_qpData.CE_dec.nonzeroSingularValues();
    //    const Matrix Z = m_qpData.CE_dec.matrixV().rightCols(m_n-r);

    // get null space basis from ColPivHouseholderQR
    //	Matrix Z = m_qpData.CE_dec.householderQ();
    //	Z = Z.rightCols(m_n-r);

    // get null space basis from COD
    // P^{-1} * y => colsPermutation() * y;
    //	Z = m_qpData.CE_dec.matrixZ(); // * m_qpData.CE_dec.colsPermutation();
    ZT.setIdentity();
    // m_qpData.CE_dec.applyZAdjointOnTheLeftInPlace(ZT);
    typedef tsid::math::Index Index;
    const Index rank = m_qpData.CE_dec.rank();
    Vector temp(m_n);
    for (Index k = 0; k < rank; ++k) {
      if (k != rank - 1) ZT.row(k).swap(ZT.row(rank - 1));
      ZT.middleRows(rank - 1, m_n - rank + 1)
          .applyHouseholderOnTheLeft(
              m_qpData.CE_dec.matrixQTZ().row(k).tail(m_n - rank).adjoint(),
              m_qpData.CE_dec.zCoeffs()(k), &temp(0));
      if (k != rank - 1) ZT.row(k).swap(ZT.row(rank - 1));
    }
    STOP_PROFILER("Eiquadprog get CE null-space basis");

    // find a solution for the equalities
    Vector x0 = m_qpData.CE_dec.solve(m_qpData.ce0);
    x0 = -x0;

    //    START_PROFILER("Eiquadprog project Hessian full");
    //    m_ZT_H_Z.noalias() = Z.transpose()*m_qpData.H*Z; // this is too slow
    //	STOP_PROFILER("Eiquadprog project Hessian full");

    START_PROFILER("Eiquadprog project Hessian incremental");
    const ConstraintLevel& cl1 = problemData[1];
    m_ZT_H_Z.setZero();
    // m_qpData.g.setZero();
    Matrix AZ;
    for (ConstraintLevel::const_iterator it = cl1.begin(); it != cl1.end();
         it++) {
      const double& w = it->first;
      auto constr = it->second;
      if (!constr->isEquality())
        PINOCCHIO_CHECK_INPUT_ARGUMENT(
            false, "Inequalities in the cost function are not implemented yet");

      AZ.noalias() = constr->matrix() * Z.rightCols(m_n - r);
      m_ZT_H_Z += w * AZ.transpose() * AZ;
      // m_qpData.g -= w*(constr->matrix().transpose()*constr->vector());
    }
    // m_ZT_H_Z.diagonal() += 1e-8*Vector::Ones(m_n);
    m_qpData.CI_Z.noalias() = m_qpData.CI * Z.rightCols(m_n - r);
    STOP_PROFILER("Eiquadprog project Hessian incremental");
  }
  STOP_PROFILER("Eiquadprog eliminate equalities");
#endif
}

const HQPOutput& SolverHQuadProg::solve(const HQPData& problemData) {
  // #ifndef NDEBUG
  //   PRINT_MATRIX(m_qpData.H);
  //   PRINT_VECTOR(m_qpData.g);
  //   PRINT_MATRIX(m_qpData.CE);
  //   PRINT_VECTOR(m_qpData.ce0);
  //   PRINT_MATRIX(m_qpData.CI);
  //   PRINT_VECTOR(m_qpData.ci0);
  // #endif
  SolverHQuadProg::retrieveQPData(problemData);

  //  min 0.5 * x G x + g0 x
  //  s.t.
  //  CE^T x + ce0 = 0
  //  CI^T x + ci0 >= 0
  m_objValue = eiquadprog::solvers::solve_quadprog(
      m_qpData.H, m_qpData.g, m_qpData.CE.transpose(), m_qpData.ce0,
      m_qpData.CI.transpose(), m_qpData.ci0, m_output.x, m_activeSet,
      m_activeSetSize);

  if (m_objValue == std::numeric_limits<double>::infinity())
    m_output.status = HQP_STATUS_INFEASIBLE;
  else {
    m_output.status = HQP_STATUS_OPTIMAL;
#ifndef NDEBUG
    const Vector& x = m_output.x;
    const ConstraintLevel& cl0 = problemData[0];
    if (cl0.size() > 0) {
      for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
           it++) {
        auto constr = it->second;
        if (constr->checkConstraint(x) == false) {
          if (constr->isEquality()) {
            sendMsg("Equality " + constr->name() + " violated: " +
                    toString((constr->matrix() * x - constr->vector()).norm()));
          } else if (constr->isInequality()) {
            sendMsg(
                "Inequality " + constr->name() + " violated: " +
                toString(
                    (constr->matrix() * x - constr->lowerBound()).minCoeff()) +
                "\n" +
                toString(
                    (constr->upperBound() - constr->matrix() * x).minCoeff()));
          } else if (constr->isBound()) {
            sendMsg("Bound " + constr->name() + " violated: " +
                    toString((x - constr->lowerBound()).minCoeff()) + "\n" +
                    toString((constr->upperBound() - x).minCoeff()));
          }
        }
      }
    }
#endif
  }

  return m_output;
}

double SolverHQuadProg::getObjectiveValue() { return m_objValue; }


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017 CNRS
3			//
4			// This file is part of tsid
5			// tsid is free software: you can redistribute it
6			// and/or modify it under the terms of the GNU Lesser General Public
7			// License as published by the Free Software Foundation, either version
8			// 3 of the License, or (at your option) any later version.
9			// tsid is distributed in the hope that it will be
10			// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11			// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12			// General Lesser Public License for more details. You should have
13			// received a copy of the GNU Lesser General Public License along with
14			// tsid If not, see
15			// <http://www.gnu.org/licenses/>.
16			//
17
18			#include "tsid/solvers/solver-HQP-eiquadprog.hpp"
19			#include "tsid/math/utils.hpp"
20			#include "eiquadprog/eiquadprog.hpp"
21			#include "tsid/utils/stop-watch.hpp"
22
23			using namespace tsid::math;
24			using namespace tsid::solvers;
25			using namespace Eigen;
26
27			SolverHQuadProg::SolverHQuadProg(const std::string& name)
28			: SolverHQPBase(name),
29			m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION) {
30			m_n = 0;
31			m_neq = 0;
32			m_nin = 0;
33			}
34
35			void SolverHQuadProg::sendMsg(const std::string& s) {
36			std::cout << "[SolverHQuadProg." << m_name << "] " << s << std::endl;
37			}
38
39			void SolverHQuadProg::resize(unsigned int n, unsigned int neq,
40			unsigned int nin) {
41			const bool resizeVar = n != m_n;
42			const bool resizeEq = (resizeVar \|\| neq != m_neq);
43			const bool resizeIn = (resizeVar \|\| nin != m_nin);
44
45			if (resizeEq) {
46			#ifndef NDEBUG
47			sendMsg("Resizing equality constraints from " + toString(m_neq) + " to " +
48			toString(neq));
49			#endif
50			m_qpData.CE.resize(neq, n);
51			m_qpData.ce0.resize(neq);
52			}
53			if (resizeIn) {
54			#ifndef NDEBUG
55			sendMsg("Resizing inequality constraints from " + toString(m_nin) + " to " +
56			toString(nin));
57			#endif
58			m_qpData.CI.resize(2 * nin, n);
59			m_qpData.ci0.resize(2 * nin);
60			}
61			if (resizeVar) {
62			#ifndef NDEBUG
63			sendMsg("Resizing Hessian from " + toString(m_n) + " to " + toString(n));
64			#endif
65			m_qpData.H.resize(n, n);
66			m_qpData.g.resize(n);
67			m_output.x.resize(n);
68			}
69
70			m_n = n;
71			m_neq = neq;
72			m_nin = nin;
73			}
74
75			void SolverHQuadProg::retrieveQPData(const HQPData& problemData,
76			const bool /hessianRegularization/) {
77			if (problemData.size() > 2) {
78			PINOCCHIO_CHECK_INPUT_ARGUMENT(
79			false, "Solver not implemented for more than 2 hierarchical levels.");
80			}
81
82			// Compute the constraint matrix sizes
83			unsigned int neq = 0, nin = 0;
84			const ConstraintLevel& cl0 = problemData[0];
85			if (cl0.size() > 0) {
86			const unsigned int n = cl0[0].second->cols();
87			for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
88			it++) {
89			auto constr = it->second;
90			assert(n == constr->cols());
91			if (constr->isEquality())
92			neq += constr->rows();
93			else
94			nin += constr->rows();
95			}
96			// If necessary, resize the constraint matrices
97			resize(n, neq, nin);
98
99			int i_eq = 0, i_in = 0;
100			for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
101			it++) {
102			auto constr = it->second;
103			if (constr->isEquality()) {
104			m_qpData.CE.middleRows(i_eq, constr->rows()) = constr->matrix();
105			m_qpData.ce0.segment(i_eq, constr->rows()) = -constr->vector();
106			i_eq += constr->rows();
107			} else if (constr->isInequality()) {
108			m_qpData.CI.middleRows(i_in, constr->rows()) = constr->matrix();
109			m_qpData.ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
110			i_in += constr->rows();
111			m_qpData.CI.middleRows(i_in, constr->rows()) = -constr->matrix();
112			m_qpData.ci0.segment(i_in, constr->rows()) = constr->upperBound();
113			i_in += constr->rows();
114			} else if (constr->isBound()) {
115			m_qpData.CI.middleRows(i_in, constr->rows()).setIdentity();
116			m_qpData.ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
117			i_in += constr->rows();
118			m_qpData.CI.middleRows(i_in, constr->rows()) =
119			-Matrix::Identity(m_n, m_n);
120			m_qpData.ci0.segment(i_in, constr->rows()) = constr->upperBound();
121			i_in += constr->rows();
122			}
123			}
124			} else
125			resize(m_n, neq, nin);
126
127			if (problemData.size() > 1) {
128			const ConstraintLevel& cl1 = problemData[1];
129			m_qpData.H.setZero();
130			m_qpData.g.setZero();
131			for (ConstraintLevel::const_iterator it = cl1.begin(); it != cl1.end();
132			it++) {
133			const double& w = it->first;
134			auto constr = it->second;
135			if (!constr->isEquality())
136			PINOCCHIO_CHECK_INPUT_ARGUMENT(
137			false, "Inequalities in the cost function are not implemented yet");
138
139			m_qpData.H += w * constr->matrix().transpose() * constr->matrix();
140			m_qpData.g -= w * (constr->matrix().transpose() * constr->vector());
141			}
142			m_qpData.H.diagonal() += m_hessian_regularization * Vector::Ones(m_n);
143			}
144
145			#ifdef ELIMINATE_EQUALITY_CONSTRAINTS
146
147			// eliminate equality constraints
148			const int r = m_neq;
149			Matrix Z(m_n, m_n - m_neq);
150			Matrix ZT(m_n, m_n);
151			m_ZT_H_Z.resize(m_n - r, m_n - r);
152
153			START_PROFILER("Eiquadprog eliminate equalities");
154			if (m_neq > 0) {
155			START_PROFILER("Eiquadprog CE decomposition");
156			// m_qpData.CE_dec.compute(m_qpData.CE, ComputeThinU \| ComputeThinV);
157			m_qpData.CE_dec.compute(m_qpData.CE);
158			STOP_PROFILER("Eiquadprog CE decomposition");
159
160			START_PROFILER("Eiquadprog get CE null-space basis");
161			// get nullspace basis from SVD
162			// const int r = m_qpData.CE_dec.nonzeroSingularValues();
163			// const Matrix Z = m_qpData.CE_dec.matrixV().rightCols(m_n-r);
164
165			// get null space basis from ColPivHouseholderQR
166			// Matrix Z = m_qpData.CE_dec.householderQ();
167			// Z = Z.rightCols(m_n-r);
168
169			// get null space basis from COD
170			// P^{-1} * y => colsPermutation() * y;
171			// Z = m_qpData.CE_dec.matrixZ(); // * m_qpData.CE_dec.colsPermutation();
172			ZT.setIdentity();
173			// m_qpData.CE_dec.applyZAdjointOnTheLeftInPlace(ZT);
174			typedef tsid::math::Index Index;
175			const Index rank = m_qpData.CE_dec.rank();
176			Vector temp(m_n);
177			for (Index k = 0; k < rank; ++k) {
178			if (k != rank - 1) ZT.row(k).swap(ZT.row(rank - 1));
179			ZT.middleRows(rank - 1, m_n - rank + 1)
180			.applyHouseholderOnTheLeft(
181			m_qpData.CE_dec.matrixQTZ().row(k).tail(m_n - rank).adjoint(),
182			m_qpData.CE_dec.zCoeffs()(k), &temp(0));
183			if (k != rank - 1) ZT.row(k).swap(ZT.row(rank - 1));
184			}
185			STOP_PROFILER("Eiquadprog get CE null-space basis");
186
187			// find a solution for the equalities
188			Vector x0 = m_qpData.CE_dec.solve(m_qpData.ce0);
189			x0 = -x0;
190
191			// START_PROFILER("Eiquadprog project Hessian full");
192			// m_ZT_H_Z.noalias() = Z.transpose()m_qpData.HZ; // this is too slow
193			// STOP_PROFILER("Eiquadprog project Hessian full");
194
195			START_PROFILER("Eiquadprog project Hessian incremental");
196			const ConstraintLevel& cl1 = problemData[1];
197			m_ZT_H_Z.setZero();
198			// m_qpData.g.setZero();
199			Matrix AZ;
200			for (ConstraintLevel::const_iterator it = cl1.begin(); it != cl1.end();
201			it++) {
202			const double& w = it->first;
203			auto constr = it->second;
204			if (!constr->isEquality())
205			PINOCCHIO_CHECK_INPUT_ARGUMENT(
206			false, "Inequalities in the cost function are not implemented yet");
207
208			AZ.noalias() = constr->matrix() * Z.rightCols(m_n - r);
209			m_ZT_H_Z += w * AZ.transpose() * AZ;
210			// m_qpData.g -= w(constr->matrix().transpose()constr->vector());
211			}
212			// m_ZT_H_Z.diagonal() += 1e-8*Vector::Ones(m_n);
213			m_qpData.CI_Z.noalias() = m_qpData.CI * Z.rightCols(m_n - r);
214			STOP_PROFILER("Eiquadprog project Hessian incremental");
215			}
216			STOP_PROFILER("Eiquadprog eliminate equalities");
217			#endif
218			}
219
220			const HQPOutput& SolverHQuadProg::solve(const HQPData& problemData) {
221			// #ifndef NDEBUG
222			// PRINT_MATRIX(m_qpData.H);
223			// PRINT_VECTOR(m_qpData.g);
224			// PRINT_MATRIX(m_qpData.CE);
225			// PRINT_VECTOR(m_qpData.ce0);
226			// PRINT_MATRIX(m_qpData.CI);
227			// PRINT_VECTOR(m_qpData.ci0);
228			// #endif
229			SolverHQuadProg::retrieveQPData(problemData);
230
231			// min 0.5 * x G x + g0 x
232			// s.t.
233			// CE^T x + ce0 = 0
234			// CI^T x + ci0 >= 0
235			m_objValue = eiquadprog::solvers::solve_quadprog(
236			m_qpData.H, m_qpData.g, m_qpData.CE.transpose(), m_qpData.ce0,
237			m_qpData.CI.transpose(), m_qpData.ci0, m_output.x, m_activeSet,
238			m_activeSetSize);
239
240			if (m_objValue == std::numeric_limits<double>::infinity())
241			m_output.status = HQP_STATUS_INFEASIBLE;
242			else {
243			m_output.status = HQP_STATUS_OPTIMAL;
244			#ifndef NDEBUG
245			const Vector& x = m_output.x;
246			const ConstraintLevel& cl0 = problemData[0];
247			if (cl0.size() > 0) {
248			for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
249			it++) {
250			auto constr = it->second;
251			if (constr->checkConstraint(x) == false) {
252			if (constr->isEquality()) {
253			sendMsg("Equality " + constr->name() + " violated: " +
254			toString((constr->matrix() * x - constr->vector()).norm()));
255			} else if (constr->isInequality()) {
256			sendMsg(
257			"Inequality " + constr->name() + " violated: " +
258			toString(
259			(constr->matrix() * x - constr->lowerBound()).minCoeff()) +
260			"\n" +
261			toString(
262			(constr->upperBound() - constr->matrix() * x).minCoeff()));
263			} else if (constr->isBound()) {
264			sendMsg("Bound " + constr->name() + " violated: " +
265			toString((x - constr->lowerBound()).minCoeff()) + "\n" +
266			toString((constr->upperBound() - x).minCoeff()));
267			}
268			}
269			}
270			}
271			#endif
272			}
273
274			return m_output;
275			}
276
277			double SolverHQuadProg::getObjectiveValue() { return m_objValue; }