GCC Code Coverage Report


Directory: ./
File: src/path-optimization/quadratic-program.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 27 54 50.0%
Branches: 43 116 37.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 #include <hpp/core/path-optimization/quadratic-program.hh>
30 #include <hpp/util/timer.hh>
31 #include <iostream>
32 #include <proxsuite/proxqp/sparse/wrapper.hpp>
33
34 #pragma GCC diagnostic push
35 #pragma GCC diagnostic ignored "-Wconversion"
36 #pragma GCC diagnostic ignored "-Wunused-variable"
37 #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
38 #include <path-optimization/spline-gradient-based/eiquadprog_2011.hpp>
39 #pragma GCC diagnostic pop
40
41 namespace hpp {
42 namespace core {
43 /// \addtogroup path_optimization
44 /// \{
45 namespace pathOptimization {
46 HPP_DEFINE_TIMECOUNTER(QuadraticProgram_decompose);
47 HPP_DEFINE_TIMECOUNTER(QuadraticProgram_computeLLT);
48 HPP_DEFINE_TIMECOUNTER(QuadraticProgram_solve_quadprog);
49
50 30 QuadraticProgram::~QuadraticProgram() {
51 HPP_DISPLAY_TIMECOUNTER(QuadraticProgram_decompose);
52 HPP_DISPLAY_TIMECOUNTER(QuadraticProgram_computeLLT);
53 HPP_DISPLAY_TIMECOUNTER(QuadraticProgram_solve_quadprog);
54 30 }
55
56 void QuadraticProgram::decompose() {
57 HPP_SCOPE_TIMECOUNTER(QuadraticProgram_decompose);
58 dec.compute(H);
59 assert(dec.rank() == H.rows());
60 }
61
62 15 void QuadraticProgram::computeLLT() {
63 HPP_SCOPE_TIMECOUNTER(QuadraticProgram_computeLLT);
64 15 trace = H.trace();
65 15 llt.compute(H);
66 15 }
67
68 18 double QuadraticProgram::solve(const LinearConstraint& ce,
69 const LinearConstraint& ci) {
70
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
18 if (ce.J.rows() > ce.J.cols())
71 throw std::runtime_error(
72 "The QP is over-constrained. QuadProg cannot handle it.");
73 HPP_SCOPE_TIMECOUNTER(QuadraticProgram_solve_quadprog);
74 // min 0.5 * x H x + g x
75 // s.t. CE x + ce0 = 0
76 // CI x + ci0 >= 0
77
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
18 if (!useProxqp_) {
78 Eigen::QuadProgStatus status;
79 double cost = solve_quadprog2(llt, trace, b, ce.J.transpose(), -ce.b,
80 ci.J.transpose(), -ci.b, xStar,
81 activeConstraint, activeSetSize, status);
82 switch (status) {
83 case Eigen::UNBOUNDED:
84 hppDout(warning, "Quadratic problem is not bounded");
85 case Eigen::CONVERGED:
86 break;
87 case Eigen::CONSTRAINT_LINEARLY_DEPENDENT:
88 hppDout(error,
89 "Constraint of quadratic problem are linearly dependent.");
90 break;
91 }
92 return cost;
93 } else {
94 using proxsuite::proxqp::QPSolverOutput;
95 proxsuite::proxqp::sparse::QP<value_type, long long> qp{
96
1/2
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
18 H.rows(), ce.b.size(), ci.b.size()};
97 18 qp.settings.eps_abs = accuracy_;
98
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
18 vector_t u = ci.b;
99
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
18 u.fill(std::numeric_limits<value_type>::infinity());
100
101 // Conversion to sparse types
102 Eigen::SparseMatrix<value_type, Eigen::ColMajor, long long> H1(
103
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
18 H.sparseView());
104 Eigen::SparseMatrix<value_type, Eigen::ColMajor, long long> A1(
105
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
18 ce.J.sparseView());
106 Eigen::SparseMatrix<value_type, Eigen::ColMajor, long long> C1(
107
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
18 ci.J.sparseView());
108
8/16
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 18 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 18 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 18 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 18 times.
✗ Branch 27 not taken.
18 qp.init(H1, b, A1, ce.b, C1, ci.b, u);
109
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
18 qp.solve();
110 18 value_type res(0);
111 Eigen::IOFormat fullPrecision(Eigen::FullPrecision, Eigen::DontAlignCols,
112
7/14
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 18 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
36 ", ", ", ", "", "", " << ", ";");
113
1/7
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
18 switch (qp.results.info.status) {
114 18 case QPSolverOutput::PROXQP_SOLVED:
115
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
18 xStar = qp.results.x;
116
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18 times.
✗ Branch 14 not taken.
18 res += (.5 * xStar.transpose() * H * xStar)(0, 0);
117
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
18 res += (b.transpose() * xStar)(0);
118 18 return res;
119 break;
120 case QPSolverOutput::PROXQP_MAX_ITER_REACHED:
121 hppDout(warning, "PROXQP_MAX_ITER_REACHED");
122 break;
123 case QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE:
124 hppDout(warning, "PROXQP_PRIMAL_INFEASIBLE");
125 break;
126 case QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE:
127 hppDout(warning, "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE");
128 break;
129 case QPSolverOutput::PROXQP_DUAL_INFEASIBLE:
130 hppDout(warning, "PROXQP_DUAL_INFEASIBLE");
131 break;
132 case QPSolverOutput::PROXQP_NOT_RUN:
133 hppDout(warning, "PROXQP_NOT_RUN");
134 break;
135 default:
136 abort();
137 }
138
6/12
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 16 not taken.
✓ Branch 17 taken 18 times.
108 }
139 return std::numeric_limits<value_type>::infinity();
140 }
141 } // namespace pathOptimization
142 } // namespace core
143 } // namespace hpp
144