GCC Code Coverage Report

Directory: ./
File: src/solver-abstract.cpp
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 46 59 78.0%
Branches: 43 110 39.1%
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 #include "hpp/bezier-com-traj/solver/solver-abstract.hpp"
19 #ifdef USE_GLPK_SOLVER
20 #include <glpk.h>
21
22 #include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
23 #endif
24 #include <Eigen/Sparse>
25 #include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>
26 #include <stdexcept>
27
28 namespace solvers {
29
30 template <typename Derived>
31 30852 inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
32
3/6
✓ Branch 2 taken 15426 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15426 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15426 times.
✗ Branch 9 not taken.
 30852 bool isnan = !((x.array() == x.array()).all());
33 30852 return isnan;
34 }
35
36 typedef Eigen::SparseMatrix<double> SpMat;
37 typedef Eigen::SparseVector<double> SpVec;
38 typedef Eigen::SparseVector<int> SpVeci;
39
40 namespace {
41 2571 void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds,
42 std::pair<MatrixXd, VectorXd>& data) {
43
2/2
✓ Branch 1 taken 2553 times.
✓ Branch 2 taken 18 times.
 2571 if (minBounds.size() == 0) return;
44 18 MatrixXd& res = data.first;
45 18 VectorXd& resv = data.second;
46
1/2
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 MatrixXd D(res.rows() + res.cols(), res.cols());
47
1/2
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 18 VectorXd d(resv.rows() + res.cols());
48
2/4
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 D.block(0, 0, res.rows(), res.cols()) = res;
49
1/2
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 D.block(res.rows(), 0, res.cols(), res.cols()) =
50
3/6
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
 36 (-1.) * MatrixXd::Identity(res.cols(), res.cols());
51
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 18 d.head(resv.size()) = resv;
52
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 d.tail(res.cols()) = -minBounds;
53
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 data.first = D;
54
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 data.second = d;
55 18 }
56
57 2571 void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds,
58 std::pair<MatrixXd, VectorXd>& data) {
59
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 if (maxBounds.size() == 0) return;
60 ✗ MatrixXd& res = data.first;
61 ✗ VectorXd& resv = data.second;
62 ✗ MatrixXd D(res.rows() + res.cols() - 3, res.cols());
63 ✗ VectorXd d(resv.rows() + res.cols());
64 ✗ D.block(0, 0, res.rows(), res.cols()) = res;
65 ✗ D.block(res.rows(), 0, res.cols(), res.cols()) =
66 ✗ MatrixXd::Identity(res.cols(), res.cols());
67 ✗ d.head(resv.size()) = resv;
68 ✗ d.tail(res.cols()) = maxBounds;
69 ✗ data.first = D;
70 ✗ data.second = d;
71 }
72
73 2571 std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(
74 solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
75 const MatrixXd& CI, const VectorXd& ci0) {
76 2571 std::pair<MatrixXd, VectorXd> data;
77
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 data.first = CI;
78
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 data.second = ci0;
79
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 addConstraintMinBoundQuadProg(minBounds, data);
80
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 addConstraintMaxBoundQuadProg(maxBounds, data);
81 2571 return data;
82 }
83 } // namespace
84
85 2571 ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D,
86 const VectorXd& d, const MatrixXd& Hess, const VectorXd& g,
87 const VectorXd& initGuess, solvers::Cref_vectorX minBounds,
88 solvers::Cref_vectorX maxBounds, const SolverType solver) {
89
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(!(is_nan(A)));
90
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(!(is_nan(b)));
91
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(!(is_nan(D)));
92
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(!(is_nan(d)));
93
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
 2571 assert(!(is_nan(initGuess)));
94 2571 ResultData res;
95
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 res.x = initGuess;
96
1/2
✓ Branch 0 taken 2571 times.
✗ Branch 1 not taken.
 2571 switch (solver) {
97 /*
98 * solves the problem
99 * min. x' Hess x + 2 g0' x
100 * s.t. CE x + ce0 = 0
101 * CI x + ci0 >= 0
102 * Thus CI = -A; ci0 = b
103 * CI = D; ce0 = -d
104 */
105 2571 case SOLVER_QUADPROG:
106 // case SOLVER_QUADPROG_SPARSE:
107 {
108
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2571 times.
 2571 assert(!(is_nan(Hess)));
109 std::pair<MatrixXd, VectorXd> CIp =
110
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
111
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 VectorXd ce0 = -d;
112 tsid::solvers::EiquadprogFast QPsolver =
113
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 tsid::solvers::EiquadprogFast();
114 tsid::solvers::EiquadprogFast_status status;
115 // if(solver == SOLVER_QUADPROG)
116
2/4
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
 2571 status = QPsolver.solve_quadprog(Hess, g, D, ce0, -CIp.first,
117
1/2
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
 2571 CIp.second, res.x);
118 /* else
119 {
120 SpMat Hsp = Hess.sparseView();
121 status = QPsolver.solve_quadprog_sparse(Hsp,g,D,ce0,CI,b,res.x);
122 }*/
123 2571 res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL);
124
2/2
✓ Branch 0 taken 557 times.
✓ Branch 1 taken 2014 times.
 2571 if (res.success_) res.cost_ = QPsolver.getObjValue();
125 5142 return res;
126 2571 }
127 #ifdef USE_GLPK_SOLVER
128 case SOLVER_GLPK: {
129 res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds,
130 res.x, res.cost_) == GLP_OPT);
131 return res;
132 }
133 #endif
134 ✗ default:
135 ✗ throw std::runtime_error("Unknown solver type in solver-asbtract");
136 }
137 }
138
139 } /* namespace solvers */
140