Directory: | ./ |
---|---|
File: | include/hpp/core/path-optimization/quadratic-program.hh |
Date: | 2024-12-13 16:14:03 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 29 | 30 | 96.7% |
Branches: | 25 | 58 | 43.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2018, Joseph Mirabel | ||
2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
3 | // | ||
4 | |||
5 | // Redistribution and use in source and binary forms, with or without | ||
6 | // modification, are permitted provided that the following conditions are | ||
7 | // met: | ||
8 | // | ||
9 | // 1. Redistributions of source code must retain the above copyright | ||
10 | // notice, this list of conditions and the following disclaimer. | ||
11 | // | ||
12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
13 | // notice, this list of conditions and the following disclaimer in the | ||
14 | // documentation and/or other materials provided with the distribution. | ||
15 | // | ||
16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
27 | // DAMAGE. | ||
28 | |||
29 | #ifndef HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH | ||
30 | #define HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH | ||
31 | |||
32 | #include <hpp/core/fwd.hh> | ||
33 | #include <hpp/core/path-optimization/linear-constraint.hh> | ||
34 | |||
35 | namespace hpp { | ||
36 | namespace core { | ||
37 | /// \addtogroup path_optimization | ||
38 | /// \{ | ||
39 | namespace pathOptimization { | ||
40 | /** Quadratic program | ||
41 | * | ||
42 | * This class stores a quadratic cost defined by | ||
43 | * \f$ \frac{1}{2} x^T H x + b^T x \f$ where \f$ (H, b) \f$ are the parameters. | ||
44 | * | ||
45 | * It can then solve the two following program: | ||
46 | * \li Program subject to linear equality constraints | ||
47 | * \f{eqnarray*}{ | ||
48 | * \min & \frac{1}{2} x^T H x + b^T x \\ | ||
49 | * A_0 x = b_0 | ||
50 | * \f} | ||
51 | * This is done via \ref reduced, \ref decompose and \ref solve methods | ||
52 | * \li Program subject to linear equality and inequality constraints: | ||
53 | * \f{eqnarray*}{ | ||
54 | * \min & \frac{1}{2} x^T H x + b^T x \\ | ||
55 | * A_0 x = b_0 \\ | ||
56 | * A_1 x \ge b_1 | ||
57 | * \f} | ||
58 | * This is done via \ref computeLLT and \ref solve methods | ||
59 | * and uses quadprog | ||
60 | **/ | ||
61 | struct QuadraticProgram { | ||
62 | typedef Eigen::JacobiSVD<matrix_t> Decomposition_t; | ||
63 | typedef Eigen::LLT<matrix_t, Eigen::Lower> LLT_t; | ||
64 | |||
65 | /// Constructor | ||
66 | /// \param inputSize dimension of the space on which the quadratic cost is | ||
67 | /// defined, | ||
68 | /// \param useProxqp whether to use proxqp instead of eiquadprog_2011 as | ||
69 | /// the internal solver. This parameter will soon be removed | ||
70 | /// and proxqp be the internal solver. | ||
71 | 15 | QuadraticProgram(size_type inputSize, bool useProxqp = true) | |
72 | 15 | : H(inputSize, inputSize), | |
73 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | b(inputSize), |
74 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | dec(inputSize, inputSize, Eigen::ComputeThinU | Eigen::ComputeThinV), |
75 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | xStar(inputSize), |
76 | 15 | accuracy_(1e-4), | |
77 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | useProxqp_(useProxqp) { |
78 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | H.setZero(); |
79 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | b.setZero(); |
80 | 15 | bIsZero = true; | |
81 | 15 | } | |
82 | |||
83 | /// Constructor | ||
84 | /// \param inputSize dimension of the space on which the quadratic cost is | ||
85 | /// defined, | ||
86 | /// \param lc linear equality constraint, | ||
87 | /// \param useProxqp whether to use proxqp instead of eiquadprog_2011 as | ||
88 | /// the internal solver. This parameter will soon be removed | ||
89 | /// and proxqp be the internal solver. | ||
90 | 15 | QuadraticProgram(const QuadraticProgram& QP, const LinearConstraint& lc, | |
91 | bool useProxqp = true) | ||
92 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
15 | : H(lc.PK.cols(), lc.PK.cols()), |
93 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | b(lc.PK.cols()), |
94 | 15 | bIsZero(false), | |
95 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
15 | dec(lc.PK.cols(), lc.PK.cols(), |
96 | Eigen::ComputeThinU | Eigen::ComputeThinV), | ||
97 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | xStar(lc.PK.cols()), |
98 | 15 | accuracy_(1e-4), | |
99 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | useProxqp_(useProxqp) { |
100 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | QP.reduced(lc, *this); |
101 | 15 | } | |
102 | |||
103 | QuadraticProgram(const QuadraticProgram& QP) | ||
104 | : H(QP.H), | ||
105 | b(QP.b), | ||
106 | bIsZero(QP.bIsZero), | ||
107 | dec(QP.dec), | ||
108 | xStar(QP.xStar), | ||
109 | accuracy_(QP.accuracy_), | ||
110 | useProxqp_(QP.useProxqp_) {} | ||
111 | |||
112 | ~QuadraticProgram(); | ||
113 | |||
114 | /// Set accuracy of internal QP solver | ||
115 | /// \param acc the desired accuracy | ||
116 | /// \note only used by proxqp | ||
117 | /// The accuracy corresponds to \f$\epsilon_{abs}\f$ in \link | ||
118 | /// https://inria.hal.science/hal-03683733/file/Yet_another_QP_solver_for_robotics_and_beyond.pdf | ||
119 | /// this paper \endlink (Equation (2)). | ||
120 | 30 | void accuracy(value_type acc) { accuracy_ = acc; } | |
121 | /// Get accuracy of internal QP solver | ||
122 | /// \note only used by proxqp | ||
123 | /// The accuracy corresponds to \f$\epsilon_{abs}\f$ in \link | ||
124 | /// https://inria.hal.science/hal-03683733/file/Yet_another_QP_solver_for_robotics_and_beyond.pdf | ||
125 | /// this paper \endlink (Equation (2)). | ||
126 | value_type accuracy() const { return accuracy_; } | ||
127 | void addRows(const std::size_t& nbRows) { | ||
128 | H.conservativeResize(H.rows() + nbRows, H.cols()); | ||
129 | b.conservativeResize(b.rows() + nbRows, b.cols()); | ||
130 | |||
131 | H.bottomRows(nbRows).setZero(); | ||
132 | } | ||
133 | |||
134 | /// \name Program subject to linear equality constraints. | ||
135 | /// \{ | ||
136 | |||
137 | /*/ Compute the problem | ||
138 | * \f{eqnarray*}{ | ||
139 | * \min & \frac{1}{2} * x^T H x + b^T x \\ | ||
140 | * lc.J * x = lc.b | ||
141 | * \f} | ||
142 | **/ | ||
143 | 15 | void reduced(const LinearConstraint& lc, QuadraticProgram& QPr) const { | |
144 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | matrix_t H_PK(H * lc.PK); |
145 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
15 | QPr.H.noalias() = lc.PK.transpose() * H_PK; |
146 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
15 | QPr.b.noalias() = H_PK.transpose() * lc.xStar; |
147 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
|
15 | if (!bIsZero) { |
148 | ✗ | QPr.b.noalias() += lc.PK.transpose() * b; | |
149 | } | ||
150 | 15 | QPr.bIsZero = false; | |
151 | 15 | } | |
152 | |||
153 | void decompose(); | ||
154 | |||
155 | void solve() { xStar.noalias() = -dec.solve(b); } | ||
156 | |||
157 | /// \} | ||
158 | |||
159 | /// \name Program subject to linear equality and inequality constraints. | ||
160 | /// \{ | ||
161 | |||
162 | void computeLLT(); | ||
163 | |||
164 | /// Compute solution using quadprog | ||
165 | /// \param ce equality constraints | ||
166 | /// \param ci inequality constraints: \f$ ci.J * x \ge ci.b \f$ | ||
167 | /// \return the cost of the solution. | ||
168 | /// | ||
169 | /// \note \ref computeLLT must have been called before. | ||
170 | /// \note if the problem is ill-conditioned, member xStar is left unchanged. | ||
171 | double solve(const LinearConstraint& ce, const LinearConstraint& ci); | ||
172 | |||
173 | /// \} | ||
174 | |||
175 | /// \name Model | ||
176 | /// \{ | ||
177 | matrix_t H; | ||
178 | vector_t b; | ||
179 | bool bIsZero; | ||
180 | /// \} | ||
181 | |||
182 | /// \name Data (for inequality constraints) | ||
183 | /// \{ | ||
184 | LLT_t llt; | ||
185 | value_type trace; | ||
186 | Eigen::VectorXi activeConstraint; | ||
187 | int activeSetSize; | ||
188 | /// \} | ||
189 | |||
190 | /// \name Data (for equality constraints) | ||
191 | /// \{ | ||
192 | Decomposition_t dec; | ||
193 | vector_t xStar; | ||
194 | /// \} | ||
195 | value_type accuracy_; | ||
196 | bool useProxqp_; | ||
197 | }; | ||
198 | } // namespace pathOptimization | ||
199 | } // namespace core | ||
200 | } // namespace hpp | ||
201 | |||
202 | #endif // HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH | ||
203 |