GCC Code Coverage Report

Directory: ./
File: src/path-optimization/linear-constraint.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 26 32 81.2%
Branches: 37 98 37.8%
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/linear-constraint.hh>
30 #include <hpp/pinocchio/util.hh>
31 #include <hpp/util/exception-factory.hh>
32 #include <hpp/util/timer.hh>
33
34 // #define USE_SVD
35 #ifdef USE_SVD
36 #include <hpp/constraints/svd.hh>
37 #endif
38
39 namespace hpp {
40 namespace core {
41 namespace pathOptimization {
42 HPP_DEFINE_TIMECOUNTER(LinearConstraint_decompose);
43
44 75 LinearConstraint::~LinearConstraint() {
45 HPP_DISPLAY_TIMECOUNTER(LinearConstraint_decompose);
46 75 }
47
48 15 bool LinearConstraint::decompose(bool check, bool throwIfNotValid) {
49 HPP_SCOPE_TIMECOUNTER(LinearConstraint_decompose);
50
51
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
 15 if (J.rows() == 0) { // No constraint
52 ✗ PK = matrix_t::Identity(J.cols(), J.cols());
53 ✗ xStar = vector_t::Zero(PK.rows());
54 ✗ return true;
55 }
56
57 #ifdef USE_SVD
58 typedef Eigen::JacobiSVD<matrix_t> Decomposition_t;
59 Decomposition_t dec(J, Eigen::ComputeThinU | Eigen::ComputeFullV);
60 rank = dec.rank();
61
62 PK.resize(J.cols(), J.cols() - rank);
63 xStar.resize(PK.rows());
64
65 xStar = dec.solve(b);
66
67 PK.noalias() = constraints::getV2(dec, rank);
68 #else // USE_SVD
69
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 Eigen::ColPivHouseholderQR<matrix_t> qr(J.transpose());
70
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 rank = qr.rank();
71
72
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 PK.resize(J.cols(), J.cols() - rank);
73
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 xStar.resize(PK.rows());
74
75
4/8
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 15 times.
✗ Branch 12 not taken.
 15 vector_t rhs((qr.colsPermutation().inverse() * b).head(rank));
76
77
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 vector_t z(J.cols());
78
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 30 z.head(rank).noalias() = qr.matrixR()
79
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 .topLeftCorner(rank, rank)
80
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 .triangularView<Eigen::Upper>()
81
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 .transpose()
82
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 .solve(rhs);
83
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 z.tail(J.cols() - rank).setZero();
84
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 xStar.noalias() = qr.householderQ() * z;
85
86
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 PK.noalias() =
87
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 qr.householderQ() *
88
3/6
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 matrix_t::Identity(J.cols(), J.cols()).rightCols(J.cols() - rank);
89 #endif // USE_SVD
90
91
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 if (check) {
92 // check that the constraints are feasible
93
3/6
✓ 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.
 15 matrix_t error = J * xStar - b;
94
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
 15 if (!error.isZero(1e-7)) {
95 ✗ if (throwIfNotValid) {
96 ✗ HPP_THROW(std::invalid_argument,
97 "Constraints are not feasible.\nError is "
98 << setpyformat << one_line(error) << unsetpyformat);
99 }
100 hppDout(warning, "Constraint not feasible: " << error.norm() << '\n'
101 << error.transpose());
102 ✗ return false;
103 }
104
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 }
105 15 return true;
106 15 }
107 } // namespace pathOptimization
108 } // namespace core
109 } // namespace hpp
110
111 #ifdef USE_SVD
112 #undef USE_SVD
113 #endif // USE_SVD
114