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 
45 ompl::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 
51 ompl::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 
59 ompl::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 
93 void 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 
140 void 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
151 void 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 
165 void 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 
315 ompl::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 
326 int 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 
372 int ompl::geometric::XXL::addState(const base::State *state)
373 {
374  return addThisState(si_->cloneState(state));
375 }
376 
377 int 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 
418 int 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 
439 void 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 
471 void 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 
485 void 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 
523 bool ompl::geometric::XXL::sampleAlongLead(Layer *layer, const std::vector<int> &lead,
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 
609 int 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
676 int 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
759 bool ompl::geometric::XXL::feasibleLead(Layer *layer, const std::vector<int> &lead,
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
847 bool ompl::geometric::XXL::connectLead(Layer *layer, const std::vector<int> &lead, std::vector<int> &candidateRegions,
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
1012 void 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 
1095 void 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 
1183 void 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 
1409 bool 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 
1417 bool 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 
1425 bool 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 
1499 void 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 
1527 void 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 
1561 void 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 
1573 struct 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
1591 bool 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 
1688 bool 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 }
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
A shared pointer wrapper for ompl::base::SpaceInformation.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: XXL.cpp:65
Definition of an abstract state.
Definition: State.h:113
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future....
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: XXL.cpp:109
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
@ INVALID_GOAL
Invalid goal state.
A class to store the exit status of Planner::solve()
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Base class for a PlannerData edge.
Definition: PlannerData.h:190
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
@ EXACT_SOLUTION
The planner found an exact solution.
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
Definition: XXL.cpp:1288
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
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...
A shared pointer wrapper for ompl::base::StateSpace.
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_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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 addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:78
@ INVALID_START
Invalid start state or no start state specified.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
A shared pointer wrapper for ompl::geometric::XXLDecomposition.