Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx	Lines:	74	111	66.7 %
Date:	2024-02-02 08:47:34	Branches:	104	352	29.5 %


//
// 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/>.
//

#ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__
#define __invdyn_solvers_hqp_eiquadprog_rt_hxx__

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

namespace eisol = eiquadprog::solvers;

#define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation"
#define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution"

namespace tsid {
namespace solvers {

template <int nVars, int nEqCon, int nIneqCon>
SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::SolverHQuadProgRT(
    const std::string& name)
    : SolverHQPBase(name),
      m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION) {
  m_n = nVars;
  m_neq = nEqCon;
  m_nin = nIneqCon;
  m_output.resize(nVars, nEqCon, 2 * nIneqCon);
}

template <int nVars, int nEqCon, int nIneqCon>
void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::sendMsg(const std::string& s) {
  std::cout << "[SolverHQuadProgRT." << m_name << "] " << s << std::endl;
}

template <int nVars, int nEqCon, int nIneqCon>
void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::resize(unsigned int n,
                                                        unsigned int neq,
                                                        unsigned int nin) {
  assert(n == nVars);
  assert(neq == nEqCon);
  assert(nin == nIneqCon);
  if ((n != nVars) || (neq != nEqCon) || (nin != nIneqCon))
    std::cerr
        << "[SolverHQuadProgRT] (n!=nVars) || (neq!=nEqCon) || (nin!=nIneqCon)"
        << std::endl;
}

template <int nVars, int nEqCon, int nIneqCon>
const HQPOutput& SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::solve(
    const HQPData& problemData) {
  using namespace tsid::math;

  // #ifndef EIGEN_RUNTIME_NO_MALLOC
  //   Eigen::internal::set_is_malloc_allowed(false);
  // #endif

  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);

  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_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
        m_ce0.segment(i_eq, constr->rows()) = -constr->vector();
        i_eq += constr->rows();
      } else if (constr->isInequality()) {
        m_CI.middleRows(i_in, constr->rows()) = constr->matrix();
        m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
        i_in += constr->rows();
        m_CI.middleRows(i_in, constr->rows()) = -constr->matrix();
        m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
        i_in += constr->rows();
      } else if (constr->isBound()) {
        m_CI.middleRows(i_in, constr->rows()).setIdentity();
        m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
        i_in += constr->rows();
        m_CI.middleRows(i_in, constr->rows()) = -Matrix::Identity(m_n, m_n);
        m_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_H.setZero();
    m_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_H.noalias() += w * constr->matrix().transpose() * constr->matrix();
      EIGEN_MALLOC_NOT_ALLOWED
      m_g.noalias() -= w * (constr->matrix().transpose() * constr->vector());
    }
    m_H.diagonal().noalias() += m_hessian_regularization * Vector::Ones(m_n);
  }

  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);

  //  // eliminate equality constraints
  //  if(m_neq>0)
  //  {
  //    m_CE_lu.compute(m_CE);
  //    sendMsg("The rank of CD is "+toString(m_CE_lu.rank());
  //    const MatrixXd & Z = m_CE_lu.kernel();

  //  }

  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_SOLUTION);

  //  min 0.5 * x G x + g0 x
  //  s.t.
  //  CE x + ce0 = 0
  //  CI x + ci0 >= 0
  typename RtVectorX<nVars>::d sol(m_n);
  EIGEN_MALLOC_ALLOWED
  eisol::RtEiquadprog_status status =
      m_solver.solve_quadprog(m_H, m_g, m_CE, m_ce0, m_CI, m_ci0, sol);
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_SOLUTION);

  m_output.x = sol;

  // #ifndef EIGEN_RUNTIME_NO_MALLOC
  //   Eigen::internal::set_is_malloc_allowed(true);
  // #endif

  if (status == eisol::RT_EIQUADPROG_OPTIMAL) {
    m_output.status = HQP_STATUS_OPTIMAL;
    m_output.lambda = m_solver.getLagrangeMultipliers();
    //    m_output.activeSet = m_solver.getActiveSet().template tail< 2*nIneqCon
    //    >().head(m_solver.getActiveSetSize());
    m_output.activeSet = m_solver.getActiveSet().segment(
        m_neq, m_solver.getActiveSetSize() - m_neq);
    m_output.iterations = m_solver.getIteratios();

#ifndef NDEBUG
    const Vector& x = m_output.x;

    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
  } else if (status == eisol::RT_EIQUADPROG_UNBOUNDED)
    m_output.status = HQP_STATUS_INFEASIBLE;
  else if (status == eisol::RT_EIQUADPROG_MAX_ITER_REACHED)
    m_output.status = HQP_STATUS_MAX_ITER_REACHED;
  else if (status == eisol::RT_EIQUADPROG_REDUNDANT_EQUALITIES)
    m_output.status = HQP_STATUS_ERROR;

  return m_output;
}

template <int nVars, int nEqCon, int nIneqCon>
double SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::getObjectiveValue() {
  return m_solver.getObjValue();
}

template <int nVars, int nEqCon, int nIneqCon>
bool SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::setMaximumIterations(
    unsigned int maxIter) {
  SolverHQPBase::setMaximumIterations(maxIter);
  return m_solver.setMaxIter(maxIter);
}

}  // namespace solvers
}  // namespace tsid

#endif  // ifndef __invdyn_solvers_hqp_eiquadprog_rt_hxx__


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