Loading...
Searching...
No Matches
XXL.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#include <queue>
38#include "ompl/geometric/planners/xxl/XXL.h"
39#include "ompl/base/goals/GoalSampleableRegion.h"
40#include "ompl/base/goals/GoalLazySamples.h"
41#include "ompl/tools/config/MagicConstants.h"
42#include "ompl/tools/config/SelfConfig.h"
43#include "ompl/util/Exception.h"
44
45ompl::geometric::XXL::XXL(const ompl::base::SpaceInformationPtr &si) : base::Planner(si, "XXL")
46{
47 xstate_ = si_->allocState();
48 Planner::declareParam<double>("rand_walk_rate", this, &XXL::setRandWalkRate, &XXL::getRandWalkRate, "0.:.05:1.");
49}
50
51ompl::geometric::XXL::XXL(const ompl::base::SpaceInformationPtr &si, const XXLDecompositionPtr &decomp)
52 : base::Planner(si, "XXL")
53{
54 xstate_ = si_->allocState();
55 setDecomposition(decomp);
56 Planner::declareParam<double>("rand_walk_rate", this, &XXL::setRandWalkRate, &XXL::getRandWalkRate, "0.:.05:1.");
57}
58
59ompl::geometric::XXL::~XXL()
60{
61 freeMemory();
62 si_->freeState(xstate_);
63}
64
66{
67 Planner::clear();
68 freeMemory();
69
70 if (decomposition_)
71 {
72 // Layer storage
73 // TODO: Implement a clear function to avoid memory reallocation
74 if (topLayer_)
75 delete topLayer_;
76 topLayer_ =
77 new Layer(-1, decomposition_->getNumRegions(), 0, nullptr); // top layer get -1 as id and a nullptr parent
78 allocateLayers(topLayer_);
79 }
80
81 motions_.clear();
82 startMotions_.clear();
83 goalMotions_.clear();
84 goalCount_.clear();
85
86 realGraph_.clear();
87 lazyGraph_.clear();
88
89 statesConnectedInRealGraph_ = 0;
90 kill_ = false;
91}
92
93void ompl::geometric::XXL::freeMemory()
94{
95 for (auto &motion : motions_)
96 {
97 si_->freeState(motion->state);
98 delete motion;
99 }
100 motions_.clear();
101
102 if (topLayer_)
103 {
104 delete topLayer_;
105 topLayer_ = nullptr;
106 }
107}
108
110{
111 if (!decomposition_)
112 {
113 OMPL_ERROR("%s: Decomposition is not set. Cannot continue setup.", getName().c_str());
114 throw;
115 }
116
117 Planner::setup();
118
119 sampler_ = si_->allocStateSampler();
120
121 statesConnectedInRealGraph_ = 0;
122
123 maxGoalStatesPerRegion_ = 100; // todo: make this a parameter
124 maxGoalStates_ = 500; // todo: Make this a parameter
125
126 kill_ = false;
127
128 // Layer storage - Allocating new storage
129 if (topLayer_)
130 delete topLayer_;
131 topLayer_ = new Layer(-1, decomposition_->getNumRegions(), 0, nullptr);
132 allocateLayers(topLayer_);
133
134 if (rand_walk_rate_ < 0.0 || rand_walk_rate_ > 1.0)
135 {
136 rand_walk_rate_ = 0.05;
137 }
138}
139
140void ompl::geometric::XXL::setDecomposition(const XXLDecompositionPtr &decomp)
141{
142 decomposition_ = decomp;
143 predecessors_.resize(decomposition_->getNumRegions());
144 closedList_.resize(decomposition_->getNumRegions());
145
146 if (decomposition_->numLayers() < 1)
147 throw ompl::Exception("Decomposition must have at least one layer of projection");
148}
149
150// TODO: Do this dynamically. In other words, only allocate a layer/region when we use it
151void ompl::geometric::XXL::allocateLayers(Layer *layer)
152{
153 if (!layer)
154 return;
155
156 if (layer->getLevel() < decomposition_->numLayers() - 1)
157 {
158 layer->allocateSublayers();
159 if (layer->getLevel() + 1 < decomposition_->numLayers() - 1)
160 for (size_t i = 0; i < layer->numRegions(); ++i)
161 allocateLayers(layer->getSublayer(i));
162 }
163}
164
165void ompl::geometric::XXL::updateRegionConnectivity(const Motion *m1, const Motion *m2, int layer)
166{
167 if (layer >= decomposition_->numLayers() || layer < 0) // recursive stop
168 return;
169
170 const ompl::base::StateSpacePtr &ss = si_->getStateSpace();
171
172 // check the projections of m1 and m2 in all layers at or below layer. If any one level isn't adjacent, we need to
173 // add states
174 std::vector<base::State *> intermediateStates;
175 std::vector<std::vector<int>> intProjections;
176 double dt = 0.05;
177 for (int i = layer; i < decomposition_->numLayers(); ++i)
178 {
179 if (m1->levels[i] != m2->levels[i])
180 {
181 std::vector<int> nbrs;
182 decomposition_->getNeighborhood(m1->levels[i], nbrs);
183 bool isNeighbor = false;
184 for (size_t j = 0; j < nbrs.size() && !isNeighbor; ++j)
185 isNeighbor = nbrs[j] == m2->levels[i];
186
187 if (!isNeighbor) // interpolate states between the motions
188 {
189 double t = dt;
190 while (t < (1.0 - dt / 2.0))
191 {
192 std::vector<int> projection;
193 ss->interpolate(m1->state, m2->state, t, xstate_);
194 decomposition_->project(xstate_, projection);
195
196 intermediateStates.push_back(si_->cloneState(xstate_));
197 intProjections.push_back(projection);
198
199 t += dt;
200 }
201 break;
202 }
203 }
204 }
205
206 if (intermediateStates.size())
207 {
208 std::vector<int> gaps;
209
210 // see if we still need to fill any gaps
211 for (size_t i = 1; i < intProjections.size(); ++i) // state
212 {
213 for (int j = layer; j < (int)intProjections[i].size(); ++j) // layer
214 {
215 if (intProjections[i - 1][j] != intProjections[i][j])
216 {
217 std::vector<int> nbrs;
218 decomposition_->getNeighborhood(intProjections[i - 1][j], nbrs);
219 bool isNeighbor = false;
220 for (size_t k = 0; k < nbrs.size() && !isNeighbor; ++k)
221 isNeighbor = nbrs[k] == intProjections[i][j];
222
223 if (!isNeighbor)
224 {
225 gaps.push_back((int)i);
226 break;
227 }
228 }
229 }
230 }
231
232 std::vector<std::vector<base::State *>> gapStates;
233 std::vector<std::vector<std::vector<int>>> gapProjections;
234
235 for (size_t i = 0; i < gaps.size(); ++i)
236 {
237 std::vector<base::State *> gapState;
238 std::vector<std::vector<int>> gapProj;
239
240 double t = (gaps[i]) * dt;
241 double end = t + dt;
242 double newdt = dt * dt;
243 while (t < end)
244 {
245 std::vector<int> projection;
246 ss->interpolate(m1->state, m2->state, t, xstate_);
247 decomposition_->project(xstate_, projection);
248
249 gapState.push_back(si_->cloneState(xstate_));
250 gapProj.push_back(projection);
251 t += newdt;
252 }
253
254 gapStates.push_back(gapState);
255 gapProjections.push_back(gapProj);
256 }
257
258 // add the gaps
259 int offset = 0;
260 for (size_t i = 0; i < gaps.size(); ++i)
261 {
262 auto start = intermediateStates.begin() + gaps[i] + offset; // insert before this position
263 intermediateStates.insert(start, gapStates[i].begin(), gapStates[i].end());
264
265 auto startproj = intProjections.begin() + gaps[i] + offset;
266 intProjections.insert(startproj, gapProjections[i].begin(), gapProjections[i].end());
267
268 offset += gapStates[i].size();
269 }
270 }
271
272 // Now, add the states where we jump regions
273 const Motion *prev = m1;
274 for (size_t i = 0; i < intermediateStates.size(); ++i)
275 {
276 for (int j = layer; j < decomposition_->numLayers(); ++j)
277 {
278 if (prev->levels[j] != intProjections[i][j])
279 {
280 // add the new state
281 int id = addState(intermediateStates[i]);
282 const Motion *newMotion = motions_[id];
283
284 // This state is placed in the real graph too
285 Layer *l = topLayer_;
286 for (size_t k = 0; k < newMotion->levels.size(); ++k)
287 {
288 l->getRegion(newMotion->levels[k]).motionsInTree.push_back(newMotion->index);
289 if (l->hasSublayers())
290 l = l->getSublayer(newMotion->levels[k]);
291 }
292
293 // Connect the new state to prev
294 lazyGraph_.removeEdge(prev->index, newMotion->index); // remove the edge so we never try this again
295 double weight = si_->distance(prev->state, newMotion->state);
296 realGraph_.addEdge(prev->index, newMotion->index, weight);
297 if (realGraph_.numNeighbors(prev->index) == 1)
298 statesConnectedInRealGraph_++;
299 if (realGraph_.numNeighbors(newMotion->index) == 1)
300 statesConnectedInRealGraph_++;
301
302 Layer *l1 = getLayer(prev->levels, j);
303 Layer *l2 = getLayer(newMotion->levels, j);
304
305 l1->getRegionGraph().addEdge(prev->levels[j], newMotion->levels[j]);
306 if (l1 != l2)
307 l2->getRegionGraph().addEdge(prev->levels[j], newMotion->levels[j]);
308
309 prev = newMotion;
310 }
311 }
312 }
313}
314
315ompl::geometric::XXL::Layer *ompl::geometric::XXL::getLayer(const std::vector<int> &regions, int layer)
316{
317 if (layer >= decomposition_->numLayers())
318 throw ompl::Exception("Requested invalid layer");
319
320 Layer *l = topLayer_;
321 for (int i = 0; i < layer; ++i)
322 l = l->getSublayer(regions[i]);
323 return l;
324}
325
326int ompl::geometric::XXL::addThisState(base::State *state)
327{
328 auto motion = new Motion();
329 motion->state = state;
330 decomposition_->project(motion->state, motion->levels);
331
332 // Adding state to both lazy graph and the real graph
333 motion->index = realGraph_.addVertex();
334 if (lazyGraph_.addVertex() != motion->index)
335 throw;
336
337 // Adding lazy edges to all states in the neighborhood
338 std::vector<int> nbrs;
339 decomposition_->getNeighbors(motion->levels[0], nbrs);
340 nbrs.push_back(motion->levels[0]); // add this region
341 for (size_t i = 0; i < nbrs.size(); ++i)
342 {
343 const std::vector<int> &nbrMotions = topLayer_->getRegion(nbrs[i]).allMotions;
344 for (size_t j = 0; j < nbrMotions.size(); ++j)
345 {
346 const Motion *nbrMotion = motions_[nbrMotions[j]];
347 double weight = si_->distance(motion->state, nbrMotion->state);
348 bool added = lazyGraph_.addEdge(motion->index, nbrMotion->index, weight);
349 if (!added)
350 throw std::runtime_error("Failed to add edge to graph");
351 }
352 }
353
354 // Inserting motion into the list
355 motions_.push_back(motion);
356 assert(motion->index == motions_.size() - 1);
357
358 // Updating sublayer storage with this motion
359 Layer *layer = topLayer_;
360 for (size_t i = 0; i < motion->levels.size(); ++i)
361 {
362 layer->getRegion(motion->levels[i]).allMotions.push_back(motion->index);
363 if (layer->hasSublayers())
364 layer = layer->getSublayer(motion->levels[i]);
365 else if (i != motion->levels.size() - 1)
366 throw;
367 }
368
369 return motion->index;
370}
371
372int ompl::geometric::XXL::addState(const base::State *state)
373{
374 return addThisState(si_->cloneState(state));
375}
376
377int ompl::geometric::XXL::addGoalState(const base::State *state)
378{
379 std::vector<int> proj;
380 decomposition_->project(state, proj);
381 auto it = goalCount_.find(proj);
382 if (it != goalCount_.end() && it->second >= (int)maxGoalStatesPerRegion_)
383 return -1;
384 else
385 {
386 if (it != goalCount_.end())
387 goalCount_[proj]++;
388 else
389 goalCount_[proj] = 1;
390 }
391
392 int id = addState(state); // adds state to the graph (no neighbors) and updates statistics
393 assert(id >= 0);
394
395 const Motion *motion = motions_[id];
396 goalMotions_.push_back(motion->index);
397 // if (goalCount_[proj] == maxGoalStatesPerRegion_ || goalMotions_.size() == 1) // some output so we know things are
398 // running
399 // OMPL_INFORM("%s: Goal state count: %lu", getName().c_str(), goalMotions_.size());
400
401 // Goal states are always placed in the real graph
402 // Updating sublayer storage with this motion
403 Layer *layer = topLayer_;
404 for (size_t i = 0; i < motion->levels.size(); ++i)
405 {
406 layer->addGoalState(motion->levels[i], id);
407
408 layer->getRegion(motion->levels[i]).motionsInTree.push_back(motion->index);
409 if (layer->hasSublayers())
410 layer = layer->getSublayer(motion->levels[i]);
411 else if (i != motion->levels.size() - 1)
412 throw;
413 }
414
415 return motion->index;
416}
417
418int ompl::geometric::XXL::addStartState(const base::State *state)
419{
420 int id = addState(state); // adds state to the graph (no neighbors) and updates statistics
421 assert(id >= 0);
422
423 const Motion *motion = motions_[id];
424 startMotions_.push_back(motion->index);
425
426 // Start states are always placed in the real graph
427 // Updating sublayer storage with this motion
428 Layer *layer = topLayer_;
429 for (size_t i = 0; i < motion->levels.size(); ++i)
430 {
431 layer->getRegion(motion->levels[i]).motionsInTree.push_back(motion->index);
432 if (layer->hasSublayers())
433 layer = layer->getSublayer(motion->levels[i]);
434 }
435
436 return motion->index;
437}
438
439void ompl::geometric::XXL::updateRegionProperties(Layer *layer, int reg)
440{
441 // SIMPLIFY
442 // Initialize all weights to 0.5
443 // Nudge weights based on:
444 // - States in region
445 // - Edges in region
446 // - Number of appearances in the lead
447
448 const Region &region = layer->getRegion(reg);
449 double &weight = layer->getWeights()[reg];
450
451 int statesInRegion = (int)region.allMotions.size();
452 int statesInLayer = (layer->getLevel() == 0 ? (int)motions_.size() :
453 (int)layer->getParent()->getRegion(layer->getID()).allMotions.size());
454 double stateRatio = statesInRegion / (double)statesInLayer;
455
456 int connectedStatesInRegion = (int)region.motionsInTree.size();
457
458 double connStateRatio = (statesInRegion > 0 ? connectedStatesInRegion / (double)statesInRegion : 0.0);
459 double leadRatio = (layer->numLeads() ? layer->leadAppearances(reg) / (double)layer->numLeads() : 0.0);
460
461 double newWeight = (exp(-stateRatio) * exp(-10.0 * connStateRatio)) + (1.0 - exp(-leadRatio));
462
463 // nudge the weight in the direction of new weight by the given factor
464 double nudgeFactor = 0.1;
465 weight += (newWeight - weight) * nudgeFactor;
466
467 // clamp weight between 0 and 1
468 weight = std::max(0.0, std::min(1.0, weight));
469}
470
471void ompl::geometric::XXL::updateRegionProperties(const std::vector<int> &regions)
472{
473 Layer *layer = getLayer(regions, 0);
474 for (size_t i = 0; i < regions.size(); ++i)
475 {
476 updateRegionProperties(layer, regions[i]);
477
478 if (layer->hasSublayers())
479 layer = layer->getSublayer(regions[i]);
480 else if (i != regions.size() - 1)
481 throw;
482 }
483}
484
485void ompl::geometric::XXL::sampleStates(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
486{
487 std::vector<int> newStates;
488 if (layer->getID() == -1) // top layer
489 {
490 // Just sample uniformly until ptc is triggered
491 while (!ptc)
492 {
493 sampler_->sampleUniform(xstate_);
494 if (si_->isValid(xstate_))
495 newStates.push_back(addState(xstate_));
496 }
497 }
498 else
499 {
500 const std::vector<int> &states = layer->getParent()->getRegion(layer->getID()).allMotions;
501 if (states.size() == 0)
502 throw ompl::Exception("Cannot sample states in a subregion where there are no states");
503
504 while (!ptc)
505 {
506 // pick a random state in the layer as the seed
507 const Motion *seedMotion = motions_[states[rng_.uniformInt(0, states.size() - 1)]];
508 const base::State *seedState = seedMotion->state;
509 // Returns a valid state
510 if (decomposition_->sampleFromRegion(layer->getID(), xstate_, seedState, layer->getLevel() - 1))
511 {
512 int idx = addState(xstate_);
513 newStates.push_back(idx);
514 }
515 }
516 }
517
518 // update weights of the regions we added states to
519 for (size_t i = 0; i < newStates.size(); ++i)
520 updateRegionProperties(motions_[newStates[i]]->levels);
521}
522
523bool ompl::geometric::XXL::sampleAlongLead(Layer *layer, const std::vector<int> &lead,
524 const ompl::base::PlannerTerminationCondition &ptc)
525{
526 // try to put a valid state in every region, a chicken for every pot
527 int numSampleAttempts = 10;
528 std::vector<int> newStates;
529
530 if (lead.size() == 1) // always sample if lead is just one cell
531 {
532 std::vector<int> nbrs;
533 decomposition_->getNeighborhood(lead[0], nbrs);
534 nbrs.push_back(lead[0]);
535 for (int j = 0; j < numSampleAttempts; ++j)
536 {
537 const ompl::base::State *seed = nullptr;
538 // if (layer->getLevel() > 0) // must find a seed. Try states in the neighborhood. There probably are some
539 {
540 rng_.shuffle(nbrs.begin(), nbrs.end());
541 for (size_t k = 0; k < nbrs.size() && !seed; ++k)
542 {
543 const Region &nbrReg = layer->getRegion(nbrs[k]);
544 if (nbrReg.allMotions.size() > 0) // just pick any old state
545 seed = motions_[nbrReg.allMotions[rng_.uniformInt(0, nbrReg.allMotions.size() - 1)]]->state;
546 }
547 if (!seed)
548 continue;
549 }
550
551 if (decomposition_->sampleFromRegion(lead[0], xstate_, seed, layer->getLevel()))
552 newStates.push_back(addState(xstate_));
553 }
554 }
555 else // normal lead with at least two cells
556 {
557 // If any region has relatively few states, sample more there
558 int maxStateCount = 0;
559 for (size_t i = 0; i < lead.size() && !ptc; ++i)
560 {
561 const Region &region = layer->getRegion(lead[i]);
562 if ((int)region.allMotions.size() > maxStateCount)
563 maxStateCount = region.allMotions.size();
564 }
565
566 for (size_t i = 0; i < lead.size() && !ptc; ++i)
567 {
568 const Region &region = layer->getRegion(lead[i]);
569 double p = 1.0 - (region.allMotions.size() / (double)maxStateCount);
570 if (rng_.uniform01() < p)
571 {
572 std::vector<int> nbrs;
573 decomposition_->getNeighborhood(lead[i], nbrs);
574 for (int j = 0; j < numSampleAttempts; ++j)
575 {
576 const ompl::base::State *seed = nullptr;
577 // if (layer->getLevel() > 0) // must find a seed. Try states in the neighborhood. There probably
578 // are some
579 {
580 rng_.shuffle(nbrs.begin(), nbrs.end());
581 for (size_t k = 0; k < nbrs.size() && !seed; ++k)
582 {
583 const Region &nbrReg = layer->getRegion(nbrs[k]);
584 if (nbrReg.allMotions.size() > 0) // just pick any old state
585 seed = motions_[nbrReg.allMotions[rng_.uniformInt(0, nbrReg.allMotions.size() - 1)]]
586 ->state;
587 }
588
589 if (!seed)
590 continue;
591 }
592
593 if (decomposition_->sampleFromRegion(lead[i], xstate_, seed, layer->getLevel()))
594 newStates.push_back(addState(xstate_));
595 }
596 }
597 }
598 }
599
600 // Update weights after sampling
601 for (size_t i = 0; i < newStates.size(); ++i)
602 updateRegionProperties(motions_[newStates[i]]->levels);
603 for (size_t i = 0; i < lead.size(); ++i)
604 updateRegionProperties(layer, lead[i]);
605
606 return newStates.size() > 0;
607}
608
609int ompl::geometric::XXL::steerToRegion(Layer *layer, int from, int to)
610{
611 if (!decomposition_->canSteer())
612 throw ompl::Exception("steerToRegion not implemented in decomposition");
613
614 Region &fromRegion = layer->getRegion(from);
615
616 if (fromRegion.motionsInTree.size() == 0)
617 {
618 OMPL_DEBUG("%s: %d -> %d, Layer %d", __func__, from, to, layer->getLevel());
619 OMPL_WARN("XXL: Logic error: No existing states in the region to expand from");
620 return -1;
621 }
622
623 // Select a motion at random in the from region
624 int random = rng_.uniformInt(0, fromRegion.motionsInTree.size() - 1);
625 const Motion *fromMotion = motions_[fromRegion.motionsInTree[random]];
626
627 std::vector<base::State *> newStates;
628 // Steer toward 'to' region. If successful, a valid path is found between newStates
629 if (decomposition_->steerToRegion(to, layer->getLevel(), fromMotion->state, newStates))
630 {
631 std::vector<int> newStateIDs(newStates.size());
632 // add all states into the real graph
633 for (size_t i = 0; i < newStates.size(); ++i)
634 newStateIDs[i] = addThisState(newStates[i]);
635
636 // Connect all states
637 int prev = fromMotion->index;
638 for (size_t i = 0; i < newStateIDs.size(); ++i)
639 {
640 // Add edge
641 lazyGraph_.removeEdge(prev, newStateIDs[i]);
642 double weight = si_->distance(motions_[prev]->state, motions_[newStateIDs[i]]->state);
643 realGraph_.addEdge(prev, newStateIDs[i], weight);
644
645 // Insert state into sublayers
646 Layer *l = topLayer_;
647 const Motion *newMotion = motions_[newStateIDs[i]];
648 for (size_t j = 0; j < newMotion->levels.size(); ++j)
649 {
650 l->getRegion(newMotion->levels[j]).motionsInTree.push_back(newMotion->index);
651 if (l->hasSublayers())
652 l = l->getSublayer(newMotion->levels[j]);
653 }
654
655 // Update states connected metric
656 if (i == 0 && realGraph_.numNeighbors(prev) == 0)
657 statesConnectedInRealGraph_++;
658 if (realGraph_.numNeighbors(newStateIDs[i]) == 0)
659 statesConnectedInRealGraph_++;
660
661 // Update connectivity
662 updateRegionConnectivity(motions_[prev], newMotion, layer->getLevel());
663 updateRegionProperties(newMotion->levels);
664
665 prev = newStateIDs[i];
666 }
667
668 return newStateIDs.back();
669 }
670 return -1;
671}
672
673// Expand the verified tree in region 'from' to region 'to' in the given layer
674// Expansion is only from states connected to the start/goal in region 'from'
675// A successful expansion will connect with an existing (connected) state in the region, or a newly sampled state
676int ompl::geometric::XXL::expandToRegion(Layer *layer, int from, int to, bool useExisting)
677{
678 Region &fromRegion = layer->getRegion(from);
679 Region &toRegion = layer->getRegion(to);
680
681 if (fromRegion.motionsInTree.size() == 0)
682 {
683 OMPL_DEBUG("%s: %d -> %d, Layer %d", __func__, from, to, layer->getLevel());
684 OMPL_WARN("XXL: Logic error: No existing states in the region to expand from");
685 return -1;
686 // throw ompl::Exception("Logic error: No existing states in the region to expand from");
687 }
688
689 if (useExisting && toRegion.motionsInTree.size() == 0)
690 {
691 OMPL_ERROR("Logic error: useExisting is true but there are no existing states");
692 return -1;
693 }
694
695 // Select a motion at random in the from region
696 int random = rng_.uniformInt(0, fromRegion.motionsInTree.size() - 1);
697 const Motion *fromMotion = motions_[fromRegion.motionsInTree[random]];
698
699 // Select a motion in the 'to' region, or sample a new state
700 const Motion *toMotion = nullptr;
701 if (useExisting ||
702 (toRegion.motionsInTree.size() > 0 && rng_.uniform01() < 0.50)) // use an existing state 50% of the time
703 {
704 for (size_t i = 0; i < toRegion.motionsInTree.size() && !toMotion; ++i)
705 if (lazyGraph_.edgeExists(fromMotion->index, motions_[toRegion.motionsInTree[i]]->index))
706 toMotion = motions_[toRegion.motionsInTree[i]];
707 }
708
709 bool newState = false;
710 if (toMotion == nullptr) // sample a new state
711 {
712 // base::State* xstate = si_->allocState();
713 if (decomposition_->sampleFromRegion(to, xstate_, fromMotion->state, layer->getLevel()))
714 {
715 int id = addState(xstate_);
716 toMotion = motions_[id];
717 newState = true;
718 }
719 // si_->freeState(xstate);
720
721 if (toMotion == nullptr)
722 return -1;
723 }
724
725 lazyGraph_.removeEdge(fromMotion->index, toMotion->index); // remove the edge so we never try this again
726 layer->selectRegion(to);
727
728 // Try to connect the states
729 if (si_->checkMotion(fromMotion->state, toMotion->state)) // Motion is valid!
730 {
731 // add edge to real graph
732 double weight = si_->distance(fromMotion->state, toMotion->state);
733
734 if (realGraph_.numNeighbors(fromMotion->index) == 0)
735 statesConnectedInRealGraph_++;
736 if (realGraph_.numNeighbors(toMotion->index) == 0)
737 statesConnectedInRealGraph_++;
738 realGraph_.addEdge(fromMotion->index, toMotion->index, weight);
739
740 updateRegionConnectivity(fromMotion, toMotion, layer->getLevel());
741
742 if (newState)
743 {
744 // Add this state to the real graph
745 Layer *l = topLayer_;
746 for (size_t i = 0; i < toMotion->levels.size(); ++i)
747 {
748 l->getRegion(toMotion->levels[i]).motionsInTree.push_back(toMotion->index);
749 if (l->hasSublayers())
750 l = l->getSublayer(toMotion->levels[i]);
751 }
752 }
753 return toMotion->index;
754 }
755 return -1;
756}
757
758// Check that each region in the lead has at least one state connected to the start and goal
759bool ompl::geometric::XXL::feasibleLead(Layer *layer, const std::vector<int> &lead,
760 const ompl::base::PlannerTerminationCondition &ptc)
761{
762 assert(lead.size() > 0);
763
764 // Don't do anything with a one-region lead
765 if (lead.size() == 1)
766 return layer->getRegion(lead[0]).motionsInTree.size() > 0;
767
768 // Figure out which regions we know how to get to (path in the tree)
769 // There should always a known state at the beginning and end of the lead (start and goal)
770 std::vector<bool> regionInTree(lead.size(), false);
771 size_t numRegionsInLead = 0;
772 for (size_t i = 0; i < lead.size(); ++i)
773 {
774 regionInTree[i] = layer->getRegion(lead[i]).motionsInTree.size() > 0;
775 if (regionInTree[i])
776 numRegionsInLead++;
777 }
778
779 // Connect unoccupied regions to an occupied neighbor in the lead
780 // Attempt connection while we are making progress along the lead (forward and backward)
781 bool expanded = true;
782 int maxAttempts = 5;
783 while (numRegionsInLead < lead.size() && expanded && !ptc)
784 {
785 expanded = false;
786 for (size_t i = 1; i < lead.size() && !ptc; ++i)
787 {
788 int from = -1, to = -1;
789 if (!regionInTree[i] && regionInTree[i - 1]) // 'forward' along lead
790 {
791 from = i - 1;
792 to = i;
793 }
794 else if (regionInTree[i] && !regionInTree[i - 1]) // 'backward along lead'
795 {
796 from = i;
797 to = i - 1;
798 }
799
800 if (from != -1) // expand into new territory from existing state
801 {
802 // bool warned = false;
803 for (int j = 0; j < maxAttempts && !ptc && !expanded; ++j)
804 {
805 int idx;
806
807 if (decomposition_->canSteer())
808 idx = steerToRegion(layer, lead[from], lead[to]);
809 else
810 idx = expandToRegion(layer, lead[from], lead[to]);
811
812 // Success
813 if (idx != -1)
814 {
815 regionInTree[to] = true;
816 numRegionsInLead++;
817 expanded = true;
818 }
819 }
820 }
821 else if (regionInTree[i] && regionInTree[i - 1]) // bolster existing connection between these regions
822 {
823 const Region &r1 = layer->getRegion(lead[i - 1]);
824 const Region &r2 = layer->getRegion(lead[i]);
825 double p1 = 1.0 - (r1.motionsInTree.size() / (double)r1.allMotions.size());
826 double p2 = 1.0 - (r2.motionsInTree.size() / (double)r2.allMotions.size());
827 double p = std::max(p1, p2);
828 if (rng_.uniform01() < p) // improve existing connections
829 connectRegions(layer, lead[i - 1], lead[i], ptc);
830 }
831 }
832 }
833
834 if (rng_.uniform01() < 0.05) // small chance to brute force connect along lead
835 {
836 for (size_t i = 1; i < lead.size(); ++i)
837 connectRegions(layer, lead[i - 1], lead[i], ptc);
838 }
839
840 for (size_t i = 0; i < lead.size(); ++i)
841 updateRegionProperties(layer, lead[i]);
842
843 return numRegionsInLead == lead.size();
844}
845
846// We have a lead that has states in every region. Try to connect the states together
847bool ompl::geometric::XXL::connectLead(Layer *layer, const std::vector<int> &lead, std::vector<int> &candidateRegions,
848 const ompl::base::PlannerTerminationCondition &ptc)
849{
850 if (lead.size() == 0)
851 throw ompl::Exception("Lead is empty");
852
853 AdjacencyList &regionGraph = layer->getRegionGraph();
854
855 // Make sure there are connections between the regions along the lead.
856 bool failed = false;
857 if (lead.size() > 1)
858 {
859 bool connected = true;
860 for (size_t i = 1; i < lead.size() && connected && !ptc; ++i)
861 {
862 bool regionsConnected = regionGraph.edgeExists(lead[i], lead[i - 1]);
863
864 const Region &r1 = layer->getRegion(lead[i - 1]);
865 const Region &r2 = layer->getRegion(lead[i]);
866 double p1 = 1.0 - (r1.motionsInTree.size() / (double)r1.allMotions.size());
867 double p2 = 1.0 - (r2.motionsInTree.size() / (double)r2.allMotions.size());
868 double p = std::max(p1, p2);
869
870 if (regionsConnected)
871 p /= 2.0;
872
873 if (!regionsConnected || rng_.uniform01() < p)
874 connectRegions(layer, lead[i - 1], lead[i], ptc);
875
876 connected &= regionGraph.edgeExists(lead[i], lead[i - 1]);
877 }
878
879 if (!connected)
880 failed = true;
881 }
882
883 // Failed to connect all regions in the lead
884 if (failed)
885 return false;
886
887 // Compute the candidate region connection point(s)
888 std::set<int> startComponents;
889 std::set<int> goalComponents;
890 for (size_t i = 0; i < startMotions_.size(); ++i)
891 startComponents.insert(realGraph_.getComponentID(startMotions_[i]));
892 for (size_t i = 0; i < goalMotions_.size(); ++i)
893 goalComponents.insert(realGraph_.getComponentID(goalMotions_[i]));
894
895 // See if there are connected components that appear in every region in the lead
896 // If so, there must be a solution path
897 std::set<int> sharedComponents;
898 for (std::set<int>::iterator it = startComponents.begin(); it != startComponents.end();)
899 {
900 std::set<int>::iterator goalIt = goalComponents.find(*it);
901 if (goalIt != goalComponents.end())
902 {
903 sharedComponents.insert(*it);
904 goalComponents.erase(goalIt);
905 startComponents.erase(it++); // VERY important to increment iterator here.
906 }
907 else
908 it++;
909 }
910
911 if (sharedComponents.size()) // there must be a path from start to goal
912 return true;
913
914 // Figure out which regions in the lead are connected to the start and/or goal
915 std::vector<bool> inStartTree(lead.size(), false);
916 std::vector<std::set<int>> validGoalComponents;
917 for (size_t i = 0; i < lead.size(); ++i)
918 {
919 validGoalComponents.push_back(std::set<int>());
920
921 const std::vector<int> &motions = layer->getRegion(lead[i]).motionsInTree;
922 for (size_t j = 0; j < motions.size(); ++j)
923 {
924 int component = realGraph_.getComponentID(motions[j]);
925 assert(component >= 0);
926
927 if (startComponents.find(component) != startComponents.end())
928 inStartTree[i] = true;
929 else if (goalComponents.find(component) != goalComponents.end())
930 validGoalComponents[i].insert(component);
931 }
932 }
933
934 // intersect the valid goal components to find components that are in every region in the lead connected to the goal
935 // start with the smallest non-empty set
936 size_t min = validGoalComponents.size() - 1;
937 assert(validGoalComponents[min].size() > 0);
938 for (size_t i = 0; i < validGoalComponents.size(); ++i)
939 if (validGoalComponents[i].size() && validGoalComponents[i].size() < validGoalComponents[min].size())
940 min = i;
941
942 // The set of goal component ids that appear in all regions that are connected to a goal state
943 std::set<int> leadGoalComponents(validGoalComponents[min].begin(), validGoalComponents[min].end());
944 for (size_t i = 0; i < lead.size(); ++i)
945 {
946 if (i == min || validGoalComponents[i].size() == 0)
947 continue;
948
949 for (std::set<int>::iterator it = leadGoalComponents.begin(); it != leadGoalComponents.end();)
950 if (validGoalComponents[i].find(*it) == validGoalComponents[i].end()) // candidate component not in the
951 // list
952 leadGoalComponents.erase(it++);
953 else
954 it++;
955 }
956
957 // see if there is a region (or regions) that have states in both the start and the goal tree
958 candidateRegions.clear();
959 for (size_t i = 0; i < lead.size(); ++i)
960 {
961 if (inStartTree[i])
962 {
963 // check for goal tree - intersect validGoalComponents[i] and leadGoalComponents. If non-empty, this is a
964 // winner
965 for (std::set<int>::iterator it = validGoalComponents[i].begin(); it != validGoalComponents[i].end(); ++it)
966 if (leadGoalComponents.find(*it) != leadGoalComponents.end())
967 {
968 candidateRegions.push_back(lead[i]);
969 break;
970 }
971 }
972 }
973
974 // internal connection within a region
975 for (size_t i = 0; i < candidateRegions.size(); ++i)
976 {
977 connectRegion(layer, candidateRegions[i], ptc);
978
979 // See if there is a solution path
980 for (size_t i = 0; i < startMotions_.size(); ++i)
981 for (size_t j = 0; j < goalMotions_.size(); ++j)
982 if (realGraph_.inSameComponent(startMotions_[i], goalMotions_[j])) // yup, solution exists
983 return true;
984 }
985
986 if (candidateRegions.size() == 0)
987 {
988 // at this point we have not yet found a solution path. see if there are other regions we can identify as
989 // candidates if all regions are connected to the start and the end of the lead is connected to the goal, mark
990 // that as a candidate
991 bool allConnectedToStart = true;
992 for (size_t i = 0; i < inStartTree.size(); ++i)
993 allConnectedToStart &= inStartTree[i];
994
995 if (allConnectedToStart && validGoalComponents[lead.size() - 1].size() > 0)
996 {
997 candidateRegions.push_back(lead.back());
998 connectRegion(layer, lead.back(), ptc);
999
1000 // See if there is a solution path
1001 for (size_t i = 0; i < startMotions_.size(); ++i)
1002 for (size_t j = 0; j < goalMotions_.size(); ++j)
1003 if (realGraph_.inSameComponent(startMotions_[i], goalMotions_[j])) // yup, solution exists
1004 return true;
1005 }
1006 }
1007
1008 return false; // failed to find a solution path
1009}
1010
1011// Try to connect nodes within a region that are not yet connected
1012void ompl::geometric::XXL::connectRegion(Layer *layer, int reg, const base::PlannerTerminationCondition &ptc)
1013{
1014 assert(layer);
1015 assert(reg >= 0 && reg < decomposition_->getNumRegions());
1016
1017 Region &region = layer->getRegion(reg);
1018 const std::vector<int> &allMotions = region.allMotions;
1019
1020 // Shuffle the motions in this regions
1021 std::vector<int> shuffledMotions(allMotions.begin(), allMotions.end());
1022 rng_.shuffle(shuffledMotions.begin(), shuffledMotions.end());
1023
1024 // size_t maxIdx = (shuffledMotions.size() > 20 ? shuffledMotions.size() / 2 : shuffledMotions.size());
1025 size_t maxIdx = shuffledMotions.size();
1026
1027 // This method will try to connect states in different connected components
1028 // of the real graph together
1029
1030 // Try to join disconnected states together within the region
1031 for (size_t i = 0; i < maxIdx && !ptc; ++i)
1032 {
1033 const Motion *m1 = motions_[shuffledMotions[i]];
1034 for (size_t j = i + 1; j < maxIdx && !ptc; ++j)
1035 {
1036 const Motion *m2 = motions_[shuffledMotions[j]];
1037 if (lazyGraph_.edgeExists(m1->index, m2->index) && !realGraph_.inSameComponent(m1->index, m2->index))
1038 {
1039 // Remove this edge so we never try and verify this edge again
1040 lazyGraph_.removeEdge(m1->index, m2->index);
1041
1042 // At this point, m1 and m2 should both be connected to start or goal, but
1043 // there is no path in the graph from m1 to m2. Try to connect them together
1044 if (si_->checkMotion(m1->state, m2->state)) // Motion is valid!
1045 {
1046 // add edge to real graph
1047 double weight = si_->distance(m1->state, m2->state);
1048 realGraph_.addEdge(m1->index, m2->index, weight); // add state to real graph
1049
1050 // Update region storage
1051 if (realGraph_.numNeighbors(m1->index) == 1 && !isStartState(m1->index) && !isGoalState(m1->index))
1052 {
1053 statesConnectedInRealGraph_++;
1054
1055 // Add this state to the real graph in all layers
1056 Layer *l = topLayer_;
1057 for (size_t i = 0; i < m1->levels.size(); ++i)
1058 {
1059 l->getRegion(m1->levels[i]).motionsInTree.push_back(m1->index);
1060 if (l->hasSublayers())
1061 l = l->getSublayer(m1->levels[i]);
1062 else if (i != m1->levels.size() - 1)
1063 throw;
1064 }
1065 }
1066 if (realGraph_.numNeighbors(m2->index) == 1 && !isStartState(m2->index) && !isGoalState(m2->index))
1067 {
1068 statesConnectedInRealGraph_++;
1069
1070 // Add this state to the real graph in all layers
1071 Layer *l = topLayer_;
1072 for (size_t i = 0; i < m2->levels.size(); ++i)
1073 {
1074 l->getRegion(m2->levels[i]).motionsInTree.push_back(m2->index);
1075 if (l->hasSublayers())
1076 l = l->getSublayer(m2->levels[i]);
1077 else if (i != m2->levels.size() - 1)
1078 throw;
1079 }
1080 }
1081
1082 updateRegionConnectivity(m1, m2, layer->getLevel());
1083
1084 // They better be connected meow
1085 assert(realGraph_.inSameComponent(m1->index, m2->index));
1086 }
1087 }
1088 }
1089 }
1090
1091 layer->connectRegion(reg);
1092 updateRegionProperties(layer, reg);
1093}
1094
1095void ompl::geometric::XXL::connectRegions(Layer *layer, int r1, int r2, const base::PlannerTerminationCondition &ptc,
1096 bool all)
1097{
1098 assert(layer);
1099 assert(r1 >= 0 && r1 < decomposition_->getNumRegions());
1100 assert(r2 >= 0 && r2 < decomposition_->getNumRegions());
1101
1102 // "select" the regions 20x
1103 layer->selectRegion(r1, 20);
1104 layer->selectRegion(r2, 20);
1105
1106 Region &reg1 = layer->getRegion(r1);
1107 const std::vector<int> &allMotions1 = reg1.allMotions;
1108 // Shuffle the motions in r1
1109 std::vector<int> shuffledMotions1(allMotions1.begin(), allMotions1.end());
1110 rng_.shuffle(shuffledMotions1.begin(), shuffledMotions1.end());
1111
1112 Region &reg2 = layer->getRegion(r2);
1113 const std::vector<int> &allMotions2 = reg2.allMotions;
1114 // Shuffle the motions in r2
1115 std::vector<int> shuffledMotions2(allMotions2.begin(), allMotions2.end());
1116 rng_.shuffle(shuffledMotions2.begin(), shuffledMotions2.end());
1117
1118 size_t maxConnections = std::numeric_limits<size_t>::max();
1119 size_t maxIdx1 = (all ? shuffledMotions1.size() : std::min(shuffledMotions1.size(), maxConnections));
1120 size_t maxIdx2 = (all ? shuffledMotions2.size() : std::min(shuffledMotions2.size(), maxConnections));
1121
1122 // Try to join different connected components within the region
1123 for (size_t i = 0; i < maxIdx1 && !ptc; ++i)
1124 {
1125 const Motion *m1 = motions_[shuffledMotions1[i]];
1126 for (size_t j = 0; j < maxIdx2 && !ptc; ++j)
1127 {
1128 const Motion *m2 = motions_[shuffledMotions2[j]];
1129 if (lazyGraph_.edgeExists(m1->index, m2->index) && !realGraph_.inSameComponent(m1->index, m2->index))
1130 {
1131 // Remove this edge so we never try and verify this edge again
1132 lazyGraph_.removeEdge(m1->index, m2->index);
1133
1134 // At this point, m1 and m2 should both be connected to start or goal, but
1135 // there is no path in the graph from m1 to m2. Try to connect them together
1136 if (si_->checkMotion(m1->state, m2->state)) // Motion is valid!
1137 {
1138 // add edge to real graph
1139 double weight = si_->distance(m1->state, m2->state);
1140 realGraph_.addEdge(m1->index, m2->index, weight); // add state to real graph
1141
1142 // Update region storage
1143 if (realGraph_.numNeighbors(m1->index) == 1 && !isStartState(m1->index) && !isGoalState(m1->index))
1144 {
1145 statesConnectedInRealGraph_++;
1146
1147 // Add this state to the real graph in all layers
1148 Layer *l = topLayer_;
1149 for (size_t i = 0; i < m1->levels.size(); ++i)
1150 {
1151 l->getRegion(m1->levels[i]).motionsInTree.push_back(m1->index);
1152 if (l->hasSublayers())
1153 l = l->getSublayer(m1->levels[i]);
1154 }
1155 }
1156 if (realGraph_.numNeighbors(m2->index) == 1 && !isStartState(m2->index) && !isGoalState(m2->index))
1157 {
1158 statesConnectedInRealGraph_++;
1159
1160 // Add this state to the real graph in all layers
1161 Layer *l = topLayer_;
1162 for (size_t i = 0; i < m2->levels.size(); ++i)
1163 {
1164 l->getRegion(m2->levels[i]).motionsInTree.push_back(m2->index);
1165 if (l->hasSublayers())
1166 l = l->getSublayer(m2->levels[i]);
1167 }
1168 }
1169
1170 updateRegionConnectivity(m1, m2, layer->getLevel());
1171
1172 // They better be connected meow
1173 assert(realGraph_.inSameComponent(m1->index, m2->index));
1174 }
1175 }
1176 }
1177 }
1178
1179 updateRegionProperties(layer, r1);
1180 updateRegionProperties(layer, r2);
1181}
1182
1183void ompl::geometric::XXL::computeLead(Layer *layer, std::vector<int> &lead)
1184{
1185 if (startMotions_.size() == 0)
1186 throw ompl::Exception("Cannot compute lead without at least one start state");
1187 if (goalMotions_.size() == 0)
1188 OMPL_WARN("No goal states to compute lead to");
1189
1190 int start, end;
1191
1192 if (goalMotions_.size() == 0)
1193 {
1194 const Motion *s = motions_[startMotions_[rng_.uniformInt(0, startMotions_.size() - 1)]];
1195 start = s->levels[layer->getLevel()];
1196
1197 if (topLayer_->numRegions() == 1)
1198 end = 0;
1199 else
1200 {
1201 do
1202 {
1203 end = rng_.uniformInt(0, topLayer_->numRegions() - 1);
1204 } while (start == end);
1205 }
1206 }
1207 else
1208 {
1209 const Motion *s = nullptr;
1210 const Motion *e = nullptr;
1211
1212 if (layer->getLevel() == 0)
1213 {
1214 s = motions_[startMotions_[rng_.uniformInt(0, startMotions_.size() - 1)]];
1215 e = motions_[goalMotions_[rng_.uniformInt(0, goalMotions_.size() - 1)]];
1216 }
1217 else // sublayers
1218 {
1219 std::set<int> startComponents;
1220 for (size_t i = 0; i < startMotions_.size(); ++i)
1221 startComponents.insert(realGraph_.getComponentID(startMotions_[i]));
1222
1223 // pick a state at random that is connected to the start
1224 do
1225 {
1226 const Region &reg = layer->getRegion(rng_.uniformInt(0, layer->numRegions() - 1));
1227 if (reg.motionsInTree.size())
1228 {
1229 int random = rng_.uniformInt(0, reg.motionsInTree.size() - 1);
1230
1231 int cid = realGraph_.getComponentID(reg.motionsInTree[random]);
1232 for (std::set<int>::const_iterator it = startComponents.begin(); it != startComponents.end(); ++it)
1233 if (cid == *it)
1234 {
1235 s = motions_[reg.motionsInTree[random]];
1236 break;
1237 }
1238 }
1239 } while (s == nullptr);
1240
1241 std::set<int> goalComponents;
1242 for (size_t i = 0; i < goalMotions_.size(); ++i)
1243 goalComponents.insert(realGraph_.getComponentID(goalMotions_[i]));
1244
1245 // pick a state at random that is connected to the goal
1246 do
1247 {
1248 const Region &reg = layer->getRegion(rng_.uniformInt(0, layer->numRegions() - 1));
1249 if (reg.motionsInTree.size())
1250 {
1251 int random = rng_.uniformInt(0, reg.motionsInTree.size() - 1);
1252
1253 int cid = realGraph_.getComponentID(reg.motionsInTree[random]);
1254 for (std::set<int>::const_iterator it = goalComponents.begin(); it != goalComponents.end(); ++it)
1255 if (cid == *it)
1256 {
1257 e = motions_[reg.motionsInTree[random]];
1258 break;
1259 }
1260 }
1261 } while (e == nullptr);
1262 }
1263
1264 start = s->levels[layer->getLevel()];
1265 end = e->levels[layer->getLevel()];
1266 }
1267
1268 bool success = false;
1269
1270 if (start == end)
1271 {
1272 lead.resize(1);
1273 lead[0] = start;
1274 success = true;
1275 }
1276 else
1277 {
1278 if (rng_.uniform01() > rand_walk_rate_)
1279 success = shortestPath(start, end, lead, layer->getWeights()) && lead.size() > 0;
1280 else
1281 success = randomWalk(start, end, lead) && lead.size() > 0;
1282 }
1283
1284 if (!success)
1285 throw ompl::Exception("Failed to compute lead", getName().c_str());
1286}
1287
1289{
1290 getGoalStates(); // non-threaded version
1291
1292 // If there are promising subregions, pick one of them most of the time
1293 double p = layer->connectibleRegions() / ((double)layer->connectibleRegions() + 1);
1294 if (layer->hasSublayers() && layer->connectibleRegions() > 0 && rng_.uniform01() < p)
1295 {
1296 // TODO: Make this non-uniform?
1297 int subregion = layer->connectibleRegion(rng_.uniformInt(0, layer->connectibleRegions() - 1));
1298 Layer *sublayer = layer->getSublayer(subregion);
1299
1300 return searchForPath(sublayer, ptc);
1301 }
1302 else
1303 {
1304 std::vector<int> lead;
1305 computeLead(layer, lead);
1306 layer->markLead(lead); // update weights along weight
1307
1308 // sample states where they are needed along lead
1309 sampleAlongLead(layer, lead, ptc);
1310
1311 // Every region in the lead has a valid state in it
1312 if (feasibleLead(layer, lead, ptc))
1313 {
1314 std::vector<int> candidates;
1315
1316 // Find a feasible path through the lead
1317 connectLead(layer, lead, candidates, ptc);
1318 if (constructSolutionPath())
1319 return true;
1320
1321 // No feasible path found, but see if we can descend in layers to focus
1323 if (layer->hasSublayers())
1324 {
1325 // see if there are candidate regions for subexpansion
1326 for (size_t i = 0; i < candidates.size() && !ptc; ++i)
1327 {
1328 Layer *sublayer = layer->getSublayer(candidates[i]);
1329 if (searchForPath(sublayer, ptc))
1330 return true;
1331 }
1332 }
1333 }
1334 }
1335
1336 return false;
1337}
1338
1340{
1341 if (!decomposition_)
1342 throw ompl::Exception("Decomposition is not set. Cannot solve");
1343
1344 checkValidity();
1345
1346 // Making sure goal object is valid
1347 base::GoalSampleableRegion *gsr = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
1348 if (!gsr)
1349 {
1350 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
1352 }
1353 if (!gsr->couldSample())
1354 {
1355 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
1357 }
1358
1359 // Start goal sampling
1360 // kill_ = false;
1361 // goalStateThread_ = boost::thread(boost::bind(&ompl::geometric::XXL::getGoalStates, this, ptc));
1362
1363 // Adding all start states
1364 while (const base::State *s = pis_.nextStart())
1365 addStartState(s);
1366
1367 // There must be at least one valid start state
1368 if (startMotions_.size() == 0)
1369 {
1370 kill_ = true;
1371 // goalStateThread_.join();
1372
1373 OMPL_ERROR("%s: No valid start states", getName().c_str());
1375 }
1376
1377 OMPL_INFORM("%s: Operating over %d dimensional, %d layer decomposition with %d regions per layer",
1378 getName().c_str(), decomposition_->getDimension(), decomposition_->numLayers(),
1379 decomposition_->getNumRegions());
1380 OMPL_INFORM("%s: Random ralk rate: %.3f", getName().c_str(), rand_walk_rate_);
1381
1382 bool foundSolution = false;
1383
1384 while (!ptc && goalMotions_.size() == 0)
1385 getGoalStates(); // make sure at least one goal state exists before planning
1386
1387 while (!ptc && !foundSolution)
1388 {
1389 // getGoalStates(); // non-threaded version
1390 foundSolution = searchForPath(topLayer_, ptc);
1391 }
1392
1393 if (!foundSolution && constructSolutionPath())
1394 {
1395 OMPL_ERROR("Tripped and fell over a solution path.");
1396 foundSolution = true;
1397 }
1398
1399 OMPL_INFORM("%s: Created %lu states (%lu start, %lu goal); %u are connected", getName().c_str(), motions_.size(),
1400 startMotions_.size(), goalMotions_.size(), statesConnectedInRealGraph_);
1401
1402 // Stop goal sampling thread, if it is still running
1403 kill_ = true;
1404 // goalStateThread_.join();
1405
1407}
1408
1409bool ompl::geometric::XXL::isStartState(int idx) const
1410{
1411 for (size_t i = 0; i < startMotions_.size(); ++i)
1412 if (idx == startMotions_[i])
1413 return true;
1414 return false;
1415}
1416
1417bool ompl::geometric::XXL::isGoalState(int idx) const
1418{
1419 for (size_t i = 0; i < goalMotions_.size(); ++i)
1420 if (idx == goalMotions_[i])
1421 return true;
1422 return false;
1423}
1424
1425bool ompl::geometric::XXL::constructSolutionPath()
1426{
1427 int start = startMotions_[0];
1428 std::vector<int> predecessors;
1429 std::vector<double> cost;
1430 realGraph_.dijkstra(start, predecessors, cost);
1431
1432 int bestGoal = -1;
1433 double bestCost = std::numeric_limits<double>::max();
1434
1435 for (size_t i = 0; i < goalMotions_.size(); ++i)
1436 {
1437 if (cost[goalMotions_[i]] < bestCost)
1438 {
1439 bestCost = cost[goalMotions_[i]];
1440 bestGoal = goalMotions_[i];
1441 }
1442 }
1443
1444 if (bestGoal == -1)
1445 return false;
1446
1447 std::vector<Motion *> slnPath;
1448 int v = bestGoal;
1449 while (predecessors[v] != v)
1450 {
1451 slnPath.push_back(motions_[v]);
1452 v = predecessors[v];
1453 }
1454 slnPath.push_back(motions_[v]); // start
1455
1456 PathGeometric *path = new PathGeometric(si_);
1457 for (int i = slnPath.size() - 1; i >= 0; --i)
1458 path->append(slnPath[i]->state);
1459 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
1460
1461 return true;
1462}
1463
1465{
1466 // Adding all vertices
1467 for (size_t i = 0; i < motions_.size(); ++i)
1468 {
1469 bool isStart = false;
1470 bool isGoal = false;
1471
1472 for (size_t j = 0; j < startMotions_.size(); ++j)
1473 if (startMotions_[j] == (int)i)
1474 isStart = true;
1475
1476 for (size_t j = 0; j < goalMotions_.size(); ++j)
1477 if (goalMotions_[j] == (int)i)
1478 isGoal = true;
1479
1480 if (!isStart && !isGoal)
1481 data.addVertex(base::PlannerDataVertex(motions_[i]->state));
1482 else if (isStart)
1483 data.addStartVertex(base::PlannerDataVertex(motions_[i]->state));
1484 else if (isGoal)
1485 data.addGoalVertex(base::PlannerDataVertex(motions_[i]->state));
1486 }
1487
1488 // Adding all edges
1489 for (size_t i = 0; i < motions_.size(); ++i)
1490 {
1491 std::vector<std::pair<int, double>> nbrs;
1492 realGraph_.getNeighbors(i, nbrs);
1493
1494 for (size_t j = 0; j < nbrs.size(); ++j)
1495 data.addEdge(i, nbrs[j].first, ompl::base::PlannerDataEdge(), ompl::base::Cost(nbrs[j].second));
1496 }
1497}
1498
1499void ompl::geometric::XXL::getGoalStates()
1500{
1501 int newCount = 0;
1502 int maxCount = 10;
1503 while (pis_.haveMoreGoalStates() && newCount < maxCount /*&& goalMotions_.size() < maxGoalStates_*/)
1504 {
1505 const base::State *st = pis_.nextGoal();
1506 if (st == nullptr)
1507 break;
1508
1509 newCount++;
1510
1511 double dist = std::numeric_limits<double>::infinity(); // min distance to another goal state
1512 for (size_t i = 0; i < goalMotions_.size(); ++i)
1513 {
1514 double d = si_->distance(motions_[goalMotions_[i]]->state, st);
1515 if (d < dist)
1516 dist = d;
1517 }
1518
1519 // Keep goals diverse. TODO: GoalLazySamples does something similar to this.
1520 if (dist > 0.5)
1521 addGoalState(st);
1522 else
1523 OMPL_WARN("XXL: Rejecting goal state that is %f from another goal", dist);
1524 }
1525}
1526
1527void ompl::geometric::XXL::getGoalStates(const base::PlannerTerminationCondition & /*ptc*/)
1528{
1529 throw ompl::Exception("Not thread safe");
1530 /*base::Goal* goal = pdef_->getGoal().get();
1531 while(!ptc && !kill_ && goalMotions_.size() < maxGoalStates_)
1532 {
1533 if (!pis_.haveMoreGoalStates())
1534 {
1535 usleep(1000);
1536 continue;
1537 }
1538
1539 const base::State* st = pis_.nextGoal(ptc);
1540 if (st == nullptr)
1541 break;
1542
1543 double dist = std::numeric_limits<double>::infinity(); // min distance to another goal state
1544 for(size_t i = 0; i < goalMotions_.size(); ++i)
1545 {
1546 double d = si_->distance(motions_[goalMotions_[i]]->state, st);
1547 if (d < dist)
1548 dist = d;
1549 }
1550
1551 // Keep goals diverse. TODO: GoalLazySamples does something similar to this.
1552 if (dist > 0.5)
1553 addGoalState(st);
1554 }
1555
1556 if (goalMotions_.size() >= maxGoalStates_)
1557 OMPL_INFORM("%s: Reached max state count in goal sampling thread", getName().c_str());
1558 OMPL_INFORM("%s: Goal sampling thread ceased", getName().c_str());*/
1559}
1560
1561void ompl::geometric::XXL::getNeighbors(int rid, const std::vector<double> &weights,
1562 std::vector<std::pair<int, double>> &neighbors) const
1563{
1564 std::vector<int> nbrs;
1565 decomposition_->getNeighbors(rid, nbrs);
1566
1567 for (size_t i = 0; i < nbrs.size(); ++i)
1568 {
1569 neighbors.push_back(std::make_pair(nbrs[i], weights[nbrs[i]]));
1570 }
1571}
1572
1573struct OpenListNode
1574{
1575 int id{-1};
1576 int parent{-1};
1577 double g{0.0}, h{std::numeric_limits<double>::infinity()};
1578
1579 OpenListNode(int _id) : id(_id)
1580 {
1581 }
1582
1583 bool operator<(const OpenListNode &other) const
1584 {
1585 return (g + h) > (other.g + other.h); // priority queue is a max heap, but we want nodes with smaller g+h to
1586 // have higher priority
1587 }
1588};
1589
1590// (weighted) A* search
1591bool ompl::geometric::XXL::shortestPath(int r1, int r2, std::vector<int> &path, const std::vector<double> &weights)
1592{
1593 if (r1 < 0 || r1 >= decomposition_->getNumRegions())
1594 {
1595 OMPL_ERROR("Start region (%d) is not valid", r1);
1596 return false;
1597 }
1598
1599 if (r2 < 0 || r2 >= decomposition_->getNumRegions())
1600 {
1601 OMPL_ERROR("Goal region (%d) is not valid", r2);
1602 return false;
1603 }
1604
1605 // 50% of time, do weighted A* instead of normal A*
1606 double weight = 1.0; // weight = 1; normal A*
1607 if (rng_.uniform01() < 0.50)
1608 {
1609 if (rng_.uniform01() < 0.50)
1610 weight = 0.01; // greedy search
1611 else
1612 weight = 50.0; // weighted A*
1613 }
1614
1615 // Initialize predecessors and open list
1616 std::fill(predecessors_.begin(), predecessors_.end(), -1);
1617 std::fill(closedList_.begin(), closedList_.end(), false);
1618
1619 // Create empty open list
1620 std::priority_queue<OpenListNode> openList;
1621
1622 // Add start node to open list
1623 OpenListNode start(r1);
1624 start.g = 0.0;
1625 start.h = weight * decomposition_->distanceHeuristic(r1, r2);
1626 start.parent = r1; // start has a self-transition to parent
1627 openList.push(start);
1628
1629 // A* search
1630 bool solution = false;
1631 while (!openList.empty())
1632 {
1633 OpenListNode node = openList.top();
1634 openList.pop();
1635
1636 // been here before
1637 if (closedList_[node.id])
1638 continue;
1639
1640 // mark node as 'been here'
1641 closedList_[node.id] = true;
1642 predecessors_[node.id] = node.parent;
1643
1644 // found solution!
1645 if (node.id == r2)
1646 {
1647 solution = true;
1648 break;
1649 }
1650
1651 // Go through neighbors and add them to open list, if necessary
1652 std::vector<std::pair<int, double>> neighbors;
1653 getNeighbors(node.id, weights, neighbors);
1654
1655 // Shuffle neighbors for variability in the search
1656 rng_.shuffle(neighbors.begin(), neighbors.end());
1657 for (size_t i = 0; i < neighbors.size(); ++i)
1658 {
1659 // only add neighbors we have not visited
1660 if (!closedList_[neighbors[i].first])
1661 {
1662 OpenListNode nbr(neighbors[i].first);
1663 nbr.g = node.g + neighbors[i].second;
1664 nbr.h = weight * decomposition_->distanceHeuristic(neighbors[i].first, r2);
1665 nbr.parent = node.id;
1666
1667 openList.push(nbr);
1668 }
1669 }
1670 }
1671
1672 if (solution)
1673 {
1674 path.clear();
1675 int current = r2;
1676 while (predecessors_[current] != current)
1677 {
1678 path.insert(path.begin(), current);
1679 current = predecessors_[current];
1680 }
1681
1682 path.insert(path.begin(), current); // add start state
1683 }
1684
1685 return solution;
1686}
1687
1688bool ompl::geometric::XXL::randomWalk(int r1, int r2, std::vector<int> &path)
1689{
1690 // Initialize predecessors and closed list
1691 std::fill(predecessors_.begin(), predecessors_.end(), -1);
1692 std::fill(closedList_.begin(), closedList_.end(), false);
1693
1694 closedList_[r1] = true;
1695 for (int i = 0; i < decomposition_->getNumRegions(); ++i)
1696 {
1697 int u = i;
1698 // doing random walk until we hit a state already in the tree
1699 while (!closedList_[u])
1700 {
1701 std::vector<int> neighbors;
1702 decomposition_->getNeighbors(u, neighbors);
1703 int nbr = neighbors[rng_.uniformInt(0, neighbors.size() - 1)]; // random successor
1704
1705 predecessors_[u] = nbr;
1706 u = nbr;
1707 }
1708
1709 // Adding the (simplified) random walk to the tree
1710 u = i;
1711 while (!closedList_[u])
1712 {
1713 closedList_[u] = true;
1714 u = predecessors_[u];
1715 }
1716 }
1717
1718 int current = r2;
1719 path.clear();
1720 while (predecessors_[current] != -1)
1721 {
1722 path.insert(path.begin(), current);
1723 current = predecessors_[current];
1724
1725 if ((int)path.size() >= decomposition_->getNumRegions())
1726 throw ompl::Exception("Serious problem in random walk");
1727 }
1728 path.insert(path.begin(), current); // add start state
1729
1730 if (path.front() != r1)
1731 throw ompl::Exception("Path does not start at correct place");
1732 if (path.back() != r2)
1733 throw ompl::Exception("Path does not end at correct place");
1734
1735 return true;
1736}
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of a goal region that can be sampled.
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future....
Base class for a PlannerData edge.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
A shared pointer wrapper for ompl::geometric::XXLDecomposition.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition XXL.cpp:109
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
Definition XXL.cpp:1288
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition XXL.cpp:1464
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition XXL.cpp:65
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition XXL.cpp:1339
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ TIMEOUT
The planner failed to find a solution.