GCC Code Coverage Report

Directory: ./
File: src/path-projector/global.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 326 361 90.3%
Branches: 307 568 54.0%
Line Branch Exec Source
1 // Copyright (c) 2015, 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-projector/global.hh"
30
31 #include <hpp/constraints/solver/by-substitution.hh>
32 #include <hpp/core/config-projector.hh>
33 #include <hpp/core/interpolated-path.hh>
34 #include <hpp/core/path-vector.hh>
35 #include <hpp/core/problem.hh>
36 #include <hpp/core/steering-method.hh>
37 #include <hpp/core/steering-method/straight.hh>
38 #include <hpp/pinocchio/configuration.hh>
39 #include <hpp/pinocchio/liegroup.hh>
40 #include <hpp/util/debug.hh>
41 #include <hpp/util/timer.hh>
42 #include <limits>
43 #include <queue>
44 #include <stack>
45
46 namespace hpp {
47 namespace core {
48 namespace pathProjector {
49 namespace {
50 HPP_DEFINE_TIMECOUNTER(globalPathProjector_initCfgList);
51 HPP_DEFINE_TIMECOUNTER(globalPathProjector_projOneStep);
52 HPP_DEFINE_TIMECOUNTER(globalPathProjector_reinterpolate);
53 HPP_DEFINE_TIMECOUNTER(globalPathProjector_createPath);
54 } // namespace
55
56 4 GlobalPtr_t Global::create(const DistancePtr_t& distance,
57 const SteeringMethodPtr_t& steeringMethod,
58 value_type step) {
59 4 value_type hessianBound = steeringMethod->problem()
60
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 ->getParameter("PathProjection/HessianBound")
61
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 .floatValue();
62 4 value_type thr_min = steeringMethod->problem()
63
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 ->getParameter("PathProjection/MinimalDist")
64
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 .floatValue();
65 hppDout(info, "Hessian bound is " << hessianBound);
66 hppDout(info, "Min Dist is " << thr_min);
67 return GlobalPtr_t(
68
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 new Global(distance, steeringMethod, step, thr_min, hessianBound));
69 }
70
71 4 GlobalPtr_t Global::create(const ProblemConstPtr_t& problem,
72 const value_type& step) {
73
1/2
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 8 return create(problem->distance(), problem->steeringMethod(), step);
74 }
75
76 4 Global::Global(const DistancePtr_t& distance,
77 const SteeringMethodPtr_t& steeringMethod, value_type step,
78 4 value_type threshold, value_type hessianBound)
79 : PathProjector(distance, steeringMethod),
80 4 step_(step),
81 4 hessianBound_(hessianBound),
82
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 thresholdMin_(threshold) {
83 // TODO Only steeringMethod::Straight has been tested so far.
84
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(HPP_DYNAMIC_PTR_CAST(hpp::core::steeringMethod::Straight,
85 steeringMethod));
86 4 }
87
88 64 bool Global::impl_apply(const PathPtr_t& path, PathPtr_t& proj) const {
89
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
 64 assert(path);
90 64 bool success = false;
91 64 PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path);
92
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 if (!pv) {
93
4/10
✓ Branch 3 taken 64 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 64 times.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 64 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 192 if (!path->constraints() || !path->constraints()->configProjector() ||
94
5/12
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 64 times.
✓ Branch 12 taken 64 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 64 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 128 path->constraints()->configProjector()->dimension() == 0) {
95 ✗ proj = path;
96 ✗ success = true;
97 } else {
98
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 32 times.
 64 if (hessianBound_ <= 0)
99
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 success = project(path, proj);
100 else
101
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 success = project2(path, proj);
102 }
103 } else {
104 PathVectorPtr_t res =
105 ✗ PathVector::create(pv->outputSize(), pv->outputDerivativeSize());
106 ✗ PathPtr_t part;
107 ✗ success = true;
108 ✗ for (size_t i = 0; i < pv->numberPaths(); i++) {
109 ✗ if (!apply(pv->pathAtRank(i), part)) {
110 // We add the path only if part is not NULL and:
111 // - either its length is not zero,
112 // - or it's not the first one.
113 ✗ if (part && (part->length() > 0 || i == 0)) {
114 ✗ res->appendPath(part);
115 }
116 ✗ success = false;
117 ✗ break;
118 }
119 ✗ res->appendPath(part);
120 }
121 ✗ proj = res;
122 }
123
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
 64 assert(proj);
124
5/10
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 64 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 64 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 64 times.
 64 assert((proj->initial() - path->initial()).isZero());
125
11/20
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 38 times.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 26 times.
✓ Branch 20 taken 26 times.
✓ Branch 21 taken 38 times.
✓ Branch 23 taken 26 times.
✓ Branch 24 taken 38 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
 64 assert(!success || (proj->end() - path->end()).isZero());
126 64 return success;
127 64 }
128
129 32 bool Global::project(const PathPtr_t& path, PathPtr_t& proj) const {
130
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 Configs_t cfgs;
131
1/2
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 32 ConfigProjector& p = *path->constraints()->configProjector();
132 HPP_START_TIMECOUNTER(globalPathProjector_initCfgList);
133
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 initialConfigList(path, cfgs);
134
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 32 times.
 32 if (cfgs.size() == 2) { // Shorter than step_
135 ✗ proj = path;
136 ✗ return true;
137 }
138
4/8
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 32 times.
 32 assert((cfgs.back() - path->end()).isZero());
139 HPP_STOP_AND_DISPLAY_TIMECOUNTER(globalPathProjector_initCfgList);
140
141
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 32 Bools_t projected(cfgs.size() - 2, false);
142
2/4
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
 32 Alphas_t alphas(cfgs.size() - 2, LineSearch_t());
143
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 32 Lengths_t lengths(cfgs.size() - 1, 0);
144 32 Configs_t::iterator last = --(cfgs.end());
145
146 32 std::size_t nbIter = 0;
147
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 const std::size_t maxIter = p.maxIterations();
148
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 32 const std::size_t maxCfgNum = 2 + (std::size_t)(10 * path->length() / step_);
149
150 hppDout(info, "start with " << cfgs.size() << " configs");
151
3/4
✓ Branch 1 taken 168 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 136 times.
✓ Branch 4 taken 32 times.
 168 while (!projectOneStep(p, cfgs, last, projected, lengths, alphas)) {
152
4/8
✓ Branch 2 taken 136 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 136 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 136 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 136 times.
 136 assert((cfgs.back() - path->end()).isZero());
153
1/2
✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
 136 if (cfgs.size() < maxCfgNum) {
154
1/2
✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
 136 const size_type newCs = reinterpolate(p.robot(), cfgs, last, projected,
155 136 lengths, alphas, step_);
156
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 104 times.
 136 if (newCs > 0) {
157 32 nbIter = 0;
158 hppDout(info, "Added " << newCs << " configs. Cur / Max = "
159 << cfgs.size() << '/' << maxCfgNum);
160 } else if (newCs < 0) {
161 hppDout(info, "Removed " << -newCs << " configs. Cur / Max = "
162 << cfgs.size() << '/' << maxCfgNum);
163 }
164 }
165
166 136 nbIter++;
167
168
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 136 times.
 136 if (nbIter > maxIter) break;
169 }
170 HPP_DISPLAY_TIMECOUNTER(globalPathProjector_projOneStep);
171 HPP_DISPLAY_TIMECOUNTER(globalPathProjector_reinterpolate);
172
173 // Build the projection
174 HPP_START_TIMECOUNTER(globalPathProjector_createPath);
175
1/2
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 32 bool ret = createPath(p.robot(), path->constraints(), cfgs, last, projected,
176 lengths, proj);
177 HPP_STOP_AND_DISPLAY_TIMECOUNTER(globalPathProjector_createPath);
178 32 return ret;
179 32 }
180
181 32 bool Global::project2(const PathPtr_t& path, PathPtr_t& proj) const {
182
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 Configs_t cfgs;
183
1/2
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 32 ConfigProjector& p = *path->constraints()->configProjector();
184
2/4
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
 32 q_.resize(p.robot()->configSize());
185
2/4
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
 32 dq_.resize(p.robot()->numberDof());
186
187 HPP_START_TIMECOUNTER(globalPathProjector_initCfgList);
188
189 32 Datas_t datas;
190
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 initialConfigList(path, p, datas);
191 32 Datas_t::iterator last = datas.end();
192
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 32 reinterpolate(p.robot(), p, datas, last);
193
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 18 times.
 32 if (datas.size() == 2) { // Shorter than step_
194 14 proj = path;
195 #if HPP_ENABLE_BENCHMARK
196 hppBenchmark("Interpolated path (global - with hessian): "
197 << 0 << ", [ " << 0 << ", " << 0 << ", " << 0 << "]");
198 #endif
199 14 return true;
200 }
201
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 18 times.
 18 assert((datas.back().q - path->end()).isZero());
202 HPP_STOP_AND_DISPLAY_TIMECOUNTER(globalPathProjector_initCfgList);
203
204 18 std::size_t nbIter = 0;
205
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 const std::size_t maxIter = 2 * p.maxIterations();
206
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 const std::size_t maxCfgNum = 2 + (std::size_t)(10 * path->length() / step_);
207
208 hppDout(info, "start with " << datas.size() << " configs");
209 18 bool repeat = true;
210
2/2
✓ Branch 0 taken 194 times.
✓ Branch 1 taken 18 times.
 212 while (repeat) {
211
1/2
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
 194 repeat = !projectOneStep(p, datas, last);
212
4/8
✓ Branch 2 taken 194 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 194 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 194 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 194 times.
 194 assert((datas.back().q - path->end()).isZero());
213
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 194 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 194 times.
✗ Branch 6 not taken.
 194 if (datas.size() < maxCfgNum || !repeat) {
214
1/2
✓ Branch 2 taken 194 times.
✗ Branch 3 not taken.
 194 const size_type newCs = reinterpolate(p.robot(), p, datas, last);
215
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 178 times.
 194 if (newCs > 0) {
216 16 nbIter = 0;
217 hppDout(info, "Added " << newCs << " configs. Cur / Max = "
218 << datas.size() << '/' << maxCfgNum);
219 16 repeat = true;
220 } else if (newCs < 0) {
221 hppDout(info, "Removed " << -newCs << " configs. Cur / Max = "
222 << datas.size() << '/' << maxCfgNum);
223 }
224 }
225
226 194 nbIter++;
227
228
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 194 times.
 194 assert(datas.size() >= 2);
229
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 194 times.
 194 if (nbIter > maxIter) break;
230 }
231 HPP_DISPLAY_TIMECOUNTER(globalPathProjector_projOneStep);
232 HPP_DISPLAY_TIMECOUNTER(globalPathProjector_reinterpolate);
233
234 // Build the projection
235 HPP_START_TIMECOUNTER(globalPathProjector_createPath);
236
1/2
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 bool ret = createPath(p.robot(), path->constraints(), datas, last, proj);
237 HPP_STOP_AND_DISPLAY_TIMECOUNTER(globalPathProjector_createPath);
238 18 return ret;
239 32 }
240
241 168 bool Global::projectOneStep(ConfigProjector& p, Configs_t& q,
242 Configs_t::iterator& last, Bools_t& b, Lengths_t& l,
243 Alphas_t& a) const {
244 HPP_START_TIMECOUNTER(globalPathProjector_projOneStep);
245 /// First and last should not be updated
246 168 const Configs_t::iterator begin = ++(q.begin());
247 168 const Configs_t::iterator end = --(q.end());
248 168 Bools_t ::iterator itB = (b.begin());
249 168 Alphas_t ::iterator itA = (a.begin());
250 168 Lengths_t::iterator itL = (l.begin());
251 168 Configs_t::iterator itCp = (q.begin());
252 168 bool allAreSatisfied = true;
253 168 bool curUpdated = false, prevUpdated = false;
254 /// Eigen matrices storage order defaults to column major.
255
2/4
✓ Branch 3 taken 168 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 168 times.
✗ Branch 7 not taken.
 168 Eigen::Matrix<value_type, Eigen::Dynamic, 2> oldQ(p.robot()->configSize(), 2);
256
2/4
✓ Branch 3 taken 168 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 168 times.
✗ Branch 7 not taken.
 168 Eigen::Matrix<value_type, Eigen::Dynamic, 2> dq(p.robot()->numberDof(), 2);
257
1/2
✓ Branch 1 taken 168 times.
✗ Branch 2 not taken.
 168 dq.setZero();
258
2/4
✓ Branch 3 taken 168 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 168 times.
✗ Branch 7 not taken.
 168 vector_t qMinusQPrev(p.robot()->numberDof());
259 168 size_type iCol = 0;
260 168 size_type iNCol = (iCol + 1) % 2;
261
2/2
✓ Branch 2 taken 4056 times.
✓ Branch 3 taken 148 times.
 4204 for (Configs_t::iterator it = begin; it != last; ++it) {
262
2/2
✓ Branch 2 taken 3402 times.
✓ Branch 3 taken 654 times.
 4056 if (!*itB) {
263
2/4
✓ Branch 2 taken 3402 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3402 times.
✗ Branch 6 not taken.
 3402 oldQ.col(iCol) = *it;
264
2/4
✓ Branch 4 taken 3402 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3402 times.
✗ Branch 8 not taken.
 3402 *itB = p.solver().oneStep(*it, *itA);
265
2/4
✓ Branch 3 taken 3402 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3402 times.
✗ Branch 7 not taken.
 3402 dq.col(iCol) = p.solver().lastStep();
266 // *itA = alphaMax - 0.8 * (alphaMax - *itA);
267
4/4
✓ Branch 0 taken 762 times.
✓ Branch 1 taken 2640 times.
✓ Branch 4 taken 626 times.
✓ Branch 5 taken 136 times.
 3402 allAreSatisfied = allAreSatisfied && *itB;
268 3402 curUpdated = true;
269
2/2
✓ Branch 0 taken 3206 times.
✓ Branch 1 taken 196 times.
 3402 if (prevUpdated) {
270 /// Detect large increase in size
271
6/12
✓ Branch 1 taken 3206 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3206 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3206 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3206 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3206 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 3206 times.
✗ Branch 18 not taken.
 3206 hpp::pinocchio::difference(p.robot(), oldQ.col(iCol), oldQ.col(iNCol),
272 qMinusQPrev);
273
4/8
✓ Branch 1 taken 3206 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3206 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3206 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3206 times.
✗ Branch 11 not taken.
 3206 const vector_t deltaDQ = dq.col(iCol) - dq.col(iNCol);
274
1/2
✓ Branch 1 taken 3206 times.
✗ Branch 2 not taken.
 3206 const value_type N2 = qMinusQPrev.squaredNorm();
275
1/2
✓ Branch 1 taken 3206 times.
✗ Branch 2 not taken.
 3206 if (sqrt(N2) < Eigen::NumTraits<value_type>::dummy_precision()) {
276 hppDout(error, "The two config should be removed:"
277 << "\noldQ = " << oldQ.transpose()
278 << "\nqMinusQPrev = " << qMinusQPrev.transpose()
279 << "\nDistance is "
280 << d(oldQ.col(iCol), oldQ.col(iNCol))
281 << "\nand in mem " << *itL);
282 } else {
283
1/2
✓ Branch 1 taken 3206 times.
✗ Branch 2 not taken.
 3206 const value_type alphaSquare = 1 - 2 * qMinusQPrev.dot(deltaDQ) / N2 +
284
1/2
✓ Branch 1 taken 3206 times.
✗ Branch 2 not taken.
 3206 qMinusQPrev.squaredNorm() / N2;
285 // alpha > 4 => the distance between the two points has been
286 // mutiplied by more than 4.
287
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 3186 times.
 3206 if (alphaSquare > 16) {
288 hppDout(error, "alpha^2 = "
289 << alphaSquare
290 << "\nqMinusQPrev = " << qMinusQPrev.transpose()
291 << "\ndeltaDQ = " << deltaDQ.transpose());
292 20 last = it;
293 20 --last;
294 20 return false;
295 }
296 }
297
298 3186 const value_type limitCos = 0.5;
299 vector_t dots =
300
6/12
✓ Branch 1 taken 3186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3186 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3186 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3186 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3186 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3186 times.
✗ Branch 17 not taken.
 3186 dq.colwise().normalized().transpose() * qMinusQPrev.normalized();
301 // Check if both updates are pointing outward
302
6/10
✓ Branch 1 taken 3186 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 268 times.
✓ Branch 4 taken 2918 times.
✓ Branch 6 taken 268 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 268 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 3186 times.
 3186 if (dots[iCol] > limitCos && dots[iNCol] < -limitCos) {
303 hppDout(error, "Descent step is going in opposite direction: "
304 << dots << ". It is likely a discontinuity.");
305 ✗ last = it;
306 ✗ --last;
307 ✗ return false;
308 }
309 3186 iNCol = iCol;
310 3186 iCol = (iCol + 1) % 2;
311
3/4
✓ Branch 1 taken 3186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3186 times.
✓ Branch 5 taken 20 times.
 3206 }
312 }
313
7/10
✓ Branch 0 taken 790 times.
✓ Branch 1 taken 3246 times.
✓ Branch 2 taken 196 times.
✓ Branch 3 taken 594 times.
✓ Branch 6 taken 3442 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 3442 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3442 times.
✗ Branch 14 not taken.
 4036 if (prevUpdated || curUpdated) *itL = d(*itCp, *it);
314 4036 prevUpdated = curUpdated;
315 4036 curUpdated = false;
316 4036 ++itCp;
317 4036 ++itB;
318 4036 ++itL;
319 }
320
5/8
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 32 times.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 116 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 116 times.
✗ Branch 12 not taken.
 148 if (prevUpdated) *itL = d(*itCp, *end);
321 HPP_STOP_TIMECOUNTER(globalPathProjector_projOneStep);
322 148 return allAreSatisfied;
323 168 }
324
325 194 bool Global::projectOneStep(ConfigProjector& p, Datas_t& ds,
326 const Datas_t::iterator& last) const {
327 HPP_START_TIMECOUNTER(globalPathProjector_projOneStep);
328 194 Datas_t::iterator _dPrev = ds.begin();
329 194 Datas_t::iterator _d = ++(ds.begin());
330 194 bool allAreSatisfied = true;
331 194 bool curUpdated = false, prevUpdated = false;
332
333
2/2
✓ Branch 2 taken 1994 times.
✓ Branch 3 taken 194 times.
 2188 for (; _d != last; ++_d) {
334
2/2
✓ Branch 1 taken 1778 times.
✓ Branch 2 taken 216 times.
 1994 if (!_d->projected) {
335
2/4
✓ Branch 4 taken 1778 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1778 times.
✗ Branch 8 not taken.
 1778 _d->projected = p.solver().oneStep(_d->q, _d->alpha);
336 // dq_ = p.solver().lastStep();
337
1/2
✓ Branch 1 taken 1778 times.
✗ Branch 2 not taken.
 1778 _d->sigma = p.sigma();
338 1778 ++_d->Niter;
339
4/4
✓ Branch 0 taken 324 times.
✓ Branch 1 taken 1454 times.
✓ Branch 3 taken 148 times.
✓ Branch 4 taken 176 times.
 1778 allAreSatisfied = allAreSatisfied && _d->projected;
340 1778 curUpdated = true;
341 }
342
7/10
✓ Branch 0 taken 408 times.
✓ Branch 1 taken 1586 times.
✓ Branch 2 taken 220 times.
✓ Branch 3 taken 188 times.
✓ Branch 6 taken 1806 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1806 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1806 times.
✗ Branch 14 not taken.
 1994 if (prevUpdated || curUpdated) _d->length = d(_dPrev->q, _d->q);
343 1994 prevUpdated = curUpdated;
344 1994 curUpdated = false;
345 1994 ++_dPrev;
346 }
347
5/8
✓ Branch 0 taken 192 times.
✓ Branch 1 taken 2 times.
✓ Branch 4 taken 192 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 192 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 192 times.
✗ Branch 12 not taken.
 194 if (prevUpdated) _d->length = d(_dPrev->q, _d->q);
348 HPP_STOP_TIMECOUNTER(globalPathProjector_projOneStep);
349 194 return allAreSatisfied;
350 }
351
352 136 size_type Global::reinterpolate(const DevicePtr_t& robot, Configs_t& q,
353 const Configs_t::iterator& last, Bools_t& b,
354 Lengths_t& l, Alphas_t& a,
355 const value_type& maxDist) const {
356 HPP_START_TIMECOUNTER(globalPathProjector_reinterpolate);
357 136 Configs_t::iterator begin = ++(q.begin());
358 136 Configs_t::iterator end = last;
359 136 ++end;
360 136 Bools_t ::iterator itB = (b.begin());
361 136 Alphas_t ::iterator itA = (a.begin());
362 136 Lengths_t::iterator itL = (l.begin());
363 136 Configs_t::iterator itCp = (q.begin());
364
2/4
✓ Branch 2 taken 136 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 136 times.
✗ Branch 6 not taken.
 136 Configuration_t newQ(robot->configSize());
365 136 size_type nbNewC = 0;
366
2/2
✓ Branch 2 taken 4146 times.
✓ Branch 3 taken 136 times.
 4282 for (Configs_t::iterator it = begin; it != last; ++it) {
367
2/2
✓ Branch 1 taken 472 times.
✓ Branch 2 taken 3674 times.
 4146 if (*itL > maxDist) {
368 472 ++nbNewC;
369
4/8
✓ Branch 1 taken 472 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 472 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 472 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 472 times.
✗ Branch 12 not taken.
 944 pinocchio::interpolate<pinocchio::RnxSOnLieGroupMap>(robot, *itCp, *it,
370 472 0.5, newQ);
371 // FIXME: make sure the iterator are valid after insertion
372 // Insert new respective elements
373
1/2
✓ Branch 2 taken 472 times.
✗ Branch 3 not taken.
 472 it = q.insert(it, newQ);
374
1/2
✓ Branch 2 taken 472 times.
✗ Branch 3 not taken.
 472 itB = b.insert(itB, false);
375
2/4
✓ Branch 1 taken 472 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 472 times.
✗ Branch 6 not taken.
 472 itA = a.insert(itA, LineSearch_t());
376
4/8
✓ Branch 2 taken 472 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 472 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 472 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 472 times.
✗ Branch 14 not taken.
 472 itL = l.insert(itL, d(*itCp, *it));
377 // Update length after
378 472 Configs_t::iterator itNC = it;
379 472 ++itNC;
380 472 Lengths_t::iterator itNL = itL;
381 472 ++itNL;
382
3/6
✓ Branch 2 taken 472 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 472 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 472 times.
✗ Branch 10 not taken.
 472 *itNL = d(*it, *itNC);
383 // FIXME: End has changed ?
384 472 end = q.end();
385 472 it = itCp;
386 472 continue;
387
2/2
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 3646 times.
 4146 } else if (*itL < maxDist * 1e-2) {
388 28 nbNewC--;
389 hppDout(warning, "Removing configuration: " << it->transpose()
390 << "\nToo close to: "
391 << itCp->transpose());
392 // The distance to the previous point is very small.
393 // This point can safely be removed.
394 28 it = q.erase(it);
395
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 itB = b.erase(itB);
396 28 itA = a.erase(itA);
397 28 itL = l.erase(itL);
398 // Update length
399
3/6
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 28 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 28 times.
✗ Branch 10 not taken.
 28 *itL = d(*itCp, *it);
400 28 it = itCp;
401 28 continue;
402 }
403 3646 ++itCp;
404 3646 ++itB;
405 3646 ++itA;
406 3646 ++itL;
407 }
408 HPP_STOP_TIMECOUNTER(globalPathProjector_reinterpolate);
409 136 return nbNewC;
410 136 }
411
412 226 size_type Global::reinterpolate(const DevicePtr_t& robot, ConfigProjector& p,
413 Datas_t& ds, Datas_t::iterator& last) const {
414 HPP_START_TIMECOUNTER(globalPathProjector_reinterpolate);
415 226 Datas_t::iterator begin = ++(ds.begin());
416 226 Datas_t::iterator _dPrev = ds.begin();
417 226 size_type nbNewC = 0;
418
1/2
✓ Branch 1 taken 226 times.
✗ Branch 2 not taken.
 226 Data newD;
419
2/4
✓ Branch 2 taken 226 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 226 times.
✗ Branch 6 not taken.
 226 newD.q.resize(robot->configSize());
420
1/2
✓ Branch 1 taken 226 times.
✗ Branch 2 not taken.
 226 const std::size_t maxIter = p.maxIterations();
421 226 const value_type K = hessianBound_, dist_min = thresholdMin_,
422 226 sigma_min = dist_min * K;
423
2/2
✓ Branch 2 taken 2216 times.
✓ Branch 3 taken 208 times.
 2424 for (Datas_t::iterator _d = begin; _d != last; ++_d) {
424
5/6
✓ Branch 1 taken 2198 times.
✓ Branch 2 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2198 times.
✓ Branch 6 taken 18 times.
✓ Branch 7 taken 2198 times.
 2216 if (_dPrev->sigma < sigma_min || _dPrev->Niter >= maxIter) {
425 hppDout(info, "Rejected sigma " << _d->sigma);
426 18 last = _dPrev;
427 18 break;
428 }
429
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2198 times.
 2198 if (_d->sigma < sigma_min) {
430 hppDout(info, "Rejected sigma " << _d->sigma);
431 ✗ last = _d;
432 ✗ break;
433 }
434 2198 const value_type delta = _dPrev->sigma + _d->sigma;
435
2/2
✓ Branch 1 taken 190 times.
✓ Branch 2 taken 2008 times.
 2198 if (_d->length > delta / K) {
436 190 ++nbNewC;
437 // const value_type t = ( 1 + (_dPrev->sigma - _d->sigma) / (K *
438 // _d->length) ) / 2;
439 190 const value_type t = _dPrev->sigma / (K * _d->length);
440
2/4
✓ Branch 0 taken 190 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 assert(t < 1 && t > 0);
441
3/6
✓ Branch 1 taken 190 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 190 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 190 times.
✗ Branch 9 not taken.
 380 pinocchio::interpolate<pinocchio::RnxSOnLieGroupMap>(robot, _dPrev->q,
442
1/2
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 _d->q, t, newD.q);
443 hppDout(info, "Add config " << newD.q.transpose());
444
445 // Insert new respective elements
446
1/2
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 initData(newD, newD.q, p, true, false, _dPrev->q);
447
1/2
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 _d = ds.insert(_d, newD);
448 // Update length after
449 190 Datas_t::iterator _dNext = _d;
450 190 ++_dNext;
451
3/6
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 190 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 190 times.
✗ Branch 10 not taken.
 190 _dNext->length = d(_d->q, _dNext->q);
452 190 _dPrev = _d;
453 190 continue;
454 190 }
455 2008 ++_dPrev;
456 }
457 HPP_STOP_TIMECOUNTER(globalPathProjector_reinterpolate);
458 226 return nbNewC;
459 226 }
460
461 32 bool Global::createPath(const DevicePtr_t& robot,
462 const ConstraintSetPtr_t& constraint,
463 const Configs_t& q, const Configs_t::iterator&,
464 const Bools_t& b, const Lengths_t& l,
465 PathPtr_t& result) const {
466 /// Compute total length
467 32 value_type length = 0;
468 32 value_type min = std::numeric_limits<value_type>::max(), max = 0;
469 32 size_type nbWaypoints = 0;
470 32 Lengths_t::const_iterator itL = (l.begin());
471 32 Bools_t ::const_iterator itB = (b.begin());
472 32 Configs_t::const_iterator itCl = (q.begin());
473 32 bool fullyProjected = true;
474
2/2
✓ Branch 3 taken 834 times.
✓ Branch 4 taken 12 times.
 846 for (; itB != b.end(); ++itB) {
475
5/6
✓ Branch 1 taken 814 times.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 814 times.
✓ Branch 6 taken 20 times.
✓ Branch 7 taken 814 times.
 834 if (!*itB || *itL > step_) {
476 20 fullyProjected = false;
477 20 break;
478 }
479 814 length += *itL;
480
481 814 ++nbWaypoints;
482 814 min = std::min(min, *itL);
483 814 max = std::max(max, *itL);
484
485 814 ++itCl;
486 814 ++itL;
487 }
488
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 20 times.
 32 if (fullyProjected) {
489 12 length += *itL;
490
491 12 ++nbWaypoints;
492 12 min = std::min(min, *itL);
493 12 max = std::max(max, *itL);
494
495 12 ++itCl;
496 12 ++itL;
497
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
 12 assert(itL == l.end());
498 }
499 #if HPP_ENABLE_BENCHMARK
500 value_type avg = (nbWaypoints == 0 ? 0 : length / (value_type)nbWaypoints);
501 hppBenchmark("Interpolated path (global - non hessian): "
502 << nbWaypoints << ", [ " << min << ", " << avg << ", " << max
503 << "]");
504 #endif
505
506 InterpolatedPathPtr_t out =
507
3/6
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 InterpolatedPath::create(robot, q.front(), *itCl, length, constraint);
508
509
2/2
✓ Branch 2 taken 28 times.
✓ Branch 3 taken 4 times.
 32 if (itCl != q.begin()) {
510 28 length = 0;
511 28 Lengths_t::const_iterator itL = (l.begin());
512 28 Configs_t::const_iterator begin = ++(q.begin());
513
2/2
✓ Branch 2 taken 798 times.
✓ Branch 3 taken 28 times.
 826 for (Configs_t::const_iterator it = begin; it != itCl; ++it) {
514 798 length += *itL;
515
2/4
✓ Branch 3 taken 798 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 798 times.
✗ Branch 7 not taken.
 798 out->insert(length, *it);
516 798 ++itL;
517 }
518 } else {
519 hppDout(info, "Path of length 0");
520
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 assert(!fullyProjected);
521 }
522 32 result = out;
523 hppDout(info, "Projection succeeded ? " << fullyProjected);
524 32 return fullyProjected;
525 32 }
526
527 18 bool Global::createPath(const DevicePtr_t& robot,
528 const ConstraintSetPtr_t& constraint, const Datas_t& ds,
529 const Datas_t::iterator& last,
530 PathPtr_t& result) const {
531 /// Compute total length
532 18 value_type length = 0;
533 18 value_type min = std::numeric_limits<value_type>::max(), max = 0;
534 18 size_type nbWaypoints = 0;
535 18 const Datas_t::const_iterator begin = ++(ds.begin());
536 18 Datas_t::const_iterator _dLast = ds.begin();
537 18 bool fullyProjected = (last == ds.end());
538
2/2
✓ Branch 2 taken 172 times.
✓ Branch 3 taken 18 times.
 190 for (Datas_t::const_iterator _d = begin; _d != last; ++_d) {
539
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 172 times.
 172 if (!_d->projected) {
540 ✗ fullyProjected = false;
541 ✗ break;
542 }
543 172 length += _d->length;
544 172 ++_dLast;
545
546 172 ++nbWaypoints;
547 172 min = std::min(min, _d->length);
548 172 max = std::max(max, _d->length);
549 }
550 #if HPP_ENABLE_BENCHMARK
551 value_type avg = (nbWaypoints == 0 ? 0 : length / (value_type)nbWaypoints);
552 hppBenchmark("Interpolated path (global - with hessian): "
553 << nbWaypoints << ", [ " << min << ", " << avg << ", " << max
554 << "]");
555 #endif
556
557 InterpolatedPathPtr_t out = InterpolatedPath::create(
558
3/6
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 36 robot, ds.front().q, _dLast->q, length, constraint);
559
560
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 2 times.
 18 if (_dLast != ds.begin()) {
561 16 length = 0;
562
2/2
✓ Branch 1 taken 156 times.
✓ Branch 2 taken 16 times.
 172 for (Datas_t::const_iterator _d = begin; _d != _dLast; ++_d) {
563 156 length += _d->length;
564
2/4
✓ Branch 3 taken 156 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 156 times.
✗ Branch 7 not taken.
 156 out->insert(length, _d->q);
565 }
566 } else {
567 hppDout(info, "Path of length 0");
568
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 assert(!fullyProjected);
569 }
570 18 result = out;
571 hppDout(info, "Projection succeeded ? " << fullyProjected);
572 18 return fullyProjected;
573 18 }
574
575 32 void Global::initialConfigList(const PathPtr_t& path, Configs_t& cfgs) const {
576 32 InterpolatedPathPtr_t ip = HPP_DYNAMIC_PTR_CAST(InterpolatedPath, path);
577
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 32 times.
 32 if (ip) {
578 // Get the waypoint of ip
579 const InterpolatedPath::InterpolationPoints_t& ips =
580 ✗ ip->interpolationPoints();
581 ✗ for (InterpolatedPath::InterpolationPoints_t::const_iterator it =
582 ✗ ips.begin();
583 ✗ it != ips.end(); ++it) {
584 ✗ cfgs.push_back(it->second);
585 }
586 } else {
587
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 32 const value_type L = path->length();
588
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 32 Configuration_t q(path->outputSize());
589
2/4
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
 32 cfgs.push_back(path->initial());
590 // Factor 0.99 is to ensure that the distance between two consecutives
591 // configurations will be smaller that step_
592
2/2
✓ Branch 0 taken 660 times.
✓ Branch 1 taken 32 times.
 692 for (value_type t = step_; t < L; t += step_ * 0.99) {
593 // Interpolate without taking care of the constraints
594 // FIXME: Path must not be a PathVector otherwise the constraints
595 // are applied.
596
2/4
✓ Branch 2 taken 660 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 660 times.
✗ Branch 6 not taken.
 660 path->at(t, q);
597
1/2
✓ Branch 1 taken 660 times.
✗ Branch 2 not taken.
 660 cfgs.push_back(q);
598 }
599
2/4
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
 32 cfgs.push_back(path->end());
600 32 }
601 32 }
602
603 32 void Global::initialConfigList(const PathPtr_t& path, ConfigProjector& p,
604 Datas_t& ds) const {
605
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 Data newD;
606
607 // Set initial point.
608
3/6
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
 32 initData(newD, path->initial(), p, true, true);
609
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 ds.push_back(newD);
610
611 32 InterpolatedPathPtr_t ip = HPP_DYNAMIC_PTR_CAST(InterpolatedPath, path);
612
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 32 times.
 32 if (ip) {
613 // Get the waypoint of ip
614 const InterpolatedPath::InterpolationPoints_t& ips =
615 ✗ ip->interpolationPoints();
616 InterpolatedPath::InterpolationPoints_t::const_iterator _ipPrev =
617 ✗ ips.begin();
618 ✗ for (InterpolatedPath::InterpolationPoints_t::const_iterator _ip =
619 ✗ ++(ips.begin());
620 ✗ _ip != ips.end(); ++_ip) {
621 ✗ initData(newD, _ip->second, p, true, false, _ipPrev->second);
622 ✗ ds.push_back(newD);
623 ✗ ++_ipPrev;
624 }
625 } else {
626
3/6
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
 32 initData(newD, path->end(), p, true, true, path->initial());
627
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 ds.push_back(newD);
628 }
629 32 }
630
631 254 void Global::initData(Data& data, const Configuration_t& q, ConfigProjector& p,
632 bool computeSigma, bool projected,
633 const Configuration_t& qLength) const {
634 254 data.q = q;
635
1/2
✓ Branch 0 taken 254 times.
✗ Branch 1 not taken.
 254 if (computeSigma) {
636
1/2
✓ Branch 1 taken 254 times.
✗ Branch 2 not taken.
 254 q_ = q;
637
1/2
✓ Branch 1 taken 254 times.
✗ Branch 2 not taken.
 254 LineSearch_t ls;
638
2/4
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 254 times.
✗ Branch 6 not taken.
 254 p.solver().oneStep(q_, ls);
639 // dq_ = p.solver().lastStep();
640
1/2
✓ Branch 1 taken 254 times.
✗ Branch 2 not taken.
 254 data.sigma = p.sigma();
641 }
642 254 data.projected = projected;
643 254 data.alpha = LineSearch_t();
644 254 data.Niter = 0;
645
2/2
✓ Branch 2 taken 222 times.
✓ Branch 3 taken 32 times.
 254 if (qLength.size() == q.size())
646
2/4
✓ Branch 2 taken 222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 222 times.
✗ Branch 6 not taken.
 222 data.length = d(qLength, q);
647 else
648 32 data.length = 0;
649 254 }
650 } // namespace pathProjector
651 } // namespace core
652 } // namespace hpp
653