Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/solvers/solver-HQP-eiquadprog-fast.cpp	Lines:	0	136	0.0 %
Date:	2024-02-02 08:47:34	Branches:	0	368	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-fast.hpp"
#include "tsid/math/utils.hpp"
#include "eiquadprog/eiquadprog-fast.hpp"
#include "tsid/utils/stop-watch.hpp"

// #define PROFILE_EIQUADPROG_FAST

using namespace eiquadprog::solvers;

namespace tsid {
namespace solvers {

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

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

void SolverHQuadProgFast::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);
  }

  if (resizeVar || resizeIn || resizeEq) {
    m_solver.reset(n, neq, nin * 2);
    m_output.resize(n, neq, 2 * nin);
  }

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

void SolverHQuadProgFast::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);

    unsigned 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);

  EIGEN_MALLOC_NOT_ALLOWED;

  // Compute the cost
  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");

      EIGEN_MALLOC_ALLOWED;
      m_qpData.H.noalias() +=
          w * constr->matrix().transpose() * constr->matrix();
      EIGEN_MALLOC_NOT_ALLOWED;

      m_qpData.g.noalias() -=
          w * constr->matrix().transpose() * constr->vector();
    }

    if (hessianRegularization) {
      double m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION);
      m_qpData.H.diagonal().array() += m_hessian_regularization;
    }
  }
}

const HQPOutput& SolverHQuadProgFast::solve(const HQPData& problemData) {
  SolverHQuadProgFast::retrieveQPData(problemData);

  START_PROFILER_EIQUADPROG_FAST(PROFILE_EIQUADPROG_SOLUTION);
  //  min 0.5 * x G x + g0 x
  //  s.t.
  //  CE x + ce0 = 0
  //  CI x + ci0 >= 0
  EIGEN_MALLOC_ALLOWED
  eiquadprog::solvers::EiquadprogFast_status status =
      m_solver.solve_quadprog(m_qpData.H, m_qpData.g, m_qpData.CE, m_qpData.ce0,
                              m_qpData.CI, m_qpData.ci0, m_output.x);

  STOP_PROFILER_EIQUADPROG_FAST(PROFILE_EIQUADPROG_SOLUTION);

  if (status == EIQUADPROG_FAST_OPTIMAL) {
    m_output.status = HQP_STATUS_OPTIMAL;
    m_output.lambda = m_solver.getLagrangeMultipliers();
    m_output.iterations = m_solver.getIteratios();
    //    m_output.activeSet =
    //    m_solver.getActiveSet().tail(2*m_nin).head(m_solver.getActiveSetSize()-m_neq);
    m_output.activeSet = m_solver.getActiveSet().segment(
        m_neq, m_solver.getActiveSetSize() - m_neq);
#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) {
          // m_output.status = HQP_STATUS_ERROR;
          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
  } else if (status == EIQUADPROG_FAST_UNBOUNDED)
    m_output.status = HQP_STATUS_INFEASIBLE;
  else if (status == EIQUADPROG_FAST_MAX_ITER_REACHED)
    m_output.status = HQP_STATUS_MAX_ITER_REACHED;
  else if (status == EIQUADPROG_FAST_REDUNDANT_EQUALITIES)
    m_output.status = HQP_STATUS_ERROR;

  return m_output;
}

double SolverHQuadProgFast::getObjectiveValue() {
  return m_solver.getObjValue();
}

