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