Directory: | ./ |
---|---|
File: | src/path-planner/bi-rrt-star.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 8 | 317 | 2.5% |
Branches: | 15 | 704 | 2.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2020 CNRS | ||
3 | // Authors: Joseph Mirabel | ||
4 | // | ||
5 | |||
6 | // Redistribution and use in source and binary forms, with or without | ||
7 | // modification, are permitted provided that the following conditions are | ||
8 | // met: | ||
9 | // | ||
10 | // 1. Redistributions of source code must retain the above copyright | ||
11 | // notice, this list of conditions and the following disclaimer. | ||
12 | // | ||
13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
14 | // notice, this list of conditions and the following disclaimer in the | ||
15 | // documentation and/or other materials provided with the distribution. | ||
16 | // | ||
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
28 | // DAMAGE. | ||
29 | |||
30 | #include <hpp/core/config-validations.hh> | ||
31 | #include <hpp/core/configuration-shooter.hh> | ||
32 | #include <hpp/core/edge.hh> | ||
33 | #include <hpp/core/path-planner/bi-rrt-star.hh> | ||
34 | #include <hpp/core/path-projector.hh> | ||
35 | #include <hpp/core/path-validation-report.hh> | ||
36 | #include <hpp/core/path-validation.hh> | ||
37 | #include <hpp/core/problem.hh> | ||
38 | #include <hpp/core/roadmap.hh> | ||
39 | #include <hpp/pinocchio/configuration.hh> | ||
40 | #include <hpp/pinocchio/liegroup-space.hh> | ||
41 | #include <queue> | ||
42 | |||
43 | namespace hpp { | ||
44 | namespace core { | ||
45 | namespace pathPlanner { | ||
46 | ✗ | BiRrtStarPtr_t BiRrtStar::create(const ProblemConstPtr_t& problem) { | |
47 | ✗ | BiRrtStarPtr_t shPtr(new BiRrtStar(problem)); | |
48 | ✗ | shPtr->init(shPtr); | |
49 | ✗ | return shPtr; | |
50 | } | ||
51 | |||
52 | ✗ | BiRrtStarPtr_t BiRrtStar::createWithRoadmap(const ProblemConstPtr_t& problem, | |
53 | const RoadmapPtr_t& roadmap) { | ||
54 | ✗ | BiRrtStarPtr_t shPtr(new BiRrtStar(problem, roadmap)); | |
55 | ✗ | shPtr->init(shPtr); | |
56 | ✗ | return shPtr; | |
57 | } | ||
58 | |||
59 | ✗ | BiRrtStar::BiRrtStar(const ProblemConstPtr_t& problem) | |
60 | : Parent_t(problem), | ||
61 | ✗ | gamma_(1.), | |
62 | ✗ | extendMaxLength_(1.), | |
63 | ✗ | minimalPathLength_(1e-5), | |
64 | ✗ | toRoot_(2) { | |
65 | ✗ | maxIterations(100); | |
66 | ✗ | stopWhenProblemIsSolved(false); | |
67 | } | ||
68 | |||
69 | ✗ | BiRrtStar::BiRrtStar(const ProblemConstPtr_t& problem, | |
70 | ✗ | const RoadmapPtr_t& roadmap) | |
71 | : Parent_t(problem, roadmap), | ||
72 | ✗ | gamma_(1.), | |
73 | ✗ | extendMaxLength_(1.), | |
74 | ✗ | minimalPathLength_(1e-5), | |
75 | ✗ | toRoot_(2) { | |
76 | ✗ | maxIterations(100); | |
77 | ✗ | stopWhenProblemIsSolved(false); | |
78 | } | ||
79 | |||
80 | ✗ | void BiRrtStar::init(const BiRrtStarWkPtr_t& weak) { | |
81 | ✗ | Parent_t::init(weak); | |
82 | ✗ | weak_ = weak; | |
83 | } | ||
84 | |||
85 | // ----------- Algorithm ---------------------------------------------- // | ||
86 | |||
87 | typedef std::pair<bool, PathPtr_t> ValidatedPath_t; | ||
88 | |||
89 | typedef std::map<NodePtr_t, EdgePtr_t> ParentMap_t; | ||
90 | ✗ | value_type computeCost(const ParentMap_t& map, NodePtr_t n) { | |
91 | typedef ParentMap_t::const_iterator It_t; | ||
92 | ✗ | value_type c = 0; | |
93 | ✗ | for (It_t current = map.find(n); current->second; | |
94 | ✗ | current = map.find(current->second->from())) { | |
95 | ✗ | if (current == map.end()) | |
96 | ✗ | throw std::logic_error( | |
97 | "BiRRT*: This node has no parent. You cannot use BiRRT* from a " | ||
98 | ✗ | "precomputed roadmap."); | |
99 | ✗ | c += current->second->path()->length(); | |
100 | } | ||
101 | ✗ | return c; | |
102 | } | ||
103 | |||
104 | /// \param map the parent map to update | ||
105 | /// \param n a roadmap node | ||
106 | /// \param e a roadmap edge that ends at \c n. | ||
107 | /// \note if \c e is NULL, then \c n is considered a root of the parent | ||
108 | /// map. | ||
109 | ✗ | void setParent(ParentMap_t& map, NodePtr_t n, EdgePtr_t e, bool newNode) { | |
110 | ✗ | if (e) { | |
111 | ✗ | assert(e->to() == n); | |
112 | ✗ | if (map.find(e->from()) == map.end()) | |
113 | ✗ | throw std::logic_error( | |
114 | "BiRRT*: Could not find node from of edge in parent map. You cannot " | ||
115 | ✗ | "use BiRRT* from a precomputed roadmap."); | |
116 | } | ||
117 | ✗ | if (newNode && map.count(n)) | |
118 | ✗ | throw std::logic_error("BiRRT*: This node already exists in the roadmap."); | |
119 | ✗ | map[n] = e; | |
120 | } | ||
121 | |||
122 | struct WeighedNode_t { | ||
123 | NodePtr_t node; | ||
124 | EdgePtr_t parent; | ||
125 | value_type cost; | ||
126 | ✗ | bool operator<(const WeighedNode_t& other) const { return cost < other.cost; } | |
127 | ✗ | WeighedNode_t(NodePtr_t node, EdgePtr_t parent, value_type cost) | |
128 | ✗ | : node(node), parent(parent), cost(cost) {} | |
129 | }; | ||
130 | typedef std::priority_queue<WeighedNode_t> Queue_t; | ||
131 | |||
132 | ✗ | ParentMap_t computeParentMap(NodePtr_t root) { | |
133 | typedef std::map<NodePtr_t, WeighedNode_t> Visited_t; | ||
134 | typedef Visited_t::iterator ItV_t; | ||
135 | ✗ | Visited_t visited; | |
136 | |||
137 | ✗ | Queue_t queue; | |
138 | ✗ | queue.push(WeighedNode_t(root, EdgePtr_t(), 0)); | |
139 | |||
140 | ✗ | while (!queue.empty()) { | |
141 | ✗ | WeighedNode_t current(queue.top()); | |
142 | ✗ | queue.pop(); | |
143 | |||
144 | std::pair<ItV_t, bool> res( | ||
145 | ✗ | visited.insert(std::make_pair(current.node, current))); | |
146 | ✗ | bool addChildren(res.second); | |
147 | ✗ | if (!addChildren) { | |
148 | // Not inserted because already visited. Check if path is better. | ||
149 | // Normally, it is not possible that the children of current are | ||
150 | // visited before all the best way of reaching current is found. | ||
151 | ✗ | if (res.first->second.cost > current.cost) { | |
152 | ✗ | res.first->second.cost = current.cost; | |
153 | ✗ | res.first->second.parent = current.parent; | |
154 | // Re-add to priority queue. | ||
155 | ✗ | addChildren = true; | |
156 | } | ||
157 | } | ||
158 | ✗ | if (addChildren) { | |
159 | ✗ | const Edges_t& edges = current.node->outEdges(); | |
160 | ✗ | for (Edges_t::const_iterator _edge = edges.begin(); _edge != edges.end(); | |
161 | ✗ | ++_edge) { | |
162 | ✗ | EdgePtr_t edge(*_edge); | |
163 | ✗ | queue.push(WeighedNode_t(edge->to(), edge, | |
164 | ✗ | current.cost + edge->path()->length())); | |
165 | } | ||
166 | } | ||
167 | } | ||
168 | |||
169 | ✗ | ParentMap_t result; | |
170 | ✗ | for (ItV_t _v = visited.begin(); _v != visited.end(); ++_v) | |
171 | ✗ | result[_v->first] = _v->second.parent; | |
172 | ✗ | return result; | |
173 | } | ||
174 | |||
175 | ✗ | void BiRrtStar::startSolve() { | |
176 | ✗ | Parent_t::startSolve(); | |
177 | |||
178 | ✗ | if (roadmap()->goalNodes().size() != 1) | |
179 | ✗ | throw std::invalid_argument("there should be only one goal node."); | |
180 | |||
181 | ✗ | extendMaxLength_ = | |
182 | ✗ | problem()->getParameter("BiRRT*/maxStepLength").floatValue(); | |
183 | ✗ | if (extendMaxLength_ <= 0) | |
184 | ✗ | extendMaxLength_ = std::sqrt(problem()->robot()->numberDof()); | |
185 | ✗ | gamma_ = problem()->getParameter("BiRRT*/gamma").floatValue(); | |
186 | ✗ | minimalPathLength_ = | |
187 | ✗ | problem()->getParameter("BiRRT*/minimalPathLength").floatValue(); | |
188 | |||
189 | ✗ | roots_[0] = roadmap()->initNode(); | |
190 | ✗ | roots_[1] = roadmap()->goalNodes()[0]; | |
191 | |||
192 | ✗ | toRoot_[0].clear(); | |
193 | ✗ | toRoot_[1].clear(); | |
194 | ✗ | setParent(toRoot_[0], roots_[0], EdgePtr_t(), true); | |
195 | ✗ | setParent(toRoot_[1], roots_[1], EdgePtr_t(), true); | |
196 | } | ||
197 | |||
198 | ✗ | void BiRrtStar::oneStep() { | |
199 | ✗ | Configuration_t q = sample(); | |
200 | |||
201 | ✗ | if (roadmap()->connectedComponents().size() == 2) { | |
202 | ✗ | if (extend(roots_[0], toRoot_[0], q)) { | |
203 | // in the unlikely event that extend connected the two graphs, | ||
204 | // then one of the connected component is not valid. | ||
205 | ✗ | if (roots_[0]->connectedComponent() == roots_[1]->connectedComponent()) | |
206 | ✗ | return; | |
207 | ✗ | connect(roots_[1], toRoot_[1], q); | |
208 | } | ||
209 | |||
210 | ✗ | std::swap(roots_[0], roots_[1]); | |
211 | ✗ | std::swap(toRoot_[0], toRoot_[1]); | |
212 | } else { | ||
213 | ✗ | if (toRoot_[1].find(roots_[0]) == toRoot_[1].end()) { | |
214 | // Fill parent map | ||
215 | ✗ | toRoot_[0] = computeParentMap(roots_[0]); | |
216 | ✗ | toRoot_[1] = computeParentMap(roots_[1]); | |
217 | } | ||
218 | |||
219 | ✗ | assert(toRoot_[0].size() == toRoot_[1].size()); | |
220 | ✗ | assert(toRoot_[0].size() == roadmap()->nodes().size()); | |
221 | ✗ | improve(q); | |
222 | } | ||
223 | } | ||
224 | |||
225 | ✗ | Configuration_t BiRrtStar::sample() { | |
226 | ✗ | Configuration_t q(problem()->robot()->configSize()); | |
227 | ✗ | ConfigurationShooterPtr_t shooter = problem()->configurationShooter(); | |
228 | |||
229 | ✗ | if (roadmap()->connectedComponents().size() == 1 && | |
230 | ✗ | value_type(rand()) / INT_MAX > (value_type)0.2) { | |
231 | // Compute best path and find one point | ||
232 | typedef ParentMap_t::const_iterator It_t; | ||
233 | ✗ | int nedges = 0; | |
234 | ✗ | for (It_t current = toRoot_[0].find(roots_[1]); current->second; | |
235 | ✗ | current = toRoot_[0].find(current->second->from())) { | |
236 | ✗ | if (current == toRoot_[0].end()) { | |
237 | ✗ | shooter->shoot(q); | |
238 | ✗ | return q; | |
239 | } | ||
240 | ✗ | ++nedges; | |
241 | } | ||
242 | ✗ | if (nedges >= 2) { | |
243 | ✗ | int i = 1 + (rand() % (nedges - 1)); | |
244 | ✗ | It_t edge1, edge0; | |
245 | ✗ | for (edge1 = toRoot_[0].find(roots_[1]); i != 1; | |
246 | ✗ | edge1 = toRoot_[0].find(edge1->second->from())) | |
247 | ✗ | --i; | |
248 | ✗ | edge0 = toRoot_[0].find(edge1->second->from()); | |
249 | ✗ | if (edge0->second->to() != edge1->second->from()) | |
250 | ✗ | throw std::logic_error("BiRRT*: wrong parent map."); | |
251 | |||
252 | // qm = (q0 + q2) / 2 | ||
253 | ✗ | pinocchio::interpolate(problem()->robot(), | |
254 | ✗ | edge0->second->from()->configuration(), | |
255 | ✗ | edge1->second->to()->configuration(), 0.5, q); | |
256 | |||
257 | // q = q1 + alpha * (qm - q1) | ||
258 | ✗ | vector_t v(problem()->robot()->numberDof()); | |
259 | ✗ | pinocchio::difference(problem()->robot(), q, | |
260 | ✗ | edge0->second->to()->configuration(), v); | |
261 | ✗ | v.normalize(); | |
262 | |||
263 | ✗ | value_type l(extendMaxLength_ - value_type(rand()) * extendMaxLength_ / | |
264 | ✗ | (10 * (value_type)INT_MAX)); | |
265 | ✗ | pinocchio::integrate(problem()->robot(), | |
266 | ✗ | edge0->second->to()->configuration(), l * v, q); | |
267 | ✗ | return q; | |
268 | } | ||
269 | } | ||
270 | |||
271 | ✗ | shooter->shoot(q); | |
272 | ✗ | return q; | |
273 | } | ||
274 | |||
275 | ✗ | bool validate(const ProblemConstPtr_t& problem, const PathPtr_t& path) { | |
276 | ✗ | PathPtr_t validPart; | |
277 | ✗ | PathValidationReportPtr_t report; | |
278 | ✗ | return problem->pathValidation()->validate(path, false, validPart, report); | |
279 | } | ||
280 | |||
281 | ✗ | bool BiRrtStar::buildPath(const Configuration_t& q0, const Configuration_t& q1, | |
282 | value_type maxLength, bool validatePath, | ||
283 | PathPtr_t& result) { | ||
284 | ✗ | result.reset(); | |
285 | ✗ | bool allValid = true; | |
286 | ✗ | PathPtr_t path = problem()->steeringMethod()->steer(q0, q1); | |
287 | ✗ | if (!path) { | |
288 | ✗ | result = path; | |
289 | ✗ | return false; | |
290 | } | ||
291 | ✗ | if (problem()->pathProjector()) { // path projection | |
292 | ✗ | PathPtr_t projected; | |
293 | ✗ | allValid = problem()->pathProjector()->apply(path, projected); | |
294 | ✗ | if (!projected) { | |
295 | ✗ | result.reset(); | |
296 | ✗ | return false; | |
297 | } | ||
298 | ✗ | path = projected; | |
299 | } | ||
300 | |||
301 | ✗ | if (maxLength > 0 && path->length() > maxLength) { | |
302 | ✗ | const interval_t& I = path->timeRange(); | |
303 | ✗ | path = path->extract(I.first, I.first + maxLength); | |
304 | } | ||
305 | |||
306 | ✗ | if (!validatePath) { | |
307 | ✗ | result = path; | |
308 | ✗ | assert(!allValid || result->end() == q1); | |
309 | ✗ | return allValid; | |
310 | } | ||
311 | |||
312 | ✗ | PathPtr_t validPart; | |
313 | ✗ | PathValidationReportPtr_t report; | |
314 | ✗ | allValid &= | |
315 | ✗ | problem()->pathValidation()->validate(path, false, validPart, report); | |
316 | ✗ | result = validPart; | |
317 | ✗ | assert(!allValid || result->end() == q1); | |
318 | ✗ | return allValid; | |
319 | } | ||
320 | |||
321 | ✗ | bool BiRrtStar::extend(NodePtr_t target, ParentMap_t& parentMap, | |
322 | Configuration_t& q) { | ||
323 | ✗ | ConnectedComponentPtr_t cc(target->connectedComponent()); | |
324 | |||
325 | value_type dist; | ||
326 | ✗ | NodePtr_t near = roadmap()->nearestNode(q, cc, dist); | |
327 | ✗ | if (dist < 1e-16) return false; | |
328 | |||
329 | ✗ | if (problem()->constraints()) { | |
330 | // if the random configuration (alias q) is not successfully projected | ||
331 | // on the constraints, replace q by the middle configuration between | ||
332 | // q_near and q. | ||
333 | // The goal is to improve the success rate of the method for | ||
334 | // constraints that have a small basin of attraction | ||
335 | ✗ | int i = 0; | |
336 | ✗ | while (!problem()->constraints()->apply(q) && i < 10) { | |
337 | ✗ | problem()->robot()->configSpace()->interpolate(near->configuration(), q, | |
338 | .5, q); | ||
339 | ✗ | ++i; | |
340 | } | ||
341 | } | ||
342 | |||
343 | ✗ | PathPtr_t path; | |
344 | ✗ | buildPath(near->configuration(), q, extendMaxLength_, true, path); | |
345 | ✗ | if (!path || path->length() < minimalPathLength_) return false; | |
346 | ✗ | q = path->end(); | |
347 | ✗ | assert(path->end() == q); | |
348 | |||
349 | ✗ | value_type n((value_type)roadmap()->nodes().size()); | |
350 | ✗ | NodeVector_t nearNodes = roadmap()->nodesWithinBall( | |
351 | q, cc, | ||
352 | ✗ | std::min( | |
353 | ✗ | gamma_ * std::pow(std::log(n) / n, | |
354 | ✗ | 1. / (value_type)problem()->robot()->numberDof()), | |
355 | ✗ | extendMaxLength_)); | |
356 | |||
357 | ✗ | value_type cost_q(computeCost(parentMap, near) + path->length()); | |
358 | ✗ | std::vector<ValidatedPath_t> paths; | |
359 | ✗ | paths.reserve(nearNodes.size()); | |
360 | ✗ | for (NodeVector_t::const_iterator _near = nearNodes.begin(); | |
361 | ✗ | _near != nearNodes.end(); ++_near) { | |
362 | ✗ | PathPtr_t near2new; | |
363 | ✗ | assert(!near2new); | |
364 | ✗ | if (*_near == near) { | |
365 | ✗ | assert(path->end() == q); | |
366 | ✗ | near2new = path; | |
367 | ✗ | assert(near2new->end() == q); | |
368 | ✗ | paths.push_back(ValidatedPath_t(true, near2new)); | |
369 | ✗ | continue; | |
370 | ✗ | } else if (buildPath((*_near)->configuration(), q, -1, false, near2new)) { | |
371 | ✗ | paths.push_back(ValidatedPath_t(false, near2new)); | |
372 | ✗ | assert(near2new->end() == q); | |
373 | } else { | ||
374 | ✗ | paths.push_back(ValidatedPath_t(false, PathPtr_t())); | |
375 | ✗ | continue; | |
376 | } | ||
377 | ✗ | assert(near2new->end() == q); | |
378 | |||
379 | ✗ | if (paths.size() > 0) { | |
380 | ✗ | value_type _cost_q = computeCost(parentMap, *_near) + near2new->length(); | |
381 | ✗ | if (_cost_q < cost_q) { | |
382 | ✗ | paths.back().first = true; | |
383 | // Run path validation | ||
384 | ✗ | if (validate(problem(), near2new)) { | |
385 | // Path is valid and shorter. | ||
386 | ✗ | cost_q = _cost_q; | |
387 | ✗ | near = *_near; | |
388 | ✗ | path = near2new; | |
389 | } else | ||
390 | ✗ | paths.back().second.reset(); | |
391 | } | ||
392 | } | ||
393 | } | ||
394 | |||
395 | ✗ | NodePtr_t nnew = roadmap()->addNode(q); | |
396 | ✗ | EdgePtr_t edge = roadmap()->addEdge(near, nnew, path); | |
397 | ✗ | roadmap()->addEdge(nnew, near, path->reverse()); | |
398 | ✗ | assert(parentMap.find(near) != parentMap.end()); | |
399 | ✗ | if (parentMap.count(nnew)) return false; | |
400 | ✗ | setParent(parentMap, nnew, edge, true); | |
401 | ✗ | assert(paths.size() == nearNodes.size()); | |
402 | ✗ | for (std::size_t i = 0; i < nearNodes.size(); ++i) { | |
403 | ✗ | if (nearNodes[i] == near || !paths[i].second) continue; | |
404 | |||
405 | ✗ | value_type cost_q_near = cost_q + paths[i].second->length(); | |
406 | ✗ | if (cost_q_near < computeCost(parentMap, nearNodes[i])) { | |
407 | ✗ | bool pathValid = paths[i].first; | |
408 | ✗ | if (!pathValid) // If path validation has not been run | |
409 | ✗ | pathValid = validate(problem(), paths[i].second); | |
410 | ✗ | if (pathValid) { | |
411 | ✗ | roadmap()->addEdge(nearNodes[i], nnew, paths[i].second); | |
412 | edge = | ||
413 | ✗ | roadmap()->addEdge(nnew, nearNodes[i], paths[i].second->reverse()); | |
414 | ✗ | setParent(parentMap, nearNodes[i], edge, false); | |
415 | } | ||
416 | } | ||
417 | } | ||
418 | ✗ | return true; | |
419 | } | ||
420 | |||
421 | ✗ | bool BiRrtStar::connect(NodePtr_t b, ParentMap_t& parentMap, | |
422 | const Configuration_t& q) { | ||
423 | ✗ | Configuration_t qnew; | |
424 | // while extend did not reach q | ||
425 | ✗ | while (roadmap()->connectedComponents().size() == 2) { | |
426 | ✗ | qnew = q; | |
427 | ✗ | if (!extend(b, parentMap, qnew)) // extend failed | |
428 | ✗ | return false; | |
429 | } | ||
430 | ✗ | return true; | |
431 | } | ||
432 | |||
433 | ✗ | bool BiRrtStar::improve(const Configuration_t& q) { | |
434 | value_type dist; | ||
435 | ✗ | const NodePtr_t nearQ = roadmap()->nearestNode(q, dist); | |
436 | ✗ | if (dist < 1e-16) return false; | |
437 | |||
438 | ✗ | PathPtr_t nearQ_qnew; | |
439 | ✗ | buildPath(nearQ->configuration(), q, extendMaxLength_, true, nearQ_qnew); | |
440 | ✗ | if (!nearQ_qnew || nearQ_qnew->length() < minimalPathLength_) return false; | |
441 | |||
442 | ✗ | const Configuration_t qnew(nearQ_qnew->end()); | |
443 | |||
444 | ✗ | const value_type n((value_type)roadmap()->nodes().size()); | |
445 | ✗ | NodeVector_t nearNodes = roadmap()->nodesWithinBall( | |
446 | ✗ | qnew, roots_[0]->connectedComponent(), | |
447 | ✗ | std::min( | |
448 | ✗ | gamma_ * std::pow(std::log(n) / n, | |
449 | ✗ | 1. / (value_type)problem()->robot()->numberDof()), | |
450 | ✗ | extendMaxLength_)); | |
451 | |||
452 | ✗ | const NodePtr_t nnew = roadmap()->addNode(qnew); | |
453 | |||
454 | ✗ | std::vector<ValidatedPath_t> paths; | |
455 | ✗ | paths.reserve(nearNodes.size()); | |
456 | |||
457 | ✗ | for (int k = 0; k < 2; ++k) { | |
458 | ✗ | paths.clear(); | |
459 | |||
460 | ✗ | NodePtr_t bestParent(nearQ); | |
461 | ✗ | PathPtr_t best_qnew(nearQ_qnew); | |
462 | ✗ | value_type cost_q(computeCost(toRoot_[k], nearQ) + nearQ_qnew->length()); | |
463 | |||
464 | ✗ | for (NodeVector_t::const_iterator _near = nearNodes.begin(); | |
465 | ✗ | _near != nearNodes.end(); ++_near) { | |
466 | ✗ | PathPtr_t near2new; | |
467 | ✗ | if (*_near == nearQ) { | |
468 | ✗ | near2new = nearQ_qnew; | |
469 | ✗ | assert(near2new->end() == qnew); | |
470 | ✗ | paths.push_back(ValidatedPath_t(true, near2new)); | |
471 | ✗ | continue; | |
472 | ✗ | } else if (buildPath((*_near)->configuration(), qnew, -1, false, | |
473 | near2new)) { | ||
474 | ✗ | paths.push_back(ValidatedPath_t(false, near2new)); | |
475 | ✗ | assert(near2new->end() == qnew); | |
476 | } else { | ||
477 | ✗ | paths.push_back(ValidatedPath_t(false, PathPtr_t())); | |
478 | ✗ | continue; | |
479 | } | ||
480 | ✗ | if (paths.size() > 0) { | |
481 | value_type _cost_q = | ||
482 | ✗ | computeCost(toRoot_[k], *_near) + near2new->length(); | |
483 | ✗ | if (_cost_q < cost_q) { | |
484 | ✗ | paths.back().first = true; | |
485 | // Run path validation | ||
486 | ✗ | if (validate(problem(), near2new)) { | |
487 | // Path is valid and shorter. | ||
488 | ✗ | cost_q = _cost_q; | |
489 | ✗ | bestParent = *_near; | |
490 | ✗ | best_qnew = near2new; | |
491 | } else | ||
492 | ✗ | paths.back().second.reset(); | |
493 | } | ||
494 | } | ||
495 | } | ||
496 | |||
497 | ✗ | EdgePtr_t edge = roadmap()->addEdge(bestParent, nnew, best_qnew); | |
498 | ✗ | roadmap()->addEdge(nnew, bestParent, best_qnew->reverse()); | |
499 | ✗ | assert(toRoot_[k].find(bestParent) != toRoot_[k].end()); | |
500 | ✗ | if (toRoot_[k].count(nnew)) continue; | |
501 | ✗ | setParent(toRoot_[k], nnew, edge, true); | |
502 | |||
503 | ✗ | assert(paths.size() == nearNodes.size()); | |
504 | ✗ | for (std::size_t i = 0; i < nearNodes.size(); ++i) { | |
505 | ✗ | if (nearNodes[i] == bestParent || !paths[i].second) continue; | |
506 | |||
507 | ✗ | value_type cost_q_near = cost_q + paths[i].second->length(); | |
508 | ✗ | if (cost_q_near < computeCost(toRoot_[k], nearNodes[i])) { | |
509 | ✗ | bool pathValid = paths[i].first; | |
510 | ✗ | if (!pathValid) // If path validation has not been run | |
511 | ✗ | pathValid = validate(problem(), paths[i].second); | |
512 | ✗ | if (pathValid) { | |
513 | ✗ | roadmap()->addEdge(nearNodes[i], nnew, paths[i].second); | |
514 | ✗ | edge = roadmap()->addEdge(nnew, nearNodes[i], | |
515 | ✗ | paths[i].second->reverse()); | |
516 | ✗ | assert(toRoot_[k].find(nnew) != toRoot_[k].end()); | |
517 | ✗ | setParent(toRoot_[k], nearNodes[i], edge, false); | |
518 | } | ||
519 | } | ||
520 | } | ||
521 | } | ||
522 | ✗ | return true; | |
523 | } | ||
524 | |||
525 | // ----------- Declare parameters ------------------------------------- // | ||
526 | |||
527 | 18 | HPP_START_PARAMETER_DECLARATION(BiRrtStar) | |
528 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
529 | Parameter::FLOAT, "BiRRT*/maxStepLength", | ||
530 | "The maximum step length when extending. If negative, uses sqrt(dimension)", | ||
531 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | Parameter(-1.))); |
532 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription(Parameter::FLOAT, "BiRRT*/gamma", |
533 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "", Parameter(1.))); |
534 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
535 | Parameter::FLOAT, "BiRRT*/minimalPathLength", | ||
536 | "The minimum length between 2 configurations in the roadmap.", | ||
537 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | Parameter(1e-4))); |
538 | 18 | HPP_END_PARAMETER_DECLARATION(BiRrtStar) | |
539 | } // namespace pathPlanner | ||
540 | } // namespace core | ||
541 | } // namespace hpp | ||
542 |