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"
47 xstate_ = si_->allocState();
48 Planner::declareParam<double>(
"rand_walk_rate",
this, &XXL::setRandWalkRate, &XXL::getRandWalkRate,
"0.:.05:1.");
52 : base::Planner(si,
"XXL")
54 xstate_ = si_->allocState();
55 setDecomposition(decomp);
56 Planner::declareParam<double>(
"rand_walk_rate",
this, &XXL::setRandWalkRate, &XXL::getRandWalkRate,
"0.:.05:1.");
59 ompl::geometric::XXL::~XXL()
62 si_->freeState(xstate_);
77 new Layer(-1, decomposition_->getNumRegions(), 0,
nullptr);
78 allocateLayers(topLayer_);
82 startMotions_.clear();
89 statesConnectedInRealGraph_ = 0;
93 void ompl::geometric::XXL::freeMemory()
95 for (
auto &motion : motions_)
97 si_->freeState(motion->state);
113 OMPL_ERROR(
"%s: Decomposition is not set. Cannot continue setup.", getName().c_str());
119 sampler_ = si_->allocStateSampler();
121 statesConnectedInRealGraph_ = 0;
123 maxGoalStatesPerRegion_ = 100;
124 maxGoalStates_ = 500;
131 topLayer_ =
new Layer(-1, decomposition_->getNumRegions(), 0,
nullptr);
132 allocateLayers(topLayer_);
134 if (rand_walk_rate_ < 0.0 || rand_walk_rate_ > 1.0)
136 rand_walk_rate_ = 0.05;
142 decomposition_ = decomp;
143 predecessors_.resize(decomposition_->getNumRegions());
144 closedList_.resize(decomposition_->getNumRegions());
146 if (decomposition_->numLayers() < 1)
147 throw ompl::Exception(
"Decomposition must have at least one layer of projection");
151 void ompl::geometric::XXL::allocateLayers(Layer *layer)
156 if (layer->getLevel() < decomposition_->numLayers() - 1)
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));
165 void ompl::geometric::XXL::updateRegionConnectivity(
const Motion *m1,
const Motion *m2,
int layer)
167 if (layer >= decomposition_->numLayers() || layer < 0)
174 std::vector<base::State *> intermediateStates;
175 std::vector<std::vector<int>> intProjections;
177 for (
int i = layer; i < decomposition_->numLayers(); ++i)
179 if (m1->levels[i] != m2->levels[i])
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];
190 while (t < (1.0 - dt / 2.0))
192 std::vector<int> projection;
193 ss->interpolate(m1->state, m2->state, t, xstate_);
194 decomposition_->project(xstate_, projection);
196 intermediateStates.push_back(si_->cloneState(xstate_));
197 intProjections.push_back(projection);
206 if (intermediateStates.size())
208 std::vector<int> gaps;
211 for (
size_t i = 1; i < intProjections.size(); ++i)
213 for (
int j = layer; j < (int)intProjections[i].size(); ++j)
215 if (intProjections[i - 1][j] != intProjections[i][j])
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];
225 gaps.push_back((
int)i);
232 std::vector<std::vector<base::State *>> gapStates;
233 std::vector<std::vector<std::vector<int>>> gapProjections;
235 for (
size_t i = 0; i < gaps.size(); ++i)
237 std::vector<base::State *> gapState;
238 std::vector<std::vector<int>> gapProj;
240 double t = (gaps[i]) * dt;
242 double newdt = dt * dt;
245 std::vector<int> projection;
246 ss->interpolate(m1->state, m2->state, t, xstate_);
247 decomposition_->project(xstate_, projection);
249 gapState.push_back(si_->cloneState(xstate_));
250 gapProj.push_back(projection);
254 gapStates.push_back(gapState);
255 gapProjections.push_back(gapProj);
260 for (
size_t i = 0; i < gaps.size(); ++i)
262 auto start = intermediateStates.begin() + gaps[i] + offset;
263 intermediateStates.insert(start, gapStates[i].begin(), gapStates[i].end());
265 auto startproj = intProjections.begin() + gaps[i] + offset;
266 intProjections.insert(startproj, gapProjections[i].begin(), gapProjections[i].end());
268 offset += gapStates[i].size();
273 const Motion *prev = m1;
274 for (
size_t i = 0; i < intermediateStates.size(); ++i)
276 for (
int j = layer; j < decomposition_->numLayers(); ++j)
278 if (prev->levels[j] != intProjections[i][j])
281 int id = addState(intermediateStates[i]);
282 const Motion *newMotion = motions_[id];
285 Layer *l = topLayer_;
286 for (
size_t k = 0; k < newMotion->levels.size(); ++k)
288 l->getRegion(newMotion->levels[k]).motionsInTree.push_back(newMotion->index);
289 if (l->hasSublayers())
290 l = l->getSublayer(newMotion->levels[k]);
294 lazyGraph_.removeEdge(prev->index, newMotion->index);
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_++;
302 Layer *l1 = getLayer(prev->levels, j);
303 Layer *l2 = getLayer(newMotion->levels, j);
305 l1->getRegionGraph().addEdge(prev->levels[j], newMotion->levels[j]);
307 l2->getRegionGraph().addEdge(prev->levels[j], newMotion->levels[j]);
317 if (layer >= decomposition_->numLayers())
320 Layer *l = topLayer_;
321 for (
int i = 0; i < layer; ++i)
322 l = l->getSublayer(regions[i]);
326 int ompl::geometric::XXL::addThisState(base::State *state)
328 auto motion =
new Motion();
329 motion->state = state;
330 decomposition_->project(motion->state, motion->levels);
333 motion->index = realGraph_.addVertex();
334 if (lazyGraph_.addVertex() != motion->index)
338 std::vector<int> nbrs;
339 decomposition_->getNeighbors(motion->levels[0], nbrs);
340 nbrs.push_back(motion->levels[0]);
341 for (
size_t i = 0; i < nbrs.size(); ++i)
343 const std::vector<int> &nbrMotions = topLayer_->getRegion(nbrs[i]).allMotions;
344 for (
size_t j = 0; j < nbrMotions.size(); ++j)
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);
350 throw std::runtime_error(
"Failed to add edge to graph");
355 motions_.push_back(motion);
356 assert(motion->index == motions_.size() - 1);
359 Layer *layer = topLayer_;
360 for (
size_t i = 0; i < motion->levels.size(); ++i)
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)
369 return motion->index;
372 int ompl::geometric::XXL::addState(
const base::State *state)
374 return addThisState(si_->cloneState(state));
377 int ompl::geometric::XXL::addGoalState(
const base::State *state)
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_)
386 if (it != goalCount_.end())
389 goalCount_[proj] = 1;
392 int id = addState(state);
395 const Motion *motion = motions_[id];
396 goalMotions_.push_back(motion->index);
403 Layer *layer = topLayer_;
404 for (
size_t i = 0; i < motion->levels.size(); ++i)
406 layer->addGoalState(motion->levels[i],
id);
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)
415 return motion->index;
418 int ompl::geometric::XXL::addStartState(
const base::State *state)
420 int id = addState(state);
423 const Motion *motion = motions_[id];
424 startMotions_.push_back(motion->index);
428 Layer *layer = topLayer_;
429 for (
size_t i = 0; i < motion->levels.size(); ++i)
431 layer->getRegion(motion->levels[i]).motionsInTree.push_back(motion->index);
432 if (layer->hasSublayers())
433 layer = layer->getSublayer(motion->levels[i]);
436 return motion->index;
439 void ompl::geometric::XXL::updateRegionProperties(Layer *layer,
int reg)
448 const Region ®ion = layer->getRegion(reg);
449 double &weight = layer->getWeights()[reg];
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;
456 int connectedStatesInRegion = (int)region.motionsInTree.size();
458 double connStateRatio = (statesInRegion > 0 ? connectedStatesInRegion / (double)statesInRegion : 0.0);
459 double leadRatio = (layer->numLeads() ? layer->leadAppearances(reg) / (double)layer->numLeads() : 0.0);
461 double newWeight = (exp(-stateRatio) * exp(-10.0 * connStateRatio)) + (1.0 - exp(-leadRatio));
464 double nudgeFactor = 0.1;
465 weight += (newWeight - weight) * nudgeFactor;
468 weight = std::max(0.0, std::min(1.0, weight));
471 void ompl::geometric::XXL::updateRegionProperties(
const std::vector<int> ®ions)
473 Layer *layer = getLayer(regions, 0);
474 for (
size_t i = 0; i < regions.size(); ++i)
476 updateRegionProperties(layer, regions[i]);
478 if (layer->hasSublayers())
479 layer = layer->getSublayer(regions[i]);
480 else if (i != regions.size() - 1)
487 std::vector<int> newStates;
488 if (layer->getID() == -1)
493 sampler_->sampleUniform(xstate_);
494 if (si_->isValid(xstate_))
495 newStates.push_back(addState(xstate_));
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");
507 const Motion *seedMotion = motions_[states[rng_.uniformInt(0, states.size() - 1)]];
508 const base::State *seedState = seedMotion->state;
510 if (decomposition_->sampleFromRegion(layer->getID(), xstate_, seedState, layer->getLevel() - 1))
512 int idx = addState(xstate_);
513 newStates.push_back(idx);
519 for (
size_t i = 0; i < newStates.size(); ++i)
520 updateRegionProperties(motions_[newStates[i]]->levels);
523 bool ompl::geometric::XXL::sampleAlongLead(Layer *layer,
const std::vector<int> &lead,
527 int numSampleAttempts = 10;
528 std::vector<int> newStates;
530 if (lead.size() == 1)
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)
540 rng_.shuffle(nbrs.begin(), nbrs.end());
541 for (
size_t k = 0; k < nbrs.size() && !seed; ++k)
543 const Region &nbrReg = layer->getRegion(nbrs[k]);
544 if (nbrReg.allMotions.size() > 0)
545 seed = motions_[nbrReg.allMotions[rng_.uniformInt(0, nbrReg.allMotions.size() - 1)]]->state;
551 if (decomposition_->sampleFromRegion(lead[0], xstate_, seed, layer->getLevel()))
552 newStates.push_back(addState(xstate_));
558 int maxStateCount = 0;
559 for (
size_t i = 0; i < lead.size() && !ptc; ++i)
561 const Region ®ion = layer->getRegion(lead[i]);
562 if ((
int)region.allMotions.size() > maxStateCount)
563 maxStateCount = region.allMotions.size();
566 for (
size_t i = 0; i < lead.size() && !ptc; ++i)
568 const Region ®ion = layer->getRegion(lead[i]);
569 double p = 1.0 - (region.allMotions.size() / (double)maxStateCount);
570 if (rng_.uniform01() < p)
572 std::vector<int> nbrs;
573 decomposition_->getNeighborhood(lead[i], nbrs);
574 for (
int j = 0; j < numSampleAttempts; ++j)
580 rng_.shuffle(nbrs.begin(), nbrs.end());
581 for (
size_t k = 0; k < nbrs.size() && !seed; ++k)
583 const Region &nbrReg = layer->getRegion(nbrs[k]);
584 if (nbrReg.allMotions.size() > 0)
585 seed = motions_[nbrReg.allMotions[rng_.uniformInt(0, nbrReg.allMotions.size() - 1)]]
593 if (decomposition_->sampleFromRegion(lead[i], xstate_, seed, layer->getLevel()))
594 newStates.push_back(addState(xstate_));
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]);
606 return newStates.size() > 0;
609 int ompl::geometric::XXL::steerToRegion(Layer *layer,
int from,
int to)
611 if (!decomposition_->canSteer())
612 throw ompl::Exception(
"steerToRegion not implemented in decomposition");
614 Region &fromRegion = layer->getRegion(from);
616 if (fromRegion.motionsInTree.size() == 0)
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");
624 int random = rng_.uniformInt(0, fromRegion.motionsInTree.size() - 1);
625 const Motion *fromMotion = motions_[fromRegion.motionsInTree[random]];
627 std::vector<base::State *> newStates;
629 if (decomposition_->steerToRegion(to, layer->getLevel(), fromMotion->state, newStates))
631 std::vector<int> newStateIDs(newStates.size());
633 for (
size_t i = 0; i < newStates.size(); ++i)
634 newStateIDs[i] = addThisState(newStates[i]);
637 int prev = fromMotion->index;
638 for (
size_t i = 0; i < newStateIDs.size(); ++i)
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);
646 Layer *l = topLayer_;
647 const Motion *newMotion = motions_[newStateIDs[i]];
648 for (
size_t j = 0; j < newMotion->levels.size(); ++j)
650 l->getRegion(newMotion->levels[j]).motionsInTree.push_back(newMotion->index);
651 if (l->hasSublayers())
652 l = l->getSublayer(newMotion->levels[j]);
656 if (i == 0 && realGraph_.numNeighbors(prev) == 0)
657 statesConnectedInRealGraph_++;
658 if (realGraph_.numNeighbors(newStateIDs[i]) == 0)
659 statesConnectedInRealGraph_++;
662 updateRegionConnectivity(motions_[prev], newMotion, layer->getLevel());
663 updateRegionProperties(newMotion->levels);
665 prev = newStateIDs[i];
668 return newStateIDs.back();
676 int ompl::geometric::XXL::expandToRegion(Layer *layer,
int from,
int to,
bool useExisting)
678 Region &fromRegion = layer->getRegion(from);
679 Region &toRegion = layer->getRegion(to);
681 if (fromRegion.motionsInTree.size() == 0)
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");
689 if (useExisting && toRegion.motionsInTree.size() == 0)
691 OMPL_ERROR(
"Logic error: useExisting is true but there are no existing states");
696 int random = rng_.uniformInt(0, fromRegion.motionsInTree.size() - 1);
697 const Motion *fromMotion = motions_[fromRegion.motionsInTree[random]];
700 const Motion *toMotion =
nullptr;
702 (toRegion.motionsInTree.size() > 0 && rng_.uniform01() < 0.50))
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]];
709 bool newState =
false;
710 if (toMotion ==
nullptr)
713 if (decomposition_->sampleFromRegion(to, xstate_, fromMotion->state, layer->getLevel()))
715 int id = addState(xstate_);
716 toMotion = motions_[id];
721 if (toMotion ==
nullptr)
725 lazyGraph_.removeEdge(fromMotion->index, toMotion->index);
726 layer->selectRegion(to);
729 if (si_->checkMotion(fromMotion->state, toMotion->state))
732 double weight = si_->distance(fromMotion->state, toMotion->state);
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);
740 updateRegionConnectivity(fromMotion, toMotion, layer->getLevel());
745 Layer *l = topLayer_;
746 for (
size_t i = 0; i < toMotion->levels.size(); ++i)
748 l->getRegion(toMotion->levels[i]).motionsInTree.push_back(toMotion->index);
749 if (l->hasSublayers())
750 l = l->getSublayer(toMotion->levels[i]);
753 return toMotion->index;
759 bool ompl::geometric::XXL::feasibleLead(Layer *layer,
const std::vector<int> &lead,
762 assert(lead.size() > 0);
765 if (lead.size() == 1)
766 return layer->getRegion(lead[0]).motionsInTree.size() > 0;
770 std::vector<bool> regionInTree(lead.size(),
false);
771 size_t numRegionsInLead = 0;
772 for (
size_t i = 0; i < lead.size(); ++i)
774 regionInTree[i] = layer->getRegion(lead[i]).motionsInTree.size() > 0;
781 bool expanded =
true;
783 while (numRegionsInLead < lead.size() && expanded && !ptc)
786 for (
size_t i = 1; i < lead.size() && !ptc; ++i)
788 int from = -1, to = -1;
789 if (!regionInTree[i] && regionInTree[i - 1])
794 else if (regionInTree[i] && !regionInTree[i - 1])
803 for (
int j = 0; j < maxAttempts && !ptc && !expanded; ++j)
807 if (decomposition_->canSteer())
808 idx = steerToRegion(layer, lead[from], lead[to]);
810 idx = expandToRegion(layer, lead[from], lead[to]);
815 regionInTree[to] =
true;
821 else if (regionInTree[i] && regionInTree[i - 1])
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)
829 connectRegions(layer, lead[i - 1], lead[i], ptc);
834 if (rng_.uniform01() < 0.05)
836 for (
size_t i = 1; i < lead.size(); ++i)
837 connectRegions(layer, lead[i - 1], lead[i], ptc);
840 for (
size_t i = 0; i < lead.size(); ++i)
841 updateRegionProperties(layer, lead[i]);
843 return numRegionsInLead == lead.size();
847 bool ompl::geometric::XXL::connectLead(Layer *layer,
const std::vector<int> &lead, std::vector<int> &candidateRegions,
850 if (lead.size() == 0)
853 AdjacencyList ®ionGraph = layer->getRegionGraph();
859 bool connected =
true;
860 for (
size_t i = 1; i < lead.size() && connected && !ptc; ++i)
862 bool regionsConnected = regionGraph.edgeExists(lead[i], lead[i - 1]);
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);
870 if (regionsConnected)
873 if (!regionsConnected || rng_.uniform01() < p)
874 connectRegions(layer, lead[i - 1], lead[i], ptc);
876 connected &= regionGraph.edgeExists(lead[i], lead[i - 1]);
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]));
897 std::set<int> sharedComponents;
898 for (std::set<int>::iterator it = startComponents.begin(); it != startComponents.end();)
900 std::set<int>::iterator goalIt = goalComponents.find(*it);
901 if (goalIt != goalComponents.end())
903 sharedComponents.insert(*it);
904 goalComponents.erase(goalIt);
905 startComponents.erase(it++);
911 if (sharedComponents.size())
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)
919 validGoalComponents.push_back(std::set<int>());
921 const std::vector<int> &motions = layer->getRegion(lead[i]).motionsInTree;
922 for (
size_t j = 0; j < motions.size(); ++j)
924 int component = realGraph_.getComponentID(motions[j]);
925 assert(component >= 0);
927 if (startComponents.find(component) != startComponents.end())
928 inStartTree[i] =
true;
929 else if (goalComponents.find(component) != goalComponents.end())
930 validGoalComponents[i].insert(component);
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())
943 std::set<int> leadGoalComponents(validGoalComponents[min].begin(), validGoalComponents[min].end());
944 for (
size_t i = 0; i < lead.size(); ++i)
946 if (i == min || validGoalComponents[i].size() == 0)
949 for (std::set<int>::iterator it = leadGoalComponents.begin(); it != leadGoalComponents.end();)
950 if (validGoalComponents[i].find(*it) == validGoalComponents[i].end())
952 leadGoalComponents.erase(it++);
958 candidateRegions.clear();
959 for (
size_t i = 0; i < lead.size(); ++i)
965 for (std::set<int>::iterator it = validGoalComponents[i].begin(); it != validGoalComponents[i].end(); ++it)
966 if (leadGoalComponents.find(*it) != leadGoalComponents.end())
968 candidateRegions.push_back(lead[i]);
975 for (
size_t i = 0; i < candidateRegions.size(); ++i)
977 connectRegion(layer, candidateRegions[i], ptc);
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]))
986 if (candidateRegions.size() == 0)
991 bool allConnectedToStart =
true;
992 for (
size_t i = 0; i < inStartTree.size(); ++i)
993 allConnectedToStart &= inStartTree[i];
995 if (allConnectedToStart && validGoalComponents[lead.size() - 1].size() > 0)
997 candidateRegions.push_back(lead.back());
998 connectRegion(layer, lead.back(), ptc);
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]))
1012 void ompl::geometric::XXL::connectRegion(Layer *layer,
int reg,
const base::PlannerTerminationCondition &ptc)
1015 assert(reg >= 0 && reg < decomposition_->getNumRegions());
1017 Region ®ion = layer->getRegion(reg);
1018 const std::vector<int> &allMotions = region.allMotions;
1021 std::vector<int> shuffledMotions(allMotions.begin(), allMotions.end());
1022 rng_.shuffle(shuffledMotions.begin(), shuffledMotions.end());
1025 size_t maxIdx = shuffledMotions.size();
1031 for (
size_t i = 0; i < maxIdx && !ptc; ++i)
1033 const Motion *m1 = motions_[shuffledMotions[i]];
1034 for (
size_t j = i + 1; j < maxIdx && !ptc; ++j)
1036 const Motion *m2 = motions_[shuffledMotions[j]];
1037 if (lazyGraph_.edgeExists(m1->index, m2->index) && !realGraph_.inSameComponent(m1->index, m2->index))
1040 lazyGraph_.removeEdge(m1->index, m2->index);
1044 if (si_->checkMotion(m1->state, m2->state))
1047 double weight = si_->distance(m1->state, m2->state);
1048 realGraph_.addEdge(m1->index, m2->index, weight);
1051 if (realGraph_.numNeighbors(m1->index) == 1 && !isStartState(m1->index) && !isGoalState(m1->index))
1053 statesConnectedInRealGraph_++;
1056 Layer *l = topLayer_;
1057 for (
size_t i = 0; i < m1->levels.size(); ++i)
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)
1066 if (realGraph_.numNeighbors(m2->index) == 1 && !isStartState(m2->index) && !isGoalState(m2->index))
1068 statesConnectedInRealGraph_++;
1071 Layer *l = topLayer_;
1072 for (
size_t i = 0; i < m2->levels.size(); ++i)
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)
1082 updateRegionConnectivity(m1, m2, layer->getLevel());
1085 assert(realGraph_.inSameComponent(m1->index, m2->index));
1091 layer->connectRegion(reg);
1092 updateRegionProperties(layer, reg);
1095 void ompl::geometric::XXL::connectRegions(Layer *layer,
int r1,
int r2,
const base::PlannerTerminationCondition &ptc,
1099 assert(r1 >= 0 && r1 < decomposition_->getNumRegions());
1100 assert(r2 >= 0 && r2 < decomposition_->getNumRegions());
1103 layer->selectRegion(r1, 20);
1104 layer->selectRegion(r2, 20);
1106 Region ®1 = layer->getRegion(r1);
1107 const std::vector<int> &allMotions1 = reg1.allMotions;
1109 std::vector<int> shuffledMotions1(allMotions1.begin(), allMotions1.end());
1110 rng_.shuffle(shuffledMotions1.begin(), shuffledMotions1.end());
1112 Region ®2 = layer->getRegion(r2);
1113 const std::vector<int> &allMotions2 = reg2.allMotions;
1115 std::vector<int> shuffledMotions2(allMotions2.begin(), allMotions2.end());
1116 rng_.shuffle(shuffledMotions2.begin(), shuffledMotions2.end());
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));
1123 for (
size_t i = 0; i < maxIdx1 && !ptc; ++i)
1125 const Motion *m1 = motions_[shuffledMotions1[i]];
1126 for (
size_t j = 0; j < maxIdx2 && !ptc; ++j)
1128 const Motion *m2 = motions_[shuffledMotions2[j]];
1129 if (lazyGraph_.edgeExists(m1->index, m2->index) && !realGraph_.inSameComponent(m1->index, m2->index))
1132 lazyGraph_.removeEdge(m1->index, m2->index);
1136 if (si_->checkMotion(m1->state, m2->state))
1139 double weight = si_->distance(m1->state, m2->state);
1140 realGraph_.addEdge(m1->index, m2->index, weight);
1143 if (realGraph_.numNeighbors(m1->index) == 1 && !isStartState(m1->index) && !isGoalState(m1->index))
1145 statesConnectedInRealGraph_++;
1148 Layer *l = topLayer_;
1149 for (
size_t i = 0; i < m1->levels.size(); ++i)
1151 l->getRegion(m1->levels[i]).motionsInTree.push_back(m1->index);
1152 if (l->hasSublayers())
1153 l = l->getSublayer(m1->levels[i]);
1156 if (realGraph_.numNeighbors(m2->index) == 1 && !isStartState(m2->index) && !isGoalState(m2->index))
1158 statesConnectedInRealGraph_++;
1161 Layer *l = topLayer_;
1162 for (
size_t i = 0; i < m2->levels.size(); ++i)
1164 l->getRegion(m2->levels[i]).motionsInTree.push_back(m2->index);
1165 if (l->hasSublayers())
1166 l = l->getSublayer(m2->levels[i]);
1170 updateRegionConnectivity(m1, m2, layer->getLevel());
1173 assert(realGraph_.inSameComponent(m1->index, m2->index));
1179 updateRegionProperties(layer, r1);
1180 updateRegionProperties(layer, r2);
1183 void ompl::geometric::XXL::computeLead(Layer *layer, std::vector<int> &lead)
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");
1192 if (goalMotions_.size() == 0)
1194 const Motion *s = motions_[startMotions_[rng_.uniformInt(0, startMotions_.size() - 1)]];
1195 start = s->levels[layer->getLevel()];
1197 if (topLayer_->numRegions() == 1)
1203 end = rng_.uniformInt(0, topLayer_->numRegions() - 1);
1204 }
while (start == end);
1209 const Motion *s =
nullptr;
1210 const Motion *e =
nullptr;
1212 if (layer->getLevel() == 0)
1214 s = motions_[startMotions_[rng_.uniformInt(0, startMotions_.size() - 1)]];
1215 e = motions_[goalMotions_[rng_.uniformInt(0, goalMotions_.size() - 1)]];
1219 std::set<int> startComponents;
1220 for (
size_t i = 0; i < startMotions_.size(); ++i)
1221 startComponents.insert(realGraph_.getComponentID(startMotions_[i]));
1226 const Region ® = layer->getRegion(rng_.uniformInt(0, layer->numRegions() - 1));
1227 if (reg.motionsInTree.size())
1229 int random = rng_.uniformInt(0, reg.motionsInTree.size() - 1);
1231 int cid = realGraph_.getComponentID(reg.motionsInTree[random]);
1232 for (std::set<int>::const_iterator it = startComponents.begin(); it != startComponents.end(); ++it)
1235 s = motions_[reg.motionsInTree[random]];
1239 }
while (s ==
nullptr);
1241 std::set<int> goalComponents;
1242 for (
size_t i = 0; i < goalMotions_.size(); ++i)
1243 goalComponents.insert(realGraph_.getComponentID(goalMotions_[i]));
1248 const Region ® = layer->getRegion(rng_.uniformInt(0, layer->numRegions() - 1));
1249 if (reg.motionsInTree.size())
1251 int random = rng_.uniformInt(0, reg.motionsInTree.size() - 1);
1253 int cid = realGraph_.getComponentID(reg.motionsInTree[random]);
1254 for (std::set<int>::const_iterator it = goalComponents.begin(); it != goalComponents.end(); ++it)
1257 e = motions_[reg.motionsInTree[random]];
1261 }
while (e ==
nullptr);
1264 start = s->levels[layer->getLevel()];
1265 end = e->levels[layer->getLevel()];
1268 bool success =
false;
1278 if (rng_.uniform01() > rand_walk_rate_)
1279 success = shortestPath(start, end, lead, layer->getWeights()) && lead.size() > 0;
1281 success = randomWalk(start, end, lead) && lead.size() > 0;
1293 double p = layer->connectibleRegions() / ((double)layer->connectibleRegions() + 1);
1294 if (layer->hasSublayers() && layer->connectibleRegions() > 0 && rng_.uniform01() < p)
1297 int subregion = layer->connectibleRegion(rng_.uniformInt(0, layer->connectibleRegions() - 1));
1298 Layer *sublayer = layer->getSublayer(subregion);
1300 return searchForPath(sublayer, ptc);
1304 std::vector<int> lead;
1305 computeLead(layer, lead);
1306 layer->markLead(lead);
1309 sampleAlongLead(layer, lead, ptc);
1312 if (feasibleLead(layer, lead, ptc))
1314 std::vector<int> candidates;
1317 connectLead(layer, lead, candidates, ptc);
1318 if (constructSolutionPath())
1323 if (layer->hasSublayers())
1326 for (
size_t i = 0; i < candidates.size() && !ptc; ++i)
1328 Layer *sublayer = layer->getSublayer(candidates[i]);
1329 if (searchForPath(sublayer, ptc))
1341 if (!decomposition_)
1350 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
1355 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
1368 if (startMotions_.size() == 0)
1373 OMPL_ERROR(
"%s: No valid start states", getName().c_str());
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_);
1382 bool foundSolution =
false;
1384 while (!ptc && goalMotions_.size() == 0)
1387 while (!ptc && !foundSolution)
1390 foundSolution = searchForPath(topLayer_, ptc);
1393 if (!foundSolution && constructSolutionPath())
1395 OMPL_ERROR(
"Tripped and fell over a solution path.");
1396 foundSolution =
true;
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_);
1409 bool ompl::geometric::XXL::isStartState(
int idx)
const
1411 for (
size_t i = 0; i < startMotions_.size(); ++i)
1412 if (idx == startMotions_[i])
1417 bool ompl::geometric::XXL::isGoalState(
int idx)
const
1419 for (
size_t i = 0; i < goalMotions_.size(); ++i)
1420 if (idx == goalMotions_[i])
1425 bool ompl::geometric::XXL::constructSolutionPath()
1427 int start = startMotions_[0];
1428 std::vector<int> predecessors;
1429 std::vector<double> cost;
1430 realGraph_.dijkstra(start, predecessors, cost);
1433 double bestCost = std::numeric_limits<double>::max();
1435 for (
size_t i = 0; i < goalMotions_.size(); ++i)
1437 if (cost[goalMotions_[i]] < bestCost)
1439 bestCost = cost[goalMotions_[i]];
1440 bestGoal = goalMotions_[i];
1447 std::vector<Motion *> slnPath;
1449 while (predecessors[v] != v)
1451 slnPath.push_back(motions_[v]);
1452 v = predecessors[v];
1454 slnPath.push_back(motions_[v]);
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());
1467 for (
size_t i = 0; i < motions_.size(); ++i)
1469 bool isStart =
false;
1470 bool isGoal =
false;
1472 for (
size_t j = 0; j < startMotions_.size(); ++j)
1473 if (startMotions_[j] == (
int)i)
1476 for (
size_t j = 0; j < goalMotions_.size(); ++j)
1477 if (goalMotions_[j] == (
int)i)
1480 if (!isStart && !isGoal)
1489 for (
size_t i = 0; i < motions_.size(); ++i)
1491 std::vector<std::pair<int, double>> nbrs;
1492 realGraph_.getNeighbors(i, nbrs);
1494 for (
size_t j = 0; j < nbrs.size(); ++j)
1499 void ompl::geometric::XXL::getGoalStates()
1503 while (pis_.haveMoreGoalStates() && newCount < maxCount )
1511 double dist = std::numeric_limits<double>::infinity();
1512 for (
size_t i = 0; i < goalMotions_.size(); ++i)
1514 double d = si_->distance(motions_[goalMotions_[i]]->state, st);
1523 OMPL_WARN(
"XXL: Rejecting goal state that is %f from another goal", dist);
1527 void ompl::geometric::XXL::getGoalStates(
const base::PlannerTerminationCondition & )
1561 void ompl::geometric::XXL::getNeighbors(
int rid,
const std::vector<double> &weights,
1562 std::vector<std::pair<int, double>> &neighbors)
const
1564 std::vector<int> nbrs;
1565 decomposition_->getNeighbors(rid, nbrs);
1567 for (
size_t i = 0; i < nbrs.size(); ++i)
1569 neighbors.push_back(std::make_pair(nbrs[i], weights[nbrs[i]]));
1577 double g{0.0}, h{std::numeric_limits<double>::infinity()};
1579 OpenListNode(
int _id) : id(_id)
1583 bool operator<(
const OpenListNode &other)
const
1585 return (g + h) > (other.g + other.h);
1591 bool ompl::geometric::XXL::shortestPath(
int r1,
int r2, std::vector<int> &path,
const std::vector<double> &weights)
1593 if (r1 < 0 || r1 >= decomposition_->getNumRegions())
1595 OMPL_ERROR(
"Start region (%d) is not valid", r1);
1599 if (r2 < 0 || r2 >= decomposition_->getNumRegions())
1601 OMPL_ERROR(
"Goal region (%d) is not valid", r2);
1606 double weight = 1.0;
1607 if (rng_.uniform01() < 0.50)
1609 if (rng_.uniform01() < 0.50)
1616 std::fill(predecessors_.begin(), predecessors_.end(), -1);
1617 std::fill(closedList_.begin(), closedList_.end(),
false);
1620 std::priority_queue<OpenListNode> openList;
1623 OpenListNode start(r1);
1625 start.h = weight * decomposition_->distanceHeuristic(r1, r2);
1627 openList.push(start);
1630 bool solution =
false;
1631 while (!openList.empty())
1633 OpenListNode node = openList.top();
1637 if (closedList_[node.id])
1641 closedList_[node.id] =
true;
1642 predecessors_[node.id] = node.parent;
1652 std::vector<std::pair<int, double>> neighbors;
1653 getNeighbors(node.id, weights, neighbors);
1656 rng_.shuffle(neighbors.begin(), neighbors.end());
1657 for (
size_t i = 0; i < neighbors.size(); ++i)
1660 if (!closedList_[neighbors[i].first])
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;
1676 while (predecessors_[current] != current)
1678 path.insert(path.begin(), current);
1679 current = predecessors_[current];
1682 path.insert(path.begin(), current);
1688 bool ompl::geometric::XXL::randomWalk(
int r1,
int r2, std::vector<int> &path)
1691 std::fill(predecessors_.begin(), predecessors_.end(), -1);
1692 std::fill(closedList_.begin(), closedList_.end(),
false);
1694 closedList_[r1] =
true;
1695 for (
int i = 0; i < decomposition_->getNumRegions(); ++i)
1699 while (!closedList_[u])
1701 std::vector<int> neighbors;
1702 decomposition_->getNeighbors(u, neighbors);
1703 int nbr = neighbors[rng_.uniformInt(0, neighbors.size() - 1)];
1705 predecessors_[u] = nbr;
1711 while (!closedList_[u])
1713 closedList_[u] =
true;
1714 u = predecessors_[u];
1720 while (predecessors_[current] != -1)
1722 path.insert(path.begin(), current);
1723 current = predecessors_[current];
1725 if ((
int)path.size() >= decomposition_->getNumRegions())
1728 path.insert(path.begin(), current);
1730 if (path.front() != r1)
1732 if (path.back() != r2)
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
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...
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...
SpaceInformationPtr si_
The space information for which planning is done.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
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...
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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.