GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/solver/impl/hierarchical-iterative.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 72 73 98.6%
Branches: 41 74 55.4%
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_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
30 #define HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
31
32 #include <hpp/constraints/config.hh>
33 #include <hpp/constraints/svd.hh>
34 #include <hpp/util/debug.hh>
35
36 namespace hpp {
37 namespace constraints {
38 namespace solver {
39 namespace lineSearch {
40 template <typename SolverType>
41 34 inline bool Constant::operator()(const SolverType& solver, vectorOut_t arg,
42 vectorOut_t darg) {
43
3/6
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 34 times.
✗ Branch 9 not taken.
 34 solver.integrate(arg, darg, arg);
44 34 return true;
45 }
46
47 template <typename SolverType>
48 224 inline bool Backtracking::operator()(const SolverType& solver, vectorOut_t arg,
49 vectorOut_t u) {
50 224 arg_darg.resize(arg.size());
51
52 224 const value_type slope = computeLocalSlope(solver);
53 224 const value_type t = 2 * c * slope;
54 224 const value_type f_arg_norm2 = solver.residualError();
55
56
1/2
✓ Branch 0 taken 224 times.
✗ Branch 1 not taken.
 224 if (t > 0) {
57 hppDout(error, "The descent direction is not valid: " << t / c);
58 } else {
59 224 value_type alpha = 1;
60 /* TODO add a threshold to avoid too large steps.
61 const value_type u2 = u.squaredNorm();
62 if (u2 > 1.) alpha = 1. / std::sqrt(u2);
63 */
64
65
2/2
✓ Branch 0 taken 248 times.
✓ Branch 1 taken 6 times.
 254 while (alpha > smallAlpha) {
66
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
 248 darg = alpha * u;
67
4/8
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 248 times.
✗ Branch 11 not taken.
 248 solver.integrate(arg, darg, arg_darg);
68
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
 248 solver.template computeValue<false>(arg_darg);
69
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 solver.computeError();
70 // Check if we are doing better than the linear approximation with coef
71 // multiplied by c < 1
72 // t < 0 must hold
73 248 const value_type f_arg_darg_norm2 = solver.residualError();
74
2/2
✓ Branch 0 taken 218 times.
✓ Branch 1 taken 30 times.
 248 if (f_arg_norm2 - f_arg_darg_norm2 >= -alpha * t) {
75
1/2
✓ Branch 1 taken 218 times.
✗ Branch 2 not taken.
 218 arg = arg_darg;
76
1/2
✓ Branch 1 taken 218 times.
✗ Branch 2 not taken.
 218 u = darg;
77 218 return true;
78 }
79 // Prepare next step
80 30 alpha *= tau;
81 }
82 hppDout(error, "Could find alpha such that ||f(q)||**2 + "
83 << c
84 << " * 2*(f(q)^T * J * dq) is doing worse than "
85 "||f(q + alpha * dq)||**2");
86 }
87
88 6 u *= smallAlpha;
89
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 6 solver.integrate(arg, u, arg);
90 6 return false;
91 }
92
93 template <typename SolverType>
94 224 inline value_type Backtracking::computeLocalSlope(
95 const SolverType& solver) const {
96 224 value_type slope = 0;
97
2/2
✓ Branch 1 taken 341 times.
✓ Branch 2 taken 224 times.
 565 for (std::size_t i = 0; i < solver.stacks_.size(); ++i) {
98 341 typename SolverType::Data& d = solver.datas_[i];
99 341 const size_type nrows = d.reducedJ.rows();
100
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 326 times.
 341 if (df.size() < nrows) df.resize(nrows);
101
3/6
✓ Branch 2 taken 341 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 341 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 341 times.
✗ Branch 9 not taken.
 341 df.head(nrows).noalias() = d.reducedJ * solver.dqSmall_;
102 341 slope +=
103
4/8
✓ Branch 2 taken 341 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 341 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 341 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 341 times.
✗ Branch 12 not taken.
 341 df.head(nrows).dot(d.activeRowsOfJ.keepRows().rview(d.error).eval());
104 }
105 224 return slope;
106 }
107
108 template <typename SolverType>
109 21589 inline bool FixedSequence::operator()(const SolverType& solver, vectorOut_t arg,
110 vectorOut_t darg) {
111 21589 darg *= alpha;
112 21589 alpha = alphaMax - K * (alphaMax - alpha);
113
3/6
✓ Branch 2 taken 21589 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21589 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 21589 times.
✗ Branch 9 not taken.
 21589 solver.integrate(arg, darg, arg);
114 21589 return true;
115 }
116
117 template <typename SolverType>
118 48 inline bool ErrorNormBased::operator()(const SolverType& solver,
119 vectorOut_t arg, vectorOut_t darg) {
120 48 const value_type r = solver.residualError() / solver.squaredErrorThreshold();
121 48 const value_type alpha = C - K * std::tanh(a * r + b);
122
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 darg *= alpha;
123
4/8
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 48 times.
✗ Branch 11 not taken.
 48 solver.integrate(arg, darg, arg);
124 48 return true;
125 }
126 } // namespace lineSearch
127
128 template <typename LineSearchType>
129 inline solver::HierarchicalIterative::Status
130 440 solver::HierarchicalIterative::solve(vectorOut_t arg,
131 LineSearchType lineSearch) const {
132 hppDout(info, "before projection: " << arg.transpose());
133 440 assert(!arg.hasNaN());
134
135 440 size_type errorDecreased = 3, iter = 0;
136 440 value_type previousSquaredNorm = std::numeric_limits<value_type>::infinity();
137 static const value_type dqMinSquaredNorm =
138 Eigen::NumTraits<value_type>::dummy_precision();
139
140 // Fill value and Jacobian
141 440 computeValue<true>(arg);
142 440 computeError();
143
144 440 if (squaredNorm_ > squaredErrorThreshold_ && reducedDimension_ == 0)
145 ✗ return INFEASIBLE;
146
147 Status status;
148 8945 while (squaredNorm_ > squaredErrorThreshold_ && errorDecreased &&
149 8892 iter < maxIterations_) {
150 8748 computeSaturation(arg);
151 8748 computeDescentDirection();
152 8748 if (dq_.squaredNorm() < dqMinSquaredNorm) {
153 // TODO INFEASIBLE means that we have reached a local minima.
154 // The problem may still be feasible from a different starting point.
155 243 status = INFEASIBLE;
156 243 break;
157 }
158 8505 lineSearch(*this, arg, dq_);
159
160 8505 computeValue<true>(arg);
161 8505 computeError();
162
163 hppDout(info, "squareNorm = " << squaredNorm_);
164 8505 --errorDecreased;
165 8505 if (squaredNorm_ < previousSquaredNorm)
166 6050 errorDecreased = 3;
167 else
168 2455 status = ERROR_INCREASED;
169 8505 previousSquaredNorm = squaredNorm_;
170 8505 ++iter;
171 }
172
173 hppDout(info, "number of iterations: " << iter);
174 440 if (squaredNorm_ > squaredErrorThreshold_) {
175 hppDout(info, "Projection failed.");
176 396 return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status;
177 }
178 hppDout(info, "After projection: " << arg.transpose());
179 44 assert(!arg.hasNaN());
180 44 return SUCCESS;
181 }
182 } // namespace solver
183 } // namespace constraints
184 } // namespace hpp
185
186 #endif // HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
187