Directory: | ./ |
---|---|
File: | include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx |
Date: | 2024-08-26 20:29:39 |
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 |