Directory: | ./ |
---|---|
File: | src/path-projector/progressive.cc |
Date: | 2024-08-10 11:29:48 |
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 |