bool SolverHQuadProgFast::setMaximumIterations(unsigned int maxIter) {
  SolverHQPBase::setMaximumIterations(maxIter);
  return m_solver.setMaxIter(maxIter);
}
}  // namespace solvers
}  // namespace tsid


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-fast.hpp"
19			#include "tsid/math/utils.hpp"
20			#include "eiquadprog/eiquadprog-fast.hpp"
21			#include "tsid/utils/stop-watch.hpp"
22
23			// #define PROFILE_EIQUADPROG_FAST
24
25			using namespace eiquadprog::solvers;
26
27			namespace tsid {
28			namespace solvers {
29
30			using namespace math;
31			SolverHQuadProgFast::SolverHQuadProgFast(const std::string& name)
32			: SolverHQPBase(name),
33			m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION) {
34			m_n = 0;
35			m_neq = 0;
36			m_nin = 0;
37			}
38
39			void SolverHQuadProgFast::sendMsg(const std::string& s) {
40			std::cout << "[SolverHQuadProgFast." << m_name << "] " << s << std::endl;
41			}
42
43			void SolverHQuadProgFast::resize(unsigned int n, unsigned int neq,
44			unsigned int nin) {
45			const bool resizeVar = n != m_n;
46			const bool resizeEq = (resizeVar \|\| neq != m_neq);
47			const bool resizeIn = (resizeVar \|\| nin != m_nin);
48
49			if (resizeEq) {
50			#ifndef NDEBUG
51			sendMsg("Resizing equality constraints from " + toString(m_neq) + " to " +
52			toString(neq));
53			#endif
54			m_qpData.CE.resize(neq, n);
55			m_qpData.ce0.resize(neq);
56			}
57			if (resizeIn) {
58			#ifndef NDEBUG
59			sendMsg("Resizing inequality constraints from " + toString(m_nin) + " to " +
60			toString(nin));
61			#endif
62			m_qpData.CI.resize(2 * nin, n);
63			m_qpData.ci0.resize(2 * nin);
64			}
65			if (resizeVar) {
66			#ifndef NDEBUG
67			sendMsg("Resizing Hessian from " + toString(m_n) + " to " + toString(n));
68			#endif
69			m_qpData.H.resize(n, n);
70			m_qpData.g.resize(n);
71			m_output.x.resize(n);
72			}
73
74			if (resizeVar \|\| resizeIn \|\| resizeEq) {
75			m_solver.reset(n, neq, nin * 2);
76			m_output.resize(n, neq, 2 * nin);
77			}
78
79			m_n = n;
80			m_neq = neq;
81			m_nin = nin;
82			}
83
84			void SolverHQuadProgFast::retrieveQPData(const HQPData& problemData,
85			const bool hessianRegularization) {
86			if (problemData.size() > 2) {
87			PINOCCHIO_CHECK_INPUT_ARGUMENT(
88			false, "Solver not implemented for more than 2 hierarchical levels.");
89			}
90
91			// Compute the constraint matrix sizes
92			unsigned int neq = 0, nin = 0;
93			const ConstraintLevel& cl0 = problemData[0];
94			if (cl0.size() > 0) {
95			const unsigned int n = cl0[0].second->cols();
96			for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
97			it++) {
98			auto constr = it->second;
99			assert(n == constr->cols());
100			if (constr->isEquality())
101			neq += constr->rows();
102			else
103			nin += constr->rows();
104			}
105			// If necessary, resize the constraint matrices
106			resize(n, neq, nin);
107
108			unsigned int i_eq = 0, i_in = 0;
109			for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
110			it++) {
111			auto constr = it->second;
112			if (constr->isEquality()) {
113			m_qpData.CE.middleRows(i_eq, constr->rows()) = constr->matrix();
114			m_qpData.ce0.segment(i_eq, constr->rows()) = -constr->vector();
115			i_eq += constr->rows();
116
117			} else if (constr->isInequality()) {
118			m_qpData.CI.middleRows(i_in, constr->rows()) = constr->matrix();
119			m_qpData.ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
120			i_in += constr->rows();
121			m_qpData.CI.middleRows(i_in, constr->rows()) = -constr->matrix();
122			m_qpData.ci0.segment(i_in, constr->rows()) = constr->upperBound();
123			i_in += constr->rows();
124			} else if (constr->isBound()) {
125			m_qpData.CI.middleRows(i_in, constr->rows()).setIdentity();
126			m_qpData.ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
127			i_in += constr->rows();
128			m_qpData.CI.middleRows(i_in, constr->rows()) =
129			-Matrix::Identity(m_n, m_n);
130			m_qpData.ci0.segment(i_in, constr->rows()) = constr->upperBound();
131			i_in += constr->rows();
132			}
133			}
134			} else
135			resize(m_n, neq, nin);
136
137			EIGEN_MALLOC_NOT_ALLOWED;
138
139			// Compute the cost
140			if (problemData.size() > 1) {
141			const ConstraintLevel& cl1 = problemData[1];
142			m_qpData.H.setZero();
143			m_qpData.g.setZero();
144
145			for (ConstraintLevel::const_iterator it = cl1.begin(); it != cl1.end();
146			it++) {
147			const double& w = it->first;
148			auto constr = it->second;
149			if (!constr->isEquality())
150			PINOCCHIO_CHECK_INPUT_ARGUMENT(
151			false, "Inequalities in the cost function are not implemented yet");
152
153			EIGEN_MALLOC_ALLOWED;
154			m_qpData.H.noalias() +=
155			w * constr->matrix().transpose() * constr->matrix();
156			EIGEN_MALLOC_NOT_ALLOWED;
157
158			m_qpData.g.noalias() -=
159			w * constr->matrix().transpose() * constr->vector();
160			}
161
162			if (hessianRegularization) {
163			double m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION);
164			m_qpData.H.diagonal().array() += m_hessian_regularization;
165			}
166			}
167			}
168
169			const HQPOutput& SolverHQuadProgFast::solve(const HQPData& problemData) {
170			SolverHQuadProgFast::retrieveQPData(problemData);
171
172			START_PROFILER_EIQUADPROG_FAST(PROFILE_EIQUADPROG_SOLUTION);
173			// min 0.5 * x G x + g0 x
174			// s.t.
175			// CE x + ce0 = 0
176			// CI x + ci0 >= 0
177			EIGEN_MALLOC_ALLOWED
178			eiquadprog::solvers::EiquadprogFast_status status =
179			m_solver.solve_quadprog(m_qpData.H, m_qpData.g, m_qpData.CE, m_qpData.ce0,
180			m_qpData.CI, m_qpData.ci0, m_output.x);
181
182			STOP_PROFILER_EIQUADPROG_FAST(PROFILE_EIQUADPROG_SOLUTION);
183
184			if (status == EIQUADPROG_FAST_OPTIMAL) {
185			m_output.status = HQP_STATUS_OPTIMAL;
186			m_output.lambda = m_solver.getLagrangeMultipliers();
187			m_output.iterations = m_solver.getIteratios();
188			// m_output.activeSet =
189			// m_solver.getActiveSet().tail(2*m_nin).head(m_solver.getActiveSetSize()-m_neq);
190			m_output.activeSet = m_solver.getActiveSet().segment(
191			m_neq, m_solver.getActiveSetSize() - m_neq);
192			#ifndef NDEBUG
193			const Vector& x = m_output.x;
194
195			const ConstraintLevel& cl0 = problemData[0];
196			if (cl0.size() > 0) {
197			for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
198			it++) {
199			auto constr = it->second;
200			if (constr->checkConstraint(x) == false) {
201			// m_output.status = HQP_STATUS_ERROR;
202			if (constr->isEquality()) {
203			sendMsg("Equality " + constr->name() + " violated: " +
204			toString((constr->matrix() * x - constr->vector()).norm()));
205			} else if (constr->isInequality()) {
206			sendMsg(
207			"Inequality " + constr->name() + " violated: " +
208			toString(
209			(constr->matrix() * x - constr->lowerBound()).minCoeff()) +
210			"\n" +
211			toString(
212			(constr->upperBound() - constr->matrix() * x).minCoeff()));
213			} else if (constr->isBound()) {
214			sendMsg("Bound " + constr->name() + " violated: " +
215			toString((x - constr->lowerBound()).minCoeff()) + "\n" +
216			toString((constr->upperBound() - x).minCoeff()));
217			}
218			}
219			}
220			}
221			#endif
222			} else if (status == EIQUADPROG_FAST_UNBOUNDED)
223			m_output.status = HQP_STATUS_INFEASIBLE;
224			else if (status == EIQUADPROG_FAST_MAX_ITER_REACHED)
225			m_output.status = HQP_STATUS_MAX_ITER_REACHED;
226			else if (status == EIQUADPROG_FAST_REDUNDANT_EQUALITIES)
227			m_output.status = HQP_STATUS_ERROR;
228
229			return m_output;
230			}
231
232			double SolverHQuadProgFast::getObjectiveValue() {
233			return m_solver.getObjValue();
234			}
235
236			bool SolverHQuadProgFast::setMaximumIterations(unsigned int maxIter) {
237			SolverHQPBase::setMaximumIterations(maxIter);
238			return m_solver.setMaxIter(maxIter);
239			}
240			} // namespace solvers
241			} // namespace tsid