GCC Code Coverage Report


Directory: ./
File: include/hpp/core/path-optimization/linear-constraint.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 28 28 100.0%
Branches: 22 40 55.0%

Line Branch Exec Source
1 // Copyright (c) 2017, 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_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH
30 #define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH
31
32 #include <hpp/core/fwd.hh>
33 #include <hpp/util/debug.hh>
34
35 namespace hpp {
36 namespace core {
37 namespace pathOptimization {
38 /// A linear constraint \f$ J \times x = b \f$
39 struct LinearConstraint {
40 75 LinearConstraint(size_type inputSize, size_type outputSize)
41
4/8
✓ Branch 2 taken 75 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 75 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 75 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 75 times.
✗ Branch 12 not taken.
75 : J(outputSize, inputSize), b(outputSize), xSol(inputSize) {
42
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
75 J.setZero();
43
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
75 b.setZero();
44 75 }
45
46 ~LinearConstraint();
47
48 void concatenate(const LinearConstraint& oc) {
49 assert(oc.J.cols() == J.cols());
50 J.conservativeResize(J.rows() + oc.J.rows(), J.cols());
51 J.bottomRows(oc.J.rows()) = oc.J;
52 b.conservativeResize(b.rows() + oc.b.rows());
53 b.tail(oc.b.rows()) = oc.b;
54 }
55
56 /// Compute one solution and a base of the kernel of matrix J.
57 /// rank is also updated.
58 /// \param check If true, checks whether the constraint is feasible.
59 /// \return whether the constraint is feasible
60 /// (alwys true when check is false)
61 bool decompose(bool check = false, bool throwIfNotValid = false);
62
63 /// Compute rank of the constraint using a LU decomposition
64 24 void computeRank() {
65
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 9 times.
24 if (J.size() == 0)
66 15 rank = 0;
67 else {
68
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
9 Eigen::FullPivLU<matrix_t> lu(J);
69
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
9 rank = lu.rank();
70 9 }
71 24 }
72
73 /// Reduced constraint into the set of solutions of this constraint.
74 /// \param[in] lc the full constraint
75 /// \param[out] lcr the reduced constraint
76 /// \return if computeRank, returns true if the reduced constraint is full
77 /// rank.
78 /// if not computeRank, returns true.
79 /// \note rank is computed using computeRank method.
80 39 bool reduceConstraint(const LinearConstraint& lc, LinearConstraint& lcr,
81 bool computeRank = true) const {
82
2/4
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 39 times.
✗ Branch 6 not taken.
39 lcr.J.noalias() = lc.J * PK;
83
3/6
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 39 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 39 times.
✗ Branch 9 not taken.
39 lcr.b.noalias() = lc.b - lc.J * xStar;
84
85 // Decompose
86
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 15 times.
39 if (computeRank) {
87 24 lcr.computeRank();
88 24 return lcr.rank == std::min(lcr.J.rows(), lcr.J.cols());
89 } else
90 15 return true;
91 }
92
93 /// Compute the unique solution derived from v into \ref xSol.
94 /// \f$ xSol \gets x^* + PK \times v \f$
95 /// \param v an element of the kernel of matrix \ref J.
96 /// \retval this->xSol
97 18 void computeSolution(const vector_t& v) {
98
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
18 xSol.noalias() = xStar + PK * v;
99 #ifdef HPP_DEBUG
100 isSatisfied(xSol);
101 #endif // HPP_DEBUG
102 18 }
103
104 /// Returns \f$ ( J \times x - b ).isZero (threshold) \f$
105 bool isSatisfied(const vector_t& x,
106 const value_type& threshold =
107 Eigen::NumTraits<value_type>::dummy_precision()) {
108 vector_t err(J * x - b);
109 if (err.isZero(threshold)) return true;
110 hppDout(error, "constraints could not be satisfied: " << err.norm() << '\n'
111 << err);
112 return false;
113 }
114
115 18 void addRows(const std::size_t& nbRows) {
116
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
18 if (nbRows > 0) {
117 18 J.conservativeResize(J.rows() + nbRows, J.cols());
118 18 b.conservativeResize(b.rows() + nbRows);
119
120
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
18 J.bottomRows(nbRows).setZero();
121 }
122 18 }
123
124 /// \name Model
125 /// \{
126 matrix_t J;
127 vector_t b;
128 /// \}
129
130 /// \name Data
131 /// Solutions are \f$ \left\{ x^* + PK \times v, v \in
132 /// \mathbb{R}^{nCols(J) - rank} \right\} \f$
133 /// \{
134
135 /// Rank of \ref J
136 size_type rank;
137
138 /// Projector onto \f$ kernel(J) \f$
139 matrix_t PK;
140 /// \f$ x^* \f$ is a particular solution.
141 vector_t xStar, xSol;
142
143 /// \}
144 };
145 } // namespace pathOptimization
146 } // namespace core
147 } // namespace hpp
148
149 #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH
150