GCC Code Coverage Report

Directory: ./
File: include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx
Date: 2024-11-10 01:12:44
Exec Total Coverage
Lines: 73 110 66.4%
Branches: 104 352 29.5%
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
8/16
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2 times.
✗ Branch 24 not taken.
 2 m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION) {
39 2 m_n = nVars;
40 2 m_neq = nEqCon;
41 2 m_nin = nIneqCon;
42
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 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
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 611 times.
 611 assert(n == nVars);
55
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 611 times.
 611 assert(neq == nEqCon);
56
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 611 times.
 611 assert(nin == nIneqCon);
57
3/6
✓ Branch 0 taken 611 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 611 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 611 times.
 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 610 times.
 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
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 if (cl0.size() > 0) {
83
1/2
✓ Branch 3 taken 610 times.
✗ Branch 4 not taken.
 610 const unsigned int n = cl0[0].second->cols();
84
2/2
✓ Branch 4 taken 1260 times.
✓ Branch 5 taken 610 times.
 3130 for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
85 1260 it++) {
86 1260 auto constr = it->second;
87
2/4
✓ Branch 2 taken 1260 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1260 times.
 1260 assert(n == constr->cols());
88
3/4
✓ Branch 2 taken 1260 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 630 times.
✓ Branch 5 taken 630 times.
 1260 if (constr->isEquality())
89
1/2
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
 630 neq += constr->rows();
90 else
91
1/2
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
 630 nin += constr->rows();
92 }
93 // If necessary, resize the constraint matrices
94
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 resize(n, neq, nin);
95
96 610 int i_eq = 0, i_in = 0;
97
2/2
✓ Branch 4 taken 1260 times.
✓ Branch 5 taken 610 times.
 3130 for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
98 1260 it++) {
99 1260 auto constr = it->second;
100
3/4
✓ Branch 2 taken 1260 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 630 times.
✓ Branch 5 taken 630 times.
 1260 if (constr->isEquality()) {
101
4/8
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 630 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 630 times.
✗ Branch 13 not taken.
 630 m_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
102
5/10
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 630 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 630 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 630 times.
✗ Branch 16 not taken.
 630 m_ce0.segment(i_eq, constr->rows()) = -constr->vector();
103
1/2
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
 630 i_eq += constr->rows();
104
2/4
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 630 times.
✗ Branch 5 not taken.
 630 } else if (constr->isInequality()) {
105
4/8
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 630 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 630 times.
✗ Branch 13 not taken.
 630 m_CI.middleRows(i_in, constr->rows()) = constr->matrix();
106
5/10
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 630 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 630 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 630 times.
✗ Branch 16 not taken.
 630 m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
107
1/2
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
 630 i_in += constr->rows();
108
5/10
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 630 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 630 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 630 times.
✗ Branch 16 not taken.
 630 m_CI.middleRows(i_in, constr->rows()) = -constr->matrix();
109
4/8
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 630 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 630 times.
✗ Branch 13 not taken.
 630 m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
110
1/2
✓ Branch 2 taken 630 times.
✗ Branch 3 not taken.
 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
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 if (problemData.size() > 1) {
124 610 const ConstraintLevel& cl1 = problemData[1];
125
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 m_H.setZero();
126
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 m_g.setZero();
127
2/2
✓ Branch 3 taken 640 times.
✓ Branch 4 taken 610 times.
 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
2/4
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 640 times.
 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
7/14
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 640 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 640 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 640 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 640 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 640 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 640 times.
✗ Branch 22 not taken.
 640 m_H.noalias() += w * constr->matrix().transpose() * constr->matrix();
137 640 EIGEN_MALLOC_NOT_ALLOWED
138
7/14
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 640 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 640 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 640 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 640 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 640 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 640 times.
✗ Branch 22 not taken.
 640 m_g.noalias() -= w * (constr->matrix().transpose() * constr->vector());
139 }
140
5/10
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 610 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 610 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 610 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 610 times.
✗ Branch 14 not taken.
 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
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 typename RtVectorX<nVars>::d sol(m_n);
161 610 EIGEN_MALLOC_ALLOWED
162 eisol::RtEiquadprog_status status =
163
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 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
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 m_output.x = sol;
167
168 // #ifndef EIGEN_RUNTIME_NO_MALLOC
169 // Eigen::internal::set_is_malloc_allowed(true);
170 // #endif
171
172
1/2
✓ Branch 0 taken 610 times.
✗ Branch 1 not taken.
 610 if (status == eisol::RT_EIQUADPROG_OPTIMAL) {
173 610 m_output.status = HQP_STATUS_OPTIMAL;
174
1/2
✓ Branch 2 taken 610 times.
✗ Branch 3 not taken.
 610 m_output.lambda = m_solver.getLagrangeMultipliers();
175 // m_output.activeSet = m_solver.getActiveSet().template tail< 2*nIneqCon
176 // >().head(m_solver.getActiveSetSize());
177
1/2
✓ Branch 2 taken 610 times.
✗ Branch 3 not taken.
 1220 m_output.activeSet = m_solver.getActiveSet().segment(
178
1/2
✓ Branch 2 taken 610 times.
✗ Branch 3 not taken.
 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
1/2
✓ Branch 1 taken 610 times.
✗ Branch 2 not taken.
 610 if (cl0.size() > 0) {
185
2/2
✓ Branch 4 taken 1260 times.
✓ Branch 5 taken 610 times.
 3130 for (ConstraintLevel::const_iterator it = cl0.begin(); it != cl0.end();
186 1260 it++) {
187 1260 auto constr = it->second;
188
3/6
✓ Branch 2 taken 1260 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1260 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1260 times.
 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__
235