GCC Code Coverage Report

Directory: ./
File: src/path-projector/progressive.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 126 150 84.0%
Branches: 205 415 49.4%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
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/progressive.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/util/timer.hh>
36
37 // TODO used to access parameters of the problem. We should not do this here.
38 // See todo of pathProjector::Global::create
39 #include <hpp/core/problem.hh>
40 #include <hpp/core/steering-method.hh>
41 #include <hpp/core/steering-method/straight.hh>
42 #include <limits>
43 #include <queue>
44 #include <stack>
45
46 namespace hpp {
47 namespace core {
48 namespace pathProjector {
49 constraints::solver::lineSearch::Constant lineSearch;
50
51 4 ProgressivePtr_t Progressive::create(const DistancePtr_t& distance,
52 const SteeringMethodPtr_t& steeringMethod,
53 value_type step) {
54 4 value_type hessianBound = steeringMethod->problem()
55
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 ->getParameter("PathProjection/HessianBound")
56
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 .floatValue();
57 4 value_type thr_min = steeringMethod->problem()
58
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 ->getParameter("PathProjection/MinimalDist")
59
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 .floatValue();
60 hppDout(info, "Hessian bound is " << hessianBound);
61 hppDout(info, "Min Dist is " << thr_min);
62 return ProgressivePtr_t(
63
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 new Progressive(distance, steeringMethod, step, thr_min, hessianBound));
64 }
65
66 4 ProgressivePtr_t Progressive::create(const ProblemConstPtr_t& problem,
67 const value_type& step) {
68
1/2
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 8 return create(problem->distance(), problem->steeringMethod(), step);
69 }
70
71 4 Progressive::Progressive(const DistancePtr_t& distance,
72 const SteeringMethodPtr_t& steeringMethod,
73 value_type step, value_type thresholdMin,
74 4 value_type hessianBound)
75 : PathProjector(distance, steeringMethod),
76 4 step_(step),
77 4 thresholdMin_(thresholdMin),
78 4 hessianBound_(hessianBound),
79 4 withHessianBound_(hessianBound > 0) {
80 steeringMethod::StraightPtr_t sm(
81 4 HPP_DYNAMIC_PTR_CAST(steeringMethod::Straight, steeringMethod));
82
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (!sm)
83 ✗ throw std::logic_error(
84 "The steering method should be of type"
85 ✗ " Straight.");
86 4 }
87
88 64 bool Progressive::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
5/12
✓ Branch 3 taken 64 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 64 times.
✓ Branch 14 taken 64 times.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 64 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 64 if (!path->constraints() || !path->constraints()->configProjector()) {
94 ✗ proj = path;
95 ✗ success = true;
96 } else {
97
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 success = project(path, proj);
98 }
99 } else {
100 PathVectorPtr_t res =
101 ✗ PathVector::create(pv->outputSize(), pv->outputDerivativeSize());
102 ✗ PathPtr_t part;
103 ✗ success = true;
104 ✗ for (size_t i = 0; i < pv->numberPaths(); i++) {
105 ✗ if (!apply(pv->pathAtRank(i), part)) {
106 // We add the path only if part is not NULL and:
107 // - either its length is not zero,
108 // - or it's not the first one.
109 ✗ if (part && (part->length() > 0 || i == 0)) {
110 ✗ res->appendPath(part);
111 }
112 ✗ success = false;
113 ✗ break;
114 }
115 ✗ res->appendPath(part);
116 }
117 ✗ proj = res;
118 }
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
 64 assert(proj);
120
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());
121
11/20
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 36 times.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 28 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 28 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 28 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 28 times.
✓ Branch 20 taken 28 times.
✓ Branch 21 taken 36 times.
✓ Branch 23 taken 28 times.
✓ Branch 24 taken 36 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());
122 64 return success;
123 64 }
124
125 64 bool Progressive::project(const PathPtr_t& path, PathPtr_t& proj) const {
126 64 ConstraintSetPtr_t constraints = path->constraints();
127
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
 64 if (!constraints) {
128 ✗ proj = path;
129 ✗ return true;
130 }
131
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 const ConfigProjectorPtr_t& cp = constraints->configProjector();
132 64 core::interval_t timeRange = path->timeRange();
133
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 const Configuration_t& q1 = path->initial();
134
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 const Configuration_t& q2 = path->end();
135
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 Configuration_t qtmp = q1;
136 64 const size_t maxDichotomyTries = 10,
137 64 maxPathSplit =
138 64 (size_t)(10 * (timeRange.second - timeRange.first) /
139 64 (double)step_);
140
3/6
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 64 times.
 64 assert(constraints->isSatisfied(q1));
141
3/6
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 64 times.
 64 if (!constraints->isSatisfied(q2)) return false;
142
4/8
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 64 times.
 64 if (!cp || cp->dimension() == 0) {
143 ✗ proj = path;
144 ✗ return true;
145 }
146
147 64 bool pathIsFullyProjected = false;
148
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 std::queue<PathPtr_t> paths;
149
3/6
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 64 times.
✗ Branch 8 not taken.
 128 PathPtr_t toSplit = steer(q1, q2);
150
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 Configuration_t qi(q1.size());
151 64 value_type curStep, curLength, totalLength = 0;
152 64 size_t c = 0;
153 64 const value_type& K = hessianBound_; // upper bound of Hessian
154
4/6
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 32 times.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
 64 if (withHessianBound_) cp->solver().oneStep(qtmp, lineSearch);
155
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 value_type sigma = cp->sigma();
156
157 64 value_type min = std::numeric_limits<value_type>::max(), max = 0;
158
159 while (true) {
160
2/2
✓ Branch 0 taken 294 times.
✓ Branch 1 taken 1650 times.
 1944 const value_type threshold = (withHessianBound_ ? sigma / K : step_);
161 1944 const value_type thr_min = thresholdMin_;
162
163
3/4
✓ Branch 2 taken 1944 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 28 times.
✓ Branch 5 taken 1916 times.
 1944 if (toSplit->length() < threshold) {
164
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 paths.push(toSplit);
165
4/8
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 28 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 28 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 28 times.
 28 assert(constraints->isSatisfied(toSplit->initial()));
166
4/8
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 28 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 28 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 28 times.
 28 assert(constraints->isSatisfied(toSplit->end()));
167
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 28 totalLength += toSplit->length();
168 28 pathIsFullyProjected = true;
169 28 break;
170 }
171
1/2
✓ Branch 2 taken 1916 times.
✗ Branch 3 not taken.
 1916 const Configuration_t& qb = toSplit->initial();
172 1916 curLength = std::numeric_limits<value_type>::max();
173 1916 size_t dicC = 0;
174
175 1916 curStep = threshold - Eigen::NumTraits<value_type>::epsilon();
176
177
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1916 times.
 1916 /* if (withHessianBound_) */ if (threshold < thr_min)
178 ✗ break;
179 // if (withHessianBound_) std::cout << threshold << std::endl;
180
181 /// Find the good length.
182 /// Here, it would be good to have an upper bound of the Hessian
183 /// of the constraint.
184 do {
185
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 2350 times.
 2386 if (dicC >= maxDichotomyTries) break;
186
2/4
✓ Branch 2 taken 2350 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2350 times.
✗ Branch 6 not taken.
 2350 toSplit->eval(qi, curStep);
187
6/12
✓ Branch 2 taken 2350 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2350 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2350 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2350 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2350 times.
✗ Branch 17 not taken.
 2350 if (constraints->apply(qi)) curLength = d(qb, qi);
188 2350 curStep /= 2;
189 2350 dicC++;
190
4/4
✓ Branch 0 taken 110 times.
✓ Branch 1 taken 2240 times.
✓ Branch 2 taken 360 times.
✓ Branch 3 taken 1880 times.
 2350 } while (curLength > threshold || curLength < 1e-3);
191
3/4
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 1880 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1880 times.
 1916 if (dicC >= maxDichotomyTries || c > maxPathSplit) break;
192 // if qi failed to be projected, stop.
193
3/6
✓ Branch 2 taken 1880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1880 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1880 times.
 1880 if (!constraints->isSatisfied(qi)) break;
194
4/8
✓ Branch 1 taken 1880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1880 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1880 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1880 times.
 1880 assert(curLength == d(qb, qi));
195
3/6
✓ Branch 2 taken 1880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1880 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1880 times.
 1880 assert(constraints->isSatisfied(qi));
196
197
2/2
✓ Branch 0 taken 262 times.
✓ Branch 1 taken 1618 times.
 1880 if (withHessianBound_) {
198 /// Update sigma
199
1/2
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
 262 qtmp = qi;
200
2/4
✓ Branch 3 taken 262 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 262 times.
✗ Branch 7 not taken.
 262 cp->solver().oneStep(qtmp, lineSearch);
201
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
 262 sigma = cp->sigma();
202 }
203
204
3/6
✓ Branch 1 taken 1880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1880 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1880 times.
✗ Branch 8 not taken.
 3760 PathPtr_t part = steer(qb, qi);
205
1/2
✓ Branch 1 taken 1880 times.
✗ Branch 2 not taken.
 1880 paths.push(part);
206
4/8
✓ Branch 3 taken 1880 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1880 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1880 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1880 times.
 1880 assert(constraints->isSatisfied(part->initial()));
207
4/8
✓ Branch 3 taken 1880 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1880 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1880 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1880 times.
 1880 assert(constraints->isSatisfied(part->end()));
208
1/2
✓ Branch 2 taken 1880 times.
✗ Branch 3 not taken.
 1880 totalLength += part->length();
209
1/2
✓ Branch 2 taken 1880 times.
✗ Branch 3 not taken.
 1880 min = std::min(min, part->length());
210
1/2
✓ Branch 2 taken 1880 times.
✗ Branch 3 not taken.
 1880 max = std::max(max, part->length());
211
3/6
✓ Branch 1 taken 1880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1880 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1880 times.
✗ Branch 8 not taken.
 1880 toSplit = steer(qi, q2);
212 1880 c++;
213
2/2
✓ Branch 2 taken 1880 times.
✓ Branch 3 taken 36 times.
 3796 }
214 #if HPP_ENABLE_BENCHMARK
215 hppBenchmark("Interpolated path (progressive): "
216 << paths.size() << ", [ " << min << ", "
217 << (paths.empty() ? 0 : totalLength / paths.size()) << ", "
218 << max << "]");
219 #endif
220
2/3
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 56 times.
 64 switch (paths.size()) {
221 8 case 0:
222 8 timeRange = path->timeRange();
223
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 proj = path->extract(std::make_pair(timeRange.first, timeRange.first));
224 8 return false;
225 break;
226 ✗ case 1:
227 ✗ proj = paths.front()->copy(constraints);
228 ✗ break;
229 56 default:
230 InterpolatedPathPtr_t p =
231
1/2
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
 112 InterpolatedPath::create(cp->robot(), q1, paths.back()->end(),
232
3/6
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 56 times.
✗ Branch 13 not taken.
 224 totalLength, path->constraints());
233
1/2
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
 56 value_type t = paths.front()->length();
234
1/2
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
 56 qi = paths.front()->end();
235 56 paths.pop();
236
2/2
✓ Branch 1 taken 1852 times.
✓ Branch 2 taken 56 times.
 1908 while (!paths.empty()) {
237
4/8
✓ Branch 3 taken 1852 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1852 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1852 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 1852 times.
 1852 assert((qi - paths.front()->initial()).isZero());
238
1/2
✓ Branch 3 taken 1852 times.
✗ Branch 4 not taken.
 1852 qi = paths.front()->end();
239
3/6
✓ Branch 4 taken 1852 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1852 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1852 times.
✗ Branch 11 not taken.
 1852 p->insert(t, paths.front()->initial());
240
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1852 times.
 1852 assert(t <= totalLength);
241
1/2
✓ Branch 3 taken 1852 times.
✗ Branch 4 not taken.
 1852 t += paths.front()->length();
242 1852 paths.pop();
243 }
244 56 proj = p;
245 56 break;
246 }
247
5/10
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 56 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 56 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 56 times.
✗ Branch 16 not taken.
 56 if (d(proj->initial(), path->initial()) != 0) {
248 hppDout(error, "proj->initial () = " << proj->initial().transpose());
249 hppDout(error, "path->initial () = " << path->initial().transpose());
250 }
251
6/12
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 56 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 56 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 56 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 56 times.
 56 assert(d(proj->initial(), path->initial()) == 0);
252
16/30
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 28 times.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 28 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 28 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 28 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 28 times.
✓ Branch 21 taken 28 times.
✓ Branch 22 taken 28 times.
✓ Branch 24 taken 28 times.
✓ Branch 25 taken 28 times.
✓ Branch 27 taken 28 times.
✓ Branch 28 taken 28 times.
✓ Branch 30 taken 28 times.
✓ Branch 31 taken 28 times.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 56 if (pathIsFullyProjected && (d(proj->end(), path->end()) != 0)) {
253 hppDout(error, "proj->end () = " << proj->end().transpose());
254 hppDout(error, "path->end () = " << path->end().transpose());
255 hppDout(error,
256 "d (proj->end (), path->end ()) = " << d(proj->end(), path->end()));
257 hppDout(error, "proj->end () - path->end () = "
258 << (proj->end() - path->end()).transpose());
259 }
260
16/30
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 28 times.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 28 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 28 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 28 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 28 times.
✓ Branch 22 taken 28 times.
✓ Branch 23 taken 28 times.
✓ Branch 25 taken 28 times.
✓ Branch 26 taken 28 times.
✓ Branch 28 taken 28 times.
✓ Branch 29 taken 28 times.
✓ Branch 31 taken 28 times.
✓ Branch 32 taken 28 times.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
 56 assert(!pathIsFullyProjected || (d(proj->end(), path->end()) == 0));
261
4/8
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 56 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 56 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 56 times.
 56 assert(proj->constraints()->isSatisfied(proj->end()));
262 #ifndef NDEBUG
263 bool success;
264
1/2
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 112 Configuration_t q = proj->eval(proj->timeRange().second, success);
265
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 56 times.
 56 if (!success) {
266 ✗ q = proj->eval(proj->timeRange().second, success);
267 }
268
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 56 times.
 56 assert(success);
269
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 56 vector_t error;
270
3/6
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 56 times.
 56 assert(proj->constraints()->isSatisfied(q, error));
271 #endif
272 56 return pathIsFullyProjected;
273 64 }
274 } // namespace pathProjector
275 } // namespace core
276 } // namespace hpp
277