GCC Code Coverage Report

Directory: ./
File: include/hpp/core/path-optimization/quadratic-program.hh
Date: 2025-03-10 11:18:21
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