GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/solver/impl/by-substitution.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 55 69 79.7%
Branches: 58 134 43.3%
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_BY_SUBSTITUTION_HH
30 #define HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH
31
32 namespace hpp {
33 namespace constraints {
34 namespace solver {
35 typedef std::numeric_limits<value_type> numeric_limits;
36 typedef Eigen::NumTraits<value_type> NumTraits;
37
38 template <typename LineSearchType>
39 3294 inline HierarchicalIterative::Status BySubstitution::impl_solve(
40 vectorOut_t arg, bool _optimize, LineSearchType lineSearch) const {
41
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3294 bool optimize = _optimize && lastIsOptional_;
42
2/4
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1647 times.
 3294 assert(!arg.hasNaN());
43
44
2/4
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1647 times.
✗ Branch 5 not taken.
 3294 explicit_.solve(arg);
45
2/4
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1647 times.
 3294 assert(!arg.hasNaN());
46
47 3294 size_type errorDecreased = 3, iter = 0;
48 value_type previousSquaredNorm;
49 static const value_type dqMinSquaredNorm = NumTraits::dummy_precision();
50 3294 value_type initSquaredNorm = 0;
51
52 // Variables for optimization only
53 3294 value_type previousCost = 0;
54 3294 value_type scaling = 1.;
55 3294 bool onlyLineSearch = false;
56
1/2
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
 3294 vector_t qopt;
57
58 // Fill value and Jacobian
59
2/4
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1647 times.
✗ Branch 5 not taken.
 3294 computeValue<true>(arg);
60
1/2
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
 3294 computeError();
61
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1647 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 3294 if (optimize) previousCost = datas_.back().error.squaredNorm();
62
63 3294 bool errorWasBelowThr = (squaredNorm_ < squaredErrorThreshold_);
64
1/2
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
 3294 vector_t initArg;
65
2/2
✓ Branch 0 taken 501 times.
✓ Branch 1 taken 1146 times.
 3294 if (errorWasBelowThr) {
66
1/2
✓ Branch 1 taken 501 times.
✗ Branch 2 not taken.
 1002 initArg = arg;
67
1/2
✓ Branch 0 taken 501 times.
✗ Branch 1 not taken.
 1002 if (!optimize) iter = std::max(maxIterations_, size_type(2)) - 2;
68 1002 initSquaredNorm = squaredNorm_;
69 }
70
71 3294 bool errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_);
72
3/4
✓ Branch 0 taken 1146 times.
✓ Branch 1 taken 501 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1146 times.
 3294 if (errorIsAboveThr && reducedDimension_ == 0) return INFEASIBLE;
73
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1647 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3294 if (optimize && !errorIsAboveThr) qopt = arg;
74
75 3294 Status status = SUCCESS;
76
5/6
✗ Branch 0 not taken.
✓ Branch 1 taken 15037 times.
✓ Branch 2 taken 13427 times.
✓ Branch 3 taken 1610 times.
✓ Branch 4 taken 13426 times.
✓ Branch 5 taken 1 times.
 30074 while ((optimize || (errorIsAboveThr && errorDecreased))) {
77 // 1. Maximum iterations
78
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 13390 times.
 26852 if (iter >= maxIterations_) {
79 72 status = MAX_ITERATION_REACHED;
80 72 break;
81 }
82 26780 status = SUCCESS;
83
84 // 2. Compute step
85 // onlyLineSearch is true when we only reduced the scaling.
86
1/2
✓ Branch 0 taken 13390 times.
✗ Branch 1 not taken.
 26780 if (!onlyLineSearch) {
87 26780 previousSquaredNorm = squaredNorm_;
88 // Update the jacobian using the jacobian of the explicit system.
89
2/4
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13390 times.
✗ Branch 5 not taken.
 26780 updateJacobian(arg);
90
2/4
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13390 times.
✗ Branch 5 not taken.
 26780 computeSaturation(arg);
91
1/2
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
 26780 computeDescentDirection();
92 }
93 // Apply scaling to avoid too large steps.
94
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 13390 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 26780 if (optimize) dq_ *= scaling;
95
2/4
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 13390 times.
 26780 if (dq_.squaredNorm() < dqMinSquaredNorm) {
96 // We assume that the algorithm reached a local minima.
97 ✗ status = INFEASIBLE;
98 ✗ break;
99 }
100 // 3. Apply line search algorithm for the computed step
101
3/6
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13390 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13390 times.
✗ Branch 8 not taken.
 26780 lineSearch(*this, arg, dq_);
102
2/4
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13390 times.
✗ Branch 5 not taken.
 26780 explicit_.solve(arg);
103
2/4
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 13390 times.
 26780 assert(!arg.hasNaN());
104
105 // 4. Evaluate the error at the new point.
106
2/4
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13390 times.
✗ Branch 5 not taken.
 26780 computeValue<true>(arg);
107
1/2
✓ Branch 1 taken 13390 times.
✗ Branch 2 not taken.
 26780 computeError();
108
109 26780 --errorDecreased;
110
2/2
✓ Branch 0 taken 13377 times.
✓ Branch 1 taken 13 times.
 26780 if (squaredNorm_ < previousSquaredNorm)
111 26754 errorDecreased = 3;
112 else
113 26 status = ERROR_INCREASED;
114
115 26780 errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_);
116 // 5. In case of optimization,
117 // - if the constraints is satisfied and the cost decreased, increase
118 // the scaling (amount of confidence in the linear approximation)
119 // - if the constraints is not satisfied, decrease the scaling and
120 // and cancel this step.
121
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 13390 times.
 26780 if (optimize) {
122 ✗ if (!errorIsAboveThr) {
123 ✗ value_type cost = datas_.back().error.squaredNorm();
124 ✗ if (cost < previousCost) {
125 ✗ qopt = arg;
126 ✗ previousCost = cost;
127 ✗ if (scaling < 0.5) scaling *= 2;
128 }
129 ✗ onlyLineSearch = false;
130 } else {
131 ✗ dq_ /= scaling;
132 ✗ scaling *= 0.5;
133 ✗ if (qopt.size() > 0) arg = qopt;
134 ✗ onlyLineSearch = true;
135 }
136 }
137
138 26780 ++iter;
139 }
140
141
3/4
✓ Branch 0 taken 1647 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 501 times.
✓ Branch 3 taken 1146 times.
 3294 if (!optimize && errorWasBelowThr) {
142
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 501 times.
 1002 if (squaredNorm_ > initSquaredNorm) {
143 ✗ arg = initArg;
144 }
145 1002 return SUCCESS;
146 }
147 // If optimizing, qopt is the visited configuration that satisfies the
148 // constraints and has lowest cost.
149
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 1146 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1146 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 2292 if (optimize && qopt.size() > 0) arg = qopt;
150
151
2/4
✓ Branch 1 taken 1146 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1146 times.
 2292 assert(!arg.hasNaN());
152 2292 return status;
153 3294 }
154 } // namespace solver
155 } // namespace constraints
156 } // namespace hpp
157
158 #endif // HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH
159